From a1c6cedc6e0e0de663002e9dd231b8c2309f2544 Mon Sep 17 00:00:00 2001
From: Nathan Wagner <nw@hydaspes.if.org>
Date: Mon, 24 Oct 2016 20:35:42 -0500
Subject: [PATCH] rudimentary smp detection

---
 smp.c | 129 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 smp.h |  58 ++++++++++++++++++++++++++
 2 files changed, 187 insertions(+)
 create mode 100644 smp.c
 create mode 100644 smp.h

diff --git a/smp.c b/smp.c
new file mode 100644
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
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
-- 
2.40.0