From: Kevin O'Connor <kevin@koconnor.net>
Date: Sat, 14 Nov 2009 18:34:27 +0000 (-0500)
Subject: Fixes for mptable generation.
X-Git-Tag: rel-0.5.0~37
X-Git-Url: http://xenbits.xensource.com/gitweb?a=commitdiff_plain;h=86916ce3de5fc16753507b964053c8476ebe6631;p=seabios.git

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 0c07cfb..8d6db59 100644
--- 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 637fd2b..5432685 100644
--- 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;