1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2021 Rockchip Electronics Co., Ltd
4 */
5
6 #include <common.h>
7 #include <amp.h>
8 #include <bidram.h>
9 #include <boot_rkimg.h>
10 #include <config.h>
11 #include <sysmem.h>
12 #include <asm/gic.h>
13 #include <asm/io.h>
14 #include <asm/arch/rockchip_smccc.h>
15
16 DECLARE_GLOBAL_DATA_PTR;
17
18 /*
19 * [Design Principles]
20 *
21 * [amp.img]
22 * The amp image with FIT format which consists of non-linux firmwares.
23 * Please refer to: driver/cpu/amp.its.
24 *
25 * amp.img generation: ./tools/mkimage -f drivers/cpu/amp.its -E -p 0xe00 amp.img
26 *
27 * [linux]
28 * We still use the traditional solution for a better compatibility:
29 * boot.img/recovery.img with FIT format or Android format.
30 *
31 * The developer need add "/configurations/conf/linux" node to configure:
32 * description, arch, cpu, thumb, hyp, udelay(optional) properties.
33 * The addresses depend on U-Boot: kernel_addr_r, fdt_addr_r and
34 * ramdisk_addr_r. Please refer to: driver/cpu/amp.its.
35 *
36 * [memory management]
37 * U-Boot is not responsible for memory distribution/fixup any more, please
38 * handle this on kernel dts "/memory".
39 *
40 * [trust]
41 * The AMP feature requires trust support.
42 */
43
44 #define AMP_PART "amp"
45 #define FIT_HEADER_SIZE SZ_4K
46
47 #define gicd_readl(offset) readl((void *)GICD_BASE + (offset))
48 #define gicd_writel(v, offset) writel(v, (void *)GICD_BASE + (offset))
49 #define ROCKCHIP_AMP_NODE "/rockchip-amp"
50 #define ROCKCHIP_AMP_CPUS_NODE "/rockchip-amp/amp-cpus"
51
52 typedef struct boot_args {
53 ulong arg0;
54 ulong arg1;
55 ulong arg2;
56 ulong arg3;
57 } boot_args_t;
58
59 typedef struct boot_cpu {
60 u32 arch;
61 u32 state;
62 u32 entry;
63 u32 linux_os;
64 u32 boot_on;
65 } boot_cpu_t;
66
67 static boot_cpu_t g_bootcpu;
68 static u32 os_amp_dispatcher_cpu[8];
69
fit_get_u32_default(const void * fit,int noffset,const char * prop,u32 def)70 static u32 fit_get_u32_default(const void *fit, int noffset,
71 const char *prop, u32 def)
72 {
73 const fdt32_t *val;
74
75 val = fdt_getprop(fit, noffset, prop, NULL);
76 if (!val)
77 return def;
78
79 return fdt32_to_cpu(*val);
80 }
81
parse_os_amp_dispatcher(void)82 static int parse_os_amp_dispatcher(void)
83 {
84 const void *fdt = gd->fdt_blob;
85 int noffset, cpu, i = 0;
86 u64 mpidr;
87
88 memset(os_amp_dispatcher_cpu, 0xff, sizeof(os_amp_dispatcher_cpu));
89
90 noffset = fdt_path_offset(fdt, ROCKCHIP_AMP_NODE);
91 if (noffset < 0)
92 return 0;
93
94 if (!fdtdec_get_is_enabled(fdt, noffset))
95 return 0;
96
97 noffset = fdt_path_offset(fdt, ROCKCHIP_AMP_CPUS_NODE);
98 if (noffset < 0)
99 return 0;
100
101 fdt_for_each_subnode(cpu, fdt, noffset) {
102 mpidr = fdtdec_get_uint64(fdt, cpu, "id", 0xffffffff);
103 if (mpidr == 0xffffffff) /* invalid */
104 continue;
105
106 os_amp_dispatcher_cpu[i++] = mpidr;
107 AMP_I("cpu[%llx] belong to os amp-dispatcher\n", mpidr);
108 }
109
110 return 0;
111 }
112
load_linux_for_nonboot_cpu(u32 cpu,u32 aarch64,u32 load,u32 * entry,boot_args_t * args)113 static int load_linux_for_nonboot_cpu(u32 cpu, u32 aarch64, u32 load,
114 u32 *entry, boot_args_t *args)
115 {
116 static const char *boot_cmd[] = {
117 "boot_fit", "boot_android ${devtype} ${devnum}" };
118 int i, ret;
119
120 env_set_hex("bootm_states_unmask", BOOTM_STATE_OS_GO);
121 for (i = 0; i < ARRAY_SIZE(boot_cmd); i++) {
122 ret = run_command(boot_cmd[i], 0);
123 if (!ret)
124 break;
125 }
126 env_set("bootm_states_unmask", NULL);
127 if (ret) {
128 AMP_E("Load linux failed, ret=%d\n", ret);
129 return ret;
130 }
131
132 /* linux boot args */
133 if (aarch64) {
134 args->arg0 = (ulong)gd->fdt_blob;
135 args->arg1 = 0;
136 args->arg2 = 0;
137 } else {
138 args->arg0 = 0;
139 args->arg1 = 0;
140 args->arg2 = (ulong)gd->fdt_blob;
141 }
142
143 /* don't need call cleanup_before_linux() as this nonboot cpu is clean */
144 board_quiesce_devices(&images);
145 flush_dcache_all();
146
147 /* fixup: ramdisk/fdt/entry depend on U-Boot */
148 *entry = (u32)images.ep;
149
150 return 0;
151 }
152
is_default_pe_state(u32 pe_state)153 static int is_default_pe_state(u32 pe_state)
154 {
155 #ifdef CONFIG_ARM64
156 return (pe_state == PE_STATE(1, 1, 0, 0));
157 #else
158 return (pe_state == PE_STATE(0, 0, 0, 0));
159 #endif
160 }
161
setup_sync_bits_for_linux(void)162 static void setup_sync_bits_for_linux(void)
163 {
164 u32 val, num_irq, offset;
165
166 val = gicd_readl(GICD_CTLR);
167 val &= ~0x3;
168 gicd_writel(val, GICD_CTLR);
169
170 num_irq = 32 * ((gicd_readl(GICD_TYPER) & 0x1F) + 1);
171 offset = ((num_irq - 1) / 4) * 4;
172 gicd_writel(0x0, GICD_IPRIORITYRn + offset);
173 }
174
smc_cpu_on(u32 cpu,u32 pe_state,u32 entry,boot_args_t * args,bool is_linux)175 static int smc_cpu_on(u32 cpu, u32 pe_state, u32 entry,
176 boot_args_t *args, bool is_linux)
177 {
178 int ret;
179
180 AMP_I("Brought up cpu[%x] with state 0x%x, entry 0x%08x ...",
181 cpu, pe_state, entry);
182
183 /* if target pe state is default arch state, power up cpu directly */
184 if (is_default_pe_state(pe_state))
185 goto finish;
186
187 ret = sip_smc_amp_cfg(AMP_PE_STATE, cpu, pe_state, 0);
188 if (ret) {
189 AMP_E("smc pe-state, ret=%d\n", ret);
190 return ret;
191 }
192
193 /* only linux needs boot args */
194 if (!is_linux)
195 goto finish;
196
197 ret = sip_smc_amp_cfg(AMP_BOOT_ARG01, cpu, args->arg0, args->arg1);
198 if (ret) {
199 AMP_E("smc boot arg01, ret=%d\n", ret);
200 return ret;
201 }
202
203 ret = sip_smc_amp_cfg(AMP_BOOT_ARG23, cpu, args->arg2, args->arg3);
204 if (ret) {
205 AMP_E("smc boot arg23, ret=%d\n", ret);
206 return ret;
207 }
208
209 finish:
210 ret = psci_cpu_on(cpu, entry);
211 if (ret) {
212 printf("cpu up failed, ret=%d\n", ret);
213 return ret;
214 }
215 printf("OK\n");
216
217 return 0;
218 }
219
fit_standalone_release(char * id,uintptr_t entry_point)220 __weak int fit_standalone_release(char *id, uintptr_t entry_point)
221 {
222 return 0;
223 }
224
standalone_handler(const char * id,u32 entry_point,int data_size)225 static int standalone_handler(const char *id, u32 entry_point, int data_size)
226 {
227 int ret;
228
229 if (!sysmem_alloc_base_by_name(id,
230 (phys_addr_t)entry_point, data_size))
231 return -ENXIO;
232
233 printf("Handle standalone: '%s' at 0x%08x ...", id, entry_point);
234
235 ret = fit_standalone_release((char *)id, entry_point);
236 if (ret) {
237 printf("failed, ret=%d\n", ret);
238 return ret;
239 }
240 printf("OK\n");
241
242 return 0;
243 }
244
brought_up_amp(void * fit,int noffset,boot_cpu_t * bootcpu,int is_linux)245 static int brought_up_amp(void *fit, int noffset,
246 boot_cpu_t *bootcpu, int is_linux)
247 {
248 const char *desc;
249 boot_args_t args;
250 u32 cpu, aarch64, hyp;
251 u32 load, load_c, thumb, us;
252 u32 pe_state, entry;
253 int boot_on;
254 int data_size;
255 int i, ret;
256 u8 type = -ENODATA;
257 u8 arch = -ENODATA;
258
259 desc = fdt_getprop(fit, noffset, "description", NULL);
260 cpu = fit_get_u32_default(fit, noffset, "cpu", -ENODATA);
261 hyp = fit_get_u32_default(fit, noffset, "hyp", 0);
262 thumb = fit_get_u32_default(fit, noffset, "thumb", 0);
263 entry = load = fit_get_u32_default(fit, noffset, "load", -ENODATA);
264 load_c = fit_get_u32_default(fit, noffset, "load_c", -ENODATA);
265 us = fit_get_u32_default(fit, noffset, "udelay", 0);
266 boot_on = fit_get_u32_default(fit, noffset, "boot-on", 1);
267 fit_image_get_arch(fit, noffset, &arch);
268 fit_image_get_type(fit, noffset, &type);
269 fit_image_get_data_size(fit, noffset, &data_size);
270 memset(&args, 0, sizeof(args));
271
272 /* standalone is simple, just handle it and then exit. Allow failure */
273 if (type == IH_TYPE_STANDALONE) {
274 if (!desc || load == -ENODATA) {
275 AMP_E("standalone: \"desc\" or \"load\" property missing!\n");
276 goto exit;
277 }
278 standalone_handler(desc, load, data_size);
279 goto exit;
280 }
281
282 if (!desc || cpu == -ENODATA || arch == -ENODATA || type == -ENODATA ||
283 (load == -ENODATA && !is_linux)) {
284 AMP_E("Property missing!\n");
285 return -EINVAL;
286 }
287
288 if (is_linux) {
289 if (load != -ENODATA)
290 env_set_hex("kernel_addr_r", load);
291 if (load_c != -ENODATA)
292 env_set_hex("kernel_addr_c", load_c);
293 }
294
295 aarch64 = (arch == IH_ARCH_ARM) ? 0 : 1;
296 pe_state = PE_STATE(aarch64, hyp, thumb, 0);
297
298 #ifdef DEBUG
299 AMP_I(" desc: %s\n", desc);
300 AMP_I(" cpu: 0x%x\n", cpu);
301 AMP_I(" aarch64: %d\n", aarch64);
302 AMP_I(" hyp: %d\n", hyp);
303 AMP_I(" thumb: %d\n", thumb);
304 AMP_I(" load: 0x%08x\n", load);
305 AMP_I(" load_c: 0x%08x\n", load_c);
306 AMP_I(" pe_state: 0x%08x\n", pe_state);
307 AMP_I(" linux-os: %d\n\n", is_linux);
308 #endif
309
310 /* If os amp-dispatcher owns this cpu, don't boot it */
311 for (i = 0; i < ARRAY_SIZE(os_amp_dispatcher_cpu); i++) {
312 if (cpu == os_amp_dispatcher_cpu[i]) {
313 boot_on = 0;
314 break;
315 }
316 }
317
318 /* this cpu is boot cpu ? */
319 if ((read_mpidr() & 0x0fff) == cpu) {
320 bootcpu->arch = arch;
321 bootcpu->entry = entry;
322 bootcpu->state = pe_state;
323 bootcpu->linux_os = is_linux;
324 bootcpu->boot_on = boot_on;
325 return 0;
326 }
327
328 /* === only nonboot cpu can reach here === */
329
330 /* load or check */
331 if (is_linux) {
332 ret = load_linux_for_nonboot_cpu(cpu,
333 aarch64, load, &entry, &args);
334 if (ret)
335 return ret;
336 /*
337 * Must setup before jump to linux.
338 * This is an appointment on RK amp solution to handle
339 * GIC configure competition.
340 */
341 setup_sync_bits_for_linux();
342 } else {
343 if (!sysmem_alloc_base_by_name(desc,
344 (phys_addr_t)load, data_size))
345 return -ENXIO;
346 }
347
348 if (!boot_on)
349 return 0;
350
351 /* boot now */
352 ret = smc_cpu_on(cpu, pe_state, entry, &args, is_linux);
353 if (ret)
354 return ret;
355 exit:
356 if (us)
357 udelay(us);
358
359 return 0;
360 }
361
brought_up_all_amp(void * fit,const char * fit_uname_cfg)362 static int brought_up_all_amp(void *fit, const char *fit_uname_cfg)
363 {
364 int loadables_index;
365 int linux_noffset;
366 int conf_noffset;
367 int cpu_noffset;
368 int ret;
369 const char *uname;
370
371 conf_noffset = fit_conf_get_node(fit, fit_uname_cfg);
372 if (conf_noffset < 0)
373 return conf_noffset;
374
375 /*
376 * If boot cpu is not assigned in amp.img, the default value 0 makes
377 * boot cpu power down itself in final process, so we must initial it.
378 */
379 g_bootcpu.boot_on = 1;
380
381 linux_noffset = fdt_subnode_offset(fit, conf_noffset, "linux");
382 if (linux_noffset > 0) {
383 ret = brought_up_amp(fit, linux_noffset, &g_bootcpu, 1);
384 if (ret)
385 return ret;
386 }
387
388 for (loadables_index = 0;
389 uname = fdt_stringlist_get(fit, conf_noffset,
390 FIT_LOADABLE_PROP, loadables_index, NULL), uname;
391 loadables_index++) {
392 cpu_noffset = fit_image_get_node(fit, uname);
393 if (cpu_noffset < 0)
394 return cpu_noffset;
395
396 ret = brought_up_amp(fit, cpu_noffset, &g_bootcpu, 0);
397 if (ret)
398 return ret;
399 }
400
401 /* === only boot cpu can reach here === */
402
403 if (!g_bootcpu.boot_on) {
404 AMP_I("Primary cpu[%x, self] with state 0x%x, entry 0x%08x ... Power down!\n",
405 (u32)read_mpidr() & 0x0fff, g_bootcpu.state, g_bootcpu.entry);
406 dsb();
407 isb();
408 /* make sure I am off before os amp-dispatcher probe */
409 psci_cpu_off(0);
410
411 /* Never reach here */
412 while (1) {
413 AMP_E("Primary cpu[%x, self] power down failed\n",
414 (u32)read_mpidr());
415 __asm("wfe");
416 }
417 }
418
419 if (!g_bootcpu.linux_os && g_bootcpu.entry) {
420 flush_dcache_all();
421 AMP_I("Brought up primary cpu[%x, self] with state 0x%x, entry 0x%08x ...",
422 (u32)read_mpidr() & 0x0fff, g_bootcpu.state, g_bootcpu.entry);
423 cleanup_before_linux();
424 printf("OK\n");
425 #ifdef CONFIG_ARM64
426 if (g_bootcpu.state & (1 << MODE_HYP_SHIFT))
427 armv8_switch_to_el2(0, 0, 0, g_bootcpu.state, (u64)g_bootcpu.entry,
428 g_bootcpu.arch == IH_ARCH_ARM ? ES_TO_AARCH32 : ES_TO_AARCH64);
429 else
430 armv8_switch_to_el1(0, 0, 0, g_bootcpu.state, (u64)g_bootcpu.entry,
431 g_bootcpu.arch == IH_ARCH_ARM ? ES_TO_AARCH32 : ES_TO_AARCH64);
432 #else
433 void (*armv7_entry)(void);
434
435 armv7_entry = (void (*)(void))g_bootcpu.entry;
436 armv7_entry();
437 #endif
438 }
439
440 /* return: boot cpu continue to boot linux */
441 return 0;
442 }
443
amp_cpus_on(void)444 int amp_cpus_on(void)
445 {
446 struct blk_desc *dev_desc;
447 bootm_headers_t images;
448 disk_partition_t part;
449 void *hdr, *fit;
450 int offset, cnt;
451 int totalsize;
452 int ret = 0;
453
454 dev_desc = rockchip_get_bootdev();
455 if (!dev_desc)
456 return -EIO;
457
458 if (part_get_info_by_name(dev_desc, AMP_PART, &part) < 0)
459 return -ENODEV;
460
461 hdr = memalign(ARCH_DMA_MINALIGN, FIT_HEADER_SIZE);
462 if (!hdr)
463 return -ENOMEM;
464
465 /* get totalsize */
466 offset = part.start;
467 cnt = DIV_ROUND_UP(FIT_HEADER_SIZE, part.blksz);
468 if (blk_dread(dev_desc, offset, cnt, hdr) != cnt) {
469 ret = -EIO;
470 goto out2;
471 }
472
473 if (fdt_check_header(hdr)) {
474 AMP_E("Not fit\n");
475 ret = -EINVAL;
476 goto out2;
477 }
478
479 if (fit_get_totalsize(hdr, &totalsize)) {
480 AMP_E("No totalsize\n");
481 ret = -EINVAL;
482 goto out2;
483 }
484
485 /* load image */
486 fit = memalign(ARCH_DMA_MINALIGN, ALIGN(totalsize, part.blksz));
487 if (!fit) {
488 printf("No memory\n");
489 ret = -ENOMEM;
490 goto out2;
491 }
492
493 memcpy(fit, hdr, FIT_HEADER_SIZE);
494
495 offset += cnt;
496 cnt = DIV_ROUND_UP(totalsize, part.blksz) - cnt;
497 if (blk_dread(dev_desc, offset, cnt, fit + FIT_HEADER_SIZE) != cnt) {
498 ret = -EIO;
499 goto out1;
500 }
501
502 ret = parse_os_amp_dispatcher();
503 if (ret < 0) {
504 ret = -EINVAL;
505 goto out1;
506 }
507
508 /* Load loadables */
509 memset(&images, 0, sizeof(images));
510 images.fit_uname_cfg = "conf";
511 images.fit_hdr_os = fit;
512 images.verify = 1;
513 ret = boot_get_loadable(0, NULL, &images, IH_ARCH_DEFAULT, NULL, NULL);
514 if (ret) {
515 AMP_E("Load loadables, ret=%d\n", ret);
516 goto out1;
517 }
518 flush_dcache_all();
519
520 /* Wakeup */
521 ret = brought_up_all_amp(images.fit_hdr_os, images.fit_uname_cfg);
522 if (ret)
523 AMP_E("Brought up amps, ret=%d\n", ret);
524 out1:
525 free(fit);
526 out2:
527 free(hdr);
528
529 return ret;
530 }
531
arm64_switch_amp_pe(bootm_headers_t * images)532 int arm64_switch_amp_pe(bootm_headers_t *images)
533 {
534 images->os.arch = g_bootcpu.arch;
535 return g_bootcpu.state;
536 }
537
538