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] = io_read32(va_base + CRU_CLKGATE_CON(i)); 89 io_write32(va_base + CRU_CLKGATE_CON(i), 90 BITS_WITH_WMASK(clks_gating_table[i], 0xffff, 0)); 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 io_write32(va_base + CRU_CLKGATE_CON(i), 101 BITS_WITH_WMASK(dram_d.cru_clkgate[i], 0xffff, 0)); 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 io_write32(va_base + CRU_MODE_CON, PLL_SLOW_MODE(pll)); 109 io_write32(va_base + CRU_PLL_CON1(pll), PLL_POWER_DOWN); 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 io_write32(va_base + CRU_PLL_CON1(pll), PLL_POWER_UP); 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 (!(io_read32(va_base + CRU_PLL_CON1(pll)) & PLL_LOCK) && 125 (loop < 500)) { 126 udelay(2); 127 loop++; 128 } 129 130 if (!(io_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 = io_read32(va_base + CRU_CLKSEL_CON(0)); 145 dram_d.cru_clksel1 = io_read32(va_base + CRU_CLKSEL_CON(1)); 146 dram_d.cru_clksel10 = io_read32(va_base + CRU_CLKSEL_CON(10)); 147 dram_d.cru_clksel21 = io_read32(va_base + CRU_CLKSEL_CON(21)); 148 dram_d.cru_mode_con = io_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 io_write32(va_base + CRU_CLKSEL_CON(0), BITS_WITH_WMASK(0, 0x1f, 0)); 156 io_write32(va_base + CRU_CLKSEL_CON(1), 157 BITS_WITH_WMASK(0, 0xf, 0) | BITS_WITH_WMASK(0, 0x7, 4)); 158 159 /* peri aclk, hclk, pclk */ 160 io_write32(va_base + CRU_CLKSEL_CON(10), 161 BITS_WITH_WMASK(0, 0x1f, 0) | BITS_WITH_WMASK(0, 0x3, 8) | 162 BITS_WITH_WMASK(0, 0x7, 12)); 163 164 /* pdbus */ 165 io_write32(va_base + CRU_CLKSEL_CON(0), BITS_WITH_WMASK(0, 0x1f, 8)); 166 io_write32(va_base + CRU_CLKSEL_CON(1), 167 BITS_WITH_WMASK(0, 0x3, 8) | BITS_WITH_WMASK(0, 0x7, 12)); 168 169 /* hdmi cec 32k */ 170 io_write32(va_base + CRU_CLKSEL_CON(21), 171 BITS_WITH_WMASK(732, 0x3fff, 0) | 172 BITS_WITH_WMASK(2, 0x3, 14)); 173 } 174 175 static void plls_restore(void) 176 { 177 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 178 179 /* power up plls */ 180 pll_power_up(APLL_ID); 181 pll_power_up(GPLL_ID); 182 pll_power_up(CPLL_ID); 183 184 udelay(200); 185 186 /* wait lock*/ 187 pll_wait_lock(APLL_ID); 188 pll_wait_lock(GPLL_ID); 189 pll_wait_lock(CPLL_ID); 190 191 /* hdmi cec 32k */ 192 io_write32(va_base + CRU_CLKSEL_CON(21), 193 dram_d.cru_clksel21 | BITS_WMSK(0x3fff, 0) | 194 BITS_WMSK(0x3, 14)); 195 196 /* pdbus */ 197 io_write32(va_base + CRU_CLKSEL_CON(0), 198 dram_d.cru_clksel0 | BITS_WMSK(0x1f, 8)); 199 io_write32(va_base + CRU_CLKSEL_CON(1), 200 dram_d.cru_clksel1 | BITS_WMSK(0x3, 8) | BITS_WMSK(0x7, 12)); 201 202 /* peri aclk, hclk, pclk */ 203 io_write32(va_base + CRU_CLKSEL_CON(10), 204 dram_d.cru_clksel10 | BITS_WMSK(0x1f, 0) | 205 BITS_WMSK(0x3, 8) | BITS_WMSK(0x7, 12)); 206 207 /* core */ 208 io_write32(va_base + CRU_CLKSEL_CON(0), 209 dram_d.cru_clksel0 | BITS_WMSK(0x1f, 0)); 210 io_write32(va_base + CRU_CLKSEL_CON(1), 211 dram_d.cru_clksel1 | BITS_WMSK(0xf, 0) | BITS_WMSK(0x7, 4)); 212 213 /* resume plls mode */ 214 io_write32(va_base + CRU_MODE_CON, 215 dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(APLL_ID))); 216 io_write32(va_base + CRU_MODE_CON, 217 dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(CPLL_ID))); 218 io_write32(va_base + CRU_MODE_CON, 219 dram_d.cru_mode_con | BITS_WMSK(0x1, PLL_MODE_BIT(GPLL_ID))); 220 } 221 222 static bool wait_core_wfe_i(uint32_t core) 223 { 224 uint32_t wfei_mask, loop = 0; 225 vaddr_t va_base = (vaddr_t)phys_to_virt_io(GRF_BASE); 226 227 wfei_mask = CORE_WFE_I_MASK(core); 228 while (!(io_read32(va_base + GRF_CPU_STATUS1) & wfei_mask) && 229 loop < 500) { 230 udelay(2); 231 loop++; 232 } 233 234 return io_read32(va_base + GRF_CPU_STATUS1) & wfei_mask; 235 } 236 237 static bool core_held_in_reset(uint32_t core) 238 { 239 uint32_t val; 240 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 241 242 val = io_read32(va_base + CRU_SOFTRST_CON(0)); 243 244 return val & CORE_HELD_IN_RESET(core); 245 } 246 247 uint32_t psci_version(void) 248 { 249 return PSCI_VERSION_1_0; 250 } 251 252 int psci_features(uint32_t psci_fid) 253 { 254 switch (psci_fid) { 255 case PSCI_PSCI_FEATURES: 256 case PSCI_VERSION: 257 case PSCI_CPU_ON: 258 case PSCI_CPU_OFF: 259 case PSCI_SYSTEM_SUSPEND: 260 case PSCI_SYSTEM_RESET: 261 return PSCI_RET_SUCCESS; 262 default: 263 return PSCI_RET_NOT_SUPPORTED; 264 } 265 } 266 267 int psci_cpu_on(uint32_t core_idx, uint32_t entry, 268 uint32_t context_id) 269 { 270 bool wfei; 271 vaddr_t cru_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 272 vaddr_t isram_base = (vaddr_t)phys_to_virt_io(ISRAM_BASE); 273 274 core_idx &= MPIDR_CPU_MASK; 275 if ((core_idx == 0) || (core_idx >= CFG_TEE_CORE_NB_CORE)) 276 return PSCI_RET_INVALID_PARAMETERS; 277 278 DMSG("core_id: %" PRIu32, core_idx); 279 280 /* set secondary cores' NS entry addresses */ 281 generic_boot_set_core_ns_entry(core_idx, entry, context_id); 282 283 /* wait */ 284 if (!core_held_in_reset(core_idx)) { 285 wfei = wait_core_wfe_i(core_idx); 286 if (!wfei) { 287 EMSG("Can't wait cpu%" PRIu32 " wfei before softrst", 288 core_idx); 289 return PSCI_RET_DENIED; 290 } 291 } 292 293 /* soft reset core */ 294 io_write32(cru_base + CRU_SOFTRST_CON(0), CORE_SOFT_RESET(core_idx)); 295 dsb(); 296 297 udelay(2); 298 299 /* soft release core */ 300 io_write32(cru_base + CRU_SOFTRST_CON(0), CORE_SOFT_RELEASE(core_idx)); 301 dsb(); 302 303 /* wait */ 304 wfei = wait_core_wfe_i(core_idx); 305 if (!wfei) { 306 EMSG("Can't wait cpu%" PRIu32 " wfei after softrst", core_idx); 307 return PSCI_RET_DENIED; 308 } 309 310 /* set secondary secure entry address and lock tag */ 311 io_write32(isram_base + BOOT_ADDR_OFFSET, TEE_LOAD_ADDR); 312 io_write32(isram_base + LOCK_ADDR_OFFSET, LOCK_TAG); 313 dsb(); 314 315 sev(); 316 dsb(); 317 318 return PSCI_RET_SUCCESS; 319 } 320 321 int psci_cpu_off(void) 322 { 323 uint32_t core = get_core_pos(); 324 325 if ((core == 0) || (core >= CFG_TEE_CORE_NB_CORE)) 326 return PSCI_RET_INVALID_PARAMETERS; 327 328 DMSG("core_id: %" PRIu32, core); 329 330 psci_armv7_cpu_off(); 331 thread_mask_exceptions(THREAD_EXCP_ALL); 332 333 while (1) 334 wfi(); 335 336 return PSCI_RET_INTERNAL_FAILURE; 337 } 338 339 int psci_affinity_info(uint32_t affinity, 340 uint32_t lowest_affnity_level __unused) 341 { 342 uint32_t core_idx = affinity & MPIDR_CPU_MASK; 343 uint32_t wfi_mask = CORE_WFI_MASK(core_idx); 344 vaddr_t va_base = (vaddr_t)phys_to_virt_io(GRF_BASE); 345 346 DMSG("core_id: %" PRIu32 " STATUS: %" PRIx32 " MASK: %" PRIx32, 347 core_idx, io_read32(va_base + GRF_CPU_STATUS1), wfi_mask); 348 349 return (io_read32(va_base + GRF_CPU_STATUS1) & wfi_mask) ? 350 PSCI_AFFINITY_LEVEL_OFF : PSCI_AFFINITY_LEVEL_ON; 351 } 352 353 void psci_system_reset(void) 354 { 355 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 356 357 /* PLLs enter slow mode */ 358 io_write32(va_base + CRU_MODE_CON, PLLS_SLOW_MODE); 359 dsb(); 360 361 /* Global second reset */ 362 io_write32(va_base + CRU_SNDRST_VAL_BASE, CRU_SNDRST_VAL); 363 dsb(); 364 } 365 366 int psci_system_suspend(uintptr_t entry __unused, 367 uint32_t context_id __unused, 368 struct sm_nsec_ctx *nsec __unused) 369 { 370 DMSG("system suspend"); 371 372 clks_disable(); 373 plls_power_down(); 374 375 cache_op_inner(DCACHE_CLEAN_INV, NULL, 0); 376 377 wfi(); 378 379 plls_restore(); 380 clks_restore(); 381 382 return PSCI_RET_SUCCESS; 383 } 384 385 /* When SMP bootup, we release cores one by one */ 386 static TEE_Result reset_nonboot_cores(void) 387 { 388 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE); 389 390 io_write32(va_base + CRU_SOFTRST_CON(0), NONBOOT_CORES_SOFT_RESET); 391 392 return TEE_SUCCESS; 393 } 394 395 service_init_late(reset_nonboot_cores); 396