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