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 <kernel/tlb_helpers.h>
16 #include <libfdt.h>
17 #include <matrix.h>
18 #include <mm/core_memprot.h>
19 #include <smc_ids.h>
20 #include <sm/pm.h>
21 #include <stdbool.h>
22 #include <tee_api_types.h>
23
24 #include "at91_pm.h"
25
26 #if CFG_ATMEL_PM_SUSPEND_MODE < AT91_PM_STANDBY || \
27 CFG_ATMEL_PM_SUSPEND_MODE > AT91_PM_BACKUP
28 #error Invalid suspend mode, please check CFG_ATMEL_PM_SUSPEND_MODE
29 #endif
30
31 #define AT91_SECUMOD_SYSR 0x04
32 #define AT91_SECUMOD_RAMRDY 0x14
33 #define AT91_SECUMOD_RAMRDY_READY BIT(0)
34
35 static struct at91_pm_data soc_pm;
36
37 /* Backup canary */
38 static uint32_t canary = 0xA5A5A5A5;
39
40 /* Backup mode information used by at91bootstrap */
41 static struct at91bootstrap_bu {
42 uint32_t suspended;
43 uint32_t reserved;
44 uint32_t *canary;
45 uint32_t resume;
46 } *at91bootstrap_bu;
47
48 static vaddr_t at91_suspend_sram_base;
49 static void (*at91_suspend_sram_fn)(struct at91_pm_data *);
50
at91_pm_set_suspend_mode(struct thread_smc_args * args)51 enum sm_handler_ret at91_pm_set_suspend_mode(struct thread_smc_args *args)
52 {
53 unsigned int mode = args->a1;
54
55 /*
56 * We don't expect this function to be called simultaneously while we
57 * are entering suspend/resume function. On sama5d2, this is not a
58 * problem since this SoC is a single core one but in order to prevent
59 * any other SoC support to be added without handling this concurrency,
60 * check that we are compiled for a single core.
61 */
62 COMPILE_TIME_ASSERT(CFG_TEE_CORE_NB_CORE == 1);
63
64 if (mode > AT91_PM_BACKUP) {
65 args->a0 = SAMA5_SMC_SIP_RETURN_EINVAL;
66 return SM_HANDLER_SMC_HANDLED;
67 }
68 DMSG("Setting suspend mode to %u", mode);
69
70 args->a0 = SAMA5_SMC_SIP_RETURN_SUCCESS;
71 soc_pm.mode = mode;
72
73 return SM_HANDLER_SMC_HANDLED;
74 }
75
at91_pm_get_suspend_mode(struct thread_smc_args * args)76 enum sm_handler_ret at91_pm_get_suspend_mode(struct thread_smc_args *args)
77 {
78 args->a1 = soc_pm.mode;
79 args->a0 = SAMA5_SMC_SIP_RETURN_SUCCESS;
80
81 return SM_HANDLER_SMC_HANDLED;
82 }
83
at91_pm_copy_suspend_to_sram(void)84 static void at91_pm_copy_suspend_to_sram(void)
85 {
86 memcpy((void *)at91_suspend_sram_base, &at91_pm_suspend_in_sram,
87 at91_pm_suspend_in_sram_sz);
88
89 cache_op_inner(DCACHE_AREA_CLEAN, (void *)at91_suspend_sram_base,
90 at91_pm_suspend_in_sram_sz);
91 cache_op_inner(ICACHE_AREA_INVALIDATE, at91_suspend_sram_fn,
92 at91_pm_suspend_in_sram_sz);
93 }
94
atmel_pm_cpu_idle(void)95 void atmel_pm_cpu_idle(void)
96 {
97 uint32_t lpr0 = 0;
98 uint32_t saved_lpr0 = 0;
99
100 saved_lpr0 = io_read32(soc_pm.ramc + AT91_DDRSDRC_LPR);
101 lpr0 = saved_lpr0 & ~AT91_DDRSDRC_LPCB;
102 lpr0 |= AT91_DDRSDRC_LPCB_POWER_DOWN;
103
104 io_write32(soc_pm.ramc + AT91_DDRSDRC_LPR, lpr0);
105
106 cpu_idle();
107
108 io_write32(soc_pm.ramc + AT91_DDRSDRC_LPR, saved_lpr0);
109 }
110
at91_sam_config_shdwc_ws(vaddr_t shdwc,uint32_t * mode,uint32_t * polarity)111 static void at91_sam_config_shdwc_ws(vaddr_t shdwc, uint32_t *mode,
112 uint32_t *polarity)
113 {
114 uint32_t val = 0;
115
116 /* SHDWC.WUIR */
117 val = io_read32(shdwc + AT91_SHDW_WUIR);
118 *mode |= val & AT91_SHDW_WKUPEN_MASK;
119 *polarity |= (val >> AT91_SHDW_WKUPT_SHIFT) & AT91_SHDW_WKUPT_MASK;
120 }
121
at91_sam_config_pmc_ws(vaddr_t pmc,uint32_t mode,uint32_t polarity)122 static int at91_sam_config_pmc_ws(vaddr_t pmc, uint32_t mode, uint32_t polarity)
123 {
124 io_write32(pmc + AT91_PMC_FSMR, mode);
125 if (IS_ENABLED(CFG_SAMA5D2))
126 io_write32(pmc + AT91_PMC_FSPR, polarity);
127
128 return 0;
129 }
130
131 struct wakeup_source_info {
132 unsigned int pmc_fsmr_bit;
133 unsigned int shdwc_mr_bit;
134 bool set_polarity;
135 };
136
137 static const struct wakeup_source_info ws_info[] = {
138 { .pmc_fsmr_bit = AT91_PMC_FSTT(10), .set_polarity = true },
139 { .pmc_fsmr_bit = AT91_PMC_RTCAL, .shdwc_mr_bit = BIT(17) },
140 { .pmc_fsmr_bit = AT91_PMC_USBAL },
141 { .pmc_fsmr_bit = AT91_PMC_SDMMC_CD },
142 { .pmc_fsmr_bit = AT91_PMC_RTTAL },
143 { .pmc_fsmr_bit = AT91_PMC_RXLP_MCE },
144 };
145
146 struct wakeup_src {
147 const char *compatible;
148 const struct wakeup_source_info *info;
149 };
150
151 static const struct wakeup_src sam_ws_ids[] = {
152 #ifdef CFG_SAMA5D2
153 { .compatible = "atmel,sama5d2-gem", .info = &ws_info[0] },
154 { .compatible = "atmel,at91rm9200-rtc", .info = &ws_info[1] },
155 { .compatible = "atmel,sama5d3-udc", .info = &ws_info[2] },
156 { .compatible = "atmel,at91rm9200-ohci", .info = &ws_info[2] },
157 { .compatible = "usb-ohci", .info = &ws_info[2] },
158 { .compatible = "atmel,at91sam9g45-ehci", .info = &ws_info[2] },
159 { .compatible = "usb-ehci", .info = &ws_info[2] },
160 { .compatible = "atmel,sama5d2-sdhci", .info = &ws_info[3] }
161 #endif
162 #ifdef CFG_SAMA7G5
163 { .compatible = "microchip,sama7g5-rtc", .info = &ws_info[1] },
164 { .compatible = "microchip,sama7g5-ohci", .info = &ws_info[2] },
165 { .compatible = "usb-ohci", .info = &ws_info[2] },
166 { .compatible = "atmel,at91sam9g45-ehci", .info = &ws_info[2] },
167 { .compatible = "usb-ehci", .info = &ws_info[2] },
168 { .compatible = "microchip,sama7g5-sdhci", .info = &ws_info[3] },
169 { .compatible = "microchip,sama7g5-rtt", .info = &ws_info[4] },
170 #endif
171 };
172
dev_is_wakeup_source(const void * fdt,int node)173 static bool dev_is_wakeup_source(const void *fdt, int node)
174 {
175 return fdt_get_property(fdt, node, "wakeup-source", NULL);
176 }
177
at91_pm_config_ws_ulp1(bool set)178 static int at91_pm_config_ws_ulp1(bool set)
179 {
180 const struct wakeup_source_info *wsi = NULL;
181 const struct wakeup_src *wsrc = NULL;
182 unsigned int polarity = 0;
183 unsigned int mode = 0;
184 unsigned int val = 0;
185 unsigned int src = 0;
186 int node = 0;
187
188 if (!set) {
189 io_write32(soc_pm.pmc + AT91_PMC_FSMR, mode);
190 return TEE_SUCCESS;
191 }
192
193 at91_sam_config_shdwc_ws(soc_pm.shdwc, &mode, &polarity);
194
195 val = io_read32(soc_pm.shdwc + AT91_SHDW_MR);
196
197 /* Loop through defined wakeup sources. */
198 for (src = 0; src < ARRAY_SIZE(sam_ws_ids); src++) {
199 wsrc = &sam_ws_ids[src];
200 wsi = wsrc->info;
201
202 node = fdt_node_offset_by_compatible(soc_pm.fdt, -1,
203 wsrc->compatible);
204 while (node >= 0) {
205 if (dev_is_wakeup_source(soc_pm.fdt, node)) {
206 /* Check if enabled on SHDWC. */
207 if (wsi->shdwc_mr_bit &&
208 !(val & wsi->shdwc_mr_bit))
209 goto next_node;
210
211 mode |= wsi->pmc_fsmr_bit;
212 if (wsi->set_polarity)
213 polarity |= wsi->pmc_fsmr_bit;
214 }
215 next_node:
216 node = fdt_node_offset_by_compatible(soc_pm.fdt, node,
217 wsrc->compatible);
218 }
219 }
220
221 if (!mode) {
222 EMSG("AT91: PM: no ULP1 wakeup sources found!");
223 return TEE_ERROR_BAD_STATE;
224 }
225
226 at91_sam_config_pmc_ws(soc_pm.pmc, mode, polarity);
227
228 return TEE_SUCCESS;
229 }
230
231 /*
232 * Verify that all the clocks are correct before entering
233 * slow-clock mode.
234 */
at91_pm_verify_clocks(void)235 static bool at91_pm_verify_clocks(void)
236 {
237 int i = 0;
238 uint32_t scsr = 0;
239
240 scsr = io_read32(soc_pm.pmc + AT91_PMC_SCSR);
241
242 /* USB must not be using PLLB */
243 if ((scsr & (AT91SAM926x_PMC_UHP | AT91SAM926x_PMC_UDP)) != 0) {
244 EMSG("AT91: PM - Suspend-to-RAM with USB still active");
245 return false;
246 }
247
248 /* PCK0..PCKx must be disabled, or configured to use clk32k */
249 for (i = 0; i < AT91_PMC_PCK_COUNT; i++) {
250 uint32_t css = 0;
251
252 if ((scsr & (AT91_PMC_PCK0 << i)) == 0)
253 continue;
254 css = io_read32(soc_pm.pmc + AT91_PMC_PCKR(i)) & AT91_PMC_CSS;
255 if (css != AT91_PMC_CSS_SLOW) {
256 EMSG("AT91: PM - Suspend-to-RAM with PCK%d src %"PRId32,
257 i, css);
258 return false;
259 }
260 }
261
262 return true;
263 }
264
at91_write_backup_data(void)265 static TEE_Result at91_write_backup_data(void)
266 {
267 uint32_t val = 0;
268
269 while (true) {
270 val = io_read32(soc_pm.secumod + AT91_SECUMOD_RAMRDY);
271 if (val & AT91_SECUMOD_RAMRDY_READY)
272 break;
273 }
274
275 io_write32((vaddr_t)&at91bootstrap_bu->suspended, 1);
276 io_write32((vaddr_t)&at91bootstrap_bu->canary, virt_to_phys(&canary));
277 io_write32((vaddr_t)&at91bootstrap_bu->resume,
278 virt_to_phys((void *)(vaddr_t)at91_pm_cpu_resume));
279
280 return TEE_SUCCESS;
281 }
282
at91_pm_change_state(enum pm_op op)283 static void at91_pm_change_state(enum pm_op op)
284 {
285 int type = 0;
286 uint32_t hint = 0;
287
288 if (soc_pm.mode == AT91_PM_STANDBY)
289 type = PM_SUSPEND_STANDBY;
290 else
291 type = PM_SUSPEND_TO_MEM;
292
293 hint = SHIFT_U32(type, PM_HINT_SUSPEND_TYPE_SHIFT);
294
295 pm_change_state(op, hint);
296 }
297
at91_enter_backup(void)298 static TEE_Result at91_enter_backup(void)
299 {
300 int ret = -1;
301 TEE_Result res = TEE_ERROR_GENERIC;
302
303 res = at91_write_backup_data();
304 if (res)
305 return res;
306
307 at91_pm_change_state(PM_OP_SUSPEND);
308 ret = sm_pm_cpu_suspend((uint32_t)&soc_pm,
309 (void *)at91_suspend_sram_fn);
310 if (ret < 0) {
311 DMSG("Suspend failed");
312 res = TEE_ERROR_BAD_STATE;
313 } else {
314 res = TEE_SUCCESS;
315 }
316
317 at91_pm_change_state(PM_OP_RESUME);
318 if (res)
319 return res;
320
321 /* SRAM content is lost after resume */
322 at91_pm_copy_suspend_to_sram();
323
324 return TEE_SUCCESS;
325 }
326
atmel_pm_suspend(uintptr_t entry,struct sm_nsec_ctx * nsec)327 TEE_Result atmel_pm_suspend(uintptr_t entry, struct sm_nsec_ctx *nsec)
328 {
329 TEE_Result res = TEE_ERROR_GENERIC;
330 uint32_t sctlr = 0;
331
332 DMSG("Entering suspend mode %d", soc_pm.mode);
333
334 if (soc_pm.mode >= AT91_PM_ULP0) {
335 if (!at91_pm_verify_clocks())
336 return TEE_ERROR_BAD_STATE;
337 }
338
339 if (soc_pm.mode == AT91_PM_ULP1)
340 at91_pm_config_ws_ulp1(true);
341
342 sm_save_unbanked_regs(&nsec->ub_regs);
343
344 /*
345 * In order to run code for low-power out of SRAM without abort,
346 * configure regions with write permission with not forced to
347 * XN (Execute-never) attribute.
348 */
349 if (IS_ENABLED(CFG_HWSUPP_MEM_PERM_WXN)) {
350 sctlr = read_sctlr();
351 if (sctlr & SCTLR_WXN) {
352 write_sctlr(sctlr & ~SCTLR_WXN);
353 tlbi_all();
354 }
355 }
356
357 if (soc_pm.mode == AT91_PM_BACKUP) {
358 res = at91_enter_backup();
359 } else {
360 at91_suspend_sram_fn(&soc_pm);
361 res = TEE_SUCCESS;
362 }
363
364 /* Restore the XN attribute */
365 if (IS_ENABLED(CFG_HWSUPP_MEM_PERM_WXN)) {
366 if (sctlr & SCTLR_WXN) {
367 write_sctlr(sctlr);
368 tlbi_all();
369 }
370 }
371
372 if (soc_pm.mode == AT91_PM_ULP1)
373 at91_pm_config_ws_ulp1(false);
374
375 sm_restore_unbanked_regs(&nsec->ub_regs);
376
377 /*
378 * If the system went to backup mode, register state was lost and must
379 * be restored by jumping to the user provided entry point
380 */
381 if (res == TEE_SUCCESS && soc_pm.mode == AT91_PM_BACKUP)
382 nsec->mon_lr = entry;
383
384 DMSG("Exiting suspend mode %d, res %"PRIx32, soc_pm.mode, res);
385
386 return res;
387 }
388
at91_pm_dt_dram_init(const void * fdt)389 static TEE_Result at91_pm_dt_dram_init(const void *fdt)
390 {
391 const struct {
392 const char *compatible;
393 vaddr_t *address;
394 } dram_map[] = {
395 #ifdef CFG_SAMA5D2
396 {
397 .compatible = "atmel,sama5d3-ddramc",
398 .address = &soc_pm.ramc,
399 },
400 #endif
401 #ifdef CFG_SAMA7G5
402 {
403 .compatible = "microchip,sama7g5-uddrc",
404 .address = &soc_pm.ramc,
405 },
406 {
407 .compatible = "microchip,sama7g5-ddr3phy",
408 .address = &soc_pm.ramc_phy,
409 },
410 #endif
411 };
412 uint32_t i = 0;
413 int node = -1;
414 size_t size = 0;
415
416 for (i = 0; i < ARRAY_SIZE(dram_map); i++) {
417 node = fdt_node_offset_by_compatible(fdt, -1,
418 dram_map[i].compatible);
419
420 if (node < 0)
421 return TEE_ERROR_ITEM_NOT_FOUND;
422
423 if (dt_map_dev(fdt, node,
424 dram_map[i].address, &size, DT_MAP_AUTO) < 0)
425 return TEE_ERROR_GENERIC;
426 }
427
428 return TEE_SUCCESS;
429 }
430
at91_pm_backup_init(const void * fdt)431 static TEE_Result at91_pm_backup_init(const void *fdt)
432 {
433 enum dt_map_dev_directive mapping = DT_MAP_AUTO;
434 int node = -1;
435 size_t size = 0;
436
437 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sfrbu");
438 if (node < 0)
439 return TEE_ERROR_ITEM_NOT_FOUND;
440
441 if (IS_ENABLED(CFG_SAMA7G5))
442 mapping = DT_MAP_SECURE;
443
444 if (dt_map_dev(fdt, node, &soc_pm.sfrbu, &size, mapping) < 0)
445 return TEE_ERROR_GENERIC;
446
447 if (fdt_get_status(fdt, node) == DT_STATUS_OK_SEC)
448 /* for SAMA7G5 SFRBU is always secured, no need to configre */
449 if (!IS_ENABLED(CFG_SAMA7G5))
450 matrix_configure_periph_secure(AT91C_ID_SFRBU);
451
452 return TEE_SUCCESS;
453 }
454
at91_pm_sram_init(const void * fdt)455 static TEE_Result at91_pm_sram_init(const void *fdt)
456 {
457 int node = -1;
458 size_t size = 0;
459 paddr_t at91_suspend_sram_pbase;
460 size_t suspend_sz = at91_pm_suspend_in_sram_sz;
461
462 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sram");
463 if (node < 0)
464 return TEE_ERROR_ITEM_NOT_FOUND;
465
466 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC)
467 return TEE_ERROR_GENERIC;
468
469 if (dt_map_dev(fdt, node, &at91_suspend_sram_base, &size,
470 DT_MAP_AUTO) < 0)
471 return TEE_ERROR_GENERIC;
472
473 at91_suspend_sram_pbase = virt_to_phys((void *)at91_suspend_sram_base);
474
475 /*
476 * Map the secure ram suspend code with the memory area type
477 * "MEM_AREA_TEE_COHERENT" to make it non-cacheable.
478 * Mapping with memory area type "MEM_AREA_TEE_RAM" would enable
479 * cacheable attribute and might cause abort in some cases.
480 */
481 at91_suspend_sram_fn = core_mmu_add_mapping(MEM_AREA_TEE_COHERENT,
482 at91_suspend_sram_pbase,
483 suspend_sz);
484 if (!at91_suspend_sram_fn) {
485 EMSG("Failed to remap sram as executable");
486 return TEE_ERROR_GENERIC;
487 }
488
489 at91_pm_copy_suspend_to_sram();
490
491 return TEE_SUCCESS;
492 }
493
at91_securam_init(const void * fdt)494 static TEE_Result at91_securam_init(const void *fdt)
495 {
496 int node = -1;
497 size_t size = 0;
498 struct clk *clk = NULL;
499 TEE_Result res = TEE_ERROR_GENERIC;
500
501 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-securam");
502 if (node < 0)
503 return TEE_ERROR_ITEM_NOT_FOUND;
504
505 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC)
506 return TEE_ERROR_GENERIC;
507
508 if (dt_map_dev(fdt, node, &soc_pm.securam, &size, DT_MAP_AUTO) < 0)
509 return TEE_ERROR_GENERIC;
510
511 res = clk_dt_get_by_index(fdt, node, 0, &clk);
512 if (res)
513 return res;
514
515 if (clk_enable(clk))
516 return TEE_ERROR_GENERIC;
517
518 if (size < sizeof(struct at91bootstrap_bu))
519 return TEE_ERROR_SHORT_BUFFER;
520
521 at91bootstrap_bu = (void *)soc_pm.securam;
522
523 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-secumod");
524 if (node < 0)
525 return TEE_ERROR_ITEM_NOT_FOUND;
526
527 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC)
528 return TEE_ERROR_GENERIC;
529
530 if (dt_map_dev(fdt, node, &soc_pm.secumod, &size, DT_MAP_AUTO) < 0)
531 return TEE_ERROR_GENERIC;
532
533 return TEE_SUCCESS;
534 }
535
sam_pm_init_all(const void * fdt,vaddr_t shdwc)536 static TEE_Result sam_pm_init_all(const void *fdt, vaddr_t shdwc)
537 {
538 TEE_Result res = TEE_ERROR_GENERIC;
539
540 soc_pm.fdt = fdt;
541 soc_pm.shdwc = shdwc;
542 soc_pm.pmc = at91_pmc_get_base();
543 if (!soc_pm.pmc)
544 return TEE_ERROR_GENERIC;
545
546 soc_pm.mode = CFG_ATMEL_PM_SUSPEND_MODE;
547
548 res = at91_securam_init(fdt);
549 if (res)
550 return res;
551
552 res = at91_pm_dt_dram_init(fdt);
553 if (res)
554 return res;
555
556 res = at91_pm_backup_init(fdt);
557 if (res)
558 return res;
559
560 res = at91_pm_sram_init(fdt);
561 if (res)
562 return res;
563
564 return TEE_SUCCESS;
565 }
566
sam_pm_init(const void * fdt,vaddr_t shdwc)567 TEE_Result sam_pm_init(const void *fdt, vaddr_t shdwc)
568 {
569 if (sam_pm_init_all(fdt, shdwc))
570 panic("Failed to setup PM for this MPU");
571
572 return TEE_SUCCESS;
573 }
574