1 /* SPDX-License-Identifier: GPL-2.0-only */
2
3 #include <types.h>
4 #include <soc/addressmap.h>
5 #include <device/mmio.h>
6 #include <soc/cpu.h>
7 #include <bdk-coreboot.h>
8 #include <console/console.h>
9 #include <timer.h>
10 #include <delay.h>
11
cpu_get_available_core_mask(void)12 uint64_t cpu_get_available_core_mask(void)
13 {
14 return read64p(RST_PP_AVAILABLE);
15 }
16
cpu_get_num_available_cores(void)17 size_t cpu_get_num_available_cores(void)
18 {
19 return bdk_dpop(cpu_get_available_core_mask());
20 }
21
22 static void (*secondary_c_entry)(size_t core_id);
23 static size_t secondary_booted;
24
secondary_cpu_init(size_t core_id)25 void secondary_cpu_init(size_t core_id)
26 {
27 write64(&secondary_booted, 1);
28 dmb();
29
30 if (secondary_c_entry)
31 secondary_c_entry(core_id);
32 else
33 asm("wfi");
34 }
35
cpu_self_get_core_id(void)36 size_t cpu_self_get_core_id(void)
37 {
38 u64 mpidr_el1;
39 asm("mrs %0, MPIDR_EL1\n\t" : "=r" (mpidr_el1) :: "memory");
40
41 /* Core is 4 bits from AFF0 and rest from AFF1 */
42 size_t core_num;
43 core_num = mpidr_el1 & 0xf;
44 core_num |= (mpidr_el1 & 0xff00) >> 4;
45
46 return core_num;
47 }
48
cpu_self_get_core_mask(void)49 uint64_t cpu_self_get_core_mask(void)
50 {
51 return 1ULL << cpu_self_get_core_id();
52 }
53
start_cpu(size_t cpu,void (* entry_64)(size_t core_id))54 size_t start_cpu(size_t cpu, void (*entry_64)(size_t core_id))
55 {
56 const uint64_t coremask = 1ULL << cpu;
57 struct stopwatch sw;
58 uint64_t pending;
59
60 printk(BIOS_DEBUG, "CPU: Starting CPU%zu @ %p.\n", cpu, entry_64);
61
62 /* Core not available */
63 if (!(coremask & cpu_get_available_core_mask()))
64 return 1;
65
66 /* Only secondary CPUs are supported */
67 if (cpu == cpu_self_get_core_id())
68 return 1;
69
70 /* Check stack here, instead of in cpu_secondary.S */
71 if ((CONFIG_STACK_SIZE * cpu) > REGION_SIZE(stack_sec))
72 return 1;
73
74 /* Write the address of the main entry point */
75 write64p(MIO_BOOT_AP_JUMP, (uintptr_t)secondary_init);
76
77 /* Get coremask of cores in reset */
78 const uint64_t reset = read64p(RST_PP_RESET);
79 printk(BIOS_INFO, "CPU: Cores currently in reset: 0x%llx\n", reset);
80
81 /* Setup entry for secondary core */
82 write64(&secondary_c_entry, (uintptr_t)entry_64);
83 write64(&secondary_booted, 0);
84 dmb();
85
86 printk(BIOS_DEBUG, "CPU: Taking core %zu out of reset.\n", cpu);
87
88 /* Release core from reset */
89 write64p(RST_PP_RESET, reset & ~coremask);
90
91 /* Wait for cores to finish coming out of reset */
92 udelay(1);
93
94 stopwatch_init_usecs_expire(&sw, 1000000);
95 do {
96 pending = read64p(RST_PP_PENDING);
97 } while (!stopwatch_expired(&sw) && (pending & coremask));
98
99 if (stopwatch_expired(&sw)) {
100 printk(BIOS_ERR, "Timeout waiting for reset "
101 "pending to clear.");
102 return 1;
103 }
104
105 stopwatch_init_usecs_expire(&sw, 1000000);
106
107 printk(BIOS_DEBUG, "CPU: Wait up to 1s for the core to boot...\n");
108 while (!stopwatch_expired(&sw) && !read64(&secondary_booted))
109 ;
110
111 /* Cleanup */
112 write64(&secondary_c_entry, 0);
113 dmb();
114
115 if (!read64(&secondary_booted)) {
116 printk(BIOS_ERR, "Core %zu failed to start.\n", cpu);
117 return 1;
118 }
119
120 printk(BIOS_INFO, "CPU: Core %zu booted\n", cpu);
121 return 0;
122 }
123