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
clks_disable(void)63 static void clks_disable(void)
64 {
65 uint32_t i;
66 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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
clks_restore(void)75 static void clks_restore(void)
76 {
77 uint32_t i;
78 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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
pll_power_down(uint32_t pll)85 static void pll_power_down(uint32_t pll)
86 {
87 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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
pll_power_up(uint32_t pll)93 static void pll_power_up(uint32_t pll)
94 {
95 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
96
97 io_write32(va_base + CRU_PLL_CON1(pll), PLL_POWER_UP);
98 }
99
pll_wait_lock(uint32_t pll)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, CRU_SIZE);
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 */
plls_power_down(void)121 static void plls_power_down(void)
122 {
123 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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
plls_restore(void)156 static void plls_restore(void)
157 {
158 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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
wait_core_wfe_i(uint32_t core)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, GRF_SIZE);
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
core_held_in_reset(uint32_t core)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, CRU_SIZE);
222
223 val = io_read32(va_base + CRU_SOFTRST_CON(0));
224
225 return val & CORE_HELD_IN_RESET(core);
226 }
227
psci_version(void)228 uint32_t psci_version(void)
229 {
230 return PSCI_VERSION_1_0;
231 }
232
psci_features(uint32_t psci_fid)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
psci_cpu_on(uint32_t core_idx,uint32_t entry,uint32_t context_id)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, CRU_SIZE);
253 vaddr_t isram_base = (vaddr_t)phys_to_virt_io(ISRAM_BASE, ISRAM_SIZE);
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
psci_cpu_off(void)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
psci_affinity_info(uint32_t affinity,uint32_t lowest_affnity_level __unused)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, GRF_SIZE);
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
psci_system_reset(void)334 void psci_system_reset(void)
335 {
336 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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
psci_system_suspend(uintptr_t entry __unused,uint32_t context_id __unused,struct sm_nsec_ctx * nsec __unused)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 */
reset_nonboot_cores(void)367 static TEE_Result reset_nonboot_cores(void)
368 {
369 vaddr_t va_base = (vaddr_t)phys_to_virt_io(CRU_BASE, CRU_SIZE);
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