Fixes for mptable generation.

Rework cpu iteration to simplify.  Also fixes sparse table generation
    when multiple cores found.
Rework entrycount generation to fix irq0override code.
Move fls() to util.h.
Add free calls when allocation fails.

diff --git a/src/mptable.c b/src/mptable.c
index 0c07cfb4809aa6219934129f32a1d50d181916da..8d6db59bd46a591fde1fc23cf95aef515a5fb441 100644 (file)
--- a/src/mptable.c
+++ b/src/mptable.c
@@ -8,16 +8,7 @@
 #include "util.h" // dprintf
 #include "config.h" // CONFIG_*
 #include "mptable.h" // MPTABLE_SIGNATURE
-#include "paravirt.h"
-
-static inline unsigned long
-fls(unsigned long word)
-{
-    asm("bsr %1,%0"
-        : "=r" (word)
-        : "rm" (word));
-    return word + 1;
-}
+#include "paravirt.h" // qemu_cfg_irq0_override
 
 void
 mptable_init(void)
@@ -27,6 +18,7 @@ mptable_init(void)
 
     dprintf(3, "init MPTable\n");
 
+    // Allocate memory
     int length = (sizeof(struct mptable_config_s)
                   + sizeof(struct mpt_cpu) * MaxCountCPUs
                   + sizeof(struct mpt_bus)
@@ -36,6 +28,8 @@ mptable_init(void)
     struct mptable_floating_s *floating = malloc_fseg(sizeof(*floating));
     if (!config || !floating) {
         dprintf(1, "No room for MPTABLE!\n");
+        free(config);
+        free(floating);
         return;
     }
 
@@ -55,32 +49,27 @@ mptable_init(void)
     memcpy(config->productid, "0.1         ", sizeof(config->productid));
     config->lapic = BUILD_APIC_ADDR;
 
-    // CPU definitions.
+    // Detect cpu info
     u32 cpuid_signature, ebx, ecx, cpuid_features;
     cpuid(1, &cpuid_signature, &ebx, &ecx, &cpuid_features);
-    struct mpt_cpu *cpus = (void*)&config[1];
-    int i, actual_cpu_count;
-    for (i = 0, actual_cpu_count = 0; i < MaxCountCPUs; i++) {
-        struct mpt_cpu *cpu = &cpus[i];
-        int log_cpus = (ebx >> 16) & 0xff;
-        log_cpus = 1UL << fls(log_cpus - 1); /* round up to power of 2 */
-
-        /* Only populate the MPS tables with the first logical CPU in each
-           package */
-        if ((cpuid_features & (1 << 28)) && (i & (log_cpus - 1)) != 0)
-            continue;
-
-        actual_cpu_count++;
+    int pkgcpus = 1;
+    if (cpuid_features & (1 << 28)) {
+        /* Only populate the MPS tables with the first logical CPU in
+           each package */
+        pkgcpus = (ebx >> 16) & 0xff;
+        pkgcpus = 1 << (__fls(pkgcpus - 1) + 1); /* round up to power of 2 */
+    }
 
+    // CPU definitions.
+    struct mpt_cpu *cpus = (void*)&config[1], *cpu = cpus;
+    int i;
+    for (i = 0; i < MaxCountCPUs; i+=pkgcpus) {
         memset(cpu, 0, sizeof(*cpu));
         cpu->type = MPT_TYPE_CPU;
         cpu->apicid = i;
         cpu->apicver = 0x11;
         /* cpu flags: enabled, bootstrap cpu */
-        if (i < CountCPUs)
-            cpu->cpuflag = 1 | ((i == 0) ? 2 : 0);
-        else
-            cpu->cpuflag = 0;
+        cpu->cpuflag = (i < CountCPUs) | ((i == 0) << 1);
         if (cpuid_signature) {
             cpu->cpusignature = cpuid_signature;
             cpu->featureflag = cpuid_features;
@@ -88,15 +77,16 @@ mptable_init(void)
             cpu->cpusignature = 0x600;
             cpu->featureflag = 0x201;
         }
+        cpu++;
     }
-
-    config->entrycount = actual_cpu_count + 2 + 16;
+    int entrycount = cpu - cpus;
 
     /* isa bus */
-    struct mpt_bus *bus = (void*)&cpus[actual_cpu_count];
+    struct mpt_bus *bus = (void*)cpu;
     memset(bus, 0, sizeof(*bus));
     bus->type = MPT_TYPE_BUS;
     memcpy(bus->bustype, "ISA   ", sizeof(bus->bustype));
+    entrycount++;
 
     /* ioapic */
     u8 ioapic_id = CountCPUs;
@@ -107,9 +97,10 @@ mptable_init(void)
     ioapic->apicver = 0x11;
     ioapic->flags = 1; // enable
     ioapic->apicaddr = BUILD_IOAPIC_ADDR;
+    entrycount++;
 
     /* irqs */
-    struct mpt_intsrc *intsrc = (void*)&ioapic[1];
+    struct mpt_intsrc *intsrcs = (void*)&ioapic[1], *intsrc = intsrcs;
     for (i = 0; i < 16; i++) {
         memset(intsrc, 0, sizeof(*intsrc));
         intsrc->type = MPT_TYPE_INTSRC;
@@ -126,8 +117,10 @@ mptable_init(void)
         }
         intsrc++;
     }
+    entrycount += intsrc - intsrcs;
 
-    // Set checksum.
+    // Finalize config structure.
+    config->entrycount = entrycount;
     config->length = (void*)intsrc - (void*)config;
     config->checksum -= checksum(config, config->length);
 

diff --git a/src/util.h b/src/util.h
index 637fd2bc6efec20259a54a47ed316ba45361535b..54326853e7944935302a578aa27ec746e87db24f 100644 (file)
--- a/src/util.h
+++ b/src/util.h
@@ -93,6 +93,13 @@ static inline u32 __ffs(u32 word)
         : "rm" (word));
     return word;
 }
+static inline u32 __fls(u32 word)
+{
+    asm("bsr %1,%0"
+        : "=r" (word)
+        : "rm" (word));
+    return word;
+}
 
 static inline void writel(void *addr, u32 val) {
     *(volatile u32 *)addr = val;