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 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 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 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 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 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 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 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 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 */ 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 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 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 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 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 353 static TEE_Result at91_pm_backup_init(const void *fdt) 354 { 355 enum dt_map_dev_directive mapping = DT_MAP_AUTO; 356 int node = -1; 357 size_t size = 0; 358 359 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sfrbu"); 360 if (node < 0) 361 return TEE_ERROR_ITEM_NOT_FOUND; 362 363 if (IS_ENABLED(CFG_SAMA7G5)) 364 mapping = DT_MAP_SECURE; 365 366 if (dt_map_dev(fdt, node, &soc_pm.sfrbu, &size, mapping) < 0) 367 return TEE_ERROR_GENERIC; 368 369 if (fdt_get_status(fdt, node) == DT_STATUS_OK_SEC) 370 /* for SAMA7G5 SFRBU is always secured, no need to configre */ 371 if (!IS_ENABLED(CFG_SAMA7G5)) 372 matrix_configure_periph_secure(AT91C_ID_SFRBU); 373 374 return TEE_SUCCESS; 375 } 376 377 static TEE_Result at91_pm_sram_init(const void *fdt) 378 { 379 int node = -1; 380 size_t size = 0; 381 paddr_t at91_suspend_sram_pbase; 382 size_t suspend_sz = at91_pm_suspend_in_sram_sz; 383 384 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-sram"); 385 if (node < 0) 386 return TEE_ERROR_ITEM_NOT_FOUND; 387 388 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC) 389 return TEE_ERROR_GENERIC; 390 391 if (dt_map_dev(fdt, node, &at91_suspend_sram_base, &size, 392 DT_MAP_AUTO) < 0) 393 return TEE_ERROR_GENERIC; 394 395 at91_suspend_sram_pbase = virt_to_phys((void *)at91_suspend_sram_base); 396 397 /* Map the secure ram suspend code to be executable */ 398 at91_suspend_sram_fn = core_mmu_add_mapping(MEM_AREA_TEE_RAM, 399 at91_suspend_sram_pbase, 400 suspend_sz); 401 if (!at91_suspend_sram_fn) { 402 EMSG("Failed to remap sram as executable"); 403 return TEE_ERROR_GENERIC; 404 } 405 406 at91_pm_copy_suspend_to_sram(); 407 408 return TEE_SUCCESS; 409 } 410 411 static TEE_Result at91_securam_init(const void *fdt) 412 { 413 int node = -1; 414 size_t size = 0; 415 struct clk *clk = NULL; 416 TEE_Result res = TEE_ERROR_GENERIC; 417 418 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-securam"); 419 if (node < 0) 420 return TEE_ERROR_ITEM_NOT_FOUND; 421 422 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC) 423 return TEE_ERROR_GENERIC; 424 425 if (dt_map_dev(fdt, node, &soc_pm.securam, &size, DT_MAP_AUTO) < 0) 426 return TEE_ERROR_GENERIC; 427 428 res = clk_dt_get_by_index(fdt, node, 0, &clk); 429 if (res) 430 return res; 431 432 if (clk_enable(clk)) 433 return TEE_ERROR_GENERIC; 434 435 if (size < sizeof(struct at91bootstrap_bu)) 436 return TEE_ERROR_SHORT_BUFFER; 437 438 at91bootstrap_bu = (void *)soc_pm.securam; 439 440 node = fdt_node_offset_by_compatible(fdt, -1, "atmel,sama5d2-secumod"); 441 if (node < 0) 442 return TEE_ERROR_ITEM_NOT_FOUND; 443 444 if (fdt_get_status(fdt, node) != DT_STATUS_OK_SEC) 445 return TEE_ERROR_GENERIC; 446 447 if (dt_map_dev(fdt, node, &soc_pm.secumod, &size, DT_MAP_AUTO) < 0) 448 return TEE_ERROR_GENERIC; 449 450 return TEE_SUCCESS; 451 } 452 453 static TEE_Result sam_pm_init_all(const void *fdt, vaddr_t shdwc) 454 { 455 TEE_Result res = TEE_ERROR_GENERIC; 456 457 soc_pm.fdt = fdt; 458 soc_pm.shdwc = shdwc; 459 soc_pm.pmc = at91_pmc_get_base(); 460 if (!soc_pm.pmc) 461 return TEE_ERROR_GENERIC; 462 463 soc_pm.mode = CFG_ATMEL_PM_SUSPEND_MODE; 464 465 res = at91_securam_init(fdt); 466 if (res) 467 return res; 468 469 res = at91_pm_dt_dram_init(fdt); 470 if (res) 471 return res; 472 473 res = at91_pm_backup_init(fdt); 474 if (res) 475 return res; 476 477 res = at91_pm_sram_init(fdt); 478 if (res) 479 return res; 480 481 return TEE_SUCCESS; 482 } 483 484 TEE_Result sam_pm_init(const void *fdt, vaddr_t shdwc) 485 { 486 if (sam_pm_init_all(fdt, shdwc)) 487 panic("Failed to setup PM for this MPU"); 488 489 return TEE_SUCCESS; 490 } 491