smp.c - chromiumos/third_party/memtest - Git at Google

 /*
  * smp.c --
  */

 #include "stddef.h"
 #include "smp.h"
 #include "cpuid.h"
 #include "test.h"
 #define DELAY_FACTOR 1
 unsigned num_cpus = 1; // There is at least one cpu, the BSP
 unsigned found_cpus = 0;

 extern void memcpy(void *dst, void *src , int len);
 extern void test_start(void);
 extern int run_cpus;

 struct barrier_s *barr;

 void barrier_init(int max)
 {
 	/* Set the adddress of the barrier structure */
 	barr = (struct barrier_s *)0x9ff00;
         barr->lck.slock = 1;
         barr->mutex.slock = 1;
         barr->maxproc = max;
         barr->count = max;
         barr->st1.slock = 1;
         barr->st2.slock = 0;
 }

 void s_barrier_init(int max)
 {
         barr->s_lck.slock = 1;
         barr->s_maxproc = max;
         barr->s_count = max;
         barr->s_st1.slock = 1;
         barr->s_st2.slock = 0;
 }

 /* Spin until var = 1 */
 void spin_wait(spinlock_t *lck)
 {
         int inc = 0x400;

         asm volatile( "1:\t"
                       "cmpb $0,%1\n\t"
                       "jne 2f\n\t"
                       "rep ; nop\n\t"
                       "jmp 1b\n"
                       "2:"
                       : : "c" (inc), "m" (lck->slock): "memory" );
 }

 void barrier()
 {
 	if (num_cpus == 1) {
 		return;
 	}
 	spin_wait(&barr->st1);     /* Wait if the barrier is active */
         spin_lock(&barr->lck);	   /* Get lock for barr struct */
         if (--barr->count == 0) {  /* Last process? */
                 barr->st1.slock = 0;   /* Hold up any processes re-entering */
                 barr->st2.slock = 1;   /* Release the other processes */
                 barr->count++;
                 spin_unlock(&barr->lck);
         } else {
                 spin_unlock(&barr->lck);
                 spin_wait(&barr->st2);	/* wait for peers to arrive */
                 spin_lock(&barr->lck);
                 if (++barr->count == barr->maxproc) {
                         barr->st1.slock = 1;
                         barr->st2.slock = 0;
                 }
                 spin_unlock(&barr->lck);
         }
 }

 void s_barrier()
 {
 	if (run_cpus == 1) {
 		return;
 	}
 	spin_wait(&barr->s_st1);     /* Wait if the barrier is active */
         spin_lock(&barr->s_lck);     /* Get lock for barr struct */
         if (--barr->s_count == 0) {  /* Last process? */
                 barr->s_st1.slock = 0;   /* Hold up any processes re-entering */
                 barr->s_st2.slock = 1;   /* Release the other processes */
                 barr->s_count++;
                 spin_unlock(&barr->s_lck);
         } else {
                 spin_unlock(&barr->s_lck);
                 spin_wait(&barr->s_st2);	/* wait for peers to arrive */
                 spin_lock(&barr->s_lck);
                 if (++barr->s_count == barr->s_maxproc) {
                         barr->s_st1.slock = 1;
                         barr->s_st2.slock = 0;
                 }
                 spin_unlock(&barr->s_lck);
         }
 }

 typedef struct {
    bool started;
 } ap_info_t;

 volatile apic_register_t *APIC = NULL;
 /* CPU number to APIC ID mapping table. CPU 0 is the BSP. */
 static unsigned cpu_num_to_apic_id[MAX_CPUS];
 volatile ap_info_t AP[MAX_CPUS];

 void PUT_MEM16(uintptr_t addr, uint16_t val)
 {
    *((volatile uint16_t *)addr) = val;
 }

 void PUT_MEM32(uintptr_t addr, uint32_t val)
 {
    *((volatile uint32_t *)addr) = val;
 }

 static void inline
 APIC_WRITE(unsigned reg, uint32_t val)
 {
    APIC[reg][0] = val;
 }

 static inline uint32_t
 APIC_READ(unsigned reg)
 {
    return APIC[reg][0];
 }


 static void
 SEND_IPI(unsigned apic_id, unsigned trigger, unsigned level, unsigned mode,
 	    uint8_t vector)
 {
    uint32_t v;

    v = APIC_READ(APICR_ICRHI) & 0x00ffffff;
    APIC_WRITE(APICR_ICRHI, v | (apic_id << 24));

    v = APIC_READ(APICR_ICRLO) & ~0xcdfff;
    v |= (APIC_DEST_DEST << APIC_ICRLO_DEST_OFFSET)
       | (trigger << APIC_ICRLO_TRIGGER_OFFSET)
       | (level << APIC_ICRLO_LEVEL_OFFSET)
       | (mode << APIC_ICRLO_DELMODE_OFFSET)
       | (vector);
    APIC_WRITE(APICR_ICRLO, v);
 }


 // Silly way of busywaiting, but we don't have a timer
 void delay(unsigned us)
 {
    unsigned freq = 1000; // in MHz, assume 1GHz CPU speed
    uint64_t cycles = us * freq;
    uint64_t t0 = RDTSC();
    uint64_t t1;
    volatile unsigned k;

    do {
       for (k = 0; k < 1000; k++) continue;
       t1 = RDTSC();
    } while (t1 - t0 < cycles);
 }

 static inline void
 memset (void *dst,
         char  value,
         int   len)
 {
    int i;
    for (i = 0 ; i < len ; i++ ) {
       *((char *) dst + i) = value;
    }
 }

 void kick_cpu(unsigned cpu_num)
 {
    unsigned num_sipi, apic_id;
    apic_id = cpu_num_to_apic_id[cpu_num];

    // clear the APIC ESR register
    APIC_WRITE(APICR_ESR, 0);
    APIC_READ(APICR_ESR);

    // asserting the INIT IPI
    SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
    delay(100000 / DELAY_FACTOR);

    // de-assert the INIT IPI
    SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);

    for (num_sipi = 0; num_sipi < 2; num_sipi++) {
       unsigned timeout;
       bool send_pending;
       unsigned err;

       APIC_WRITE(APICR_ESR, 0);

       SEND_IPI(apic_id, 0, 0, APIC_DELMODE_STARTUP, (unsigned)startup_32 >> 12);

       timeout = 0;
       do {
 	 delay(10);
 	 timeout++;
 	 send_pending = (APIC_READ(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
       } while (send_pending && timeout < 1000);

       if (send_pending) {
 	 cprint(LINE_STATUS+1, 0, "SMP: STARTUP IPI was never sent");
       }

       delay(100000 / DELAY_FACTOR);

       err = APIC_READ(APICR_ESR) & 0xef;
       if (err) {
 	 cprint(LINE_STATUS+1, 0, "SMP: After STARTUP IPI: err = 0x");
          hprint(LINE_STATUS+1, COL_MID, err);
       }
    }
 }

 // These memory locations are used for the trampoline code and data.

 #define BOOTCODESTART 0x9000
 #define GDTPOINTERADDR 0x9100
 #define GDTADDR 0x9110

 void boot_ap(unsigned cpu_num)
 {
    unsigned num_sipi, apic_id;
    extern uint8_t gdt;
    extern uint8_t _ap_trampoline_start;
    extern uint8_t _ap_trampoline_protmode;
    unsigned len = &_ap_trampoline_protmode - &_ap_trampoline_start;
    apic_id = cpu_num_to_apic_id[cpu_num];


    memcpy((uint8_t*)BOOTCODESTART, &_ap_trampoline_start, len);

    // Fixup the LGDT instruction to point to GDT pointer.
    PUT_MEM16(BOOTCODESTART + 3, GDTPOINTERADDR);

    // Copy a pointer to the temporary GDT to addr GDTPOINTERADDR.
    // The temporary gdt is at addr GDTADDR
    PUT_MEM16(GDTPOINTERADDR, 4 * 8);
    PUT_MEM32(GDTPOINTERADDR + 2, GDTADDR);

    // Copy the first 4 gdt entries from the currently used GDT to the
    // temporary GDT.
    memcpy((uint8_t *)GDTADDR, &gdt, 32);

    // clear the APIC ESR register
    APIC_WRITE(APICR_ESR, 0);
    APIC_READ(APICR_ESR);

    // asserting the INIT IPI
    SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
    delay(100000 / DELAY_FACTOR);

    // de-assert the INIT IPI
    SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);

    for (num_sipi = 0; num_sipi < 2; num_sipi++) {
       unsigned timeout;
       bool send_pending;
       unsigned err;

       APIC_WRITE(APICR_ESR, 0);

       SEND_IPI(apic_id, 0, 0, APIC_DELMODE_STARTUP, BOOTCODESTART >> 12);

       timeout = 0;
       do {
 	 delay(10);
 	 timeout++;
 	 send_pending = (APIC_READ(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
       } while (send_pending && timeout < 1000);

       if (send_pending) {
 	 cprint(LINE_STATUS+1, 0, "SMP: STARTUP IPI was never sent");
       }

       delay(100000 / DELAY_FACTOR);

       err = APIC_READ(APICR_ESR) & 0xef;
       if (err) {
 	 cprint(LINE_STATUS+1, 0, "SMP: After STARTUP IPI: err = 0x");
          hprint(LINE_STATUS+1, COL_MID, err);
       }
    }
 }

 static int checksum(unsigned char *mp, int len)
 {
    int sum = 0;

    while (len--) {
        sum += *mp++;
    }
    return (sum & 0xFF);
 }

 /* Parse an MP config table for CPU information */
 bool read_mp_config_table(uintptr_t addr)
 {
    mp_config_table_header_t *mpc = (mp_config_table_header_t*)addr;
    uint8_t *tab_entry_ptr;
    uint8_t *mpc_table_end;

    if (mpc->signature != MPCSignature) {
       return FALSE;
    }
    if (checksum((unsigned char*)mpc, mpc->length) != 0) {
       return FALSE;
    }

    /* FIXME: the uintptr_t cast here works around a compilation problem on
     * AMD64, but it ignores the real problem, which is that lapic_addr
     * is only 32 bits.  Maybe that's OK, but it should be investigated.
     */
    APIC = (volatile apic_register_t*)(uintptr_t)mpc->lapic_addr;

    tab_entry_ptr = ((uint8_t*)mpc) + sizeof(mp_config_table_header_t);
    mpc_table_end = ((uint8_t*)mpc) + mpc->length;
    while (tab_entry_ptr < mpc_table_end) {
       switch (*tab_entry_ptr) {
       case MP_PROCESSOR: {
 	 mp_processor_entry_t *pe = (mp_processor_entry_t*)tab_entry_ptr;

 	 if (pe->cpu_flag & CPU_BOOTPROCESSOR) {
 	    // BSP is CPU 0
 	    cpu_num_to_apic_id[0] = pe->apic_id;
 	 } else if (num_cpus < MAX_CPUS) {
 	    cpu_num_to_apic_id[num_cpus] = pe->apic_id;
 	    num_cpus++;
 	 }
 	 found_cpus++;

 	 // we cannot handle non-local 82489DX apics
 	 if ((pe->apic_ver & 0xf0) != 0x10) {
 	    return 0;
 	 }

 	 tab_entry_ptr += sizeof(mp_processor_entry_t);
 	 break;
       }
       case MP_BUS: {
 	 tab_entry_ptr += sizeof(mp_bus_entry_t);
 	 break;
       }
       case MP_IOAPIC: {
 	 tab_entry_ptr += sizeof(mp_io_apic_entry_t);
 	 break;
       }
       case MP_INTSRC:
 	 tab_entry_ptr += sizeof(mp_interrupt_entry_t);
       case MP_LINTSRC:
 	 tab_entry_ptr += sizeof(mp_local_interrupt_entry_t);
 	 break;
       default:
 	 return FALSE;
       }
    }
    return TRUE;
 }

 /* Search for a Floating Pointer structure */
 floating_pointer_struct_t *
 scan_for_floating_ptr_struct(uintptr_t addr, uint32_t length)
 {
    floating_pointer_struct_t *fp;
    uintptr_t end = addr + length;


    while ((uintptr_t)addr < end) {
       fp = (floating_pointer_struct_t*)addr;
       if (*(unsigned int *)addr == FPSignature && fp->length == 1 &&
 		checksum((unsigned char*)addr, 16) == 0 &&
 		((fp->spec_rev == 1) || (fp->spec_rev == 4))) {

 	   return fp;
       }
       addr += 4;
    }
    return NULL;
 }

 /* Search for a Root System Descriptor Pointer */
 rsdp_t *scan_for_rsdp(uintptr_t addr, uint32_t length)
 {
    rsdp_t *rp;
    uintptr_t end = addr + length;


    while ((uintptr_t)addr < end) {
       rp = (rsdp_t*)addr;
       if (*(unsigned int *)addr == RSDPSignature &&
 		checksum((unsigned char*)addr, rp->length) == 0) {
 	   return rp;
       }
       addr += 4;
    }
    return NULL;
 }

 /* Parse a MADT table for processor entries */
 int parse_madt(uintptr_t addr) {

    mp_config_table_header_t *mpc = (mp_config_table_header_t*)addr;
    uint8_t *tab_entry_ptr;
    uint8_t *mpc_table_end;

    if (checksum((unsigned char*)mpc, mpc->length) != 0) {
       return FALSE;
    }

    APIC = (volatile apic_register_t*)(uintptr_t)mpc->lapic_addr;

    tab_entry_ptr = ((uint8_t*)mpc) + sizeof(mp_config_table_header_t);
    mpc_table_end = ((uint8_t*)mpc) + mpc->length;
    while (tab_entry_ptr < mpc_table_end) {

       madt_processor_entry_t *pe = (madt_processor_entry_t*)tab_entry_ptr;
       if (pe->type == MP_PROCESSOR) {
 	 if (pe->enabled) {
 	     if (num_cpus < MAX_CPUS) {
 		cpu_num_to_apic_id[num_cpus] = pe->apic_id;

 		/* the first CPU is the BSP, don't increment */
 		if (found_cpus) {
 		    num_cpus++;
 		}
 	     }
 	     found_cpus++;
 	 }
       }
        tab_entry_ptr += pe->length;
    }
    return TRUE;
 }

 /* This is where we search for SMP information in the following order
  * look for a floating MP pointer
  *   found:
  *     check for a default configuration
  * 	 found:
  *	   setup config, return
  *     check for a MP config table
  *	 found:
  *	   validate:
  *           good:
  *	        parse the MP config table
  *		  good:
  *		    setup config, return
  *
  * find & validate ACPI RSDP (Root System Descriptor Pointer)
  *   found:
  *     find & validate RSDT (Root System Descriptor Table)
  *       found:
  *         find & validate MSDT
  *	     found:
  *             parse the MADT table
  *               good:
  *		   setup config, return
  */
 void smp_init_bsp()
 {
    floating_pointer_struct_t *fp;
    rsdp_t *rp;
    rsdt_t *rt;
    uint8_t *tab_ptr, *tab_end;
    unsigned int *ptr;

    memset(&AP, 0, sizeof AP);

    /* Search for the Floating MP structure pointer */
    fp = scan_for_floating_ptr_struct(0x0, 0x400);
    if (fp == NULL) {
       fp = scan_for_floating_ptr_struct(639*0x400, 0x400);
    }
    if (fp == NULL) {
          fp = scan_for_floating_ptr_struct(0xf0000, 0x10000);
    }
    if (fp == NULL) {
         /* Search the BIOS ESDS area */
         unsigned int address = *(unsigned short *)0x40E;
         address <<= 4;
 	if (address) {
        		fp = scan_for_floating_ptr_struct(address, 0x400);
         }
    }

    if (fp != NULL) {
 	/* We have a floating MP pointer */

 	/* Is this a default configuration? */
 	if (fp->feature[0] > 0 && fp->feature[0] <=7) {
 	    /* This is a default config so plug in the numbers */
 	    num_cpus = 2;
 	    APIC = 0xfee00000;
 	    cpu_num_to_apic_id[0] = 0;
 	    cpu_num_to_apic_id[1] = 1;
 	    return;
 	}

 	/* Do we have a pointer to a MP configuration table? */
 	if ( fp->phys_addr != 0) {
 	    if (read_mp_config_table(fp->phys_addr)) {
 		/* Found a good MP table, done */
 		return;
 	    }
 	}
     }

    /* No MP table so far, try to find an ACPI MADT table
     * We try to use the MP table first since there is no way to distinguish
     * real cores from hyper-threads in the MADT */

    /* Search for the RSDP */
    rp = scan_for_rsdp(0xe0000, 0x20000);
    if (rp == NULL) {
         /* Search the BIOS ESDS area */
         unsigned int address = *(unsigned short *)0x40E;
         address <<= 4;
 	if (address) {
        		rp = scan_for_rsdp(address, 0x400);
         }
    }

    if (rp == NULL) {
 	/* RSDP not found, give up */
 	return;
    }

    /* Found the RSDP, now get either the RSDP or XRSDP */
    if (rp->revision >= 2) {
 		rt = (rsdt_t *)rp->xrsdt[0];
 		if (rt == 0) {
 			return;
 		}
 		/* Validate the XSDT */
 		if (*(unsigned int *)rt != XSDTSignature) {
 			return;
 		}
 		if ( checksum((unsigned char*)rt, rt->length) != 0) {
 			return;
 		}
     } else {
 		rt = (rsdt_t *)rp->rsdt;
 		if (rt == 0) {
 			return;
 		}
 		/* Validate the RSDT */
 		if (*(unsigned int *)rt != RSDTSignature) {
 			return;
 		}
 		if ( checksum((unsigned char*)rt, rt->length) != 0) {
 			return;
 		}
     }

     /* Scan the RSDT or XSDT for a pointer to the MADT */
     tab_ptr = ((uint8_t*)rt) + sizeof(rsdt_t);
     tab_end = ((uint8_t*)rt) + rt->length;

     while (tab_ptr < tab_end) {
 	ptr = *(unsigned int *)tab_ptr;
 	/* Check for the MADT signature */
 	if (ptr && *ptr == MADTSignature) {

 	    /* Found it, now parse it */
 	    if (parse_madt((uintptr_t)ptr)) {
 		return;
 	    }
 	}
         tab_ptr += 4;
     }
 }

 unsigned my_apic_id()
 {
    return (APIC[APICR_ID][0]) >> 24;
 }

 void smp_ap_booted(unsigned cpu_num)
 {
    AP[cpu_num].started = TRUE;
 }

 void smp_boot_ap(unsigned cpu_num)
 {
    unsigned timeout;
    extern bool smp_mode;
    boot_ap(cpu_num);
    timeout = 0;
    do {
       delay(1000 / DELAY_FACTOR);
       timeout++;
    } while (!AP[cpu_num].started && timeout < 100000 / DELAY_FACTOR);

    if (!AP[cpu_num].started) {
       cprint(LINE_STATUS+1, 0, "SMP: Boot timeout for");
       dprint(LINE_STATUS+1, COL_MID, cpu_num,2,1);
       cprint(LINE_STATUS+1, 26, "Turning off SMP");
       smp_mode = FALSE;
    }
 }

 unsigned smp_my_cpu_num()
 {
    unsigned apicid = my_apic_id();
    unsigned i;

    for (i = 0; i < MAX_CPUS; i++) {
       if (apicid == cpu_num_to_apic_id[i]) {
 	 break;
       }
    }
    if (i == MAX_CPUS) {
       i = 0;
    }
    return i;
 }
	/*
	* smp.c --
	*/

	#include "stddef.h"
	#include "smp.h"
	#include "cpuid.h"
	#include "test.h"
	#define DELAY_FACTOR 1
	unsigned num_cpus = 1; // There is at least one cpu, the BSP
	unsigned found_cpus = 0;

	extern void memcpy(void dst, void src , int len);
	extern void test_start(void);
	extern int run_cpus;

	struct barrier_s *barr;

	void barrier_init(int max)
	{
	/* Set the adddress of the barrier structure */
	barr = (struct barrier_s *)0x9ff00;
	barr->lck.slock = 1;
	barr->mutex.slock = 1;
	barr->maxproc = max;
	barr->count = max;
	barr->st1.slock = 1;
	barr->st2.slock = 0;
	}

	void s_barrier_init(int max)
	{
	barr->s_lck.slock = 1;
	barr->s_maxproc = max;
	barr->s_count = max;
	barr->s_st1.slock = 1;
	barr->s_st2.slock = 0;
	}

	/* Spin until var = 1 */
	void spin_wait(spinlock_t *lck)
	{
	int inc = 0x400;

	asm volatile( "1:\t"
	"cmpb $0,%1\n\t"
	"jne 2f\n\t"
	"rep ; nop\n\t"
	"jmp 1b\n"
	"2:"
	: : "c" (inc), "m" (lck->slock): "memory" );
	}

	void barrier()
	{
	if (num_cpus == 1) {
	return;
	}
	spin_wait(&barr->st1); /* Wait if the barrier is active */
	spin_lock(&barr->lck); /* Get lock for barr struct */
	if (--barr->count == 0) { /* Last process? */
	barr->st1.slock = 0; /* Hold up any processes re-entering */
	barr->st2.slock = 1; /* Release the other processes */
	barr->count++;
	spin_unlock(&barr->lck);
	} else {
	spin_unlock(&barr->lck);
	spin_wait(&barr->st2); /* wait for peers to arrive */
	spin_lock(&barr->lck);
	if (++barr->count == barr->maxproc) {
	barr->st1.slock = 1;
	barr->st2.slock = 0;
	}
	spin_unlock(&barr->lck);
	}
	}

	void s_barrier()
	{
	if (run_cpus == 1) {
	return;
	}
	spin_wait(&barr->s_st1); /* Wait if the barrier is active */
	spin_lock(&barr->s_lck); /* Get lock for barr struct */
	if (--barr->s_count == 0) { /* Last process? */
	barr->s_st1.slock = 0; /* Hold up any processes re-entering */
	barr->s_st2.slock = 1; /* Release the other processes */
	barr->s_count++;
	spin_unlock(&barr->s_lck);
	} else {
	spin_unlock(&barr->s_lck);
	spin_wait(&barr->s_st2); /* wait for peers to arrive */
	spin_lock(&barr->s_lck);
	if (++barr->s_count == barr->s_maxproc) {
	barr->s_st1.slock = 1;
	barr->s_st2.slock = 0;
	}
	spin_unlock(&barr->s_lck);
	}
	}

	typedef struct {
	bool started;
	} ap_info_t;

	volatile apic_register_t *APIC = NULL;
	/* CPU number to APIC ID mapping table. CPU 0 is the BSP. */
	static unsigned cpu_num_to_apic_id[MAX_CPUS];
	volatile ap_info_t AP[MAX_CPUS];

	void PUT_MEM16(uintptr_t addr, uint16_t val)
	{
	((volatile uint16_t )addr) = val;
	}

	void PUT_MEM32(uintptr_t addr, uint32_t val)
	{
	((volatile uint32_t )addr) = val;
	}

	static void inline
	APIC_WRITE(unsigned reg, uint32_t val)
	{
	APIC[reg][0] = val;
	}

	static inline uint32_t
	APIC_READ(unsigned reg)
	{
	return APIC[reg][0];
	}


	static void
	SEND_IPI(unsigned apic_id, unsigned trigger, unsigned level, unsigned mode,
	uint8_t vector)
	{
	uint32_t v;

	v = APIC_READ(APICR_ICRHI) & 0x00ffffff;
	APIC_WRITE(APICR_ICRHI, v \| (apic_id << 24));

	v = APIC_READ(APICR_ICRLO) & ~0xcdfff;
	v \|= (APIC_DEST_DEST << APIC_ICRLO_DEST_OFFSET)
	\| (trigger << APIC_ICRLO_TRIGGER_OFFSET)
	\| (level << APIC_ICRLO_LEVEL_OFFSET)
	\| (mode << APIC_ICRLO_DELMODE_OFFSET)
	\| (vector);
	APIC_WRITE(APICR_ICRLO, v);
	}


	// Silly way of busywaiting, but we don't have a timer
	void delay(unsigned us)
	{
	unsigned freq = 1000; // in MHz, assume 1GHz CPU speed
	uint64_t cycles = us * freq;
	uint64_t t0 = RDTSC();
	uint64_t t1;
	volatile unsigned k;

	do {
	for (k = 0; k < 1000; k++) continue;
	t1 = RDTSC();
	} while (t1 - t0 < cycles);
	}

	static inline void
	memset (void *dst,
	char value,
	int len)
	{
	int i;
	for (i = 0 ; i < len ; i++ ) {
	((char ) dst + i) = value;
	}
	}

	void kick_cpu(unsigned cpu_num)
	{
	unsigned num_sipi, apic_id;
	apic_id = cpu_num_to_apic_id[cpu_num];

	// clear the APIC ESR register
	APIC_WRITE(APICR_ESR, 0);
	APIC_READ(APICR_ESR);

	// asserting the INIT IPI
	SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
	delay(100000 / DELAY_FACTOR);

	// de-assert the INIT IPI
	SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);

	for (num_sipi = 0; num_sipi < 2; num_sipi++) {
	unsigned timeout;
	bool send_pending;
	unsigned err;

	APIC_WRITE(APICR_ESR, 0);

	SEND_IPI(apic_id, 0, 0, APIC_DELMODE_STARTUP, (unsigned)startup_32 >> 12);

	timeout = 0;
	do {
	delay(10);
	timeout++;
	send_pending = (APIC_READ(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
	} while (send_pending && timeout < 1000);

	if (send_pending) {
	cprint(LINE_STATUS+1, 0, "SMP: STARTUP IPI was never sent");
	}

	delay(100000 / DELAY_FACTOR);

	err = APIC_READ(APICR_ESR) & 0xef;
	if (err) {
	cprint(LINE_STATUS+1, 0, "SMP: After STARTUP IPI: err = 0x");
	hprint(LINE_STATUS+1, COL_MID, err);
	}
	}
	}

	// These memory locations are used for the trampoline code and data.

	#define BOOTCODESTART 0x9000
	#define GDTPOINTERADDR 0x9100
	#define GDTADDR 0x9110

	void boot_ap(unsigned cpu_num)
	{
	unsigned num_sipi, apic_id;
	extern uint8_t gdt;
	extern uint8_t _ap_trampoline_start;
	extern uint8_t _ap_trampoline_protmode;
	unsigned len = &_ap_trampoline_protmode - &_ap_trampoline_start;
	apic_id = cpu_num_to_apic_id[cpu_num];


	memcpy((uint8_t*)BOOTCODESTART, &_ap_trampoline_start, len);

	// Fixup the LGDT instruction to point to GDT pointer.
	PUT_MEM16(BOOTCODESTART + 3, GDTPOINTERADDR);

	// Copy a pointer to the temporary GDT to addr GDTPOINTERADDR.
	// The temporary gdt is at addr GDTADDR
	PUT_MEM16(GDTPOINTERADDR, 4 * 8);
	PUT_MEM32(GDTPOINTERADDR + 2, GDTADDR);

	// Copy the first 4 gdt entries from the currently used GDT to the
	// temporary GDT.
	memcpy((uint8_t *)GDTADDR, &gdt, 32);

	// clear the APIC ESR register
	APIC_WRITE(APICR_ESR, 0);
	APIC_READ(APICR_ESR);

	// asserting the INIT IPI
	SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 1, APIC_DELMODE_INIT, 0);
	delay(100000 / DELAY_FACTOR);

	// de-assert the INIT IPI
	SEND_IPI(apic_id, APIC_TRIGGER_LEVEL, 0, APIC_DELMODE_INIT, 0);

	for (num_sipi = 0; num_sipi < 2; num_sipi++) {
	unsigned timeout;
	bool send_pending;
	unsigned err;

	APIC_WRITE(APICR_ESR, 0);

	SEND_IPI(apic_id, 0, 0, APIC_DELMODE_STARTUP, BOOTCODESTART >> 12);

	timeout = 0;
	do {
	delay(10);
	timeout++;
	send_pending = (APIC_READ(APICR_ICRLO) & APIC_ICRLO_STATUS_MASK) != 0;
	} while (send_pending && timeout < 1000);

	if (send_pending) {
	cprint(LINE_STATUS+1, 0, "SMP: STARTUP IPI was never sent");
	}

	delay(100000 / DELAY_FACTOR);

	err = APIC_READ(APICR_ESR) & 0xef;
	if (err) {
	cprint(LINE_STATUS+1, 0, "SMP: After STARTUP IPI: err = 0x");
	hprint(LINE_STATUS+1, COL_MID, err);
	}
	}
	}

	static int checksum(unsigned char *mp, int len)
	{
	int sum = 0;

	while (len--) {
	sum += *mp++;
	}
	return (sum & 0xFF);
	}

	/* Parse an MP config table for CPU information */
	bool read_mp_config_table(uintptr_t addr)
	{
	mp_config_table_header_t mpc = (mp_config_table_header_t)addr;
	uint8_t *tab_entry_ptr;
	uint8_t *mpc_table_end;

	if (mpc->signature != MPCSignature) {
	return FALSE;
	}
	if (checksum((unsigned char*)mpc, mpc->length) != 0) {
	return FALSE;
	}

	/* FIXME: the uintptr_t cast here works around a compilation problem on
	* AMD64, but it ignores the real problem, which is that lapic_addr
	* is only 32 bits. Maybe that's OK, but it should be investigated.
	*/
	APIC = (volatile apic_register_t*)(uintptr_t)mpc->lapic_addr;

	tab_entry_ptr = ((uint8_t*)mpc) + sizeof(mp_config_table_header_t);
	mpc_table_end = ((uint8_t*)mpc) + mpc->length;
	while (tab_entry_ptr < mpc_table_end) {
	switch (*tab_entry_ptr) {
	case MP_PROCESSOR: {
	mp_processor_entry_t pe = (mp_processor_entry_t)tab_entry_ptr;

	if (pe->cpu_flag & CPU_BOOTPROCESSOR) {
	// BSP is CPU 0
	cpu_num_to_apic_id[0] = pe->apic_id;
	} else if (num_cpus < MAX_CPUS) {
	cpu_num_to_apic_id[num_cpus] = pe->apic_id;
	num_cpus++;
	}
	found_cpus++;

	// we cannot handle non-local 82489DX apics
	if ((pe->apic_ver & 0xf0) != 0x10) {
	return 0;
	}

	tab_entry_ptr += sizeof(mp_processor_entry_t);
	break;
	}
	case MP_BUS: {
	tab_entry_ptr += sizeof(mp_bus_entry_t);
	break;
	}
	case MP_IOAPIC: {
	tab_entry_ptr += sizeof(mp_io_apic_entry_t);
	break;
	}
	case MP_INTSRC:
	tab_entry_ptr += sizeof(mp_interrupt_entry_t);
	case MP_LINTSRC:
	tab_entry_ptr += sizeof(mp_local_interrupt_entry_t);
	break;
	default:
	return FALSE;
	}
	}
	return TRUE;
	}

	/* Search for a Floating Pointer structure */
	floating_pointer_struct_t *
	scan_for_floating_ptr_struct(uintptr_t addr, uint32_t length)
	{
	floating_pointer_struct_t *fp;
	uintptr_t end = addr + length;


	while ((uintptr_t)addr < end) {
	fp = (floating_pointer_struct_t*)addr;
	if ((unsigned int )addr == FPSignature && fp->length == 1 &&
	checksum((unsigned char*)addr, 16) == 0 &&
	((fp->spec_rev == 1) \|\| (fp->spec_rev == 4))) {

	return fp;
	}
	addr += 4;
	}
	return NULL;
	}

	/* Search for a Root System Descriptor Pointer */
	rsdp_t *scan_for_rsdp(uintptr_t addr, uint32_t length)
	{
	rsdp_t *rp;
	uintptr_t end = addr + length;


	while ((uintptr_t)addr < end) {
	rp = (rsdp_t*)addr;
	if ((unsigned int )addr == RSDPSignature &&
	checksum((unsigned char*)addr, rp->length) == 0) {
	return rp;
	}
	addr += 4;
	}
	return NULL;
	}

	/* Parse a MADT table for processor entries */
	int parse_madt(uintptr_t addr) {

	mp_config_table_header_t mpc = (mp_config_table_header_t)addr;
	uint8_t *tab_entry_ptr;
	uint8_t *mpc_table_end;

	if (checksum((unsigned char*)mpc, mpc->length) != 0) {
	return FALSE;
	}

	APIC = (volatile apic_register_t*)(uintptr_t)mpc->lapic_addr;

	tab_entry_ptr = ((uint8_t*)mpc) + sizeof(mp_config_table_header_t);
	mpc_table_end = ((uint8_t*)mpc) + mpc->length;
	while (tab_entry_ptr < mpc_table_end) {

	madt_processor_entry_t pe = (madt_processor_entry_t)tab_entry_ptr;
	if (pe->type == MP_PROCESSOR) {
	if (pe->enabled) {
	if (num_cpus < MAX_CPUS) {
	cpu_num_to_apic_id[num_cpus] = pe->apic_id;

	/* the first CPU is the BSP, don't increment */
	if (found_cpus) {
	num_cpus++;
	}
	}
	found_cpus++;
	}
	}
	tab_entry_ptr += pe->length;
	}
	return TRUE;
	}

	/* This is where we search for SMP information in the following order
	* look for a floating MP pointer
	* found:
	* check for a default configuration
	* found:
	* setup config, return
	* check for a MP config table
	* found:
	* validate:
	* good:
	* parse the MP config table
	* good:
	* setup config, return
	*
	* find & validate ACPI RSDP (Root System Descriptor Pointer)
	* found:
	* find & validate RSDT (Root System Descriptor Table)
	* found:
	* find & validate MSDT
	* found:
	* parse the MADT table
	* good:
	* setup config, return
	*/
	void smp_init_bsp()
	{
	floating_pointer_struct_t *fp;
	rsdp_t *rp;
	rsdt_t *rt;
	uint8_t tab_ptr, tab_end;
	unsigned int *ptr;

	memset(&AP, 0, sizeof AP);

	/* Search for the Floating MP structure pointer */
	fp = scan_for_floating_ptr_struct(0x0, 0x400);
	if (fp == NULL) {
	fp = scan_for_floating_ptr_struct(639*0x400, 0x400);
	}
	if (fp == NULL) {
	fp = scan_for_floating_ptr_struct(0xf0000, 0x10000);
	}
	if (fp == NULL) {
	/* Search the BIOS ESDS area */
	unsigned int address = (unsigned short )0x40E;
	address <<= 4;
	if (address) {
	fp = scan_for_floating_ptr_struct(address, 0x400);
	}
	}

	if (fp != NULL) {
	/* We have a floating MP pointer */

	/* Is this a default configuration? */
	if (fp->feature[0] > 0 && fp->feature[0] <=7) {
	/* This is a default config so plug in the numbers */
	num_cpus = 2;
	APIC = 0xfee00000;
	cpu_num_to_apic_id[0] = 0;
	cpu_num_to_apic_id[1] = 1;
	return;
	}

	/* Do we have a pointer to a MP configuration table? */
	if ( fp->phys_addr != 0) {
	if (read_mp_config_table(fp->phys_addr)) {
	/* Found a good MP table, done */
	return;
	}
	}
	}

	/* No MP table so far, try to find an ACPI MADT table
	* We try to use the MP table first since there is no way to distinguish
	* real cores from hyper-threads in the MADT */

	/* Search for the RSDP */
	rp = scan_for_rsdp(0xe0000, 0x20000);
	if (rp == NULL) {
	/* Search the BIOS ESDS area */
	unsigned int address = (unsigned short )0x40E;
	address <<= 4;
	if (address) {
	rp = scan_for_rsdp(address, 0x400);
	}
	}

	if (rp == NULL) {
	/* RSDP not found, give up */
	return;
	}

	/* Found the RSDP, now get either the RSDP or XRSDP */
	if (rp->revision >= 2) {
	rt = (rsdt_t *)rp->xrsdt[0];
	if (rt == 0) {
	return;
	}
	/* Validate the XSDT */
	if ((unsigned int )rt != XSDTSignature) {
	return;
	}
	if ( checksum((unsigned char*)rt, rt->length) != 0) {
	return;
	}
	} else {
	rt = (rsdt_t *)rp->rsdt;
	if (rt == 0) {
	return;
	}
	/* Validate the RSDT */
	if ((unsigned int )rt != RSDTSignature) {
	return;
	}
	if ( checksum((unsigned char*)rt, rt->length) != 0) {
	return;
	}
	}

	/* Scan the RSDT or XSDT for a pointer to the MADT */
	tab_ptr = ((uint8_t*)rt) + sizeof(rsdt_t);
	tab_end = ((uint8_t*)rt) + rt->length;

	while (tab_ptr < tab_end) {
	ptr = (unsigned int )tab_ptr;
	/* Check for the MADT signature */
	if (ptr && *ptr == MADTSignature) {

	/* Found it, now parse it */
	if (parse_madt((uintptr_t)ptr)) {
	return;
	}
	}
	tab_ptr += 4;
	}
	}

	unsigned my_apic_id()
	{
	return (APIC[APICR_ID][0]) >> 24;
	}

	void smp_ap_booted(unsigned cpu_num)
	{
	AP[cpu_num].started = TRUE;
	}

	void smp_boot_ap(unsigned cpu_num)
	{
	unsigned timeout;
	extern bool smp_mode;
	boot_ap(cpu_num);
	timeout = 0;
	do {
	delay(1000 / DELAY_FACTOR);
	timeout++;
	} while (!AP[cpu_num].started && timeout < 100000 / DELAY_FACTOR);

	if (!AP[cpu_num].started) {
	cprint(LINE_STATUS+1, 0, "SMP: Boot timeout for");
	dprint(LINE_STATUS+1, COL_MID, cpu_num,2,1);
	cprint(LINE_STATUS+1, 26, "Turning off SMP");
	smp_mode = FALSE;
	}
	}

	unsigned smp_my_cpu_num()
	{
	unsigned apicid = my_apic_id();
	unsigned i;

	for (i = 0; i < MAX_CPUS; i++) {
	if (apicid == cpu_num_to_apic_id[i]) {
	break;
	}
	}
	if (i == MAX_CPUS) {
	i = 0;
	}
	return i;
	}