Separate out smp detection and mp table generation from rombios32.c

Also, change smp_probe() to return the cpu count on each call.

6 files changed, 283 insertions, 259 deletions

diff --git a/src/acpi.c b/src/acpi.c
index 63ac319..51f8161 100644
--- a/src/acpi.c
+++ b/src/acpi.c
@@ -1,4 +1,4 @@
-// Support for enabling/disabling BIOS ram shadowing.
+// Support for generating ACPI tables (on emulators)
 //
 // Copyright (C) 2008  Kevin O'Connor <kevin@koconnor.net>
 // Copyright (C) 2006 Fabrice Bellard
@@ -228,6 +228,7 @@ acpi_build_processor_ssdt(u8 *ssdt)
 {
     u8 *ssdt_ptr = ssdt;
     int i, length;
+    int smp_cpus = smp_probe();
     int acpi_cpus = smp_cpus > 0xff ? 0xff : smp_cpus;
 
     ssdt_ptr[9] = 0; // checksum;
@@ -327,6 +328,7 @@ void acpi_bios_init(void)
     ssdt = (void *)(addr);
     addr += acpi_build_processor_ssdt(ssdt);
 
+    int smp_cpus = smp_probe();
     addr = (addr + 7) & ~7;
     madt_addr = addr;
     madt_size = sizeof(*madt) +
diff --git a/src/acpi.h b/src/acpi.h
index 50ba135..a5c58b5 100644
--- a/src/acpi.h
+++ b/src/acpi.h
@@ -5,9 +5,6 @@
 
 void acpi_bios_init(void);
 
-// XXX - move to better header.
-extern int smp_cpus;
-
 #define RSDP_SIGNATURE 0x2052545020445352LL // "RSD PTR "
 
 struct rsdp_descriptor         /* Root System Descriptor Pointer */
diff --git a/src/mptable.c b/src/mptable.c
new file mode 100644
index 0000000..b1b05e1
--- /dev/null
+++ b/src/mptable.c
@@ -0,0 +1,159 @@
+// MPTable generation (on emulators)
+//
+// Copyright (C) 2008  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2006 Fabrice Bellard
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "util.h" // dprintf
+#include "memmap.h" // bios_table_cur_addr
+
+static void putb(u8 **pp, int val)
+{
+    u8 *q;
+    q = *pp;
+    *q++ = val;
+    *pp = q;
+}
+
+static void putstr(u8 **pp, const char *str)
+{
+    u8 *q;
+    q = *pp;
+    while (*str)
+        *q++ = *str++;
+    *pp = q;
+}
+
+static void putle16(u8 **pp, int val)
+{
+    u8 *q;
+    q = *pp;
+    *q++ = val;
+    *q++ = val >> 8;
+    *pp = q;
+}
+
+static void putle32(u8 **pp, int val)
+{
+    u8 *q;
+    q = *pp;
+    *q++ = val;
+    *q++ = val >> 8;
+    *q++ = val >> 16;
+    *q++ = val >> 24;
+    *pp = q;
+}
+
+void
+mptable_init(void)
+{
+    u8 *mp_config_table, *q, *float_pointer_struct;
+    int ioapic_id, i, len;
+    int mp_config_table_size;
+
+    int smp_cpus = smp_probe();
+    if (CONFIG_QEMU && smp_cpus <= 1)
+        return;
+
+    bios_table_cur_addr = ALIGN(bios_table_cur_addr, 16);
+    mp_config_table = (u8 *)bios_table_cur_addr;
+    q = mp_config_table;
+    putstr(&q, "PCMP"); /* "PCMP signature */
+    putle16(&q, 0); /* table length (patched later) */
+    putb(&q, 4); /* spec rev */
+    putb(&q, 0); /* checksum (patched later) */
+    if (CONFIG_QEMU)
+        putstr(&q, "QEMUCPU "); /* OEM id */
+    else
+        putstr(&q, "BOCHSCPU");
+    putstr(&q, "0.1         "); /* vendor id */
+    putle32(&q, 0); /* OEM table ptr */
+    putle16(&q, 0); /* OEM table size */
+    putle16(&q, smp_cpus + 18); /* entry count */
+    putle32(&q, 0xfee00000); /* local APIC addr */
+    putle16(&q, 0); /* ext table length */
+    putb(&q, 0); /* ext table checksum */
+    putb(&q, 0); /* reserved */
+
+    for(i = 0; i < smp_cpus; i++) {
+        putb(&q, 0); /* entry type = processor */
+        putb(&q, i); /* APIC id */
+        putb(&q, 0x11); /* local APIC version number */
+        if (i == 0)
+            putb(&q, 3); /* cpu flags: enabled, bootstrap cpu */
+        else
+            putb(&q, 1); /* cpu flags: enabled */
+        putb(&q, 0); /* cpu signature */
+        putb(&q, 6);
+        putb(&q, 0);
+        putb(&q, 0);
+        putle16(&q, 0x201); /* feature flags */
+        putle16(&q, 0);
+
+        putle16(&q, 0); /* reserved */
+        putle16(&q, 0);
+        putle16(&q, 0);
+        putle16(&q, 0);
+    }
+
+    /* isa bus */
+    putb(&q, 1); /* entry type = bus */
+    putb(&q, 0); /* bus ID */
+    putstr(&q, "ISA   ");
+
+    /* ioapic */
+    ioapic_id = smp_cpus;
+    putb(&q, 2); /* entry type = I/O APIC */
+    putb(&q, ioapic_id); /* apic ID */
+    putb(&q, 0x11); /* I/O APIC version number */
+    putb(&q, 1); /* enable */
+    putle32(&q, 0xfec00000); /* I/O APIC addr */
+
+    /* irqs */
+    for(i = 0; i < 16; i++) {
+        putb(&q, 3); /* entry type = I/O interrupt */
+        putb(&q, 0); /* interrupt type = vectored interrupt */
+        putb(&q, 0); /* flags: po=0, el=0 */
+        putb(&q, 0);
+        putb(&q, 0); /* source bus ID = ISA */
+        putb(&q, i); /* source bus IRQ */
+        putb(&q, ioapic_id); /* dest I/O APIC ID */
+        putb(&q, i); /* dest I/O APIC interrupt in */
+    }
+    /* patch length */
+    len = q - mp_config_table;
+    mp_config_table[4] = len;
+    mp_config_table[5] = len >> 8;
+
+    mp_config_table[7] = -checksum(mp_config_table, q - mp_config_table);
+
+    mp_config_table_size = q - mp_config_table;
+
+    bios_table_cur_addr += mp_config_table_size;
+
+    /* floating pointer structure */
+    bios_table_cur_addr = ALIGN(bios_table_cur_addr, 16);
+    float_pointer_struct = (u8 *)bios_table_cur_addr;
+    q = float_pointer_struct;
+    putstr(&q, "_MP_");
+    /* pointer to MP config table */
+    putle32(&q, (unsigned long)mp_config_table);
+
+    putb(&q, 1); /* length in 16 byte units */
+    putb(&q, 4); /* MP spec revision */
+    putb(&q, 0); /* checksum (patched later) */
+    putb(&q, 0); /* MP feature byte 1 */
+
+    putb(&q, 0);
+    putb(&q, 0);
+    putb(&q, 0);
+    putb(&q, 0);
+    float_pointer_struct[10] = -checksum(float_pointer_struct
+                                         , q - float_pointer_struct);
+    bios_table_cur_addr += (q - float_pointer_struct);
+    dprintf(1, "MP table addr=0x%08lx MPC table addr=0x%08lx size=0x%x\n",
+            (unsigned long)float_pointer_struct,
+            (unsigned long)mp_config_table,
+            mp_config_table_size);
+}
diff --git a/src/rombios32.c b/src/rombios32.c
index bb4fd3a..089b49d 100644
--- a/src/rombios32.c
+++ b/src/rombios32.c
@@ -24,54 +24,6 @@
 #include "memmap.h" // bios_table_cur_addr
 #include "acpi.h" // acpi_bios_init
 
-#define cpuid(index, eax, ebx, ecx, edx) \
-  asm volatile ("cpuid" \
-                : "=a" (eax), "=b" (ebx), "=c" (ecx), "=d" (edx) \
-                : "0" (index))
-
-#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_ENABLED 0x0100
-
-#define MPTABLE_MAX_SIZE  0x00002000
-
-static inline void writel(void *addr, u32 val)
-{
-    *(volatile u32 *)addr = val;
-}
-
-static inline void writew(void *addr, u16 val)
-{
-    *(volatile u16 *)addr = val;
-}
-
-static inline void writeb(void *addr, u8 val)
-{
-    *(volatile u8 *)addr = val;
-}
-
-static inline u32 readl(const void *addr)
-{
-    return *(volatile const u32 *)addr;
-}
-
-static inline u16 readw(const void *addr)
-{
-    return *(volatile const u16 *)addr;
-}
-
-static inline u8 readb(const void *addr)
-{
-    return *(volatile const u8 *)addr;
-}
-
-int smp_cpus;
 u32 cpuid_signature;
 u32 cpuid_features;
 u32 cpuid_ext_features;
@@ -107,66 +59,13 @@ void uuid_probe(void)
 void cpu_probe(void)
 {
     u32 eax, ebx, ecx, edx;
-    cpuid(1, eax, ebx, ecx, edx);
+    cpuid(1, &eax, &ebx, &ecx, &edx);
     cpuid_signature = eax;
     cpuid_features = edx;
     cpuid_ext_features = ecx;
 }
 
 /****************************************************/
-/* SMP probe */
-
-asm(
-    ".globl smp_ap_boot_code_start\n"
-    ".globl smp_ap_boot_code_end\n"
-    "  .code16\n"
-
-    "smp_ap_boot_code_start:\n"
-    "  xor %ax, %ax\n"
-    "  mov %ax, %ds\n"
-    "  incw " __stringify(BUILD_CPU_COUNT_ADDR) "\n"
-    "1:\n"
-    "  hlt\n"
-    "  jmp 1b\n"
-    "smp_ap_boot_code_end:\n"
-
-    "  .code32\n"
-    );
-
-extern u8 smp_ap_boot_code_start;
-extern u8 smp_ap_boot_code_end;
-
-/* find the number of CPUs by launching a SIPI to them */
-void smp_probe(void)
-{
-    u32 val, sipi_vector;
-
-    smp_cpus = 1;
-    if (cpuid_features & CPUID_APIC) {
-
-        /* enable local APIC */
-        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);
-        sipi_vector = BUILD_AP_BOOT_ADDR >> 12;
-        writel(APIC_BASE + APIC_ICR_LOW, 0x000C4600 | sipi_vector);
-
-        usleep(10*1000);
-
-        smp_cpus = readw((void *)BUILD_CPU_COUNT_ADDR);
-    }
-    dprintf(1, "Found %d cpu(s)\n", smp_cpus);
-}
-
-/****************************************************/
 /* PCI init */
 
 #define PCI_ADDRESS_SPACE_MEM		0x00
