1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2017-2022, STMicroelectronics
4 */
5
6 #include <arm.h>
7 #include <boot_api.h>
8 #include <console.h>
9 #include <drivers/clk.h>
10 #include <drivers/rstctrl.h>
11 #include <drivers/stm32mp1_pmic.h>
12 #include <drivers/stm32mp1_rcc.h>
13 #include <drivers/stpmic1.h>
14 #include <drivers/stm32mp_dt_bindings.h>
15 #include <io.h>
16 #include <kernel/cache_helpers.h>
17 #include <kernel/delay.h>
18 #include <kernel/boot.h>
19 #include <kernel/interrupt.h>
20 #include <kernel/misc.h>
21 #include <kernel/panic.h>
22 #include <kernel/spinlock.h>
23 #include <mm/core_memprot.h>
24 #include <platform_config.h>
25 #include <sm/psci.h>
26 #include <sm/std_smc.h>
27 #include <stm32_util.h>
28 #include <trace.h>
29
30 #define CONSOLE_FLUSH_DELAY_MS 10
31
32 /*
33 * SMP boot support and access to the mailbox
34 */
35
36 enum core_state_id {
37 CORE_OFF = 0,
38 CORE_RET,
39 CORE_AWAKE,
40 CORE_ON,
41 };
42
43 static enum core_state_id core_state[CFG_TEE_CORE_NB_CORE];
44 static unsigned int __maybe_unused state_lock = SPINLOCK_UNLOCK;
45
lock_state_access(void)46 static uint32_t __maybe_unused lock_state_access(void)
47 {
48 return may_spin_lock(&state_lock);
49 }
50
unlock_state_access(uint32_t exceptions)51 static void __maybe_unused unlock_state_access(uint32_t exceptions)
52 {
53 may_spin_unlock(&state_lock, exceptions);
54 }
55
psci_affinity_info(uint32_t affinity,uint32_t lowest_affinity_level)56 int psci_affinity_info(uint32_t affinity, uint32_t lowest_affinity_level)
57 {
58 unsigned int pos = get_core_pos_mpidr(affinity);
59
60 DMSG("core %zu, state %u", pos, core_state[pos]);
61
62 if ((pos >= CFG_TEE_CORE_NB_CORE) ||
63 (lowest_affinity_level > PSCI_AFFINITY_LEVEL_ON)) {
64 return PSCI_RET_INVALID_PARAMETERS;
65 }
66
67 switch (core_state[pos]) {
68 case CORE_OFF:
69 case CORE_RET:
70 return PSCI_AFFINITY_LEVEL_OFF;
71 case CORE_AWAKE:
72 return PSCI_AFFINITY_LEVEL_ON_PENDING;
73 case CORE_ON:
74 return PSCI_AFFINITY_LEVEL_ON;
75 default:
76 panic();
77 }
78 }
79
80 #if CFG_TEE_CORE_NB_CORE == 1
81 /*
82 * Function called when a CPU is booted through the OP-TEE.
83 * All cores shall register when online.
84 */
stm32mp_register_online_cpu(void)85 void stm32mp_register_online_cpu(void)
86 {
87 assert(core_state[0] == CORE_OFF);
88 core_state[0] = CORE_ON;
89 }
90 #else
stm32_pm_cpu_power_down_wfi(void)91 static void __noreturn stm32_pm_cpu_power_down_wfi(void)
92 {
93 dcache_op_level1(DCACHE_OP_CLEAN);
94
95 io_write32(stm32_rcc_base() + RCC_MP_GRSTCSETR,
96 RCC_MP_GRSTCSETR_MPUP1RST);
97
98 dsb();
99 isb();
100 wfi();
101 panic();
102 }
103
stm32mp_register_online_cpu(void)104 void stm32mp_register_online_cpu(void)
105 {
106 size_t pos = get_core_pos();
107 uint32_t exceptions = lock_state_access();
108
109 if (pos == 0) {
110 assert(core_state[pos] == CORE_OFF);
111 } else {
112 if (core_state[pos] != CORE_AWAKE) {
113 core_state[pos] = CORE_OFF;
114 unlock_state_access(exceptions);
115 stm32_pm_cpu_power_down_wfi();
116 panic();
117 }
118 clk_disable(stm32mp_rcc_clock_id_to_clk(RTCAPB));
119 }
120
121 core_state[pos] = CORE_ON;
122 unlock_state_access(exceptions);
123 }
124
125 #define GICD_SGIR 0xF00
raise_sgi0_as_secure(void)126 static void raise_sgi0_as_secure(void)
127 {
128 dsb_ishst();
129 io_write32(get_gicd_base() + GICD_SGIR,
130 GIC_NON_SEC_SGI_0 | SHIFT_U32(TARGET_CPU1_GIC_MASK, 16));
131 }
132
release_secondary_early_hpen(size_t __unused pos)133 static void release_secondary_early_hpen(size_t __unused pos)
134 {
135 /* Need to send SIG#0 over Group0 after individual core 1 reset */
136 raise_sgi0_as_secure();
137 udelay(20);
138
139 io_write32(stm32mp_bkpreg(BCKR_CORE1_BRANCH_ADDRESS),
140 TEE_LOAD_ADDR);
141 io_write32(stm32mp_bkpreg(BCKR_CORE1_MAGIC_NUMBER),
142 BOOT_API_A7_CORE1_MAGIC_NUMBER);
143
144 dsb_ishst();
145 itr_raise_sgi(GIC_SEC_SGI_0, TARGET_CPU1_GIC_MASK);
146 }
147
148 /* Override default psci_cpu_on() with platform specific sequence */
psci_cpu_on(uint32_t core_id,uint32_t entry,uint32_t context_id)149 int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id)
150 {
151 size_t pos = get_core_pos_mpidr(core_id);
152 uint32_t exceptions = 0;
153 int rc = 0;
154
155 if (!pos || pos >= CFG_TEE_CORE_NB_CORE)
156 return PSCI_RET_INVALID_PARAMETERS;
157
158 DMSG("core %zu, ns_entry 0x%" PRIx32 ", state %u",
159 pos, entry, core_state[pos]);
160
161 exceptions = lock_state_access();
162
163 switch (core_state[pos]) {
164 case CORE_ON:
165 rc = PSCI_RET_ALREADY_ON;
166 break;
167 case CORE_AWAKE:
168 rc = PSCI_RET_ON_PENDING;
169 break;
170 case CORE_RET:
171 rc = PSCI_RET_DENIED;
172 break;
173 case CORE_OFF:
174 core_state[pos] = CORE_AWAKE;
175 rc = PSCI_RET_SUCCESS;
176 break;
177 default:
178 panic();
179 }
180
181 unlock_state_access(exceptions);
182
183 if (rc == PSCI_RET_SUCCESS) {
184 boot_set_core_ns_entry(pos, entry, context_id);
185 release_secondary_early_hpen(pos);
186 }
187
188 return rc;
189 }
190
191 /* Override default psci_cpu_off() with platform specific sequence */
psci_cpu_off(void)192 int psci_cpu_off(void)
193 {
194 unsigned int pos = get_core_pos();
195 uint32_t exceptions = 0;
196
197 if (pos == 0) {
198 EMSG("PSCI_CPU_OFF not supported for core #0");
199 return PSCI_RET_INTERNAL_FAILURE;
200 }
201
202 DMSG("core %u", pos);
203
204 exceptions = lock_state_access();
205
206 assert(core_state[pos] == CORE_ON);
207 core_state[pos] = CORE_OFF;
208
209 unlock_state_access(exceptions);
210
211 /* Enable BKPREG access for the disabled CPU */
212 if (clk_enable(stm32mp_rcc_clock_id_to_clk(RTCAPB)))
213 panic();
214
215 thread_mask_exceptions(THREAD_EXCP_ALL);
216 stm32_pm_cpu_power_down_wfi();
217 panic();
218 }
219 #endif
220
221 /* Override default psci_system_off() with platform specific sequence */
psci_system_off(void)222 void __noreturn psci_system_off(void)
223 {
224 DMSG("core %u", get_core_pos());
225
226 if (TRACE_LEVEL >= TRACE_DEBUG) {
227 console_flush();
228 mdelay(CONSOLE_FLUSH_DELAY_MS);
229 }
230
231 if (stm32mp_with_pmic()) {
232 stm32mp_get_pmic();
233 stpmic1_switch_off();
234 udelay(100);
235 }
236
237 panic();
238 }
239
240 /* Override default psci_system_reset() with platform specific sequence */
psci_system_reset(void)241 void __noreturn psci_system_reset(void)
242 {
243 rstctrl_assert(stm32mp_rcc_reset_id_to_rstctrl(MPSYST_R));
244 udelay(100);
245 panic();
246 }
247
248 /* Override default psci_cpu_on() with platform supported features */
psci_features(uint32_t psci_fid)249 int psci_features(uint32_t psci_fid)
250 {
251 switch (psci_fid) {
252 case ARM_SMCCC_VERSION:
253 case PSCI_PSCI_FEATURES:
254 case PSCI_SYSTEM_RESET:
255 case PSCI_VERSION:
256 return PSCI_RET_SUCCESS;
257 case PSCI_CPU_ON:
258 case PSCI_CPU_OFF:
259 if (CFG_TEE_CORE_NB_CORE > 1)
260 return PSCI_RET_SUCCESS;
261 return PSCI_RET_NOT_SUPPORTED;
262 case PSCI_SYSTEM_OFF:
263 if (stm32mp_with_pmic())
264 return PSCI_RET_SUCCESS;
265 return PSCI_RET_NOT_SUPPORTED;
266 default:
267 return PSCI_RET_NOT_SUPPORTED;
268 }
269 }
270
271 /* Override default psci_version() to enable PSCI_VERSION_1_0 API */
psci_version(void)272 uint32_t psci_version(void)
273 {
274 return PSCI_VERSION_1_0;
275 }
276