Add linker magic to ensure 16bit variables aren't repeated in 32bit code.

[seabios.git] / src / smpdetect.c

diff --git a/src/smpdetect.c b/src/smpdetect.c
index eda0ecfd87ea6be7866eef9188c48eedbd47d5a3..202a82a7e344072566d0086f572171e59379275b 100644 (file)
--- a/src/smpdetect.c
+++ b/src/smpdetect.c
@@ -7,14 +7,12 @@
 
 #include "util.h" // dprintf
 #include "config.h" // CONFIG_*
+#include "cmos.h" // CMOS_BIOS_SMP_COUNT
 
 #define CPUID_APIC (1 << 9)
 
-#define APIC_BASE    ((u8 *)0xfee00000)
-#define APIC_ICR_LOW 0x300
-#define APIC_SVR     0x0F0
-#define APIC_ID      0x020
-#define APIC_LVT3    0x370
+#define APIC_ICR_LOW ((u8*)BUILD_APIC_ADDR + 0x300)
+#define APIC_SVR     ((u8*)BUILD_APIC_ADDR + 0x0F0)
 
 #define APIC_ENABLED 0x0100
 
@@ -48,28 +46,19 @@ static inline u8 readb(const void *addr)
     return *(volatile const u8 *)addr;
 }
 
-asm(
-    ".global smp_ap_boot_code_start\n"
-    ".global smp_ap_boot_code_end\n"
-    "  .code16\n"
-
-    "smp_ap_boot_code_start:\n"
-    // Increament the counter at BUILD_CPU_COUNT_ADDR
-    "  xorw %ax, %ax\n"
+u32 smp_cpus VAR16;
+extern void smp_ap_boot_code();
+ASM16(
+    "  .global smp_ap_boot_code\n"
+    "smp_ap_boot_code:\n"
+    // Increment the cpu counter
+    "  movw $" __stringify(SEG_BIOS) ", %ax\n"
     "  movw %ax, %ds\n"
-    "  lock incw " __stringify(BUILD_CPU_COUNT_ADDR) "\n"
+    "  lock incl smp_cpus\n"
     // Halt the processor.
-    "  ljmpl $" __stringify(SEG_BIOS) ", $(permanent_halt - " __stringify(BUILD_BIOS_ADDR) ")\n"
-    "smp_ap_boot_code_end:\n"
-
-    "  .code32\n"
+    "  jmp permanent_halt\n"
     );
 
-extern u8 smp_ap_boot_code_start;
-extern u8 smp_ap_boot_code_end;
-
-static int smp_cpus;
-
 /* find the number of CPUs by launching a SIPI to them */
 int
 smp_probe(void)
@@ -77,31 +66,51 @@ smp_probe(void)
     if (smp_cpus)
         return smp_cpus;
 
-    smp_cpus = 1;
-
     u32 eax, ebx, ecx, cpuid_features;
     cpuid(1, &eax, &ebx, &ecx, &cpuid_features);
-    if (cpuid_features & CPUID_APIC) {
-        /* enable local APIC */
-        u32 val = readl(APIC_BASE + APIC_SVR);
-        val |= APIC_ENABLED;
-        writel(APIC_BASE + APIC_SVR, val);
-
-        writew((void *)BUILD_CPU_COUNT_ADDR, 1);
-        /* copy AP boot code */
-        memcpy((void *)BUILD_AP_BOOT_ADDR, &smp_ap_boot_code_start,
-               &smp_ap_boot_code_end - &smp_ap_boot_code_start);
-
-        /* broadcast SIPI */
-        writel(APIC_BASE + APIC_ICR_LOW, 0x000C4500);
-        u32 sipi_vector = BUILD_AP_BOOT_ADDR >> 12;
-        writel(APIC_BASE + APIC_ICR_LOW, 0x000C4600 | sipi_vector);
+    if (! (cpuid_features & CPUID_APIC)) {
+        // No apic - only the main cpu is present.
+        smp_cpus = 1;
+        return 1;
+    }
+
+    // Init the counter.
+    writel(&smp_cpus, 1);
 
+    // Setup jump trampoline to counter code.
+    u64 old = *(u64*)BUILD_AP_BOOT_ADDR;
+    // ljmpw $SEG_BIOS, $(smp_ap_boot_code - BUILD_BIOS_ADDR)
+    u64 new = (0xea | ((u64)SEG_BIOS<<24)
+               | (((u32)smp_ap_boot_code - BUILD_BIOS_ADDR) << 8));
+    *(u64*)BUILD_AP_BOOT_ADDR = new;
+
+    // enable local APIC
+    u32 val = readl(APIC_SVR);
+    writel(APIC_SVR, val | APIC_ENABLED);
+
+    // broadcast SIPI
+    writel(APIC_ICR_LOW, 0x000C4500);
+    u32 sipi_vector = BUILD_AP_BOOT_ADDR >> 12;
+    writel(APIC_ICR_LOW, 0x000C4600 | sipi_vector);
+
+    // Wait for other CPUs to process the SIPI.
+    if (CONFIG_COREBOOT)
         mdelay(10);
+    else
+        while (inb_cmos(CMOS_BIOS_SMP_COUNT) + 1 != readl(&smp_cpus))
+            ;
 
-        smp_cpus = readw((void *)BUILD_CPU_COUNT_ADDR);
-    }
-    dprintf(1, "Found %d cpu(s)\n", smp_cpus);
+    // Restore memory.
+    *(u64*)BUILD_AP_BOOT_ADDR = old;
 
-    return smp_cpus;
+    u32 count = readl(&smp_cpus);
+    dprintf(1, "Found %d cpu(s)\n", count);
+    return count;
+}
+
+// Reset smp_cpus to zero (forces a recheck on reboots).
+void
+smp_probe_setup(void)
+{
+    smp_cpus = 0;
 }