@@ -377,157 +276,6 @@ void pci_bios_init(void)
     pci_for_each_device(pci_bios_init_device);
 }
 
-/****************************************************/
-/* Multi Processor table init */
-
-static void putb(u8 **pp, int val)
-{
-    u8 *q;
-    q = *pp;
-    *q++ = val;
-    *pp = q;
-}
-
-static void putstr(u8 **pp, const char *str)
-{
-    u8 *q;
-    q = *pp;
-    while (*str)
-        *q++ = *str++;
-    *pp = q;
-}
-
-static void putle16(u8 **pp, int val)
-{
-    u8 *q;
-    q = *pp;
-    *q++ = val;
-    *q++ = val >> 8;
-    *pp = q;
-}
-
-static void putle32(u8 **pp, int val)
-{
-    u8 *q;
-    q = *pp;
-    *q++ = val;
-    *q++ = val >> 8;
-    *q++ = val >> 16;
-    *q++ = val >> 24;
-    *pp = q;
-}
-
-static void mptable_init(void)
-{
-    u8 *mp_config_table, *q, *float_pointer_struct;
-    int ioapic_id, i, len;
-    int mp_config_table_size;
-
-    if (CONFIG_QEMU && smp_cpus <= 1)
-        return;
-
-    bios_table_cur_addr = ALIGN(bios_table_cur_addr, 16);
-    mp_config_table = (u8 *)bios_table_cur_addr;
-    q = mp_config_table;
-    putstr(&q, "PCMP"); /* "PCMP signature */
-    putle16(&q, 0); /* table length (patched later) */
-    putb(&q, 4); /* spec rev */
-    putb(&q, 0); /* checksum (patched later) */
-    if (CONFIG_QEMU)
-        putstr(&q, "QEMUCPU "); /* OEM id */
-    else
-        putstr(&q, "BOCHSCPU");
-    putstr(&q, "0.1         "); /* vendor id */
-    putle32(&q, 0); /* OEM table ptr */
-    putle16(&q, 0); /* OEM table size */
-    putle16(&q, smp_cpus + 18); /* entry count */
-    putle32(&q, 0xfee00000); /* local APIC addr */
-    putle16(&q, 0); /* ext table length */
-    putb(&q, 0); /* ext table checksum */
-    putb(&q, 0); /* reserved */
-
-    for(i = 0; i < smp_cpus; i++) {
-        putb(&q, 0); /* entry type = processor */
-        putb(&q, i); /* APIC id */
-        putb(&q, 0x11); /* local APIC version number */
-        if (i == 0)
-            putb(&q, 3); /* cpu flags: enabled, bootstrap cpu */
-        else
-            putb(&q, 1); /* cpu flags: enabled */
-        putb(&q, 0); /* cpu signature */
-        putb(&q, 6);
-        putb(&q, 0);
-        putb(&q, 0);
-        putle16(&q, 0x201); /* feature flags */
-        putle16(&q, 0);
-
-        putle16(&q, 0); /* reserved */
-        putle16(&q, 0);
-        putle16(&q, 0);
-        putle16(&q, 0);
-    }
-
-    /* isa bus */
-    putb(&q, 1); /* entry type = bus */
-    putb(&q, 0); /* bus ID */
-    putstr(&q, "ISA   ");
-
-    /* ioapic */
-    ioapic_id = smp_cpus;
-    putb(&q, 2); /* entry type = I/O APIC */
-    putb(&q, ioapic_id); /* apic ID */
-    putb(&q, 0x11); /* I/O APIC version number */
-    putb(&q, 1); /* enable */
-    putle32(&q, 0xfec00000); /* I/O APIC addr */
-
-    /* irqs */
-    for(i = 0; i < 16; i++) {
-        putb(&q, 3); /* entry type = I/O interrupt */
-        putb(&q, 0); /* interrupt type = vectored interrupt */
-        putb(&q, 0); /* flags: po=0, el=0 */
-        putb(&q, 0);
-        putb(&q, 0); /* source bus ID = ISA */
-        putb(&q, i); /* source bus IRQ */
-        putb(&q, ioapic_id); /* dest I/O APIC ID */
-        putb(&q, i); /* dest I/O APIC interrupt in */
-    }
-    /* patch length */
-    len = q - mp_config_table;
-    mp_config_table[4] = len;
-    mp_config_table[5] = len >> 8;
-
-    mp_config_table[7] = -checksum(mp_config_table, q - mp_config_table);
-
-    mp_config_table_size = q - mp_config_table;
-
-    bios_table_cur_addr += mp_config_table_size;
-
-    /* floating pointer structure */
-    bios_table_cur_addr = ALIGN(bios_table_cur_addr, 16);
-    float_pointer_struct = (u8 *)bios_table_cur_addr;
-    q = float_pointer_struct;
-    putstr(&q, "_MP_");
-    /* pointer to MP config table */
-    putle32(&q, (unsigned long)mp_config_table);
-
-    putb(&q, 1); /* length in 16 byte units */
-    putb(&q, 4); /* MP spec revision */
-    putb(&q, 0); /* checksum (patched later) */
-    putb(&q, 0); /* MP feature byte 1 */
-
-    putb(&q, 0);
-    putb(&q, 0);
-    putb(&q, 0);
-    putb(&q, 0);
-    float_pointer_struct[10] = -checksum(float_pointer_struct
-                                         , q - float_pointer_struct);
-    bios_table_cur_addr += (q - float_pointer_struct);
-    dprintf(1, "MP table addr=0x%08lx MPC table addr=0x%08lx size=0x%x\n",
-            (unsigned long)float_pointer_struct,
-            (unsigned long)mp_config_table,
-            mp_config_table_size);
-}
-
 /* SMBIOS entry point -- must be written to a 16-bit aligned address
    between 0xf0000 and 0xfffff.
  */
