1 // © 2021 Qualcomm Innovation Center, Inc. All rights reserved.
2 //
3 // SPDX-License-Identifier: BSD-3-Clause
4
5 #include <assert.h>
6 #include <hyptypes.h>
7
8 #include <compiler.h>
9 #include <cpulocal.h>
10 #include <idle.h>
11 #include <panic.h>
12 #include <partition.h>
13 #include <platform_cpu.h>
14 #include <platform_psci.h>
15 #include <preempt.h>
16 #include <psci.h>
17 #include <thread.h>
18 #include <util.h>
19
20 #include "event_handlers.h"
21 #include "psci_smc.h"
22
23 // The entry points are really functions, but we don't use function types for
24 // them because they are never directly called from C, and using function types
25 // here would force us to break MISRA required rule 11.1 in platform_cpu_on().
26 extern const char soc_qemu_entry_cold_secondary;
27 extern const char soc_qemu_entry_warm;
28
29 CPULOCAL_DECLARE_STATIC(bool, cpu_started);
30
31 void
soc_qemu_handle_boot_cpu_cold_init(cpu_index_t cpu)32 soc_qemu_handle_boot_cpu_cold_init(cpu_index_t cpu)
33 {
34 CPULOCAL_BY_INDEX(cpu_started, cpu) = true;
35 }
36
37 bool
platform_cpu_exists(cpu_index_t cpu)38 platform_cpu_exists(cpu_index_t cpu)
39 {
40 assert(cpu < PLATFORM_MAX_CORES);
41
42 return compiler_expected((util_bit(cpu) & PLATFORM_USABLE_CORES) != 0U);
43 }
44
45 error_t
platform_cpu_on(cpu_index_t cpu)46 platform_cpu_on(cpu_index_t cpu)
47 {
48 MPIDR_EL1_t mpidr = platform_cpu_index_to_mpidr(cpu);
49 thread_t *thread = idle_thread_for(cpu);
50 uintptr_t entry_virt =
51 CPULOCAL_BY_INDEX(cpu_started, cpu)
52 ? (uintptr_t)&soc_qemu_entry_warm
53 : (uintptr_t)&soc_qemu_entry_cold_secondary;
54 psci_mpidr_t psci_mpidr = psci_mpidr_default();
55 psci_mpidr_set_Aff0(&psci_mpidr, MPIDR_EL1_get_Aff0(&mpidr));
56 psci_mpidr_set_Aff1(&psci_mpidr, MPIDR_EL1_get_Aff1(&mpidr));
57 psci_mpidr_set_Aff2(&psci_mpidr, MPIDR_EL1_get_Aff2(&mpidr));
58 psci_mpidr_set_Aff3(&psci_mpidr, MPIDR_EL1_get_Aff3(&mpidr));
59 return psci_smc_cpu_on(psci_mpidr,
60 partition_virt_to_phys(partition_get_private(),
61 entry_virt),
62 (uintptr_t)thread);
63 }
64
65 static noreturn register_t
psci_smc_system_reset_arg(register_t unused)66 psci_smc_system_reset_arg(register_t unused)
67 {
68 (void)unused;
69
70 psci_smc_system_reset();
71
72 panic("psci_smc_system_reset failed!");
73 }
74
75 void
platform_system_reset(void)76 platform_system_reset(void)
77 {
78 thread_freeze(psci_smc_system_reset_arg, 0, 0);
79 }
80
81 static noreturn register_t
psci_smc_cpu_off_arg(register_t unused)82 psci_smc_cpu_off_arg(register_t unused)
83 {
84 (void)unused;
85
86 psci_smc_cpu_off();
87
88 panic("psci_smc_cpu_off failed!");
89 }
90
91 void
platform_cpu_off(void)92 platform_cpu_off(void)
93 {
94 assert(idle_is_current());
95
96 thread_freeze(psci_smc_cpu_off_arg, 0U, 0U);
97 }
98
99 static register_t
psci_smc_cpu_suspend_arg(register_t power_state)100 psci_smc_cpu_suspend_arg(register_t power_state) REQUIRE_PREEMPT_DISABLED
101 {
102 thread_t *idle = idle_thread();
103
104 paddr_t entry_phys = partition_virt_to_phys(
105 partition_get_private(), (uintptr_t)&soc_qemu_entry_warm);
106
107 error_t ret =
108 psci_smc_cpu_suspend(power_state, entry_phys, (register_t)idle);
109
110 return (register_t)ret;
111 }
112
113 bool_result_t
platform_cpu_suspend(psci_suspend_powerstate_t power_state)114 platform_cpu_suspend(psci_suspend_powerstate_t power_state)
115 {
116 register_t ret;
117
118 assert(idle_is_current());
119
120 ret = thread_freeze(psci_smc_cpu_suspend_arg,
121 psci_suspend_powerstate_raw(power_state), ~0UL);
122
123 return (ret == 0UL) ? bool_result_ok(false)
124 : (ret == ~0UL) ? bool_result_ok(true)
125 : bool_result_error((error_t)ret);
126 }
127
128 error_t
platform_psci_set_suspend_mode(psci_mode_t mode)129 platform_psci_set_suspend_mode(psci_mode_t mode)
130 {
131 return psci_smc_psci_set_suspend_mode(mode);
132 }
133
134 #if defined(PLATFORM_PSCI_DEFAULT_SUSPEND)
135 static register_t
psci_smc_cpu_default_suspend_arg(register_t unused)136 psci_smc_cpu_default_suspend_arg(register_t unused)
137 {
138 (void)unused;
139
140 thread_t *idle = idle_thread();
141
142 paddr_t entry_phys = partition_virt_to_phys(
143 partition_get_private(), (uintptr_t)&soc_qemu_entry_warm);
144
145 error_t ret =
146 psci_smc_cpu_default_suspend(entry_phys, (register_t)idle);
147
148 return (register_t)ret;
149 }
150
151 bool_result_t
platform_cpu_default_suspend(void)152 platform_cpu_default_suspend(void)
153 {
154 register_t ret;
155
156 assert(idle_is_current());
157 ret = thread_freeze(psci_smc_cpu_default_suspend_arg, 0UL, ~0UL);
158
159 return (ret == 0UL) ? bool_result_ok(false)
160 : (ret == ~0UL) ? bool_result_ok(true)
161 : bool_result_error((error_t)ret);
162 }
163 #endif
164