1
2 #include <libcflat.h>
3
4 #include <asm/barrier.h>
5
6 #include "processor.h"
7 #include "atomic.h"
8 #include "smp.h"
9 #include "apic.h"
10 #include "fwcfg.h"
11 #include "desc.h"
12 #include "alloc_page.h"
13 #include "asm/page.h"
14
15 #define IPI_VECTOR 0x20
16
17 typedef void (*ipi_function_type)(void *data);
18
19 static struct spinlock ipi_lock;
20 static volatile ipi_function_type ipi_function;
21 static void *volatile ipi_data;
22 static volatile int ipi_done;
23 static volatile bool ipi_wait;
24 static int _cpu_count;
25 static atomic_t active_cpus;
26 extern u8 rm_trampoline, rm_trampoline_end;
27 #if defined(__i386__) || defined(CONFIG_EFI)
28 extern u8 ap_rm_gdt_descr;
29 #endif
30
31 #ifdef CONFIG_EFI
32 extern u8 ap_rm_gdt, ap_rm_gdt_end;
33 extern u8 ap_start32;
34 extern u32 smp_stacktop;
35 extern u8 stacktop;
36 #endif
37
38 /* The BSP is online from time zero. */
39 atomic_t cpu_online_count = { .counter = 1 };
40 unsigned char online_cpus[(MAX_TEST_CPUS + 7) / 8];
41
ipi(void)42 static __attribute__((used)) void ipi(void)
43 {
44 void (*function)(void *data) = ipi_function;
45 void *data = ipi_data;
46 bool wait = ipi_wait;
47
48 if (!wait) {
49 ipi_done = 1;
50 apic_write(APIC_EOI, 0);
51 }
52 function(data);
53 atomic_dec(&active_cpus);
54 if (wait) {
55 ipi_done = 1;
56 apic_write(APIC_EOI, 0);
57 }
58 }
59
60 asm (
61 "ipi_entry: \n"
62 " call ipi \n"
63 #ifndef __x86_64__
64 " iret"
65 #else
66 " iretq"
67 #endif
68 );
69
cpu_count(void)70 int cpu_count(void)
71 {
72 return _cpu_count;
73 }
74
smp_id(void)75 int smp_id(void)
76 {
77 return this_cpu_read_smp_id();
78 }
79
setup_smp_id(void * data)80 static void setup_smp_id(void *data)
81 {
82 this_cpu_write_smp_id(apic_id());
83 }
84
ap_online(void)85 void ap_online(void)
86 {
87 sti();
88
89 printf("setup: CPU %" PRId32 " online\n", apic_id());
90 atomic_inc(&cpu_online_count);
91
92 /* Only the BSP runs the test's main(), APs are given work via IPIs. */
93 for (;;)
94 asm volatile("hlt");
95 }
96
__on_cpu(int cpu,void (* function)(void * data),void * data,int wait)97 static void __on_cpu(int cpu, void (*function)(void *data), void *data, int wait)
98 {
99 const u32 ipi_icr = APIC_INT_ASSERT | APIC_DEST_PHYSICAL | APIC_DM_FIXED | IPI_VECTOR;
100 unsigned int target = id_map[cpu];
101
102 spin_lock(&ipi_lock);
103 if (target == smp_id()) {
104 function(data);
105 } else {
106 atomic_inc(&active_cpus);
107 ipi_done = 0;
108 ipi_function = function;
109 ipi_data = data;
110 ipi_wait = wait;
111 apic_icr_write(ipi_icr, target);
112 while (!ipi_done)
113 ;
114 }
115 spin_unlock(&ipi_lock);
116 }
117
on_cpu(int cpu,void (* function)(void * data),void * data)118 void on_cpu(int cpu, void (*function)(void *data), void *data)
119 {
120 __on_cpu(cpu, function, data, 1);
121 }
122
on_cpu_async(int cpu,void (* function)(void * data),void * data)123 void on_cpu_async(int cpu, void (*function)(void *data), void *data)
124 {
125 __on_cpu(cpu, function, data, 0);
126 }
127
on_cpus(void (* function)(void * data),void * data)128 void on_cpus(void (*function)(void *data), void *data)
129 {
130 int cpu;
131
132 for (cpu = cpu_count() - 1; cpu >= 0; --cpu)
133 on_cpu_async(cpu, function, data);
134
135 while (cpus_active() > 1)
136 pause();
137 }
138
cpus_active(void)139 int cpus_active(void)
140 {
141 return atomic_read(&active_cpus);
142 }
143
smp_init(void)144 void smp_init(void)
145 {
146 int i;
147 void ipi_entry(void);
148
149 init_apic_map();
150 set_idt_entry(IPI_VECTOR, ipi_entry, 0);
151
152 setup_smp_id(0);
153 for (i = 1; i < cpu_count(); ++i)
154 on_cpu(i, setup_smp_id, 0);
155
156 atomic_inc(&active_cpus);
157 }
158
do_reset_apic(void * data)159 static void do_reset_apic(void *data)
160 {
161 reset_apic();
162 }
163
smp_reset_apic(void)164 void smp_reset_apic(void)
165 {
166 int i;
167
168 reset_apic();
169 for (i = 1; i < cpu_count(); ++i)
170 on_cpu(i, do_reset_apic, 0);
171
172 atomic_inc(&active_cpus);
173 }
174
setup_rm_gdt(void)175 static void setup_rm_gdt(void)
176 {
177 #ifdef __i386__
178 struct descriptor_table_ptr *rm_gdt =
179 (struct descriptor_table_ptr *) (&ap_rm_gdt_descr - &rm_trampoline);
180 /*
181 * On i386, place the gdt descriptor to be loaded from SIPI vector right after
182 * the vector code.
183 */
184 sgdt(rm_gdt);
185 #elif defined(CONFIG_EFI)
186 idt_entry_t *gate_descr;
187
188 /*
189 * The realmode trampoline on EFI has the following layout:
190 *
191 * |rm_trampoline:
192 * |sipi_entry:
193 * | <AP bootstrapping code called from SIPI>
194 * |ap_rm_gdt:
195 * | <GDT used for 16-bit -> 32-bit trasition>
196 * |ap_rm_gdt_descr:
197 * | <GDT descriptor for ap_rm_gdt>
198 * |sipi_end:
199 * | <End of trampoline>
200 * |rm_trampoline_end:
201 *
202 * After relocating to the lowmem address pointed to by realmode_trampoline,
203 * the realmode GDT descriptor needs to contain the relocated address of
204 * ap_rm_gdt.
205 */
206 volatile struct descriptor_table_ptr *rm_gdt_descr =
207 (struct descriptor_table_ptr *) (&ap_rm_gdt_descr - &rm_trampoline);
208 rm_gdt_descr->base = (ulong) ((u32) (&ap_rm_gdt - &rm_trampoline));
209 rm_gdt_descr->limit = (u16) (&ap_rm_gdt_end - &ap_rm_gdt - 1);
210
211 /*
212 * Since 1. compile time calculation of offsets is not allowed when
213 * building with -shared, and 2. rip-relative addressing is not supported in
214 * 16-bit mode, the relocated address of ap_rm_gdt_descr needs to be stored at
215 * a location known to / accessible from the trampoline.
216 *
217 * Use the last two bytes of the trampoline page (REALMODE_GDT_LOWMEM) to store
218 * a pointer to relocated ap_rm_gdt_descr addr. This way, the trampoline code can
219 * find the relocated descriptor using the lowmem address at pa=REALMODE_GDT_LOWMEM,
220 * and this relocated descriptor points to the relocated GDT.
221 */
222 *((u16 *)(REALMODE_GDT_LOWMEM)) = (u16) (u64) rm_gdt_descr;
223
224 /*
225 * Set up a call gate to the 32-bit entrypoint (ap_start32) within GDT, since
226 * EFI may not load the 32-bit AP entrypoint (ap_start32) low enough
227 * to be reachable from the SIPI vector.
228 *
229 * Since kvm-unit-tests builds with -shared, this location needs to be fetched
230 * at runtime, and rip-relative addressing is not supported in 16-bit mode. This
231 * prevents using a long jump to ap_start32 (`ljmpl $cs, $ap_start32`).
232 *
233 * As an alternative, a far return via `push $cs; push $label; lret` would require
234 * an intermediate trampoline since $label must still be within 0 - 0xFFFF for
235 * 16-bit far return to work.
236 *
237 * Using a call gate allows for an easier 16-bit -> 32-bit transition via `lcall`.
238 *
239 * GDT layout:
240 *
241 * Entry | Segment
242 * 0 | NULL descr
243 * 1 | Code segment descr
244 * 2 | Data segment descr
245 * 3 | Call gate descr
246 *
247 * This layout is only used for reaching 32-bit mode. APs load a 64-bit GDT
248 * later during boot, which does not need to follow this layout.
249 */
250 gate_descr = ((void *)(&ap_rm_gdt - &rm_trampoline) + 3 * sizeof(gdt_entry_t));
251 set_desc_entry(gate_descr, sizeof(gdt_entry_t), (void *) &ap_start32,
252 0x8 /* sel */, 0xc /* type */, 0 /* dpl */);
253 #endif
254 }
255
bringup_aps(void)256 void bringup_aps(void)
257 {
258 void *rm_trampoline_dst = RM_TRAMPOLINE_ADDR;
259 size_t rm_trampoline_size = (&rm_trampoline_end - &rm_trampoline) + 1;
260 assert(rm_trampoline_size < PAGE_SIZE);
261
262 asm volatile("cld");
263
264 /*
265 * Fill the trampoline page with with INT3 (0xcc) so that any AP
266 * that goes astray within the first page gets a fault.
267 */
268 memset(rm_trampoline_dst, 0xcc /* INT3 */, PAGE_SIZE);
269
270 memcpy(rm_trampoline_dst, &rm_trampoline, rm_trampoline_size);
271
272 setup_rm_gdt();
273
274 #ifdef CONFIG_EFI
275 smp_stacktop = ((u64) (&stacktop)) - PER_CPU_SIZE;
276 #endif
277
278 /* INIT */
279 apic_icr_write(APIC_DEST_ALLBUT | APIC_DEST_PHYSICAL | APIC_DM_INIT | APIC_INT_ASSERT, 0);
280
281 /* SIPI */
282 apic_icr_write(APIC_DEST_ALLBUT | APIC_DEST_PHYSICAL | APIC_DM_STARTUP, 0);
283
284 _cpu_count = fwcfg_get_nb_cpus();
285
286 printf("smp: waiting for %d APs\n", _cpu_count - 1);
287 while (_cpu_count != atomic_read(&cpu_online_count))
288 cpu_relax();
289 }
290