rudimentary smp detection

diff --git a/smp.c b/smp.c
new file mode 100644 (file)
index 0000000..a6d1867
--- /dev/null
+++ b/smp.c
@@ -0,0 +1,129 @@
+#include <kernel.h>
+#include <mem.h>
+#include <smp.h>
+
+static uint8_t checksum(void *start, size_t n) {
+       uint8_t sum = 0;
+       uint8_t *bytes;
+       int i;
+
+       bytes = start;
+       for (i=0; i<n; i++) {
+               sum += bytes[i];
+       }
+       return sum;
+}
+
+#if 0
+static void *scanfordword(paddr_t start, paddr_t max, size_t by, uint32_t val) {
+       while (start <= max - by) {
+               if (*(uint32_t *)(PHY2VIRTP(start)) == val) {
+                       return PHY2VIRTP(start);
+               }
+               start += by;
+       }
+       return 0;
+}
+#endif
+
+/* TODO this could probably be a kernel thread, and just power up
+ * processors as though they were hot-plugged.  Except that the
+ * local apic on the BSP needs to be configured
+ */
+void init_smp(void) {
+       uint16_t *bda;
+       uint32_t mpsig = 0x5f504d5f;
+       uint32_t tabsig = 0x504d4350;
+       uintptr_t mem;
+       struct mpfp *mp = 0;
+       struct mpconfig *config = 0;
+       int i;
+
+       /* search for base table */
+       
+       /* search first KB of the extended bios data area */
+       bda = PHY2VIRTP(0x40E);
+       mem = (uintptr_t)((*bda) << 4);
+
+       for (i=0;i <= 1024 - 16 ;i+=16) {
+               mp = PHY2VIRTP(mem+i);
+               if ( mp->signature == mpsig && checksum(mp, 16) == 0) {
+                       goto found;
+               }
+       }
+
+       /* search last kb of base mem, 639-640k */
+       mem = KB(639);
+       for (i=0;i<1024-16;i+=16) {
+               mp = PHY2VIRTP(mem+i);
+               if ( mp->signature == mpsig && checksum(mp, 16) == 0) {
+                       goto found;
+               }
+       }
+
+       /* search bios rom address space, f0000 - fffff */
+       mem = 0xf0000;
+       for (i=0;i<KB(64)-16;i+=16) {
+               mp = PHY2VIRTP(mem+i);
+               if ( mp->signature == mpsig && checksum(mp, 16) == 0) {
+                       goto found;
+               }
+       }
+
+       printk("mpft not found\n");
+       return;
+found:
+       printk("found mpft at %llx: features1 %x, features2 %x\n", (uintptr_t)mp,
+                       (uint32_t)mp->features1, (uint32_t)mp->features2);
+       config = PHY2VIRTP(mp->mpconfigptr);
+       if (config->signature != tabsig) {
+               printk("tabsig bad\n");
+               return;
+       }
+#if 0
+       if (checksum(config, (size_t)config->tablelength) != 0) {
+               printk("config table checksum bad\n");
+               return;
+       }
+#endif
+
+       vaddr_t entry = (vaddr_t)config + sizeof *config;
+       printk("mpct local apic: %x, entries: %x, len = %x\n", config->localapicaddr, config->entrycount, config->tablelength);
+       int procnum = 0;
+       for (i=0;i<config->entrycount;i++) {
+               uint8_t etype;
+               struct processorentry *proc;
+               struct ioapic *io;
+
+               etype = *(uint8_t *)entry;
+               switch (etype) {
+                       case 0: /* processor */
+                               proc = (struct processorentry *)entry;
+                               printk("processor %x, apic = %x:%s%s\n", procnum,
+                                               (uint32_t)proc->apicid,
+                                               proc->enabled ? " enabled" : "",
+                                               proc->bootstrap ? " bsp" : ""
+                                       );
+                               procnum++;
+                               entry += 20;
+                               break;
+                       case 1: /* bus */
+                               //printk("mp bus\n");
+                               entry += 8;
+                               break;
+                       case 2: /* io apic */
+                               io = (struct ioapic *)entry;
+                               if (io->enabled) {
+                                       printk("ioapic %x %llx\n", io->id, io->address);
+                               } else {
+                                       printk("ioapic %x disabledx\n", io->id);
+                               }
+                               entry += 8;
+                               break;
+                       default:
+                               //printk(" type: %x\n", etype);
+                               entry += 8;
+                               break;
+               }
+       }
+}

diff --git a/smp.h b/smp.h
new file mode 100644 (file)
index 0000000..6760345
--- /dev/null
+++ b/smp.h
@@ -0,0 +1,58 @@
+#ifndef SMP_H_
+#define SMP_H_ 1
+
+#include <stdint.h>
+
+void init_smp(void);
+
+struct mpfp {
+       uint32_t signature;
+       uint32_t mpconfigptr;
+       uint8_t length;
+       uint8_t version;
+       uint8_t checksum;
+       uint8_t features1;
+       uint8_t features2;
+       uint8_t features3;
+       uint8_t features4;
+       uint8_t features5;
+};
+
+struct mpconfig {
+       uint32_t signature;
+       uint16_t tablelength;
+       uint8_t revision;
+       uint8_t checksum;
+       char    oemid[8];
+       char    productid[12];
+       uint32_t oemtableptr;
+       uint16_t oemtablesize;
+       uint16_t entrycount;
+       uint32_t localapicaddr;
+       uint16_t extendedtablen;
+       uint8_t extendedchecksum;
+       uint8_t reserved;
+};
+
+struct processorentry {
+       uint8_t etype;
+       uint8_t apicid;
+       uint8_t apicversion;
+       uint8_t enabled:1;
+       uint8_t bootstrap:1;
+       uint8_t unused:6;
+       uint32_t signature;
+       uint32_t features;
+       char    unused2[8];
+};
+
+struct ioapic {
+       uint8_t etype;
+       uint8_t id;
+       uint8_t ioapicversion;
+       uint8_t enabled:1;
+       uint8_t unused:7;
+       uint32_t address;
+};
+
+#endif