Correct the logical_cpus calculation in mptable_hyperthread_fixup()
[dragonfly.git] / sys / platform / pc64 / x86_64 / mp_machdep.c
index 8778d02..4d7883a 100644 (file)
@@ -160,6 +160,12 @@ typedef struct BASETABLE_ENTRY {
        char    name[16];
 }       basetable_entry;
 
+struct mptable_pos {
+       mpfps_t         mp_fps;
+       mpcth_t         mp_cth;
+       vm_size_t       mp_cth_mapsz;   
+};
+
 /*
  * this code MUST be enabled here and in mpboot.s.
  * it follows the very early stages of AP boot by placing values in CMOS ram.
@@ -216,6 +222,9 @@ static int need_hyperthreading_fixup;
 static u_int logical_cpus;
 u_int  logical_cpus_mask;
 
+static int madt_probe_test;
+TUNABLE_INT("hw.madt_probe_test", &madt_probe_test);
+
 /** XXX FIXME: where does this really belong, isa.h/isa.c perhaps? */
 int    current_postcode;
 
@@ -228,7 +237,6 @@ int mp_nbusses;             /* # of busses */
 #ifdef APIC_IO
 int    mp_napics;              /* # of IO APICs */
 #endif
-int    boot_cpu_id;            /* designated BSP */
 vm_offset_t cpu_apic_address;
 #ifdef APIC_IO
 vm_offset_t io_apic_address[NAPICID];  /* NAPICID is more than enough */
@@ -240,6 +248,8 @@ u_int32_t cpu_apic_versions[MAXCPU];
 int64_t tsc0_offset;
 extern int64_t tsc_offsets[];
 
+extern u_long ebda_addr;
+
 #ifdef APIC_IO
 struct apic_intmapinfo int_to_apicintpin[APIC_INTMAPSIZE];
 #endif
@@ -258,9 +268,6 @@ int     apic_id_to_logical[NAPICID];
 char *bootSTK;
 static int bootAP;
 
-/* Hotwire a 0->4MB V==P mapping */
-extern pt_entry_t *KPTphys;
-
 /*
  * SMP page table page.  Setup by locore to point to a page table
  * page from which we allocate per-cpu privatespace areas io_apics,
@@ -280,20 +287,22 @@ extern inthand_t IDTVEC(fast_syscall), IDTVEC(fast_syscall32);
  * Local data and functions.
  */
 
-static int     mp_capable;
 static u_int   boot_address;
 static u_int   base_memory;
 static int     mp_finish;
 
-static mpfps_t mpfps;
-static long    search_for_sig(u_int32_t target, int count);
 static void    mp_enable(u_int boot_addr);
 
+static int     mptable_probe(void);
+static long    mptable_search_sig(u_int32_t target, int count);
 static void    mptable_hyperthread_fixup(u_int id_mask);
-static void    mptable_pass1(void);
-static int     mptable_pass2(void);
-static void    default_mp_table(int type);
-static void    fix_mp_table(void);
+static void    mptable_pass1(struct mptable_pos *);
+static int     mptable_pass2(struct mptable_pos *);
+static void    mptable_default(int type);
+static void    mptable_fix(void);
+static void    mptable_map(struct mptable_pos *, vm_paddr_t);
+static void    mptable_unmap(struct mptable_pos *);
+
 #ifdef APIC_IO
 static void    setup_apic_irq_mapping(void);
 static int     apic_int_is_bus_type(int intr, int bus_type);
@@ -302,7 +311,8 @@ static int  start_all_aps(u_int boot_addr);
 #if 0
 static void    install_ap_tramp(u_int boot_addr);
 #endif
-static int     start_ap(struct mdglobaldata *gd, u_int boot_addr);
+static int     start_ap(struct mdglobaldata *gd, u_int boot_addr, int smibest);
+static int     smitest(void);
 
 static cpumask_t smp_startup_mask = 1; /* which cpus have been started */
 cpumask_t smp_active_mask = 1; /* which cpus are ready for IPIs etc? */
@@ -333,11 +343,10 @@ mp_bootaddress(u_int basemem)
 /*
  * Look for an Intel MP spec table (ie, SMP capable hardware).
  */
