1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (C) 2017, Fuzhou Rockchip Electronics Co., Ltd. 4 */ 5 6 #include <common.h> 7 #include <console.h> 8 #include <cru.h> 9 #include <grf.h> 10 #include <initcall.h> 11 #include <io.h> 12 #include <kernel/delay.h> 13 #include <kernel/boot.h> 14 #include <kernel/misc.h> 15 #include <kernel/panic.h> 16 #include <mm/core_mmu.h> 17 #include <mm/core_memprot.h> 18 #include <platform_config.h> 19 #include <sm/optee_smc.h> 20 #include <sm/psci.h> 21 #include <stdint.h> 22 #include <tee/entry_std.h> 23 #include <tee/entry_fast.h> 24 25 struct dram_data { 26 uint32_t cru_mode_con; 27 uint32_t cru_clksel0; 28 uint32_t cru_clksel1; 29 uint32_t cru_clksel10; 30 uint32_t cru_clksel21; 31 uint32_t cru_clkgate[CRU_CLKGATE_CON_CNT]; 32 }; 33 34 static struct dram_data dram_d; 35 36 register_phys_mem_pgdir(MEM_AREA_IO_SEC, CRU_BASE, CRU_SIZE); 37 register_phys_mem_pgdir(MEM_AREA_IO_SEC, GRF_BASE, GRF_SIZE); 38 register_phys_mem_pgdir(MEM_AREA_IO_NSEC, ISRAM_BASE, ISRAM_SIZE); 39 40 static const uint32_t clks_gating_table[CRU_CLKGATE_CON_CNT] = { 41 /* gate: 0-3 */ 42 0xefb8, 43 0x0ff7, 44 0xfff4, 45 0x887f, 46 /* gate: 4-7 */ 47 0x0030, 48 0x00f8, 49 0x07e0, 50 0xc000, 51 /* gate: 8-11 */ 52 0xff84, 53 0xb047, 54 0x1ca0, 55 0x57ff, 56 /* gate: 12-15 */ 57 0x0000, 58 0x00ff, 59 0x1cc0, 60 0x000f, 61 }; 62 63 static void clks_disable(void) 64 { 65 uint32_t i; 66 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 67 68 for (i = 0; i < CRU_CLKGATE_CON_CNT; i++) { 69 dram_d.cru_clkgate[i] = io_read32(va_base + CRU_CLKGATE_CON(i)); 70 io_write32(va_base + CRU_CLKGATE_CON(i), 71 BITS_WITH_WMASK(clks_gating_table[i], 0xffff, 0)); 72 } 73 } 74 75 static void clks_restore(void) 76 { 77 uint32_t i; 78 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 79 80 for (i = 0; i < CRU_CLKGATE_CON_CNT; i++) 81 io_write32(va_base + CRU_CLKGATE_CON(i), 82 BITS_WITH_WMASK(dram_d.cru_clkgate[i], 0xffff, 0)); 83 } 84 85 static void pll_power_down(uint32_t pll) 86 { 87 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 88 89 io_write32(va_base + CRU_MODE_CON, PLL_SLOW_MODE(pll)); 90 io_write32(va_base + CRU_PLL_CON1(pll), PLL_POWER_DOWN); 91 } 92 93 static void pll_power_up(uint32_t pll) 94 { 95 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 96 97 io_write32(va_base + CRU_PLL_CON1(pll), PLL_POWER_UP); 98 } 99 100 static void pll_wait_lock(uint32_t pll) 101 { 102 uint32_t loop = 0; 103 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 104 105 while (!(io_read32(va_base + CRU_PLL_CON1(pll)) & PLL_LOCK) && 106 (loop < 500)) { 107 udelay(2); 108 loop++; 109 } 110 111 if (!(io_read32(va_base + CRU_PLL_CON1(pll)) & PLL_LOCK)) { 112 EMSG("PLL can't lock, index = %" PRIu32, pll); 113 panic(); 114 } 115 } 116 117 /* 118 * Select clock from external 24MHz OSC(slow mode) and power down plls, 119 * then set frequency division of relevant bus to 24MHz. 120 */ 121 static void plls_power_down(void) 122 { 123 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 124 125 dram_d.cru_clksel0 = io_read32(va_base + CRU_CLKSEL_CON(0)); 126 dram_d.cru_clksel1 = io_read32(va_base + CRU_CLKSEL_CON(1)); 127 dram_d.cru_clksel10 = io_read32(va_base + CRU_CLKSEL_CON(10)); 128 dram_d.cru_clksel21 = io_read32(va_base + CRU_CLKSEL_CON(21)); 129 dram_d.cru_mode_con = io_read32(va_base + CRU_MODE_CON); 130 131 pll_power_down(GPLL_ID); 132 pll_power_down(CPLL_ID); 133 pll_power_down(APLL_ID); 134 135 /* core */ 136 io_write32(va_base + CRU_CLKSEL_CON(0), BITS_WITH_WMASK(0, 0x1f, 0)); 137 io_write32(va_base + CRU_CLKSEL_CON(1), 138 BITS_WITH_WMASK(0, 0xf, 0) | BITS_WITH_WMASK(0, 0x7, 4)); 139 140 /* peri aclk, hclk, pclk */ 141 io_write32(va_base + CRU_CLKSEL_CON(10), 142 BITS_WITH_WMASK(0, 0x1f, 0) | BITS_WITH_WMASK(0, 0x3, 8) | 143 BITS_WITH_WMASK(0, 0x7, 12)); 144 145 /* pdbus */ 146 io_write32(va_base + CRU_CLKSEL_CON(0), BITS_WITH_WMASK(0, 0x1f, 8)); 147 io_write32(va_base + CRU_CLKSEL_CON(1), 148 BITS_WITH_WMASK(0, 0x3, 8) | BITS_WITH_WMASK(0, 0x7, 12)); 149 150 /* hdmi cec 32k */ 151 io_write32(va_base + CRU_CLKSEL_CON(21), 152 BITS_WITH_WMASK(732, 0x3fff, 0) | 153 BITS_WITH_WMASK(2, 0x3, 14)); 154 } 155 156 static void plls_restore(void) 157 { 158 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 159 160 /* power up plls */ 161 pll_power_up(APLL_ID); 162 pll_power_up(GPLL_ID); 163 pll_power_up(CPLL_ID); 164 165 udelay(200); 166 167 /* wait lock*/ 168 pll_wait_lock(APLL_ID); 169 pll_wait_lock(GPLL_ID); 170 pll_wait_lock(CPLL_ID); 171 172 /* hdmi cec 32k */ 173 io_write32(va_base + CRU_CLKSEL_CON(21), 174 dram_d.cru_clksel21 | BITS_WMSK(0x3fff, 0) | 175 BITS_WMSK(0x3, 14)); 176 177 /* pdbus */ 178 io_write32(va_base + CRU_CLKSEL_CON(0), 179 dram_d.cru_clksel0 | BITS_WMSK(0x1f, 8)); 180 io_write32(va_base + CRU_CLKSEL_CON(1), 181 dram_d.cru_clksel1 | BITS_WMSK(0x3, 8) | BITS_WMSK(0x7, 12)); 182 183 /* peri aclk, hclk, pclk */ 184 io_write32(va_base + CRU_CLKSEL_CON(10), 185 dram_d.cru_clksel10 | BITS_WMSK(0x1f, 0) | 186 BITS_WMSK(0x3, 8) | BITS_WMSK(0x7, 12)); 187 188 /* core */ 189 io_write32(va_base + CRU_CLKSEL_CON(0), 190 dram_d.cru_clksel0 | BITS_WMSK(0x1f, 0)); 191 io_write32(va_base + CRU_CLKSEL_CON(1), 192 dram_d.cru_clksel1 | BITS_WMSK(0xf, 0) | BITS_WMSK(0x7, 4)); 193 194 /* resume plls mode */ 195 io_write32(va_base + CRU_MODE_CON, 196 dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(APLL_ID))); 197 io_write32(va_base + CRU_MODE_CON, 198 dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(CPLL_ID))); 199 io_write32(va_base + CRU_MODE_CON, 200 dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(GPLL_ID))); 201 } 202 203 static bool wait_core_wfe_i(uint32_t core) 204 { 205 uint32_t wfei_mask, loop = 0; 206 vaddr_t va_base = (vaddr_t)phys_to_virt_io(GRF_BASE); 207 208 wfei_mask = CORE_WFE_I_MASK(core); 209 while (!(io_read32(va_base + GRF_CPU_STATUS1) & wfei_mask) && 210 loop < 500) { 211 udelay(2); 212 loop++; 213 } 214 215 return io_read32(va_base + GRF_CPU_STATUS1) & wfei_mask; 216 } 217 218 static bool core_held_in_reset(uint32_t core) 219 { 220 uint32_t val; 221 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 222 223 val = io_read32(va_base + CRU_SOFTRST_CON(0)); 224 225 return val & CORE_HELD_IN_RESET(core); 226 } 227 228 uint32_t psci_version(void) 229 { 230 return PSCI_VERSION_1_0; 231 } 232 233 int psci_features(uint32_t psci_fid) 234 { 235 switch (psci_fid) { 236 case PSCI_PSCI_FEATURES: 237 case PSCI_VERSION: 238 case PSCI_CPU_ON: 239 case PSCI_CPU_OFF: 240 case PSCI_SYSTEM_SUSPEND: 241 case PSCI_SYSTEM_RESET: 242 return PSCI_RET_SUCCESS; 243 default: 244 return PSCI_RET_NOT_SUPPORTED; 245 } 246 } 247 248 int psci_cpu_on(uint32_t core_idx, uint32_t entry, 249 uint32_t context_id) 250 { 251 bool wfei; 252 vaddr_t cru_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 253 vaddr_t isram_base = (vaddr_t)phys_to_virt_io(ISRAM_BASE); 254 255 core_idx &= MPIDR_CPU_MASK; 256 if ((core_idx == 0) || (core_idx >= CFG_TEE_CORE_NB_CORE)) 257 return PSCI_RET_INVALID_PARAMETERS; 258 259 DMSG("core_id: %" PRIu32, core_idx); 260 261 /* set secondary cores' NS entry addresses */ 262 boot_set_core_ns_entry(core_idx, entry, context_id); 263 264 /* wait */ 265 if (!core_held_in_reset(core_idx)) { 266 wfei = wait_core_wfe_i(core_idx); 267 if (!wfei) { 268 EMSG("Can't wait cpu%" PRIu32 " wfei before softrst", 269 core_idx); 270 return PSCI_RET_DENIED; 271 } 272 } 273 274 /* soft reset core */ 275 io_write32(cru_base + CRU_SOFTRST_CON(0), CORE_SOFT_RESET(core_idx)); 276 dsb(); 277 278 udelay(2); 279 280 /* soft release core */ 281 io_write32(cru_base + CRU_SOFTRST_CON(0), CORE_SOFT_RELEASE(core_idx)); 282 dsb(); 283 284 /* wait */ 285 wfei = wait_core_wfe_i(core_idx); 286 if (!wfei) { 287 EMSG("Can't wait cpu%" PRIu32 " wfei after softrst", core_idx); 288 return PSCI_RET_DENIED; 289 } 290 291 /* set secondary secure entry address and lock tag */ 292 io_write32(isram_base + BOOT_ADDR_OFFSET, TEE_LOAD_ADDR); 293 io_write32(isram_base + LOCK_ADDR_OFFSET, LOCK_TAG); 294 dsb(); 295 296 sev(); 297 dsb(); 298 299 return PSCI_RET_SUCCESS; 300 } 301 302 int psci_cpu_off(void) 303 { 304 uint32_t core = get_core_pos(); 305 306 if ((core == 0) || (core >= CFG_TEE_CORE_NB_CORE)) 307 return PSCI_RET_INVALID_PARAMETERS; 308 309 DMSG("core_id: %" PRIu32, core); 310 311 psci_armv7_cpu_off(); 312 thread_mask_exceptions(THREAD_EXCP_ALL); 313 314 while (1) 315 wfi(); 316 317 return PSCI_RET_INTERNAL_FAILURE; 318 } 319 320 int psci_affinity_info(uint32_t affinity, 321 uint32_t lowest_affnity_level __unused) 322 { 323 uint32_t core_idx = affinity & MPIDR_CPU_MASK; 324 uint32_t wfi_mask = CORE_WFI_MASK(core_idx); 325 vaddr_t va_base = (vaddr_t)phys_to_virt_io(GRF_BASE); 326 327 DMSG("core_id: %" PRIu32 " STATUS: %" PRIx32 " MASK: %" PRIx32, 328 core_idx, io_read32(va_base + GRF_CPU_STATUS1), wfi_mask); 329 330 return (io_read32(va_base + GRF_CPU_STATUS1) & wfi_mask) ? 331 PSCI_AFFINITY_LEVEL_OFF : PSCI_AFFINITY_LEVEL_ON; 332 } 333 334 void psci_system_reset(void) 335 { 336 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 337 338 /* PLLs enter slow mode */ 339 io_write32(va_base + CRU_MODE_CON, PLLS_SLOW_MODE); 340 dsb(); 341 342 /* Global second reset */ 343 io_write32(va_base + CRU_SNDRST_VAL_BASE, CRU_SNDRST_VAL); 344 dsb(); 345 } 346 347 int psci_system_suspend(uintptr_t entry __unused, 348 uint32_t context_id __unused, 349 struct sm_nsec_ctx *nsec __unused) 350 { 351 DMSG("system suspend"); 352 353 clks_disable(); 354 plls_power_down(); 355 356 cache_op_inner(DCACHE_CLEAN_INV, NULL, 0); 357 358 wfi(); 359 360 plls_restore(); 361 clks_restore(); 362 363 return PSCI_RET_SUCCESS; 364 } 365 366 /* When SMP bootup, we release cores one by one */ 367 static TEE_Result reset_nonboot_cores(void) 368 { 369 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 370 371 io_write32(va_base + CRU_SOFTRST_CON(0), NONBOOT_CORES_SOFT_RESET); 372 373 return TEE_SUCCESS; 374 } 375 376 service_init_late(reset_nonboot_cores); 377