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 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 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 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 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 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 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 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 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 */ 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 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 283 static TEE_Result at91_enter_backup(void) 284 { 285 int ret = -1; 286 TEE_Result res = TEE_ERROR_GENERIC; 287 288 res = at91_write_backup_data(); 289 if (res) 290 return res; 291 292 pm_change_state(PM_OP_SUSPEND, 0); 293 ret = sm_pm_cpu_suspend((uint32_t)&soc_pm, 294 (void *)at91_suspend_sram_fn); 295 if (ret < 0) { 296 DMSG("Suspend failed"); 297 res = TEE_ERROR_BAD_STATE; 298 } else { 299 res = TEE_SUCCESS; 300 } 301 302 pm_change_state(PM_OP_RESUME, 0); 303 if (res) 304 return res; 305 306 /* SRAM content is lost after resume */ 307 at91_pm_copy_suspend_to_sram(); 308 309 return TEE_SUCCESS; 310 } 311 312 TEE_Result atmel_pm_suspend(uintptr_t entry, struct sm_nsec_ctx *nsec) 313 { 314 TEE_Result res = TEE_ERROR_GENERIC; 315 uint32_t sctlr = 0; 316 317 DMSG("Entering suspend mode %d", soc_pm.mode); 318 319 if (soc_pm.mode >= AT91_PM_ULP0) { 320 if (!at91_pm_verify_clocks()) 321 return TEE_ERROR_BAD_STATE; 322 } 323 324 if (soc_pm.mode == AT91_PM_ULP1) 325 at91_pm_config_ws_ulp1(true); 326 327 sm_save_unbanked_regs(&nsec->ub_regs); 328 329 /* 330 * In order to run code for low-power out of SRAM without abort, 331 * configure regions with write permission with not forced to 332 * XN (Execute-never) attribute. 333 */ 334 if (IS_ENABLED(CFG_HWSUPP_MEM_PERM_WXN)) { 335 sctlr = read_sctlr(); 336 if (sctlr & SCTLR_WXN) { 337 write_sctlr(sctlr & ~SCTLR_WXN); 338 tlbi_all(); 339 } 340 } 341 342 if (soc_pm.mode == AT91_PM_BACKUP) { 343 res = at91_enter_backup(); 344 } else { 345 at91_suspend_sram_fn(&soc_pm); 346 res = TEE_SUCCESS; 347 } 348 349 /* Restore the XN attribute */ 350 if (IS_ENABLED(CFG_HWSUPP_MEM_PERM_WXN)) { 351 if (sctlr & SCTLR_WXN) { 352 write_sctlr(sctlr); 353 tlbi_all(); 354 } 355 } 356 357 if (soc_pm.mode == AT91_PM_ULP1) 358 at91_pm_config_ws_ulp1(false); 359 360 sm_restore_unbanked_regs(&nsec->ub_regs); 361 362 /* 363 * If the system went to backup mode, register state was lost and must 364 * be restored by jumping to the user provided entry point 365 */ 366 if (res == TEE_SUCCESS && soc_pm.mode == AT91_PM_BACKUP) 367 nsec->mon_lr = entry; 368 369 DMSG("Exiting suspend mode %d, res %"PRIx32, soc_pm.mode, res); 370 371 return res; 372 } 373 374 static TEE_Result at91_pm_dt_dram_init(const void *fdt) 375 { 376 const struct { 377 const char *compatible; 378 vaddr_t *address; 379 } dram_map[] = { 380 #ifdef CFG_SAMA5D2 381 { 382 .compatible = "atmel,sama5d3-ddramc", 383 .address = &soc_pm.ramc, 384 }, 385 #endif 386 #ifdef CFG_SAMA7G5 387 { 388 .compatible = "microchip,sama7g5-uddrc", 389 .address = &soc_pm.ramc, 390 }, 391 { 392 .compatible = "microchip,sama7g5-ddr3phy", 393 .address = &soc_pm.ramc_phy, 394 }, 395 #endif 396 }; 397 uint32_t i = 0; 398 int node = -1; 399 size_t size = 0; 400 401 for (i = 0; i < ARRAY_SIZE(dram_map); i++) { 402 node = fdt_node_offset_by_compatible(fdt, -1, 403 dram_map[i].compatible); 404 405 if (node < 0) 406 return TEE_ERROR_ITEM_NOT_FOUND; 407 408 if (dt_map_dev(fdt, node, 409 dram_map[i].address, &size, DT_MAP_AUTO) < 0) 410 return TEE_ERROR_GENERIC; 411 } 412 413 return TEE_SUCCESS; 414 } 415 416 static TEE_Result at91_pm_backup_init(const void *fdt) 417 { 418 enum dt_map_dev_directive mapping = DT_MAP_AUTO; 419 int node = -1; 420 size_t size = 0; 421 422 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sfrbu"); 423 if (node < 0) 424 return TEE_ERROR_ITEM_NOT_FOUND; 425 426 if (IS_ENABLED(CFG_SAMA7G5)) 427 mapping = DT_MAP_SECURE; 428 429 if (dt_map_dev(fdt, node, &soc_pm.sfrbu, &size, mapping) < 0) 430 return TEE_ERROR_GENERIC; 431 432 if (fdt_get_status(fdt, node) == DT_STATUS_OK_SEC) 433 /* for SAMA7G5 SFRBU is always secured, no need to configre */ 434 if (!IS_ENABLED(CFG_SAMA7G5)) 435 matrix_configure_periph_secure(AT91C_ID_SFRBU); 436 437 return TEE_SUCCESS; 438 } 439 440 static TEE_Result at91_pm_sram_init(const void *fdt) 441 { 442 int node = -1; 443 size_t size = 0; 444 paddr_t at91_suspend_sram_pbase; 445 size_t suspend_sz = at91_pm_suspend_in_sram_sz; 446 447 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sram"); 448 if (node < 0) 449 return TEE_ERROR_ITEM_NOT_FOUND; 450 451 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC) 452 return TEE_ERROR_GENERIC; 453 454 if (dt_map_dev(fdt, node, &at91_suspend_sram_base, &size, 455 DT_MAP_AUTO) < 0) 456 return TEE_ERROR_GENERIC; 457 458 at91_suspend_sram_pbase = virt_to_phys((void *)at91_suspend_sram_base); 459 460 /* 461 * Map the secure ram suspend code with the memory area type 462 * "MEM_AREA_TEE_COHERENT" to make it non-cacheable. 463 * Mapping with memory area type "MEM_AREA_TEE_RAM" would enable 464 * cacheable attribute and might cause abort in some cases. 465 */ 466 at91_suspend_sram_fn = core_mmu_add_mapping(MEM_AREA_TEE_COHERENT, 467 at91_suspend_sram_pbase, 468 suspend_sz); 469 if (!at91_suspend_sram_fn) { 470 EMSG("Failed to remap sram as executable"); 471 return TEE_ERROR_GENERIC; 472 } 473 474 at91_pm_copy_suspend_to_sram(); 475 476 return TEE_SUCCESS; 477 } 478 479 static TEE_Result at91_securam_init(const void *fdt) 480 { 481 int node = -1; 482 size_t size = 0; 483 struct clk *clk = NULL; 484 TEE_Result res = TEE_ERROR_GENERIC; 485 486 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-securam"); 487 if (node < 0) 488 return TEE_ERROR_ITEM_NOT_FOUND; 489 490 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC) 491 return TEE_ERROR_GENERIC; 492 493 if (dt_map_dev(fdt, node, &soc_pm.securam, &size, DT_MAP_AUTO) < 0) 494 return TEE_ERROR_GENERIC; 495 496 res = clk_dt_get_by_index(fdt, node, 0, &clk); 497 if (res) 498 return res; 499 500 if (clk_enable(clk)) 501 return TEE_ERROR_GENERIC; 502 503 if (size < sizeof(struct at91bootstrap_bu)) 504 return TEE_ERROR_SHORT_BUFFER; 505 506 at91bootstrap_bu = (void *)soc_pm.securam; 507 508 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-secumod"); 509 if (node < 0) 510 return TEE_ERROR_ITEM_NOT_FOUND; 511 512 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC) 513 return TEE_ERROR_GENERIC; 514 515 if (dt_map_dev(fdt, node, &soc_pm.secumod, &size, DT_MAP_AUTO) < 0) 516 return TEE_ERROR_GENERIC; 517 518 return TEE_SUCCESS; 519 } 520 521 static TEE_Result sam_pm_init_all(const void *fdt, vaddr_t shdwc) 522 { 523 TEE_Result res = TEE_ERROR_GENERIC; 524 525 soc_pm.fdt = fdt; 526 soc_pm.shdwc = shdwc; 527 soc_pm.pmc = at91_pmc_get_base(); 528 if (!soc_pm.pmc) 529 return TEE_ERROR_GENERIC; 530 531 soc_pm.mode = CFG_ATMEL_PM_SUSPEND_MODE; 532 533 res = at91_securam_init(fdt); 534 if (res) 535 return res; 536 537 res = at91_pm_dt_dram_init(fdt); 538 if (res) 539 return res; 540 541 res = at91_pm_backup_init(fdt); 542 if (res) 543 return res; 544 545 res = at91_pm_sram_init(fdt); 546 if (res) 547 return res; 548 549 return TEE_SUCCESS; 550 } 551 552 TEE_Result sam_pm_init(const void *fdt, vaddr_t shdwc) 553 { 554 if (sam_pm_init_all(fdt, shdwc)) 555 panic("Failed to setup PM for this MPU"); 556 557 return TEE_SUCCESS; 558 } 559