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