-int
-mp_probe(void)
+static int
+mptable_probe(void)
 {
        long    x;
-       u_long  segment;
        u_int32_t target;
  
        /*
@@ -349,39 +358,25 @@ mp_probe(void)
        POSTCODE(MP_PROBE_POST);
 
        /* see if EBDA exists */
-       if ((segment = (u_long) * (u_short *) (KERNBASE + 0x40e)) != 0) {
+       if (ebda_addr != 0) {
                /* search first 1K of EBDA */
-               target = (u_int32_t) (segment << 4);
-               if ((x = search_for_sig(target, 1024 / 4)) != -1L)
-                       goto found;
+               target = (u_int32_t)ebda_addr;
+               if ((x = mptable_search_sig(target, 1024 / 4)) > 0)
+                       return x;
        } else {
                /* last 1K of base memory, effective 'top of base' passed in */
-               target = (u_int32_t) (base_memory - 0x400);
-               if ((x = search_for_sig(target, 1024 / 4)) != -1L)
-                       goto found;
+               target = (u_int32_t)(base_memory - 0x400);
+               if ((x = mptable_search_sig(target, 1024 / 4)) > 0)
+                       return x;
        }
 
        /* search the BIOS */
-       target = (u_int32_t) BIOS_BASE;
-       if ((x = search_for_sig(target, BIOS_COUNT)) != -1L)
-               goto found;
+       target = (u_int32_t)BIOS_BASE;
+       if ((x = mptable_search_sig(target, BIOS_COUNT)) > 0)
+               return x;
 
        /* nothing found */
-       mpfps = (mpfps_t)0;
-       mp_capable = 0;
        return 0;
-
-found:
-       /*
-        * Calculate needed resources.  We can safely map physical
-        * memory into SMPpt after mptable_pass1() completes.
-        */
-       mpfps = (mpfps_t)x;
-       mptable_pass1();
-
-       /* flag fact that we are running multiple processors */
-       mp_capable = 1;
-       return 1;
 }
 
 
@@ -392,12 +387,7 @@ void
 mp_start(void)
 {
        POSTCODE(MP_START_POST);
-
-       /* look for MP capable motherboard */
-       if (mp_capable)
-               mp_enable(boot_address);
-       else
-               panic("MP hardware not found!");
+       mp_enable(boot_address);
 }
 
 
@@ -549,29 +539,73 @@ mp_enable(u_int boot_addr)
        int     apic;
        u_int   ux;
 #endif /* APIC_IO */
+       vm_paddr_t mpfps_paddr;
 
        POSTCODE(MP_ENABLE_POST);
 
-#if 0 /* JGXXX */
-       /* turn on 4MB of V == P addressing so we can get to MP table */
-       *(int *)PTD = PG_V | PG_RW | ((uintptr_t)(void *)KPTphys & PG_FRAME);
-       cpu_invltlb();
-#endif
+       if (madt_probe_test)
+               mpfps_paddr = 0;
+       else
+               mpfps_paddr = mptable_probe();
 
-       /* examine the MP table for needed info, uses physical addresses */
-       x = mptable_pass2();
+       if (mpfps_paddr) {
+               struct mptable_pos mpt;
 
-#if 0 /* JGXXX */
-       *(int *)PTD = 0;
-       cpu_invltlb();
-#endif /* 0 JGXXX */
+               mptable_map(&mpt, mpfps_paddr);
 
-       /* can't process default configs till the CPU APIC is pmapped */
-       if (x)
-               default_mp_table(x);
+               /*
+                * We can safely map physical memory into SMPpt after
+                * mptable_pass1() completes.
+                */
+               mptable_pass1(&mpt);
+
+               if (cpu_apic_address == 0)
+                       panic("mp_enable: no local apic!\n");
+
+               /* examine the MP table for needed info */
+               x = mptable_pass2(&mpt);
+
+               mptable_unmap(&mpt);
+
+               /*
+                * can't process default configs till the
+                * CPU APIC is pmapped
+                */
+               if (x)
+                       mptable_default(x);
 
-       /* post scan cleanup */
-       fix_mp_table();
+               /* post scan cleanup */
+               mptable_fix();
+
+               /*
+                * lapic not mapped yet (pmap_init is called too late)
+                */
+               lapic = pmap_mapdev_uncacheable(cpu_apic_address,
+                                               sizeof(struct LAPIC));
+       } else {
+               vm_paddr_t madt_paddr;
+               int bsp_apic_id;
+
+               madt_paddr = madt_probe();
+               if (madt_paddr == 0)
+                       panic("mp_enable: madt_probe failed\n");
+
+               cpu_apic_address = madt_pass1(madt_paddr);
+               if (cpu_apic_address == 0)
+                       panic("mp_enable: no local apic (madt)!\n");
+
+               /*
+                * lapic not mapped yet (pmap_init is called too late)
+                *
+                * XXX: where is the best place to set lapic?
+                */
+               lapic = pmap_mapdev_uncacheable(cpu_apic_address,
+                                               sizeof(struct LAPIC));
+
+               bsp_apic_id = (lapic->id & 0xff000000) >> 24;
+               if (madt_pass2(madt_paddr, bsp_apic_id))
+                       panic("mp_enable: madt_pass2 failed\n");
+       }
 
 #if defined(APIC_IO)
 
@@ -628,17 +662,28 @@ mp_enable(u_int boot_addr)
 #define MP_SIG         0x5f504d5f      /* _MP_ */
 #define NEXT(X)                ((X) += 4)
 static long
-search_for_sig(u_int32_t target, int count)
+mptable_search_sig(u_int32_t target, int count)
 {
-       int     x;
-       u_int32_t *addr = (u_int32_t *) (KERNBASE + target);
+       vm_size_t map_size;
+       u_int32_t *addr;
+       int x, ret;
 
-       for (x = 0; x < count; NEXT(x))
-               if (addr[x] == MP_SIG)
+       KKASSERT(target != 0);
+
+       map_size = count * sizeof(u_int32_t);
+       addr = pmap_mapdev((vm_paddr_t)target, map_size);
+
+       ret = 0;
+       for (x = 0; x < count; NEXT(x)) {
+               if (addr[x] == MP_SIG) {
                        /* make array index a byte index */
-                       return (long)(&addr[x]);
+                       ret = target + (x * sizeof(u_int32_t));
+                       break;
+               }
+       }
 
-       return -1;
+       pmap_unmapdev((vm_offset_t)addr, map_size);
+       return ret;
 }
 
 
@@ -728,9 +773,6 @@ static int lookup_bus_type  (char *name);
 /*
  * 1st pass on motherboard's Intel MP specification table.
  *
- * initializes:
- *     ncpus = 1
- *
  * determines:
  *     cpu_apic_address (common to all CPUs)
  *     io_apic_address[N]
@@ -738,13 +780,16 @@ static int lookup_bus_type        (char *name);
  *     mp_nbusses
  *     mp_napics
  *     nintrs
+ *     need_hyperthreading_fixup
+ *     logical_cpus
  */
 static void
-mptable_pass1(void)
+mptable_pass1(struct mptable_pos *mpt)
 {
 #ifdef APIC_IO
        int     x;
 #endif
+       mpfps_t fps;
        mpcth_t cth;
        int     totalSize;
        void*   position;
@@ -754,6 +799,9 @@ mptable_pass1(void)
 
        POSTCODE(MPTABLE_PASS1_POST);
 
+       fps = mpt->mp_fps;
+       KKASSERT(fps != NULL);
+
 #ifdef APIC_IO
        /* clear various tables */
        for (x = 0; x < NAPICID; ++x) {
@@ -771,7 +819,7 @@ mptable_pass1(void)
        id_mask = 0;
 
        /* check for use of 'default' configuration */
-       if (mpfps->mpfb1 != 0) {
+       if (fps->mpfb1 != 0) {
                /* use default addresses */
                cpu_apic_address = DEFAULT_APIC_BASE;
 #ifdef APIC_IO
@@ -780,16 +828,16 @@ mptable_pass1(void)
 
                /* fill in with defaults */
                mp_naps = 2;            /* includes BSP */
-               mp_nbusses = default_data[mpfps->mpfb1 - 1][0];
+               mp_nbusses = default_data[fps->mpfb1 - 1][0];
 #if defined(APIC_IO)
                mp_napics = 1;
                nintrs = 16;
 #endif /* APIC_IO */
        }
        else {
-               if (mpfps->pap == 0)
+               cth = mpt->mp_cth;
+               if (cth == NULL)
                        panic("MP Configuration Table Header MISSING!");
-               cth = (void *)PHYS_TO_DMAP(mpfps->pap);
 
                cpu_apic_address = (vm_offset_t) cth->apic_address;
 
@@ -847,12 +895,6 @@ mptable_pass1(void)
 
        /* See if we need to fixup HT logical CPUs. */
        mptable_hyperthread_fixup(id_mask);
-       
-       /*
-        * Count the BSP.
-        * This is also used as a counter while starting the APs.
-        */
-       ncpus = 1;
 
        --mp_naps;      /* subtract the BSP */
 }
@@ -862,7 +904,7 @@ mptable_pass1(void)
  * 2nd pass on motherboard's Intel MP specification table.
  *
  * sets:
- *     boot_cpu_id
+ *     logical_cpus_mask
  *     ID_TO_IO(N), phy APIC ID to log CPU/IO table
  *     CPU_TO_ID(N), logical CPU to APIC ID table
  *     IO_TO_ID(N), logical IO to APIC ID table
@@ -870,10 +912,11 @@ mptable_pass1(void)
  *     io_apic_ints[N]
  */
 static int
-mptable_pass2(void)
+mptable_pass2(struct mptable_pos *mpt)
 {
        struct PROCENTRY proc;
        int     x;
+       mpfps_t fps;
        mpcth_t cth;
        int     totalSize;
        void*   position;
@@ -884,6 +927,9 @@ mptable_pass2(void)
 
        POSTCODE(MPTABLE_PASS2_POST);
 
+       fps = mpt->mp_fps;
+       KKASSERT(fps != NULL);
+
        /* Initialize fake proc entry for use with HT fixup. */
        bzero(&proc, sizeof(proc));
        proc.type = 0;
@@ -927,20 +973,17 @@ mptable_pass2(void)
        }
 #endif
 
-       /* setup the cpu/apic mapping arrays */
-       boot_cpu_id = -1;
-
        /* record whether PIC or virtual-wire mode */
-       machintr_setvar_simple(MACHINTR_VAR_IMCR_PRESENT, mpfps->mpfb2 & 0x80);
+       machintr_setvar_simple(MACHINTR_VAR_IMCR_PRESENT, fps->mpfb2 & 0x80);
 
        /* check for use of 'default' configuration */
-       if (mpfps->mpfb1 != 0)
-               return mpfps->mpfb1;    /* return default configuration type */
+       if (fps->mpfb1 != 0)
+               return fps->mpfb1;      /* return default configuration type */
 
-       if (mpfps->pap == 0)
+       cth = mpt->mp_cth;
+       if (cth == NULL)
                panic("MP Configuration Table Header MISSING!");
 
-       cth = (void *)PHYS_TO_DMAP(mpfps->pap);
        /* walk the table, recording info of interest */
        totalSize = cth->base_table_length - sizeof(struct MPCTH);
        position = (u_char *) cth + sizeof(struct MPCTH);
@@ -997,13 +1040,14 @@ mptable_pass2(void)
                position = (uint8_t *)position + basetable_entry_types[type].length;
        }
 
-       if (boot_cpu_id == -1)
+       if (CPU_TO_ID(0) < 0)
                panic("NO BSP found!");
 
        /* report fact that its NOT a default configuration */
        return 0;
 }
 
+
 /*
  * Check if we should perform a hyperthreading "fix-up" to
  * enumerate any logical CPU's that aren't already listed
@@ -1019,14 +1063,82 @@ mptable_pass2(void)
 static void
 mptable_hyperthread_fixup(u_int id_mask)
 {
-       u_int i, id;
+       int i, id, lcpus_max;
 
-       /* Nothing to do if there is no HTT support. */
        if ((cpu_feature & CPUID_HTT) == 0)
                return;
-       logical_cpus = (cpu_procinfo & CPUID_HTT_CORES) >> 16;
-       if (logical_cpus <= 1)
+
+       lcpus_max = (cpu_procinfo & CPUID_HTT_CORES) >> 16;
+       if (lcpus_max <= 1)
+               return;
+
+       if (strcmp(cpu_vendor, "GenuineIntel") == 0) {
+               /*
+                * INSTRUCTION SET REFERENCE, A-M (#253666)
+                * Page 3-181, Table 3-20
+                * "The nearest power-of-2 integer that is not smaller
+                *  than EBX[23:16] is the number of unique initial APIC
+                *  IDs reserved for addressing different logical
+                *  processors in a physical package."
+                */
+               for (i = 0; ; ++i) {
+                       if ((1 << i) >= lcpus_max) {
+                               lcpus_max = 1 << i;
+                               break;
+                       }
+               }
+       }
+
+       if (mp_naps == lcpus_max) {
+               /* We have nothing to fix */
                return;
+       } else if (mp_naps == 1) {
+               /* XXX this may be incorrect */
+               logical_cpus = lcpus_max;
+       } else {
+               int cur, prev, dist;
+
+               /*
+                * Calculate the distances between two nearest
+                * APIC IDs.  If all such distances are same,
+                * then it is the number of missing cpus that
+                * we are going to fill later.
+                */
+               dist = cur = prev = -1;
+               for (id = 0; id < MAXCPU; ++id) {
+                       if ((id_mask & 1 << id) == 0)
+                               continue;
+
+                       cur = id;
+                       if (prev >= 0) {
+                               int new_dist = cur - prev;
+
+                               if (dist < 0)
+                                       dist = new_dist;
+
+                               /*
+                                * Make sure that all distances
+                                * between two nearest APIC IDs
+                                * are same.
+                                */
+                               if (dist != new_dist)
+                                       return;
+                       }
+                       prev = cur;
+               }
+               if (dist == 1)
+                       return;
+
+               /* Must be power of 2 */
+               if (dist & (dist - 1))
+                       return;
+
+               /* Can't exceed CPU package capacity */
+               if (dist > lcpus_max)
+                       logical_cpus = lcpus_max;
+               else
+                       logical_cpus = dist;
+       }
 
        /*
         * For each APIC ID of a CPU that is set in the mask,
@@ -1034,7 +1146,7 @@ mptable_hyperthread_fixup(u_int id_mask)
         * physical processor.  If any of those ID's are
         * already in the table, then kill the fixup.
         */
-       for (id = 0; id <= MAXCPU; id++) {
+       for (id = 0; id < MAXCPU; id++) {
                if ((id_mask & 1 << id) == 0)
                        continue;
                /* First, make sure we are on a logical_cpus boundary. */
@@ -1053,6 +1165,48 @@ mptable_hyperthread_fixup(u_int id_mask)
        mp_naps *= logical_cpus;
 }
 
+static void
+mptable_map(struct mptable_pos *mpt, vm_paddr_t mpfps_paddr)
+{
+       mpfps_t fps = NULL;
+       mpcth_t cth = NULL;
+       vm_size_t cth_mapsz = 0;
+
+       fps = pmap_mapdev(mpfps_paddr, sizeof(*fps));
+       if (fps->pap != 0) {
+               /*
+                * Map configuration table header to get
+                * the base table size
+                */
+               cth = pmap_mapdev(fps->pap, sizeof(*cth));
+               cth_mapsz = cth->base_table_length;
+               pmap_unmapdev((vm_offset_t)cth, sizeof(*cth));
+
+               /*
+                * Map the base table
+                */
+               cth = pmap_mapdev(fps->pap, cth_mapsz);
+       }
+
+       mpt->mp_fps = fps;
+       mpt->mp_cth = cth;
+       mpt->mp_cth_mapsz = cth_mapsz;
+}
+
+static void
+mptable_unmap(struct mptable_pos *mpt)
+{
+       if (mpt->mp_cth != NULL) {
+               pmap_unmapdev((vm_offset_t)mpt->mp_cth, mpt->mp_cth_mapsz);
+               mpt->mp_cth = NULL;
+               mpt->mp_cth_mapsz = 0;
+       }
+       if (mpt->mp_fps != NULL) {
+               pmap_unmapdev((vm_offset_t)mpt->mp_fps, sizeof(*mpt->mp_fps));
+               mpt->mp_fps = NULL;
+       }
+}
+
 #ifdef APIC_IO
 
 void
@@ -1257,7 +1411,7 @@ io_apic_find_int_entry(int apic, int pin)
  * parse an Intel MP specification table
  */
 static void
-fix_mp_table(void)
+mptable_fix(void)
 {
        int     x;
 #ifdef APIC_IO
@@ -1457,9 +1611,18 @@ setup_apic_irq_mapping(void)
 
 #endif
 
+void
+mp_set_cpuids(int cpu_id, int apic_id)
+{
+       CPU_TO_ID(cpu_id) = apic_id;
+       ID_TO_CPU(apic_id) = cpu_id;
+}
+
 static int
 processor_entry(proc_entry_ptr entry, int cpu)
 {
+       KKASSERT(cpu > 0);
+
        /* check for usability */
        if (!(entry->cpu_flags & PROCENTRY_FLAG_EN))
                return 0;
@@ -1468,16 +1631,13 @@ processor_entry(proc_entry_ptr entry, int cpu)
                panic("CPU APIC ID out of range (0..%d)", NAPICID - 1);
        /* check for BSP flag */
        if (entry->cpu_flags & PROCENTRY_FLAG_BP) {
-               boot_cpu_id = entry->apic_id;
-               CPU_TO_ID(0) = entry->apic_id;
-               ID_TO_CPU(entry->apic_id) = 0;
+               mp_set_cpuids(0, entry->apic_id);
                return 0;       /* its already been counted */
        }
 
        /* add another AP to list, if less than max number of CPUs */
        else if (cpu < MAXCPU) {
-               CPU_TO_ID(cpu) = entry->apic_id;
-               ID_TO_CPU(entry->apic_id) = cpu;
+               mp_set_cpuids(cpu, entry->apic_id);
                return 1;
        }
 
@@ -1923,9 +2083,9 @@ apic_polarity(int apic, int pin)
  * FIXME: probably not complete yet...
  */
 static void
-default_mp_table(int type)
+mptable_default(int type)
 {
-       int     ap_cpu_id;
+       int     ap_cpu_id, boot_cpu_id;
 #if defined(APIC_IO)
        int     io_apic_id;
        int     pin;
@@ -2066,6 +2226,9 @@ start_all_aps(u_int boot_addr)
        u_int64_t *pt4, *pt3, *pt2;
        int     x, i, pg;
        int     shift;
+       int     smicount;
+       int     smibest;
+       int     smilast;
        u_char  mpbiosreason;
        u_long  mpbioswarmvec;
        struct mdglobaldata *gd;
@@ -2113,6 +2276,32 @@ start_all_aps(u_int boot_addr)
        outb(CMOS_REG, BIOS_RESET);
        outb(CMOS_DATA, BIOS_WARM);     /* 'warm-start' */
 
+       /*
+        * If we have a TSC we can figure out the SMI interrupt rate.
+        * The SMI does not necessarily use a constant rate.  Spend
+        * up to 250ms trying to figure it out.
+        */
+       smibest = 0;
+       if (cpu_feature & CPUID_TSC) {
+               set_apic_timer(275000);
+               smilast = read_apic_timer();
+               for (x = 0; x < 20 && read_apic_timer(); ++x) {
+                       smicount = smitest();
+                       if (smibest == 0 || smilast - smicount < smibest)
+                               smibest = smilast - smicount;
+                       smilast = smicount;
+               }
+               if (smibest > 250000)
+                       smibest = 0;
+               if (smibest) {
+                       smibest = smibest * (int64_t)1000000 /
+                                 get_apic_timer_frequency();
+               }
+       }
+       if (smibest)
+               kprintf("SMI Frequency (worst case): %d Hz (%d us)\n",
+                       1000000 / smibest, smibest);
+
        /* start each AP */
        for (x = 1; x <= mp_naps; ++x) {
 
@@ -2157,7 +2346,7 @@ start_all_aps(u_int boot_addr)
 
                /* attempt to start the Application Processor */
                CHECK_INIT(99); /* setup checkpoints */
-               if (!start_ap(gd, boot_addr)) {
+               if (!start_ap(gd, boot_addr, smibest)) {
                        kprintf("AP #%d (PHY# %d) failed!\n", x, CPU_TO_ID(x));
                        CHECK_PRINT("trace");   /* show checkpoints */
                        /* better panic as the AP may be running loose */
@@ -2205,10 +2394,6 @@ start_all_aps(u_int boot_addr)
         * NOTE!  The idlestack for the BSP was setup by locore.  Finish
         * up, clean out the P==V mapping we did earlier.
         */
-#if JGXXX
-       for (x = 0; x < NKPT; x++)
-               PTD[x] = 0;
-#endif
        pmap_set_opt();
 
        /* number of APs actually started */
@@ -2280,7 +2465,7 @@ install_ap_tramp(u_int boot_addr)
 #endif
 
 /*
- * this function starts the AP (application processor) identified
+ * This function starts the AP (application processor) identified
  * by the APIC ID 'physicalCpu'.  It does quite a "song and dance"
  * to accomplish this.  This is necessary because of the nuances
  * of the different hardware we might encounter.  It ain't pretty,
@@ -2290,7 +2475,7 @@ install_ap_tramp(u_int boot_addr)
  * before the AP goes into the LWKT scheduler's idle loop.
  */
 static int
-start_ap(struct mdglobaldata *gd, u_int boot_addr)
+start_ap(struct mdglobaldata *gd, u_int boot_addr, int smibest)
 {
        int     physical_cpu;
        int     vector;
@@ -2304,63 +2489,119 @@ start_ap(struct mdglobaldata *gd, u_int boot_addr)
        /* calculate the vector */
        vector = (boot_addr >> 12) & 0xff;
 
+       /* We don't want anything interfering */
+       cpu_disable_intr();
+
        /* Make sure the target cpu sees everything */
        wbinvd();
 
+       /*
+        * Try to detect when a SMI has occurred, wait up to 200ms.
+        *
+        * If a SMI occurs during an AP reset but before we issue
+        * the STARTUP command, the AP may brick.  To work around
+        * this problem we hold off doing the AP startup until
+        * after we have detected the SMI.  Hopefully another SMI
+        * will not occur before we finish the AP startup.
+        *
+        * Retries don't seem to help.  SMIs have a window of opportunity
+        * and if USB->legacy keyboard emulation is enabled in the BIOS
+        * the interrupt rate can be quite high.
+        *
+        * NOTE: Don't worry about the L1 cache load, it might bloat
+        *       ldelta a little but ndelta will be so huge when the SMI
+        *       occurs the detection logic will still work fine.
+        */
+       if (smibest) {
+               set_apic_timer(200000);
+               smitest();
+       }
+
        /*
         * first we do an INIT/RESET IPI this INIT IPI might be run, reseting
         * and running the target CPU. OR this INIT IPI might be latched (P5
         * bug), CPU waiting for STARTUP IPI. OR this INIT IPI might be
         * ignored.
+        *
+        * see apic/apicreg.h for icr bit definitions.
+        *
+        * TIME CRITICAL CODE, DO NOT DO ANY KPRINTFS IN THE HOT PATH.
         */
 
-       /* setup the address for the target AP */
+       /*
+        * Setup the address for the target AP.  We can setup
+        * icr_hi once and then just trigger operations with
+        * icr_lo.
+        */
        icr_hi = lapic->icr_hi & ~APIC_ID_MASK;
        icr_hi |= (physical_cpu << 24);
-       lapic->icr_hi = icr_hi;
-
-       /* do an INIT IPI: assert RESET */
        icr_lo = lapic->icr_lo & 0xfff00000;
-       lapic->icr_lo = icr_lo | 0x0000c500;
+       lapic->icr_hi = icr_hi;
 
-       /* wait for pending status end */
+       /*
+        * Do an INIT IPI: assert RESET
+        *
+        * Use edge triggered mode to assert INIT
+        */
+       lapic->icr_lo = icr_lo | 0x00004500;
        while (lapic->icr_lo & APIC_DELSTAT_MASK)
                 /* spin */ ;
 
-       /* do an INIT IPI: deassert RESET */
-       lapic->icr_lo = icr_lo | 0x00008500;
+       /*
+        * The spec calls for a 10ms delay but we may have to use a
+        * MUCH lower delay to avoid bricking an AP due to a fast SMI
+        * interrupt.  We have other loops here too and dividing by 2
+        * doesn't seem to be enough even after subtracting 350us,
+        * so we divide by 4.
+        *
+        * Our minimum delay is 150uS, maximum is 10ms.  If no SMI
+        * interrupt was detected we use the full 10ms.
+        */
+       if (smibest == 0)
+               u_sleep(10000);
+       else if (smibest < 150 * 4 + 350)
+               u_sleep(150);
+       else if ((smibest - 350) / 4 < 10000)
+               u_sleep((smibest - 350) / 4);
+       else
+               u_sleep(10000);
 
-       /* wait for pending status end */
-       u_sleep(10000);         /* wait ~10mS */
+       /*
+        * Do an INIT IPI: deassert RESET
+        *
+        * Use level triggered mode to deassert.  It is unclear
+        * why we need to do this.
+        */
+       lapic->icr_lo = icr_lo | 0x00008500;
        while (lapic->icr_lo & APIC_DELSTAT_MASK)
                 /* spin */ ;
+       u_sleep(150);                           /* wait 150us */
 
        /*
-        * next we do a STARTUP IPI: the previous INIT IPI might still be
+        * Next we do a STARTUP IPI: the previous INIT IPI might still be
         * latched, (P5 bug) this 1st STARTUP would then terminate
         * immediately, and the previously started INIT IPI would continue. OR
         * the previous INIT IPI has already run. and this STARTUP IPI will
         * run. OR the previous INIT IPI was ignored. and this STARTUP IPI
         * will run.
         */
-
-       /* do a STARTUP IPI */
        lapic->icr_lo = icr_lo | 0x00000600 | vector;
        while (lapic->icr_lo & APIC_DELSTAT_MASK)
                 /* spin */ ;
        u_sleep(200);           /* wait ~200uS */
 
        /*
-        * finally we do a 2nd STARTUP IPI: this 2nd STARTUP IPI should run IF
+        * Finally we do a 2nd STARTUP IPI: this 2nd STARTUP IPI should run IF
         * the previous STARTUP IPI was cancelled by a latched INIT IPI. OR
         * this STARTUP IPI will be ignored, as only ONE STARTUP IPI is
         * recognized after hardware RESET or INIT IPI.
         */
-
        lapic->icr_lo = icr_lo | 0x00000600 | vector;
        while (lapic->icr_lo & APIC_DELSTAT_MASK)
                 /* spin */ ;
-       u_sleep(200);           /* wait ~200uS */
+
+       /* Resume normal operation */
+       cpu_enable_intr();
 
        /* wait for it to start, see ap_init() */
        set_apic_timer(5000000);/* == 5 seconds */
@@ -2368,9 +2609,38 @@ start_ap(struct mdglobaldata *gd, u_int boot_addr)
                if (smp_startup_mask & (1 << gd->mi.gd_cpuid))
                        return 1;       /* return SUCCESS */
        }
+
        return 0;               /* return FAILURE */
 }
 
+static
+int
+smitest(void)
+{
+       int64_t ltsc;
+       int64_t ntsc;
+       int64_t ldelta;
+       int64_t ndelta;
+       int count;
+
+       ldelta = 0;
+       ndelta = 0;
+       while (read_apic_timer()) {
+               ltsc = rdtsc();
+               for (count = 0; count < 100; ++count)
+                       ntsc = rdtsc(); /* force loop to occur */
+               if (ldelta) {
+                       ndelta = ntsc - ltsc;
+                       if (ldelta > ndelta)
+                               ldelta = ndelta;
+                       if (ndelta > ldelta * 2)
+                               break;
+               } else {
+                       ldelta = ntsc - ltsc;
+               }
+       }
+       return(read_apic_timer());
+}
 
 /*
  * Lazy flush the TLB on all other CPU's.  DEPRECATED.