--- /dev/null
+#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;
+ }
+ }
+}