@@ -999,6 +747,7 @@ void smbios_init(void)
     add_struct(smbios_type_0_init(p));
     add_struct(smbios_type_1_init(p));
     add_struct(smbios_type_3_init(p));
+    int smp_cpus = smp_probe();
     for (cpu_num = 1; cpu_num <= smp_cpus; cpu_num++)
         add_struct(smbios_type_4_init(p, cpu_num));
     add_struct(smbios_type_16_init(p, memsize));
@@ -1031,8 +780,6 @@ void rombios32_init(void)
 
     cpu_probe();
 
-    smp_probe();
-
     pci_bios_init();
 
     smm_init();
diff --git a/src/smpdetect.c b/src/smpdetect.c
new file mode 100644
index 0000000..2d03d7c
--- /dev/null
+++ b/src/smpdetect.c
@@ -0,0 +1,106 @@
+// CPU count detection
+//
+// Copyright (C) 2008  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2006 Fabrice Bellard
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "util.h" // dprintf
+
+#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_ENABLED 0x0100
+
+static inline void writel(void *addr, u32 val)
+{
+    *(volatile u32 *)addr = val;
+}
+
+static inline void writew(void *addr, u16 val)
+{
+    *(volatile u16 *)addr = val;
+}
+
+static inline void writeb(void *addr, u8 val)
+{
+    *(volatile u8 *)addr = val;
+}
+
+static inline u32 readl(const void *addr)
+{
+    return *(volatile const u32 *)addr;
+}
+
+static inline u16 readw(const void *addr)
+{
+    return *(volatile const u16 *)addr;
+}
+
+static inline u8 readb(const void *addr)
+{
+    return *(volatile const u8 *)addr;
+}
+
+asm(
+    ".globl smp_ap_boot_code_start\n"
+    ".globl smp_ap_boot_code_end\n"
+    "  .code16\n"
+
+    "smp_ap_boot_code_start:\n"
+    "  xor %ax, %ax\n"
+    "  mov %ax, %ds\n"
+    "  incw " __stringify(BUILD_CPU_COUNT_ADDR) "\n"
+    "1:\n"
+    "  hlt\n"
+    "  jmp 1b\n"
+    "smp_ap_boot_code_end:\n"
+
+    "  .code32\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)
+{
+    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);
+
+        usleep(10*1000);
+
+        smp_cpus = readw((void *)BUILD_CPU_COUNT_ADDR);
+    }
+    dprintf(1, "Found %d cpu(s)\n", smp_cpus);
+
+    return smp_cpus;
+}
diff --git a/src/util.h b/src/util.h
index 0522d14..bccf750 100644
--- a/src/util.h
+++ b/src/util.h
@@ -52,6 +52,13 @@ static inline void wbinvd(void)
     asm volatile("wbinvd");
 }
 
+static inline void cpuid(u32 index, u32 *eax, u32 *ebx, u32 *ecx, u32 *edx)
+{
+    asm("cpuid"
+        : "=a" (*eax), "=b" (*ebx), "=c" (*ecx), "=d" (*edx)
+        : "0" (index));
+}
+
 void *memset(void *s, int c, size_t n);
 void *memcpy(void *d1, const void *s1, size_t len);
 void *memmove(void *d, const void *s, size_t len);
@@ -193,6 +200,12 @@ void rombios32_init(void);
 // smm.c
 void smm_init();
 
+// smpdetect.c
+int smp_probe(void);
+
+// mptable.c
+void mptable_init(void);
+
 // boot.c
 void printf_bootdev(u16 bootdev);