blob: ef2675ad6af0e3105d30c5b059c40e612725b05d [file] [log] [blame]
Martin Roth9b1b3352016-02-24 12:27:06 -08001/*
2 * MemTest86+ V5 Specific code (GPL V2.0)
3 * By Samuel DEMEULEMEESTER, sdemeule@memtest.org
4 * http://www.canardpc.com - http://www.memtest.org
5 * ------------------------------------------------
6 * smp.c - MemTest-86 Version 3.5
7 *
8 * Released under version 2 of the Gnu Public License.
9 * By Chris Brady
10 */
11
12#include "stddef.h"
13#include "stdint.h"
14#include "cpuid.h"
15#include "smp.h"
16#include "test.h"
17
18#define DELAY_FACTOR 1
19unsigned num_cpus = 1; // There is at least one cpu, the BSP
20int act_cpus;
21unsigned found_cpus = 0;
22
23extern void memcpy(void *dst, void *src , int len);
24extern void test_start(void);
25extern int run_cpus;
26extern int maxcpus;
27extern char cpu_mask[];
28extern struct cpu_ident cpu_id;
29
30struct barrier_s *barr;
31
Martin Roth69593432016-03-27 19:46:03 -060032void smp_find_cpus(void);
Martin Roth9b1b3352016-02-24 12:27:06 -080033
34void barrier_init(int max)
35{
Elyes HAOUAS54506bf2018-08-24 09:17:59 +020036 /* Set the address of the barrier structure */
Martin Roth9b1b3352016-02-24 12:27:06 -080037 barr = (struct barrier_s *)0x9ff00;
38 barr->lck.slock = 1;
39 barr->mutex.slock = 1;
40 barr->maxproc = max;
41 barr->count = max;
42 barr->st1.slock = 1;
43 barr->st2.slock = 0;
44}
45
46void s_barrier_init(int max)
47{
48 barr->s_lck.slock = 1;
49 barr->s_maxproc = max;
50 barr->s_count = max;
51 barr->s_st1.slock = 1;
52 barr->s_st2.slock = 0;
53}
54
Martin Roth69593432016-03-27 19:46:03 -060055void barrier(void)
Martin Roth9b1b3352016-02-24 12:27:06 -080056{
57 if (num_cpus == 1 || v->fail_safe & 3) {
58 return;
59 }
60 spin_wait(&barr->st1); /* Wait if the barrier is active */
61 spin_lock(&barr->lck); /* Get lock for barr struct */
62 if (--barr->count == 0) { /* Last process? */
63 barr->st1.slock = 0; /* Hold up any processes re-entering */
64 barr->st2.slock = 1; /* Release the other processes */
65 barr->count++;
Martin Roth4dcd13d2016-02-24 13:53:07 -080066 spin_unlock(&barr->lck);
Martin Roth9b1b3352016-02-24 12:27:06 -080067 } else {
Martin Roth4dcd13d2016-02-24 13:53:07 -080068 spin_unlock(&barr->lck);
Martin Roth9b1b3352016-02-24 12:27:06 -080069 spin_wait(&barr->st2); /* wait for peers to arrive */
Martin Roth4dcd13d2016-02-24 13:53:07 -080070 spin_lock(&barr->lck);
71 if (++barr->count == barr->maxproc) {
72 barr->st1.slock = 1;
73 barr->st2.slock = 0;
Martin Roth9b1b3352016-02-24 12:27:06 -080074 }
Martin Roth4dcd13d2016-02-24 13:53:07 -080075 spin_unlock(&barr->lck);
Martin Roth9b1b3352016-02-24 12:27:06 -080076 }
77}
78
Martin Roth69593432016-03-27 19:46:03 -060079void s_barrier(void)
Martin Roth9b1b3352016-02-24 12:27:06 -080080{
81 if (run_cpus == 1 || v->fail_safe & 3) {
82 return;
83 }
84 spin_wait(&barr->s_st1); /* Wait if the barrier is active */
85 spin_lock(&barr->s_lck); /* Get lock for barr struct */
86 if (--barr->s_count == 0) { /* Last process? */
87 barr->s_st1.slock = 0; /* Hold up any processes re-entering */
88 barr->s_st2.slock = 1; /* Release the other processes */
89 barr->s_count++;
Martin Roth4dcd13d2016-02-24 13:53:07 -080090 spin_unlock(&barr->s_lck);
Martin Roth9b1b3352016-02-24 12:27:06 -080091 } else {
Martin Roth4dcd13d2016-02-24 13:53:07 -080092 spin_unlock(&barr->s_lck);
Martin Roth9b1b3352016-02-24 12:27:06 -080093 spin_wait(&barr->s_st2); /* wait for peers to arrive */
Martin Roth4dcd13d2016-02-24 13:53:07 -080094 spin_lock(&barr->s_lck);
95 if (++barr->s_count == barr->s_maxproc) {
96 barr->s_st1.slock = 1;
97 barr->s_st2.slock = 0;
Martin Roth9b1b3352016-02-24 12:27:06 -080098 }
Martin Roth4dcd13d2016-02-24 13:53:07 -080099 spin_unlock(&barr->s_lck);
Martin Roth9b1b3352016-02-24 12:27:06 -0800100 }
101}
102
103typedef struct {
104 bool started;
105} ap_info_t;
106
107volatile apic_register_t *APIC = NULL;
108/* CPU number to APIC ID mapping table. CPU 0 is the BSP. */
109static unsigned cpu_num_to_apic_id[MAX_CPUS];
110volatile ap_info_t AP[MAX_CPUS];
111
112void PUT_MEM16(uintptr_t addr, uint16_t val)
113{
114 *((volatile uint16_t *)addr) = val;
115}
116
117void PUT_MEM32(uintptr_t addr, uint32_t val)
118{
119 *((volatile uint32_t *)addr) = val;
120}
121
Martin Roth4dcd13d2016-02-24 13:53:07 -0800122static void inline
Martin Roth9b1b3352016-02-24 12:27:06 -0800123APIC_WRITE(unsigned reg, uint32_t val)
124{
125 APIC[reg][0] = val;
126}
127
Martin Roth4dcd13d2016-02-24 13:53:07 -0800128static inline uint32_t
Martin Roth9b1b3352016-02-24 12:27:06 -0800129APIC_READ(unsigned reg)
130{
131 return APIC[reg][0];
132}
133
134
Martin Roth4dcd13d2016-02-24 13:53:07 -0800135static void
Martin Roth9b1b3352016-02-24 12:27:06 -0800136SEND_IPI(unsigned apic_id, unsigned trigger, unsigned level, unsigned mode,
137 uint8_t vector)
138{
139 uint32_t v;
140
141 v = APIC_READ(APICR_ICRHI) & 0x00ffffff;
142 APIC_WRITE(APICR_ICRHI, v | (apic_id << 24));
143
144 v = APIC_READ(APICR_ICRLO) & ~0xcdfff;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800145 v |= (APIC_DEST_DEST << APIC_ICRLO_DEST_OFFSET)
Martin Roth9b1b3352016-02-24 12:27:06 -0800146 | (trigger << APIC_ICRLO_TRIGGER_OFFSET)
147 | (level << APIC_ICRLO_LEVEL_OFFSET)
148 | (mode << APIC_ICRLO_DELMODE_OFFSET)
149 | (vector);
150 APIC_WRITE(APICR_ICRLO, v);
151}
152
153
154// Silly way of busywaiting, but we don't have a timer
Martin Roth4dcd13d2016-02-24 13:53:07 -0800155void delay(unsigned us)
Martin Roth9b1b3352016-02-24 12:27:06 -0800156{
157 unsigned freq = 1000; // in MHz, assume 1GHz CPU speed
Ben Gardner1848c282016-03-30 16:32:11 -0500158 uint64_t cycles = (uint64_t)us * freq;
Martin Roth9b1b3352016-02-24 12:27:06 -0800159 uint64_t t0 = RDTSC();
160 uint64_t t1;
161 volatile unsigned k;
162
163 do {
164 for (k = 0; k < 1000; k++) continue;
165 t1 = RDTSC();
166 } while (t1 - t0 < cycles);
167}
168
169static inline void
170memset (void *dst,
171 char value,
172 int len)
173{
174 int i;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800175 for (i = 0 ; i < len ; i++ ) {
Martin Roth9b1b3352016-02-24 12:27:06 -0800176 *((char *) dst + i) = value;
177 }
178}
179void initialise_cpus(void)
180{
181 int i;
182
183 act_cpus = 0;
184
185 if (maxcpus > 1) {
186 smp_find_cpus();
187 /* The total number of CPUs may be limited */
188 if (num_cpus > maxcpus) {
189 num_cpus = maxcpus;
190 }
191 /* Determine how many cpus have been selected */
192 for(i = 0; i < num_cpus; i++) {
193 if (cpu_mask[i]) {
194 act_cpus++;
195 }
196 }
197 } else {
198 act_cpus = found_cpus = num_cpus = 1;
199 }
200
201 /* Initialize the barrier before starting AP's */
202 barrier_init(act_cpus);
203
204 /* let the BSP initialise the APs. */
205 for(i = 1; i < num_cpus; i++) {
206 /* Only start this CPU if it is selected by the mask */
207 if (cpu_mask[i]) {
208 smp_boot_ap(i);
209 }
210 }
211
212}
213void kick_cpu(unsigned cpu_num)
214{
215 unsigned num_sipi, apic_id;
216 apic_id = cpu_num_to_apic_id[cpu_num];
217
218 // clear the APIC ESR register
219 APIC_WRITE(APICR_ESR, 0);
220 APIC_READ(APICR_ESR);
221
222 // asserting the INIT IPI
223 SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
224 delay(100000 / DELAY_FACTOR);
225
226 // de-assert the INIT IPI
227 SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);
228
229 for (num_sipi = 0; num_sipi < 2; num_sipi++) {
230 unsigned timeout;
231 bool send_pending;
232 unsigned err;
233
234 APIC_WRITE(APICR_ESR, 0);
235
236 SEND_IPI(apic_id, 0, 0, APIC_DELMODE_STARTUP, (unsigned)startup_32 >> 12);
237
238 timeout = 0;
239 do {
240 delay(10);
241 timeout++;
242 send_pending = (APIC_READ(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
243 } while (send_pending && timeout < 1000);
244
245 if (send_pending) {
246 cprint(LINE_STATUS+3, 0, "SMP: STARTUP IPI was never sent");
247 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800248
Martin Roth9b1b3352016-02-24 12:27:06 -0800249 delay(100000 / DELAY_FACTOR);
250
251 err = APIC_READ(APICR_ESR) & 0xef;
252 if (err) {
253 cprint(LINE_STATUS+3, 0, "SMP: After STARTUP IPI: err = 0x");
254 hprint(LINE_STATUS+3, COL_MID, err);
255 }
256 }
257}
258
259// These memory locations are used for the trampoline code and data.
260
261#define BOOTCODESTART 0x9000
262#define GDTPOINTERADDR 0x9100
263#define GDTADDR 0x9110
264
265void boot_ap(unsigned cpu_num)
266{
267 unsigned num_sipi, apic_id;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800268 extern uint8_t gdt;
Martin Roth9b1b3352016-02-24 12:27:06 -0800269 extern uint8_t _ap_trampoline_start;
270 extern uint8_t _ap_trampoline_protmode;
271 unsigned len = &_ap_trampoline_protmode - &_ap_trampoline_start;
272 apic_id = cpu_num_to_apic_id[cpu_num];
273
274
275 memcpy((uint8_t*)BOOTCODESTART, &_ap_trampoline_start, len);
276
277 // Fixup the LGDT instruction to point to GDT pointer.
278 PUT_MEM16(BOOTCODESTART + 3, GDTPOINTERADDR);
279
280 // Copy a pointer to the temporary GDT to addr GDTPOINTERADDR.
281 // The temporary gdt is at addr GDTADDR
282 PUT_MEM16(GDTPOINTERADDR, 4 * 8);
283 PUT_MEM32(GDTPOINTERADDR + 2, GDTADDR);
284
285 // Copy the first 4 gdt entries from the currently used GDT to the
286 // temporary GDT.
287 memcpy((uint8_t *)GDTADDR, &gdt, 32);
288
289 // clear the APIC ESR register
290 APIC_WRITE(APICR_ESR, 0);
291 APIC_READ(APICR_ESR);
292
293 // asserting the INIT IPI
294 SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
295 delay(100000 / DELAY_FACTOR);
296
297 // de-assert the INIT IPI
298 SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);
299
300 for (num_sipi = 0; num_sipi < 2; num_sipi++) {
301 unsigned timeout;
302 bool send_pending;
303 unsigned err;
304
305 APIC_WRITE(APICR_ESR, 0);
306
307 SEND_IPI(apic_id, 0, 0, APIC_DELMODE_STARTUP, BOOTCODESTART >> 12);
308
309 timeout = 0;
310 do {
311 delay(10);
312 timeout++;
313 send_pending = (APIC_READ(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
314 } while (send_pending && timeout < 1000);
315
316 if (send_pending) {
317 cprint(LINE_STATUS+3, 0, "SMP: STARTUP IPI was never sent");
318 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800319
Martin Roth9b1b3352016-02-24 12:27:06 -0800320 delay(100000 / DELAY_FACTOR);
321
322 err = APIC_READ(APICR_ESR) & 0xef;
323 if (err) {
324 cprint(LINE_STATUS+3, 0, "SMP: After STARTUP IPI: err = 0x");
325 hprint(LINE_STATUS+3, COL_MID, err);
326 }
327 }
328}
329
330static int checksum(unsigned char *mp, int len)
331{
332 int sum = 0;
333
334 while (len--) {
335 sum += *mp++;
336 }
337 return (sum & 0xFF);
338}
339
340/* Parse an MP config table for CPU information */
341bool read_mp_config_table(uintptr_t addr)
342{
343 mp_config_table_header_t *mpc = (mp_config_table_header_t*)addr;
344 uint8_t *tab_entry_ptr;
345 uint8_t *mpc_table_end;
346
347 if (mpc->signature != MPCSignature) {
348 return FALSE;
349 }
350 if (checksum((unsigned char*)mpc, mpc->length) != 0) {
351 return FALSE;
352 }
353
354 /* FIXME: the uintptr_t cast here works around a compilation problem on
355 * AMD64, but it ignores the real problem, which is that lapic_addr
356 * is only 32 bits. Maybe that's OK, but it should be investigated.
357 */
358 APIC = (volatile apic_register_t*)(uintptr_t)mpc->lapic_addr;
359
360 tab_entry_ptr = ((uint8_t*)mpc) + sizeof(mp_config_table_header_t);
361 mpc_table_end = ((uint8_t*)mpc) + mpc->length;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800362
Martin Roth9b1b3352016-02-24 12:27:06 -0800363 while (tab_entry_ptr < mpc_table_end) {
364 switch (*tab_entry_ptr) {
365 case MP_PROCESSOR: {
366 mp_processor_entry_t *pe = (mp_processor_entry_t*)tab_entry_ptr;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800367
Martin Roth9b1b3352016-02-24 12:27:06 -0800368 if (pe->cpu_flag & CPU_BOOTPROCESSOR) {
369 // BSP is CPU 0
370 cpu_num_to_apic_id[0] = pe->apic_id;
371 } else if (num_cpus < MAX_CPUS) {
372 cpu_num_to_apic_id[num_cpus] = pe->apic_id;
373 num_cpus++;
374 }
375 found_cpus++;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800376
Martin Roth9b1b3352016-02-24 12:27:06 -0800377 // we cannot handle non-local 82489DX apics
378 if ((pe->apic_ver & 0xf0) != 0x10) {
379 return 0;
380 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800381
Martin Roth9b1b3352016-02-24 12:27:06 -0800382 tab_entry_ptr += sizeof(mp_processor_entry_t);
383 break;
384 }
385 case MP_BUS: {
386 tab_entry_ptr += sizeof(mp_bus_entry_t);
387 break;
388 }
389 case MP_IOAPIC: {
390 tab_entry_ptr += sizeof(mp_io_apic_entry_t);
391 break;
392 }
393 case MP_INTSRC:
394 tab_entry_ptr += sizeof(mp_interrupt_entry_t);
395 break;
396 case MP_LINTSRC:
397 tab_entry_ptr += sizeof(mp_local_interrupt_entry_t);
398 break;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800399 default:
Martin Roth9b1b3352016-02-24 12:27:06 -0800400 return FALSE;
401 }
402 }
403 return TRUE;
404}
405
406/* Search for a Floating Pointer structure */
407floating_pointer_struct_t *
408scan_for_floating_ptr_struct(uintptr_t addr, uint32_t length)
409{
410 floating_pointer_struct_t *fp;
411 uintptr_t end = addr + length;
412
413
414 while ((uintptr_t)addr < end) {
415 fp = (floating_pointer_struct_t*)addr;
416 if (*(unsigned int *)addr == FPSignature && fp->length == 1 && checksum((unsigned char*)addr, 16) == 0 && ((fp->spec_rev == 1) || (fp->spec_rev == 4))) {
417 return fp;
418 }
419 addr += 4;
420 }
421 return NULL;
422}
423
424/* Search for a Root System Descriptor Pointer */
425rsdp_t *scan_for_rsdp(uintptr_t addr, uint32_t length)
426{
427 rsdp_t *rp;
428 uintptr_t end = addr + length;
429
430
431 while ((uintptr_t)addr < end) {
432 rp = (rsdp_t*)addr;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800433 if (*(unsigned int *)addr == RSDPSignature &&
Martin Roth9b1b3352016-02-24 12:27:06 -0800434 checksum((unsigned char*)addr, rp->length) == 0) {
435 return rp;
436 }
437 addr += 4;
438 }
439 return NULL;
440}
441
442/* Parse a MADT table for processor entries */
443int parse_madt(uintptr_t addr) {
444
445 mp_config_table_header_t *mpc = (mp_config_table_header_t*)addr;
446 uint8_t *tab_entry_ptr;
447 uint8_t *mpc_table_end;
448
449 if (checksum((unsigned char*)mpc, mpc->length) != 0) {
450 return FALSE;
451 }
452
453 APIC = (volatile apic_register_t*)(uintptr_t)mpc->lapic_addr;
454
455 tab_entry_ptr = ((uint8_t*)mpc) + sizeof(mp_config_table_header_t);
456 mpc_table_end = ((uint8_t*)mpc) + mpc->length;
457 while (tab_entry_ptr < mpc_table_end) {
Martin Roth4dcd13d2016-02-24 13:53:07 -0800458
Martin Roth9b1b3352016-02-24 12:27:06 -0800459 madt_processor_entry_t *pe = (madt_processor_entry_t*)tab_entry_ptr;
460 if (pe->type == MP_PROCESSOR) {
461 if (pe->enabled) {
462 if (num_cpus < MAX_CPUS) {
463 cpu_num_to_apic_id[num_cpus] = pe->apic_id;
Martin Roth4dcd13d2016-02-24 13:53:07 -0800464
Martin Roth9b1b3352016-02-24 12:27:06 -0800465 /* the first CPU is the BSP, don't increment */
466 if (found_cpus) {
467 num_cpus++;
468 }
469 }
470 found_cpus++;
471 }
472 }
473 tab_entry_ptr += pe->length;
474 }
475 return TRUE;
476}
477
478/* This is where we search for SMP information in the following order
479 * look for a floating MP pointer
480 * found:
481 * check for a default configuration
482 * found:
483 * setup config, return
484 * check for a MP config table
485 * found:
486 * validate:
487 * good:
488 * parse the MP config table
489 * good:
490 * setup config, return
491 *
492 * find & validate ACPI RSDP (Root System Descriptor Pointer)
493 * found:
494 * find & validate RSDT (Root System Descriptor Table)
495 * found:
496 * find & validate MSDT
497 * found:
498 * parse the MADT table
499 * good:
500 * setup config, return
501 */
Martin Roth69593432016-03-27 19:46:03 -0600502void smp_find_cpus(void)
Martin Roth9b1b3352016-02-24 12:27:06 -0800503{
504 floating_pointer_struct_t *fp;
505 rsdp_t *rp;
506 rsdt_t *rt;
507 uint8_t *tab_ptr, *tab_end;
508 unsigned int *ptr;
509 unsigned int uiptr;
510
511 if(v->fail_safe & 3) { return; }
512
Ben Gardner22030752016-03-04 17:47:36 -0600513 memset((void *)&AP, 0, sizeof AP);
Martin Roth9b1b3352016-02-24 12:27:06 -0800514
515 if(v->fail_safe & 8)
Martin Roth4dcd13d2016-02-24 13:53:07 -0800516 {
Martin Roth9b1b3352016-02-24 12:27:06 -0800517 // Search for the Floating MP structure pointer
518 fp = scan_for_floating_ptr_struct(0x0, 0x400);
519 if (fp == NULL) {
520 fp = scan_for_floating_ptr_struct(639*0x400, 0x400);
521 }
522 if (fp == NULL) {
523 fp = scan_for_floating_ptr_struct(0xf0000, 0x10000);
524 }
525 if (fp == NULL) {
526 // Search the BIOS ESDS area
527 unsigned int address = *(unsigned short *)0x40E;
528 address <<= 4;
529 if (address) {
530 fp = scan_for_floating_ptr_struct(address, 0x400);
531 }
532 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800533
Martin Roth9b1b3352016-02-24 12:27:06 -0800534 if (fp != NULL) {
535 // We have a floating MP pointer
536 // Is this a default configuration?
Martin Roth4dcd13d2016-02-24 13:53:07 -0800537
Martin Roth9b1b3352016-02-24 12:27:06 -0800538 if (fp->feature[0] > 0 && fp->feature[0] <=7) {
539 // This is a default config so plug in the numbers
540 num_cpus = 2;
541 APIC = (volatile apic_register_t*)0xFEE00000;
542 cpu_num_to_apic_id[0] = 0;
543 cpu_num_to_apic_id[1] = 1;
544 return;
545 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800546
Martin Roth9b1b3352016-02-24 12:27:06 -0800547 // Do we have a pointer to a MP configuration table?
548 if ( fp->phys_addr != 0) {
549 if (read_mp_config_table(fp->phys_addr)) {
550 // Found a good MP table, done
551 return;
552 }
553 }
554 }
555 }
556
557
558 /* No MP table so far, try to find an ACPI MADT table
559 * We try to use the MP table first since there is no way to distinguish
560 * real cores from hyper-threads in the MADT */
561
562 /* Search for the RSDP */
563 rp = scan_for_rsdp(0xE0000, 0x20000);
564 if (rp == NULL) {
565 /* Search the BIOS ESDS area */
566 unsigned int address = *(unsigned short *)0x40E;
567 address <<= 4;
568 if (address) {
569 rp = scan_for_rsdp(address, 0x400);
570 }
571 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800572
Martin Roth9b1b3352016-02-24 12:27:06 -0800573 if (rp == NULL) {
574 /* RSDP not found, give up */
575 return;
576 }
577
578 /* Found the RSDP, now get either the RSDT or XSDT */
579 if (rp->revision >= 2) {
580 rt = (rsdt_t *)rp->xrsdt[0];
Martin Roth4dcd13d2016-02-24 13:53:07 -0800581
Martin Roth9b1b3352016-02-24 12:27:06 -0800582 if (rt == 0) {
583 return;
584 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800585 // Validate the XSDT
Martin Roth9b1b3352016-02-24 12:27:06 -0800586 if (*(unsigned int *)rt != XSDTSignature) {
587 return;
588 }
589 if ( checksum((unsigned char*)rt, rt->length) != 0) {
590 return;
591 }
Martin Roth4dcd13d2016-02-24 13:53:07 -0800592
Martin Roth9b1b3352016-02-24 12:27:06 -0800593 } else {
594 rt = (rsdt_t *)rp->rsdt;
595 if (rt == 0) {
596 return;
597 }
598 /* Validate the RSDT */
599 if (*(unsigned int *)rt != RSDTSignature) {
600 return;
601 }
602 if ( checksum((unsigned char*)rt, rt->length) != 0) {
603 return;
604 }
605 }
606
607 /* Scan the RSDT or XSDT for a pointer to the MADT */
608 tab_ptr = ((uint8_t*)rt) + sizeof(rsdt_t);
609 tab_end = ((uint8_t*)rt) + rt->length;
610
611 while (tab_ptr < tab_end) {
612
613 uiptr = *((unsigned int *)tab_ptr);
614 ptr = (unsigned int *)uiptr;
615
616 /* Check for the MADT signature */
617 if (ptr && *ptr == MADTSignature) {
618 /* Found it, now parse it */
619 if (parse_madt((uintptr_t)ptr)) {
620 return;
621 }
622 }
623 tab_ptr += 4;
624 }
625}
Martin Roth4dcd13d2016-02-24 13:53:07 -0800626
Martin Roth69593432016-03-27 19:46:03 -0600627unsigned my_apic_id(void)
Martin Roth9b1b3352016-02-24 12:27:06 -0800628{
629 return (APIC[APICR_ID][0]) >> 24;
630}
631
Martin Roth4dcd13d2016-02-24 13:53:07 -0800632void smp_ap_booted(unsigned cpu_num)
Martin Roth9b1b3352016-02-24 12:27:06 -0800633{
634 AP[cpu_num].started = TRUE;
635}
636
637void smp_boot_ap(unsigned cpu_num)
638{
639 unsigned timeout;
640
641 boot_ap(cpu_num);
642 timeout = 0;
643 do {
644 delay(1000 / DELAY_FACTOR);
645 timeout++;
646 } while (!AP[cpu_num].started && timeout < 100000 / DELAY_FACTOR);
647
648 if (!AP[cpu_num].started) {
649 cprint(LINE_STATUS+3, 0, "SMP: Boot timeout for");
650 dprint(LINE_STATUS+3, COL_MID, cpu_num,2,1);
651 cprint(LINE_STATUS+3, 26, "Turning off SMP");
652 }
653}
654
Martin Roth69593432016-03-27 19:46:03 -0600655unsigned smp_my_cpu_num(void)
Martin Roth9b1b3352016-02-24 12:27:06 -0800656{
657 unsigned apicid = my_apic_id();
658 unsigned i;
659
660 for (i = 0; i < MAX_CPUS; i++) {
661 if (apicid == cpu_num_to_apic_id[i]) {
662 break;
663 }
664 }
665 if (i == MAX_CPUS) {
666 i = 0;
667 }
668 return i;
669}
670
671/* A set of simple functions used to preserve assigned CPU ordinals since
672 * they are lost after relocation (the stack is reloaded).
673 */
674int num_to_ord[MAX_CPUS];
675void smp_set_ordinal(int me, int ord)
676{
677 num_to_ord[me] = ord;
678}
679
680int smp_my_ord_num(int me)
681{
682 return num_to_ord[me];
683}
684
685int smp_ord_to_cpu(int me)
686{
687 int i;
688 for (i=0; i<MAX_CPUS; i++) {
689 if (num_to_ord[i] == me) return i;
690 }
691 return -1;
Elyes HAOUAS54506bf2018-08-24 09:17:59 +0200692}