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