Nucleus

diff --git a/kernel/acpi/apic.c b/kernel/acpi/apic.c
new file mode 100644
index 0000000..2e786c0
--- /dev/null
+++ b/kernel/acpi/apic.c
@@ -0,0 +1,222 @@
+/*
+ * This file contains code dealing with the Advanced Programmable Interrupt
+ * Controller system.  It should provide a drop-in replacement for the standard
+ * PIC system, abstracting it in the same way.  It should also be able to handle
+ * a non-SMP system, and fall back on the PIC.
+ */
+
+#include <stdint.h>
+#include <stddef.h>
+#include <string.h>
+#include <nucleus/memory.h>
+#include <nucleus/cpu.h>
+#include <io.h>
+#include "acpi.h"
+
+void cpu_load(void);
+extern void ap_trampoline(void);
+
+/* Entry types */
+enum EntryType {
+	ENTRY_LAPIC,
+	ENTRY_IOAPIC,
+	ENTRY_IOAPIC_ISO,
+	ENTRY_IOAPIC_NMI,
+	ENTRY_LAPIC_NMI,
+	ENTRY_LAPIC_ADDR,
+	ENTRY_X2APIC = 9,
+};
+
+/* Generic Entry */
+struct EntryHeader {
+	uint8_t type, length;
+};
+
+/* Local APIC */
+struct LAPICEntry {
+	uint8_t type, length;
+	uint8_t procId, id;
+	uint32_t flags;
+};
+
+/* I/O APIC */
+struct IOAPICEntry {
+	uint8_t type, length;
+	uint8_t id, reserved;
+	uint32_t addr, gsiBase;
+};
+
+/* IOAPIC Interrupt Source Override */
+struct ISOEntry {
+	uint8_t type, length;
+	uint8_t bus, irq;
+	uint8_t gsi, flags;
+};
+
+size_t numCores = 1;
+uintptr_t lapicPtr, ioapicPtr;
+uint8_t lapicIds[MAX_CPUS], lapicNums[MAX_CPUS];
+
+/* Enable APIC */
+static void
+enable_apic(void)
+{
+	LAPIC(0xF0) = 0x1FF;
+	LAPIC(0x80) = 0;
+}
+
+/* Read from the IOAPIC */
+static uint32_t
+read_io_apic(uint32_t reg)
+{
+	uint32_t volatile *ioapic = (uint32_t volatile *) ioapicPtr;
+	ioapic[0] = (reg & 0xFF);
+	return ioapic[4];
+}
+
+/* Write to the IOAPIC */
+static void
+write_io_apic(uint32_t reg, uint32_t value)
+{
+	uint32_t volatile *ioapic = (uint32_t volatile *) ioapicPtr;
+	ioapic[0] = (reg & 0xFF);
+	ioapic[4] = value;
+}
+
+/* Delay in gaps of 100 microseconds */
+static void
+delay(uint32_t time)
+{
+	float calc = (1193182 / 10000) * time;
+	uint32_t div = (uint32_t) calc;
+	outb(0x43, 0x30);
+	outb(0x40, div & 0xFF);
+	outb(0x40, (div >> 8) & 0xFF);
+	do outb(0x43, 0xE2); while (!(inb(0x40) & (1 << 7)));
+}
+
+/* Start the APIC timer */
+static void
+apic_start_timer(void)
+{
+	/* Wait for this processor's "turn" */
+	static uint8_t c = 0;
+	while (c != CPUID);
+
+	/* Start LAPIC countdown from -1 */
+	LAPIC(0x3E0) = 3;
+	LAPIC(0x380) = 0xFFFFFFFF;
+	delay(10);
+
+	/* Stop after PIT fires, count LAPIC ticks */
+	LAPIC(0x320) = 0x10000;
+	uint32_t ticks = 0xFFFFFFFF - LAPIC(0x390);
+
+	/* Tick every 1ms */
+	LAPIC(0x320) = 0x20000 | 32;
+	LAPIC(0x3E0) = 3;
+	LAPIC(0x380) = ticks;
+
+	/* Signal next processor */
+	c++;
+}
+
+/* Initialise SMP (RSDT method) */
+void
+init_apic(struct SDTHeader *header)
+{
+	numCores = 0;
+
+	uint8_t overrides[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+
+	/* Iterate the entries */
+	lapicPtr = *(uint32_t *) (header + 1);
+	struct EntryHeader *entry = (void *) (header + 1) + 8;
+	struct LAPICEntry *lapic;
+	struct IOAPICEntry *ioapic;
+	struct ISOEntry *iso;
+	while ((void *) entry < (((void *) header) + header->length)) {
+		switch (entry->type) {
+		case ENTRY_LAPIC:
+			if (numCores >= MAX_CPUS)
+				break;
+			lapic = (void *) entry;
+			if (!(lapic->flags & 1))
+				break;
+			lapicNums[lapic->id] = numCores;
+			lapicIds[numCores] = lapic->id;
+			numCores++;
+			break;
+		case ENTRY_IOAPIC:
+			ioapic = (void *) entry;
+			ioapicPtr = ioapic->addr;
+			break;
+		case ENTRY_IOAPIC_ISO:
+			iso = (void *) entry;
+			overrides[iso->irq] = iso->gsi;
+			break;
+		}
+		entry = (void *) entry + entry->length;
+	}
+
+	/* Send INT#1 to IRQ#33 on CPU#0 */
+	write_io_apic(0x10 + (2 * overrides[1] + 0), 33);
+	write_io_apic(0x10 + (2 * overrides[1] + 1), lapicIds[0]);
+
+	/* Initialise APs */
+	uint32_t i, j;
+	uintptr_t apTrampoline = 0x1000, stack;
+	memcpy((void *) apTrampoline, &ap_trampoline, PAGE_SIZE);
+	for (i = 1; i < numCores; i++) {
+		/* Give each processor a separate stack */
+		stack = alloc_frame() + PAGE_SIZE - sizeof(uintptr_t);
+		*((uint32_t *) (apTrampoline + 0xF00 + i)) = stack;
+		/* Send INIT IPI */
+		LAPIC(0x280) = 0;
+		LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
+		             | (lapicIds[i] << 24);
+		LAPIC(0x300) = (LAPIC(0x300) & 0xFFF00000)
+		             | 0xC500;
+		do asm("pause":::"memory"); while (LAPIC(0x300) & (1 << 12));
+		LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
+		             | (lapicIds[i] << 24);
+		LAPIC(0x300) = (LAPIC(0x300) & 0xFFF00000)
+		             | 0x8500;
+		do asm("pause":::"memory"); while (LAPIC(0x300) & (1 << 12));
+		delay(100);
+		/* Send STARTUP IPI (twice) */
+		for (j = 0; j < 2; j++) {
+			LAPIC(0x280) = 0;
+			LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
+			             | (lapicIds[i] << 24);
+			LAPIC(0x300) = (LAPIC(0x300) & 0xFFF0F800)
+			             | (0x600 + ((apTrampoline >> 12) & 0xFF));
+			delay(2);
+			do
+				asm("pause":::"memory");
+			while (LAPIC(0x300) & (1 << 12));
+		}
+	}
+
+	/* Enable APIC */
+	enable_apic();
+	/* Disable PIC */
+	outb(0x21, 0xFF);
+	outb(0xA1, 0xFF);
+
+	apic_start_timer();
+}
+
+/* AP initialisation routine */
+_Noreturn void
+ap_startup(void)
+{
+	enable_apic();
+	cpu_load();
+	apic_start_timer();
+
+	/* Do nothing */
+	asm volatile("cli");
+	while (1)
+		asm volatile("hlt");
+}