1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2021, Microchip
4 */
5
6 #include <assert.h>
7 #include <at91_clk.h>
8 #include <drivers/atmel_shdwc.h>
9 #include <drivers/pm/sam/atmel_pm.h>
10 #include <drivers/sam/at91_ddr.h>
11 #include <io.h>
12 #include <kernel/boot.h>
13 #include <kernel/dt.h>
14 #include <kernel/pm.h>
15 #include <libfdt.h>
16 #include <matrix.h>
17 #include <mm/core_memprot.h>
18 #include <smc_ids.h>
19 #include <sm/pm.h>
20 #include <stdbool.h>
21 #include <tee_api_types.h>
22
23 #include "at91_pm.h"
24
25 #if CFG_ATMEL_PM_SUSPEND_MODE < AT91_PM_STANDBY || \
26 CFG_ATMEL_PM_SUSPEND_MODE > AT91_PM_BACKUP
27 #error Invalid suspend mode, please check CFG_ATMEL_PM_SUSPEND_MODE
28 #endif
29
30 #define AT91_SECUMOD_SYSR 0x04
31 #define AT91_SECUMOD_RAMRDY 0x14
32 #define AT91_SECUMOD_RAMRDY_READY BIT(0)
33
34 static struct at91_pm_data soc_pm;
35
36 /* Backup canary */
37 static uint32_t canary = 0xA5A5A5A5;
38
39 /* Backup mode information used by at91bootstrap */
40 static struct at91bootstrap_bu {
41 uint32_t suspended;
42 uint32_t reserved;
43 uint32_t *canary;
44 uint32_t resume;
45 } *at91bootstrap_bu;
46
47 static vaddr_t at91_suspend_sram_base;
48 static void (*at91_suspend_sram_fn)(struct at91_pm_data *);
49
at91_pm_set_suspend_mode(struct thread_smc_args * args)50 enum sm_handler_ret at91_pm_set_suspend_mode(struct thread_smc_args *args)
51 {
52 unsigned int mode = args->a1;
53
54 /*
55 * We don't expect this function to be called simultaneously while we
56 * are entering suspend/resume function. On sama5d2, this is not a
57 * problem since this SoC is a single core one but in order to prevent
58 * any other SoC support to be added without handling this concurrency,
59 * check that we are compiled for a single core.
60 */
61 COMPILE_TIME_ASSERT(CFG_TEE_CORE_NB_CORE == 1);
62
63 if (mode > AT91_PM_BACKUP) {
64 args->a0 = SAMA5_SMC_SIP_RETURN_EINVAL;
65 return SM_HANDLER_SMC_HANDLED;
66 }
67 DMSG("Setting suspend mode to %u", mode);
68
69 args->a0 = SAMA5_SMC_SIP_RETURN_SUCCESS;
70 soc_pm.mode = mode;
71
72 return SM_HANDLER_SMC_HANDLED;
73 }
74
at91_pm_get_suspend_mode(struct thread_smc_args * args)75 enum sm_handler_ret at91_pm_get_suspend_mode(struct thread_smc_args *args)
76 {
77 args->a1 = soc_pm.mode;
78 args->a0 = SAMA5_SMC_SIP_RETURN_SUCCESS;
79
80 return SM_HANDLER_SMC_HANDLED;
81 }
82
at91_pm_copy_suspend_to_sram(void)83 static void at91_pm_copy_suspend_to_sram(void)
84 {
85 memcpy((void *)at91_suspend_sram_base, &at91_pm_suspend_in_sram,
86 at91_pm_suspend_in_sram_sz);
87
88 cache_op_inner(DCACHE_AREA_CLEAN, (void *)at91_suspend_sram_base,
89 at91_pm_suspend_in_sram_sz);
90 cache_op_inner(ICACHE_AREA_INVALIDATE, at91_suspend_sram_fn,
91 at91_pm_suspend_in_sram_sz);
92 }
93
atmel_pm_cpu_idle(void)94 void atmel_pm_cpu_idle(void)
95 {
96 uint32_t lpr0 = 0;
97 uint32_t saved_lpr0 = 0;
98
99 saved_lpr0 = io_read32(soc_pm.ramc + AT91_DDRSDRC_LPR);
100 lpr0 = saved_lpr0 & ~AT91_DDRSDRC_LPCB;
101 lpr0 |= AT91_DDRSDRC_LPCB_POWER_DOWN;
102
103 io_write32(soc_pm.ramc + AT91_DDRSDRC_LPR, lpr0);
104
105 cpu_idle();
106
107 io_write32(soc_pm.ramc + AT91_DDRSDRC_LPR, saved_lpr0);
108 }
109
at91_sama5d2_config_shdwc_ws(vaddr_t shdwc,uint32_t * mode,uint32_t * polarity)110 static void at91_sama5d2_config_shdwc_ws(vaddr_t shdwc, uint32_t *mode,
111 uint32_t *polarity)
112 {
113 uint32_t val = 0;
114
115 /* SHDWC.WUIR */
116 val = io_read32(shdwc + AT91_SHDW_WUIR);
117 *mode |= val & AT91_SHDW_WKUPEN_MASK;
118 *polarity |= (val >> AT91_SHDW_WKUPT_SHIFT) & AT91_SHDW_WKUPT_MASK;
119 }
120
at91_sama5d2_config_pmc_ws(vaddr_t pmc,uint32_t mode,uint32_t polarity)121 static int at91_sama5d2_config_pmc_ws(vaddr_t pmc, uint32_t mode,
122 uint32_t polarity)
123 {
124 io_write32(pmc + AT91_PMC_FSMR, mode);
125 io_write32(pmc + AT91_PMC_FSPR, polarity);
126
127 return 0;
128 }
129
130 struct wakeup_source_info {
131 unsigned int pmc_fsmr_bit;
132 unsigned int shdwc_mr_bit;
133 bool set_polarity;
134 };
135
136 static const struct wakeup_source_info ws_info[] = {
137 { .pmc_fsmr_bit = AT91_PMC_FSTT(10), .set_polarity = true },
138 { .pmc_fsmr_bit = AT91_PMC_RTCAL, .shdwc_mr_bit = BIT(17) },
139 { .pmc_fsmr_bit = AT91_PMC_USBAL },
140 { .pmc_fsmr_bit = AT91_PMC_SDMMC_CD },
141 };
142
143 struct wakeup_src {
144 const char *compatible;
145 const struct wakeup_source_info *info;
146 };
147
148 static const struct wakeup_src sama5d2_ws_ids[] = {
149 { .compatible = "atmel,sama5d2-gem", .info = &ws_info[0] },
150 { .compatible = "atmel,at91rm9200-rtc", .info = &ws_info[1] },
151 { .compatible = "atmel,sama5d3-udc", .info = &ws_info[2] },
152 { .compatible = "atmel,at91rm9200-ohci", .info = &ws_info[2] },
153 { .compatible = "usb-ohci", .info = &ws_info[2] },
154 { .compatible = "atmel,at91sam9g45-ehci", .info = &ws_info[2] },
155 { .compatible = "usb-ehci", .info = &ws_info[2] },
156 { .compatible = "atmel,sama5d2-sdhci", .info = &ws_info[3] }
157 };
158
dev_is_wakeup_source(const void * fdt,int node)159 static bool dev_is_wakeup_source(const void *fdt, int node)
160 {
161 return fdt_get_property(fdt, node, "wakeup-source", NULL);
162 }
163
at91_pm_config_ws_ulp1(bool set)164 static int at91_pm_config_ws_ulp1(bool set)
165 {
166 const struct wakeup_source_info *wsi = NULL;
167 const struct wakeup_src *wsrc = NULL;
168 unsigned int polarity = 0;
169 unsigned int mode = 0;
170 unsigned int val = 0;
171 unsigned int src = 0;
172 int node = 0;
173
174 if (!set) {
175 io_write32(soc_pm.pmc + AT91_PMC_FSMR, mode);
176 return TEE_SUCCESS;
177 }
178
179 at91_sama5d2_config_shdwc_ws(soc_pm.shdwc, &mode, &polarity);
180
181 val = io_read32(soc_pm.shdwc + AT91_SHDW_MR);
182
183 /* Loop through defined wakeup sources. */
184 for (src = 0; src < ARRAY_SIZE(sama5d2_ws_ids); src++) {
185 wsrc = &sama5d2_ws_ids[src];
186 wsi = wsrc->info;
187
188 node = fdt_node_offset_by_compatible(soc_pm.fdt, -1,
189 wsrc->compatible);
190 while (node >= 0) {
191 if (dev_is_wakeup_source(soc_pm.fdt, node)) {
192 /* Check if enabled on SHDWC. */
193 if (wsi->shdwc_mr_bit &&
194 !(val & wsi->shdwc_mr_bit))
195 goto next_node;
196
197 mode |= wsi->pmc_fsmr_bit;
198 if (wsi->set_polarity)
199 polarity |= wsi->pmc_fsmr_bit;
200 }
201 next_node:
202 node = fdt_node_offset_by_compatible(soc_pm.fdt, node,
203 wsrc->compatible);
204 }
205 }
206
207 if (!mode) {
208 EMSG("AT91: PM: no ULP1 wakeup sources found!");
209 return TEE_ERROR_BAD_STATE;
210 }
211
212 at91_sama5d2_config_pmc_ws(soc_pm.pmc, mode, polarity);
213
214 return TEE_SUCCESS;
215 }
216
217 /*
218 * Verify that all the clocks are correct before entering
219 * slow-clock mode.
220 */
at91_pm_verify_clocks(void)221 static bool at91_pm_verify_clocks(void)
222 {
223 int i = 0;
224 uint32_t scsr = 0;
225
226 scsr = io_read32(soc_pm.pmc + AT91_PMC_SCSR);
227
228 /* USB must not be using PLLB */
229 if ((scsr & (AT91SAM926x_PMC_UHP | AT91SAM926x_PMC_UDP)) != 0) {
230 EMSG("AT91: PM - Suspend-to-RAM with USB still active");
231 return false;
232 }
233
234 /* PCK0..PCK3 must be disabled, or configured to use clk32k */
235 for (i = 0; i < 4; i++) {
236 uint32_t css = 0;
237
238 if ((scsr & (AT91_PMC_PCK0 << i)) == 0)
239 continue;
240 css = io_read32(soc_pm.pmc + AT91_PMC_PCKR(i)) & AT91_PMC_CSS;
241 if (css != AT91_PMC_CSS_SLOW) {
242 EMSG("AT91: PM - Suspend-to-RAM with PCK%d src %"PRId32,
243 i, css);
244 return false;
245 }
246 }
247
248 return true;
249 }
250
at91_write_backup_data(void)251 static TEE_Result at91_write_backup_data(void)
252 {
253 uint32_t val = 0;
254
255 while (true) {
256 val = io_read32(soc_pm.secumod + AT91_SECUMOD_RAMRDY);
257 if (val & AT91_SECUMOD_RAMRDY_READY)
258 break;
259 }
260
261 io_write32((vaddr_t)&at91bootstrap_bu->suspended, 1);
262 io_write32((vaddr_t)&at91bootstrap_bu->canary, virt_to_phys(&canary));
263 io_write32((vaddr_t)&at91bootstrap_bu->resume,
264 virt_to_phys((void *)(vaddr_t)at91_pm_cpu_resume));
265
266 return TEE_SUCCESS;
267 }
268
at91_enter_backup(void)269 static TEE_Result at91_enter_backup(void)
270 {
271 int ret = -1;
272 TEE_Result res = TEE_ERROR_GENERIC;
273
274 res = at91_write_backup_data();
275 if (res)
276 return res;
277
278 pm_change_state(PM_OP_SUSPEND, 0);
279 ret = sm_pm_cpu_suspend((uint32_t)&soc_pm,
280 (void *)at91_suspend_sram_fn);
281 if (ret < 0) {
282 DMSG("Suspend failed");
283 res = TEE_ERROR_BAD_STATE;
284 } else {
285 res = TEE_SUCCESS;
286 }
287
288 pm_change_state(PM_OP_RESUME, 0);
289 if (res)
290 return res;
291
292 /* SRAM content is lost after resume */
293 at91_pm_copy_suspend_to_sram();
294
295 return TEE_SUCCESS;
296 }
297
atmel_pm_suspend(uintptr_t entry,struct sm_nsec_ctx * nsec)298 TEE_Result atmel_pm_suspend(uintptr_t entry, struct sm_nsec_ctx *nsec)
299 {
300 TEE_Result res = TEE_ERROR_GENERIC;
301
302 DMSG("Entering suspend mode %d", soc_pm.mode);
303
304 if (soc_pm.mode >= AT91_PM_ULP0) {
305 if (!at91_pm_verify_clocks())
306 return TEE_ERROR_BAD_STATE;
307 }
308
309 if (soc_pm.mode == AT91_PM_ULP1)
310 at91_pm_config_ws_ulp1(true);
311
312 sm_save_unbanked_regs(&nsec->ub_regs);
313
314 if (soc_pm.mode == AT91_PM_BACKUP) {
315 res = at91_enter_backup();
316 } else {
317 at91_suspend_sram_fn(&soc_pm);
318 res = TEE_SUCCESS;
319 }
320
321 if (soc_pm.mode == AT91_PM_ULP1)
322 at91_pm_config_ws_ulp1(false);
323
324 sm_restore_unbanked_regs(&nsec->ub_regs);
325
326 /*
327 * If the system went to backup mode, register state was lost and must
328 * be restored by jumping to the user provided entry point
329 */
330 if (res == TEE_SUCCESS && soc_pm.mode == AT91_PM_BACKUP)
331 nsec->mon_lr = entry;
332
333 DMSG("Exiting suspend mode %d, res %"PRIx32, soc_pm.mode, res);
334
335 return res;
336 }
337
at91_pm_dt_dram_init(const void * fdt)338 static TEE_Result at91_pm_dt_dram_init(const void *fdt)
339 {
340 int node = -1;
341 size_t size = 0;
342
343 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d3-ddramc");
344 if (node < 0)
345 return TEE_ERROR_ITEM_NOT_FOUND;
346
347 if (dt_map_dev(fdt, node, &soc_pm.ramc, &size, DT_MAP_AUTO) < 0)
348 return TEE_ERROR_GENERIC;
349
350 return TEE_SUCCESS;
351 }
352
at91_pm_backup_init(const void * fdt)353 static TEE_Result at91_pm_backup_init(const void *fdt)
354 {
355 int node = -1;
356 size_t size = 0;
357
358 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sfrbu");
359 if (node < 0)
360 return TEE_ERROR_ITEM_NOT_FOUND;
361
362 if (dt_map_dev(fdt, node, &soc_pm.sfrbu, &size, DT_MAP_AUTO) < 0)
363 return TEE_ERROR_GENERIC;
364
365 if (_fdt_get_status(fdt, node) == DT_STATUS_OK_SEC)
366 matrix_configure_periph_secure(AT91C_ID_SFRBU);
367
368 return TEE_SUCCESS;
369 }
370
at91_pm_sram_init(const void * fdt)371 static TEE_Result at91_pm_sram_init(const void *fdt)
372 {
373 int node = -1;
374 size_t size = 0;
375 paddr_t at91_suspend_sram_pbase;
376 size_t suspend_sz = at91_pm_suspend_in_sram_sz;
377
378 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sram");
379 if (node < 0)
380 return TEE_ERROR_ITEM_NOT_FOUND;
381
382 if (_fdt_get_status(fdt, node) != DT_STATUS_OK_SEC)
383 return TEE_ERROR_GENERIC;
384
385 if (dt_map_dev(fdt, node, &at91_suspend_sram_base, &size,
386 DT_MAP_AUTO) < 0)
387 return TEE_ERROR_GENERIC;
388
389 at91_suspend_sram_pbase = virt_to_phys((void *)at91_suspend_sram_base);
390
391 /* Map the secure ram suspend code to be executable */
392 at91_suspend_sram_fn = core_mmu_add_mapping(MEM_AREA_TEE_RAM,
393 at91_suspend_sram_pbase,
394 suspend_sz);
395 if (!at91_suspend_sram_fn) {
396 EMSG("Failed to remap sram as executable");
397 return TEE_ERROR_GENERIC;
398 }
399
400 at91_pm_copy_suspend_to_sram();
401
402 return TEE_SUCCESS;
403 }
404
at91_securam_init(const void * fdt)405 static TEE_Result at91_securam_init(const void *fdt)
406 {
407 int node = -1;
408 size_t size = 0;
409 struct clk *clk = NULL;
410 TEE_Result res = TEE_ERROR_GENERIC;
411
412 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-securam");
413 if (node < 0)
414 return TEE_ERROR_ITEM_NOT_FOUND;
415
416 if (_fdt_get_status(fdt, node) != DT_STATUS_OK_SEC)
417 return TEE_ERROR_GENERIC;
418
419 if (dt_map_dev(fdt, node, &soc_pm.securam, &size, DT_MAP_AUTO) < 0)
420 return TEE_ERROR_GENERIC;
421
422 res = clk_dt_get_by_index(fdt, node, 0, &clk);
423 if (res)
424 return res;
425
426 if (clk_enable(clk))
427 return TEE_ERROR_GENERIC;
428
429 if (size < sizeof(struct at91bootstrap_bu))
430 return TEE_ERROR_SHORT_BUFFER;
431
432 at91bootstrap_bu = (void *)soc_pm.securam;
433
434 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-secumod");
435 if (node < 0)
436 return TEE_ERROR_ITEM_NOT_FOUND;
437
438 if (_fdt_get_status(fdt, node) != DT_STATUS_OK_SEC)
439 return TEE_ERROR_GENERIC;
440
441 if (dt_map_dev(fdt, node, &soc_pm.secumod, &size, DT_MAP_AUTO) < 0)
442 return TEE_ERROR_GENERIC;
443
444 return TEE_SUCCESS;
445 }
446
sama5d2_pm_init_all(const void * fdt,vaddr_t shdwc)447 static TEE_Result sama5d2_pm_init_all(const void *fdt, vaddr_t shdwc)
448 {
449 TEE_Result res = TEE_ERROR_GENERIC;
450
451 soc_pm.fdt = fdt;
452 soc_pm.shdwc = shdwc;
453 soc_pm.pmc = at91_pmc_get_base();
454 if (!soc_pm.pmc)
455 return TEE_ERROR_GENERIC;
456
457 soc_pm.mode = CFG_ATMEL_PM_SUSPEND_MODE;
458
459 res = at91_securam_init(fdt);
460 if (res)
461 return res;
462
463 res = at91_pm_dt_dram_init(fdt);
464 if (res)
465 return res;
466
467 res = at91_pm_backup_init(fdt);
468 if (res)
469 return res;
470
471 res = at91_pm_sram_init(fdt);
472 if (res)
473 return res;
474
475 return TEE_SUCCESS;
476 }
477
sama5d2_pm_init(const void * fdt,vaddr_t shdwc)478 TEE_Result sama5d2_pm_init(const void *fdt, vaddr_t shdwc)
479 {
480 if (sama5d2_pm_init_all(fdt, shdwc))
481 panic("Failed to setup PM for sama5d2");
482
483 return TEE_SUCCESS;
484 }
485