From: John Hodge (sonata) <tpg@mutabah.net>
Date: Sat, 9 Feb 2013 09:59:32 +0000 (+0800)
Subject: Kernel/x86 - Moved MP Table parsing into its own file
X-Git-Tag: rel0.15~584
X-Git-Url: https://git.ucc.asn.au/?p=tpg%2Facess2.git;a=commitdiff_plain;h=8f870b810f0416ae051e6a3785a5e74bdd3cb513

Kernel/x86 - Moved MP Table parsing into its own file
---

diff --git a/KernelLand/Kernel/arch/x86/Makefile b/KernelLand/Kernel/arch/x86/Makefile
index a9368f4b..126ebfbb 100644
--- a/KernelLand/Kernel/arch/x86/Makefile
+++ b/KernelLand/Kernel/arch/x86/Makefile
@@ -24,6 +24,9 @@ CPPFLAGS += -DUSE_MP=$(USE_MP) -DUSE_ACPICA=$(USE_ACPICA)
 A_OBJ  = start.ao main.o mboot.o lib.o desctab.ao errors.o irq.o
 A_OBJ += mm_phys.o mm_virt.o
 A_OBJ += proc.o proc.ao time.o vm8086.o
+ifeq ($(USE_MP),1)
+A_OBJ += mptable.o
+endif
 A_OBJ += kpanic.o pci.o vpci.o
 
 ifeq ($(USE_ACPICA),1)
diff --git a/KernelLand/Kernel/arch/x86/include/apic.h b/KernelLand/Kernel/arch/x86/include/apic.h
new file mode 100644
index 00000000..689e72e6
--- /dev/null
+++ b/KernelLand/Kernel/arch/x86/include/apic.h
@@ -0,0 +1,78 @@
+/*
+ * Acess2 Kernel (x86 Port)
+ * - By John Hodge (thePowersGang)
+ *
+ * include/apic.h
+ * - Definitions for APIC/IOAPIC
+ */
+#ifndef _APIC_H_
+#define _APIC_H_
+
+typedef volatile struct sIOAPIC	tIOAPIC;
+typedef volatile struct sAPICReg tAPICReg;
+typedef struct sAPIC	tAPIC;
+
+extern tAPIC	*gpMP_LocalAPIC;
+
+struct sIOAPIC
+{
+	Uint32	Addr;
+	Uint32	Resvd1[3];
+	union {
+		Uint8	Byte;
+		Uint16	Word;
+		Uint32	DWord;
+	}	Value;
+	Uint32	Resvd2[3];
+};
+
+struct sAPICReg
+{
+	Uint32	Val;
+	Uint32	Padding[3];
+};
+
+struct sAPIC
+{
+	tAPICReg	_unused1[2];
+	tAPICReg	ID;
+	tAPICReg	Version;
+	tAPICReg	_unused2[4];
+	tAPICReg	TPR;	// Task Priority Register
+	tAPICReg	APR;	// Arbitration Priority Register (RO)
+	tAPICReg	PPR;	// Processor Priority Register (RO)
+	tAPICReg	EOI;	// EOI Register (Write Only)
+	tAPICReg	_unused3[1];
+	tAPICReg	LogDest;	// Logical Destination Register
+	tAPICReg	DestFmt;	// Destination Format Register (0-27: RO, 28-31: RW)
+	tAPICReg	SIV;	// Spurious Interrupt Vector Register (0-8: RW, 9-31: RO)
+	tAPICReg	ISR[8];	// In-Service Register - Total 256 Bits (RO)
+	tAPICReg	TMR[8];	// Trigger Mode Register - Total 256 Bits (RO)
+	tAPICReg	IRR[8];	// Interrupt Request Register - Total 256 Bits (RO)
+	tAPICReg	ErrorStatus;	// Error Status Register (RO)
+	tAPICReg	_unused4[6];
+	tAPICReg	LVTCMI;	// LVT CMI Registers
+	// 0x300
+	tAPICReg	ICR[2];	// Interrupt Command Register (RW)
+	// LVT Registers (Controls Local Vector Table)
+	// Structure:
+	// 0-7:   Vector - IDT Vector for the interrupt
+	// 12:    Delivery Status (0: Idle, 1: Send Pending)
+	// 16:    Mask (0: Enabled, 1: Disabled)
+	// 0x320
+	tAPICReg	LVTTimer;	// LVT Timer Register (RW)
+	tAPICReg	LVTThermalSensor;	// LVT Thermal Sensor Register (RW)
+	tAPICReg	LVTPerfMonCounters;	// LVT Performance Monitor Counters Register (RW)
+	tAPICReg	LVTLInt0;	// LVT Local Interrupt (LINT) #0 Register (RW);
+	tAPICReg	LVTLInt1;	// LVT Local Interrupt (LINT) #1 Register (RW);
+	tAPICReg	LVTError;	// LVT Error Register (RW);
+	// 0x380
+	tAPICReg	InitialCount;	// Initial Count Register (Used for the timer) (RW)
+	tAPICReg	CurrentCount;	// Current Count Register (Used for the timer) (RW)
+	tAPICReg	_unused5[4];
+	// 0x3E0
+	tAPICReg	DivideConifg;	// Divide Configuration Register (RW)
+	tAPICReg	_unused6[1];
+};
+
+#endif
diff --git a/KernelLand/Kernel/arch/x86/include/mp.h b/KernelLand/Kernel/arch/x86/include/mp.h
deleted file mode 100644
index 0dd5a1e7..00000000
--- a/KernelLand/Kernel/arch/x86/include/mp.h
+++ /dev/null
@@ -1,140 +0,0 @@
-/*
- */
-#ifndef _MP_H
-#define _MP_H
-
-#define MPPTR_IDENT	('_'|('M'<<8)|('P'<<16)|('_'<<24))
-#define MPTABLE_IDENT	('P'|('C'<<8)|('M'<<16)|('P'<<24))
-
-typedef struct sMPInfo {
-	Uint32	Sig;	// '_MP_'
-	Uint32	MPConfig;
-	Uint8	Length;
-	Uint8	Version;
-	Uint8	Checksum;
-	Uint8	Features[5];	// 2-4 are unused
-} tMPInfo;
-
-typedef union uMPTable_Ent {
-	Uint8	Type;
-	struct {
-		Uint8	Type;
-		Uint8	APICID;
-		Uint8	APICVer;
-		Uint8	CPUFlags;	// bit 0: Enabled, bit 1: Boot Processor
-		Uint32	CPUSignature;	// Stepping, Model, Family
-		Uint32	FeatureFlags;
-		Uint32	Reserved[2];
-	} __attribute__((packed))	Proc;	// 0x00
-	struct {
-		Uint8	Type;
-		Uint8	ID;
-		char	TypeString[6];
-	} __attribute__((packed))	Bus;	// 0x01
-	struct {
-		Uint8	Type;
-		Uint8	ID;
-		Uint8	Version;
-		Uint8	Flags;	// bit 0: Enabled
-		Uint32	Addr;
-	} __attribute__((packed))	IOAPIC;	// 0x02
-	struct {
-		Uint8	Type;
-		Uint8	IntType;
-		Uint16	Flags;	// 0,1: Polarity, 2,3: Trigger Mode
-		Uint8	SourceBusID;
-		Uint8	SourceBusIRQ;
-		Uint8	DestAPICID;
-		Uint8	DestAPICIRQ;
-	} __attribute__((packed))	IOInt;
-	struct {
-		Uint8	Type;
-		Uint8	IntType;
-		Uint16	Flags;	// 0,1: Polarity, 2,3: Trigger Mode
-		Uint8	SourceBusID;
-		Uint8	SourceBusIRQ;
-		Uint8	DestLocalAPICID;
-		Uint8	DestLocalAPICIRQ;
-	} __attribute__((packed))	LocalInt;
-} __attribute__((packed)) tMPTable_Ent;
-
-typedef struct sMPTable {
-	Uint32	Sig;
-	Uint16	BaseTableLength;
-	Uint8	SpecRev;
-	Uint8	Checksum;
-	
-	char	OemID[8];
-	char	ProductID[12];
-	
-	Uint32	OEMTablePtr;
-	Uint16	OEMTableSize;
-	Uint16	EntryCount;
-	
-	Uint32	LocalAPICMemMap;	//!< Address used to access the local APIC
-	Uint16	ExtendedTableLen;
-	Uint8	ExtendedTableChecksum;
-	Uint8	Reserved;
-	
-	tMPTable_Ent	Entries[];
-} tMPTable;
-
-typedef volatile struct {
-	Uint32	Addr;
-	Uint32	Resvd1[3];
-	union {
-		Uint8	Byte;
-		Uint16	Word;
-		Uint32	DWord;
-	}	Value;
-	Uint32	Resvd2[3];
-}	tIOAPIC;
-
-typedef struct {
-	Uint32	Val;
-	Uint32	Padding[3];
-} volatile	tReg;
-
-typedef volatile struct {
-	tReg	_unused1[2];
-	tReg	ID;
-	tReg	Version;
-	tReg	_unused2[4];
-	tReg	TPR;	// Task Priority Register
-	tReg	APR;	// Arbitration Priority Register (RO)
-	tReg	PPR;	// Processor Priority Register (RO)
-	tReg	EOI;	// EOI Register (Write Only)
-	tReg	_unused3[1];
-	tReg	LogDest;	// Logical Destination Register
-	tReg	DestFmt;	// Destination Format Register (0-27: RO, 28-31: RW)
-	tReg	SIV;	// Spurious Interrupt Vector Register (0-8: RW, 9-31: RO)
-	tReg	ISR[8];	// In-Service Register - Total 256 Bits (RO)
-	tReg	TMR[8];	// Trigger Mode Register - Total 256 Bits (RO)
-	tReg	IRR[8];	// Interrupt Request Register - Total 256 Bits (RO)
-	tReg	ErrorStatus;	// Error Status Register (RO)
-	tReg	_unused4[6];
-	tReg	LVTCMI;	// LVT CMI Registers
-	// 0x300
-	tReg	ICR[2];	// Interrupt Command Register (RW)
-	// LVT Registers (Controls Local Vector Table)
-	// Structure:
-	// 0-7:   Vector - IDT Vector for the interrupt
-	// 12:    Delivery Status (0: Idle, 1: Send Pending)
-	// 16:    Mask (0: Enabled, 1: Disabled)
-	// 0x320
-	tReg	LVTTimer;	// LVT Timer Register (RW)
-	tReg	LVTThermalSensor;	// LVT Thermal Sensor Register (RW)
-	tReg	LVTPerfMonCounters;	// LVT Performance Monitor Counters Register (RW)
-	tReg	LVTLInt0;	// LVT Local Interrupt (LINT) #0 Register (RW);
-	tReg	LVTLInt1;	// LVT Local Interrupt (LINT) #1 Register (RW);
-	tReg	LVTError;	// LVT Error Register (RW);
-	// 0x380
-	tReg	InitialCount;	// Initial Count Register (Used for the timer) (RW)
-	tReg	CurrentCount;	// Current Count Register (Used for the timer) (RW)
-	tReg	_unused5[4];
-	// 0x3E0
-	tReg	DivideConifg;	// Divide Configuration Register (RW)
-	tReg	_unused6[1];
-} volatile	tAPIC;
-
-#endif
diff --git a/KernelLand/Kernel/arch/x86/include/mptable.h b/KernelLand/Kernel/arch/x86/include/mptable.h
new file mode 100644
index 00000000..a184a089
--- /dev/null
+++ b/KernelLand/Kernel/arch/x86/include/mptable.h
@@ -0,0 +1,83 @@
+/*
+ * Acess2 Kernel (x86 Port)
+ * - By John Hodge (thePowersGang)
+ *
+ * include/mptable.h
+ * - MPTable definitions
+ */
+
+#define MPPTR_IDENT	('_'|('M'<<8)|('P'<<16)|('_'<<24))
+#define MPTABLE_IDENT	('P'|('C'<<8)|('M'<<16)|('P'<<24))
+
+typedef struct sMPInfo {
+	Uint32	Sig;	// '_MP_'
+	Uint32	MPConfig;
+	Uint8	Length;
+	Uint8	Version;
+	Uint8	Checksum;
+	Uint8	Features[5];	// 2-4 are unused
+} tMPInfo;
+
+typedef union uMPTable_Ent {
+	Uint8	Type;
+	struct {
+		Uint8	Type;
+		Uint8	APICID;
+		Uint8	APICVer;
+		Uint8	CPUFlags;	// bit 0: Enabled, bit 1: Boot Processor
+		Uint32	CPUSignature;	// Stepping, Model, Family
+		Uint32	FeatureFlags;
+		Uint32	Reserved[2];
+	} __attribute__((packed))	Proc;	// 0x00
+	struct {
+		Uint8	Type;
+		Uint8	ID;
+		char	TypeString[6];
+	} __attribute__((packed))	Bus;	// 0x01
+	struct {
+		Uint8	Type;
+		Uint8	ID;
+		Uint8	Version;
+		Uint8	Flags;	// bit 0: Enabled
+		Uint32	Addr;
+	} __attribute__((packed))	IOAPIC;	// 0x02
+	struct {
+		Uint8	Type;
+		Uint8	IntType;
+		Uint16	Flags;	// 0,1: Polarity, 2,3: Trigger Mode
+		Uint8	SourceBusID;
+		Uint8	SourceBusIRQ;
+		Uint8	DestAPICID;
+		Uint8	DestAPICIRQ;
+	} __attribute__((packed))	IOInt;
+	struct {
+		Uint8	Type;
+		Uint8	IntType;
+		Uint16	Flags;	// 0,1: Polarity, 2,3: Trigger Mode
+		Uint8	SourceBusID;
+		Uint8	SourceBusIRQ;
+		Uint8	DestLocalAPICID;
+		Uint8	DestLocalAPICIRQ;
+	} __attribute__((packed))	LocalInt;
+} __attribute__((packed)) tMPTable_Ent;
+
+typedef struct sMPTable {
+	Uint32	Sig;
+	Uint16	BaseTableLength;
+	Uint8	SpecRev;
+	Uint8	Checksum;
+	
+	char	OemID[8];
+	char	ProductID[12];
+	
+	Uint32	OEMTablePtr;
+	Uint16	OEMTableSize;
+	Uint16	EntryCount;
+	
+	Uint32	LocalAPICMemMap;	//!< Address used to access the local APIC
+	Uint16	ExtendedTableLen;
+	Uint8	ExtendedTableChecksum;
+	Uint8	Reserved;
+	
+	tMPTable_Ent	Entries[];
+} tMPTable;
diff --git a/KernelLand/Kernel/arch/x86/include/proc_int.h b/KernelLand/Kernel/arch/x86/include/proc_int.h
new file mode 100644
index 00000000..a03bf5d8
--- /dev/null
+++ b/KernelLand/Kernel/arch/x86/include/proc_int.h
@@ -0,0 +1,40 @@
+/*
+ * Acess2 Kernel (x86 Port)
+ * - By John Hodge (thePowersGang)
+ * 
+ * include/proc_int.h
+ * - Low-level threading internals
+ */
+#ifndef _PROC_INT_H_
+#define _PROC_INT_H_
+
+#include <acess.h>
+#include <threads.h>
+#include <apic.h>
+
+// === TYPES ===
+typedef struct sCPU	tCPU;
+
+// === STRUCTS ===
+struct sCPU
+{
+	Uint8	APICID;
+	Uint8	State;	// 0: Unavaliable, 1: Idle, 2: Active
+	Uint16	Resvd;
+	tThread	*Current;
+	tThread	*LastTimerThread;	// Used to do preeemption
+};
+
+// === FUNCTIONS ===
+// - mptable.c - MP Table parsing code
+extern const void	*MPTable_LocateFloatPtr(void);
+extern int	MPTable_FillCPUs(const void *FloatPtr, tCPU *CPUs, int MaxCPUs, int *BSPIndex);
+
+// - proc.asm
+extern void	SwitchTasks(Uint NewSP, Uint *OldSP, Uint NewIP, Uint *OldIO, Uint CR3);
+extern void	Proc_InitialiseSSE(void);
+extern void	Proc_SaveSSE(Uint DestPtr);
+extern void	Proc_DisableSSE(void);
+
+#endif
+
diff --git a/KernelLand/Kernel/arch/x86/main.c b/KernelLand/Kernel/arch/x86/main.c
index 97a7c75b..865e53e4 100644
--- a/KernelLand/Kernel/arch/x86/main.c
+++ b/KernelLand/Kernel/arch/x86/main.c
@@ -8,7 +8,6 @@
 #include <multiboot2.h>
 #include <init.h>
 #include <mm_virt.h>
-#include <mp.h>
 #include <pmemmap.h>
 
 #define	VGA_ERRORS	0
diff --git a/KernelLand/Kernel/arch/x86/mptable.c b/KernelLand/Kernel/arch/x86/mptable.c
new file mode 100644
index 00000000..5e9d76a0
--- /dev/null
+++ b/KernelLand/Kernel/arch/x86/mptable.c
@@ -0,0 +1,191 @@
+/*
+ * Acess2 Kernel (x86 Port)
+ * - By John Hodge (thePowersGang)
+ *
+ * mptable.c
+ * - MP Table parsing
+ */
+#include <proc_int.h>
+#include <mptable.h>
+
+// === PROTOTYPES ===
+const void	*MPTable_int_LocateFloatPtr(const void *Base, size_t Size);
+
+// === CODE ===
+const void *MPTable_int_LocateFloatPtr(const void *Base, size_t Size)
+{
+	const Uint32	*ptr = Base;
+
+	ASSERT( !(Size & (16-1)) );
+
+	for( ptr = Base; Size -= 16; ptr += 16/4 )
+	{
+		if( *ptr != MPPTR_IDENT )
+			continue ;
+		Log("Possible table at %p", ptr);
+		if( ByteSum(ptr, sizeof(tMPInfo)) != 0 )
+			continue ;
+	
+		return ptr;
+	}
+	return NULL;
+}
+
+const void *MPTable_LocateFloatPtr(void)
+{
+	const void *ret;
+	// Find MP Floating Table
+	
+	// - EBDA/Last 1Kib (640KiB)
+	ret = MPTable_int_LocateFloatPtr( (void*)(KERNEL_BASE|0x9F000), 0x1000 );
+	if(ret)	return ret;
+	
+	// - Last KiB (512KiB base mem)
+	ret = MPTable_int_LocateFloatPtr( (void*)(KERNEL_BASE|0x7F000), 0x1000 );
+	if(ret)	return ret;
+
+	// - BIOS ROM
+	ret = MPTable_int_LocateFloatPtr( (void*)(KERNEL_BASE|0xE0000), 0x20000 );
+	if(ret)	return ret;
+	
+	return NULL;
+}
+
+int MPTable_FillCPUs(const void *FloatPtr, tCPU *CPUs, int MaxCPUs, int *BSPIndex)
+{
+	 int	cpu_count = 0;
+	
+	const tMPInfo	*mpfloatptr = FloatPtr;
+	#if DUMP_MP_TABLE
+	Log("*mpfloatptr = %p {", mpfloatptr);
+	Log("\t.Sig = 0x%08x", mpfloatptr->Sig);
+	Log("\t.MPConfig = 0x%08x", mpfloatptr->MPConfig);
+	Log("\t.Length = 0x%02x", mpfloatptr->Length);
+	Log("\t.Version = 0x%02x", mpfloatptr->Version);
+	Log("\t.Checksum = 0x%02x", mpfloatptr->Checksum);
+	Log("\t.Features = [0x%02x,0x%02x,0x%02x,0x%02x,0x%02x]",
+		mpfloatptr->Features[0],	mpfloatptr->Features[1],
+		mpfloatptr->Features[2],	mpfloatptr->Features[3],
+		mpfloatptr->Features[4]
+		);
+	Log("}");
+	#endif		
+
+	const tMPTable *mptable = (void*)( KERNEL_BASE|mpfloatptr->MPConfig );
+	#if DUMP_MP_TABLE
+	Log("*mptable = %p {", mptable);
+	Log("\t.Sig = 0x%08x", mptable->Sig);
+	Log("\t.BaseTableLength = 0x%04x", mptable->BaseTableLength);
+	Log("\t.SpecRev = 0x%02x", mptable->SpecRev);
+	Log("\t.Checksum = 0x%02x", mptable->Checksum);
+	Log("\t.OEMID = '%8c'", mptable->OemID);
+	Log("\t.ProductID = '%8c'", mptable->ProductID);
+	Log("\t.OEMTablePtr = %p'", mptable->OEMTablePtr);
+	Log("\t.OEMTableSize = 0x%04x", mptable->OEMTableSize);
+	Log("\t.EntryCount = 0x%04x", mptable->EntryCount);
+	Log("\t.LocalAPICMemMap = 0x%08x", mptable->LocalAPICMemMap);
+	Log("\t.ExtendedTableLen = 0x%04x", mptable->ExtendedTableLen);
+	Log("\t.ExtendedTableChecksum = 0x%02x", mptable->ExtendedTableChecksum);
+	Log("}");
+	#endif
+	
+	gpMP_LocalAPIC = (void*)MM_MapHWPages(mptable->LocalAPICMemMap, 1);
+	
+	const tMPTable_Ent	*ents = mptable->Entries;
+	
+	cpu_count = 0;
+	for( int i = 0; i < mptable->EntryCount; i ++ )
+	{
+		size_t	entSize = 4;
+		switch( ents->Type )
+		{
+		case 0:	// Processor
+			entSize = 20;
+			#if DUMP_MP_TABLE
+			Log("%i: Processor", i);
+			Log("\t.APICID = %i", ents->Proc.APICID);
+			Log("\t.APICVer = 0x%02x", ents->Proc.APICVer);
+			Log("\t.CPUFlags = 0x%02x", ents->Proc.CPUFlags);
+			Log("\t.CPUSignature = 0x%08x", ents->Proc.CPUSignature);
+			Log("\t.FeatureFlags = 0x%08x", ents->Proc.FeatureFlags);
+			#endif
+			
+			if( !(ents->Proc.CPUFlags & 1) ) {
+				Log("DISABLED");
+				break;
+			}
+			
+			// Check if there is too many processors
+			if((cpu_count) >= MaxCPUs) {
+				(cpu_count) ++;	// If `giNumCPUs` > MaxCPUs later, it will be clipped
+				break;
+			}
+			
+			// Initialise CPU Info
+			CPUs[cpu_count].APICID = ents->Proc.APICID;
+			CPUs[cpu_count].State = 0;
+			
+			// Set BSP Variable
+			if( ents->Proc.CPUFlags & 2 )
+			{
+				if( *BSPIndex >= 0 )
+					Warning("Multiple CPUs with the BSP flag set, %i+%i", *BSPIndex, cpu_count);
+				else
+					*BSPIndex = cpu_count;
+			}
+			
+			(cpu_count) ++;
+			break;
+		
+		#if DUMP_MP_TABLE >= 2
+		case 1:	// Bus
+			entSize = 8;
+			Log("%i: Bus", i);
+			Log("\t.ID = %i", ents->Bus.ID);
+			Log("\t.TypeString = '%6C'", ents->Bus.TypeString);
+			break;
+		case 2:	// I/O APIC
+			entSize = 8;
+			Log("%i: I/O APIC", i);
+			Log("\t.ID = %i", ents->IOAPIC.ID);
+			Log("\t.Version = 0x%02x", ents->IOAPIC.Version);
+			Log("\t.Flags = 0x%02x", ents->IOAPIC.Flags);
+			Log("\t.Addr = 0x%08x", ents->IOAPIC.Addr);
+			break;
+		case 3:	// I/O Interrupt Assignment
+			entSize = 8;
+			Log("%i: I/O Interrupt Assignment", i);
+			Log("\t.IntType = %i", ents->IOInt.IntType);
+			Log("\t.Flags = 0x%04x", ents->IOInt.Flags);
+			Log("\t.SourceBusID = 0x%02x", ents->IOInt.SourceBusID);
+			Log("\t.SourceBusIRQ = 0x%02x", ents->IOInt.SourceBusIRQ);
+			Log("\t.DestAPICID = 0x%02x", ents->IOInt.DestAPICID);
+			Log("\t.DestAPICIRQ = 0x%02x", ents->IOInt.DestAPICIRQ);
+			break;
+		case 4:	// Local Interrupt Assignment
+			entSize = 8;
+			Log("%i: Local Interrupt Assignment", i);
+			Log("\t.IntType = %i", ents->LocalInt.IntType);
+			Log("\t.Flags = 0x%04x", ents->LocalInt.Flags);
+			Log("\t.SourceBusID = 0x%02x", ents->LocalInt.SourceBusID);
+			Log("\t.SourceBusIRQ = 0x%02x", ents->LocalInt.SourceBusIRQ);
+			Log("\t.DestLocalAPICID = 0x%02x", ents->LocalInt.DestLocalAPICID);
+			Log("\t.DestLocalAPICIRQ = 0x%02x", ents->LocalInt.DestLocalAPICIRQ);
+			break;
+		default:
+			Log("%i: Unknown (%i)", i, ents->Type);
+			break;
+		#endif
+		}
+		ents = (void*)( (Uint)ents + entSize );
+	}
+	
+	if( cpu_count > MaxCPUs )
+	{
+		Warning("Too many CPUs detected (%i), only using %i of them", cpu_count, MaxCPUs);
+		cpu_count = MaxCPUs;
+	}
+	
+	return cpu_count;
+}
+
diff --git a/KernelLand/Kernel/arch/x86/proc.c b/KernelLand/Kernel/arch/x86/proc.c
index d3aea161..8b05488b 100644
--- a/KernelLand/Kernel/arch/x86/proc.c
+++ b/KernelLand/Kernel/arch/x86/proc.c
@@ -8,11 +8,12 @@
 #include <desctab.h>
 #include <mm_virt.h>
 #include <errno.h>
-#if USE_MP
-# include <mp.h>
-#endif
 #include <hal_proc.h>
 #include <arch_int.h>
+#include <proc_int.h>
+#if USE_MP
+# include <apic.h>
+#endif
 
 // === FLAGS ===
 #define DEBUG_TRACE_SWITCH	0
@@ -30,28 +31,17 @@
 #endif
 
 // === TYPES ===
-typedef struct sCPU
-{
-	Uint8	APICID;
-	Uint8	State;	// 0: Unavaliable, 1: Idle, 2: Active
-	Uint16	Resvd;
-	tThread	*Current;
-	tThread	*LastTimerThread;	// Used to do preeemption
-}	tCPU;
 
 // === IMPORTS ===
 extern tGDT	gGDT[];
 extern tIDT	gIDT[];
 extern void	APWait(void);	// 16-bit AP pause code
 extern void	APStartup(void);	// 16-bit AP startup code
-extern Uint	GetEIP(void);	// start.asm
-extern Uint	GetEIP_Sched(void);	// proc.asm
 extern void	NewTaskHeader(tThread *Thread, void *Fcn, int nArgs, ...);	// Actually takes cdecl args
 extern Uint	Proc_CloneInt(Uint *ESP, Uint32 *CR3, int bNoUserClone);
 extern Uint32	gaInitPageDir[1024];	// start.asm
 extern char	Kernel_Stack_Top[];
 extern int	giNumCPUs;
-extern int	giNextTID;
 extern tThread	gThreadZero;
 extern tProcess	gProcessZero;
 extern void	Isr8(void);	// Double Fault
@@ -60,10 +50,6 @@ extern char	scheduler_return[];	// Return address in SchedulerBase
 extern char	IRQCommon[];	// Common IRQ handler code
 extern char	IRQCommon_handled[];	// IRQCommon call return location
 extern char	GetEIP_Sched_ret[];	// GetEIP call return location
-extern void	SwitchTasks(Uint NewSP, Uint *OldSP, Uint NewIP, Uint *OldIO, Uint CR3);
-extern void	Proc_InitialiseSSE(void);
-extern void	Proc_SaveSSE(Uint DestPtr);
-extern void	Proc_DisableSSE(void);
 
 // === PROTOTYPES ===
 //void	ArchThreads_Init(void);
@@ -81,7 +67,6 @@ void	Proc_ChangeStack(void);
 Uint	Proc_MakeUserStack(void);
 //void	Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize);
 void	Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP) NORETURN;
- int	Proc_Demote(Uint *Err, int Dest, tRegs *Regs);
 //void	Proc_CallFaultHandler(tThread *Thread);
 //void	Proc_DumpThreadCPUState(tThread *Thread);
 void	Proc_Scheduler(int CPU);
@@ -90,7 +75,6 @@ void	Proc_Scheduler(int CPU);
 // --- Multiprocessing ---
 #if USE_MP
 volatile int	giNumInitingCPUs = 0;
-tMPInfo	*gMPFloatPtr = NULL;
 volatile Uint32	giMP_TimerCount;	// Start Count for Local APIC Timer
 tAPIC	*gpMP_LocalAPIC = NULL;
 Uint8	gaAPIC_to_CPU[256] = {0};
@@ -122,184 +106,25 @@ tTSS	gDoubleFault_TSS = {
  */
 void ArchThreads_Init(void)
 {
-	Uint	pos = 0;
-	
-	#if USE_MP
-	tMPTable	*mptable;
-	
 	// Mark BSP as active
 	gaCPUs[0].State = 2;
 	
+	#if USE_MP
 	// -- Initialise Multiprocessing
-	// Find MP Floating Table
-	// - EBDA/Last 1Kib (640KiB)
-	for(pos = KERNEL_BASE|0x9F000; pos < (KERNEL_BASE|0xA0000); pos += 16) {
-		if( *(Uint*)(pos) == MPPTR_IDENT ) {
-			Log("Possible %p", pos);
-			if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 )	continue;
-			gMPFloatPtr = (void*)pos;
-			break;
-		}
-	}
-	// - Last KiB (512KiB base mem)
-	if(!gMPFloatPtr) {
-		for(pos = KERNEL_BASE|0x7F000; pos < (KERNEL_BASE|0x80000); pos += 16) {
-			if( *(Uint*)(pos) == MPPTR_IDENT ) {
-				Log("Possible %p", pos);
-				if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 )	continue;
-				gMPFloatPtr = (void*)pos;
-				break;
-			}
-		}
-	}
-	// - BIOS ROM
-	if(!gMPFloatPtr) {
-		for(pos = KERNEL_BASE|0xE0000; pos < (KERNEL_BASE|0x100000); pos += 16) {
-			if( *(Uint*)(pos) == MPPTR_IDENT ) {
-				Log("Possible %p", pos);
-				if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 )	continue;
-				gMPFloatPtr = (void*)pos;
-				break;
-			}
-		}
-	}
-	
-	// If the MP Table Exists, parse it
-	if(gMPFloatPtr)
+	const void *mpfloatptr = MPTable_LocateFloatPtr();
+	if( mpfloatptr )
 	{
-		 int	i;
-	 	tMPTable_Ent	*ents;
-		#if DUMP_MP_TABLE
-		Log("gMPFloatPtr = %p", gMPFloatPtr);
-		Log("*gMPFloatPtr = {");
-		Log("\t.Sig = 0x%08x", gMPFloatPtr->Sig);
-		Log("\t.MPConfig = 0x%08x", gMPFloatPtr->MPConfig);
-		Log("\t.Length = 0x%02x", gMPFloatPtr->Length);
-		Log("\t.Version = 0x%02x", gMPFloatPtr->Version);
-		Log("\t.Checksum = 0x%02x", gMPFloatPtr->Checksum);
-		Log("\t.Features = [0x%02x,0x%02x,0x%02x,0x%02x,0x%02x]",
-			gMPFloatPtr->Features[0],	gMPFloatPtr->Features[1],
-			gMPFloatPtr->Features[2],	gMPFloatPtr->Features[3],
-			gMPFloatPtr->Features[4]
-			);
-		Log("}");
-		#endif		
-
-		mptable = (void*)( KERNEL_BASE|gMPFloatPtr->MPConfig );
-		#if DUMP_MP_TABLE
-		Log("mptable = %p", mptable);
-		Log("*mptable = {");
-		Log("\t.Sig = 0x%08x", mptable->Sig);
-		Log("\t.BaseTableLength = 0x%04x", mptable->BaseTableLength);
-		Log("\t.SpecRev = 0x%02x", mptable->SpecRev);
-		Log("\t.Checksum = 0x%02x", mptable->Checksum);
-		Log("\t.OEMID = '%8c'", mptable->OemID);
-		Log("\t.ProductID = '%8c'", mptable->ProductID);
-		Log("\t.OEMTablePtr = %p'", mptable->OEMTablePtr);
-		Log("\t.OEMTableSize = 0x%04x", mptable->OEMTableSize);
-		Log("\t.EntryCount = 0x%04x", mptable->EntryCount);
-		Log("\t.LocalAPICMemMap = 0x%08x", mptable->LocalAPICMemMap);
-		Log("\t.ExtendedTableLen = 0x%04x", mptable->ExtendedTableLen);
-		Log("\t.ExtendedTableChecksum = 0x%02x", mptable->ExtendedTableChecksum);
-		Log("}");
-		#endif
-		
-		gpMP_LocalAPIC = (void*)MM_MapHWPages(mptable->LocalAPICMemMap, 1);
-		
-		ents = mptable->Entries;
-		giNumCPUs = 0;
-		
-		for( i = 0; i < mptable->EntryCount; i ++ )
+		giNumCPUs = MPTable_FillCPUs(mpfloatptr, gaCPUs, MAX_CPUS, &giProc_BootProcessorID);
+		for( int i = 0; i < giNumCPUs; i ++ )
 		{
-			 int	entSize = 0;
-			switch( ents->Type )
-			{
-			case 0:	// Processor
-				entSize = 20;
-				#if DUMP_MP_TABLE
-				Log("%i: Processor", i);
-				Log("\t.APICID = %i", ents->Proc.APICID);
-				Log("\t.APICVer = 0x%02x", ents->Proc.APICVer);
-				Log("\t.CPUFlags = 0x%02x", ents->Proc.CPUFlags);
-				Log("\t.CPUSignature = 0x%08x", ents->Proc.CPUSignature);
-				Log("\t.FeatureFlags = 0x%08x", ents->Proc.FeatureFlags);
-				#endif
-				
-				if( !(ents->Proc.CPUFlags & 1) ) {
-					Log("DISABLED");
-					break;
-				}
-				
-				// Check if there is too many processors
-				if(giNumCPUs >= MAX_CPUS) {
-					giNumCPUs ++;	// If `giNumCPUs` > MAX_CPUS later, it will be clipped
-					break;
-				}
-				
-				// Initialise CPU Info
-				gaAPIC_to_CPU[ents->Proc.APICID] = giNumCPUs;
-				gaCPUs[giNumCPUs].APICID = ents->Proc.APICID;
-				gaCPUs[giNumCPUs].State = 0;
-				giNumCPUs ++;
-				
-				// Set BSP Variable
-				if( ents->Proc.CPUFlags & 2 ) {
-					giProc_BootProcessorID = giNumCPUs-1;
-				}
-				
-				break;
-			
-			#if DUMP_MP_TABLE >= 2
-			case 1:	// Bus
-				entSize = 8;
-				Log("%i: Bus", i);
-				Log("\t.ID = %i", ents->Bus.ID);
-				Log("\t.TypeString = '%6C'", ents->Bus.TypeString);
-				break;
-			case 2:	// I/O APIC
-				entSize = 8;
-				Log("%i: I/O APIC", i);
-				Log("\t.ID = %i", ents->IOAPIC.ID);
-				Log("\t.Version = 0x%02x", ents->IOAPIC.Version);
-				Log("\t.Flags = 0x%02x", ents->IOAPIC.Flags);
-				Log("\t.Addr = 0x%08x", ents->IOAPIC.Addr);
-				break;
-			case 3:	// I/O Interrupt Assignment
-				entSize = 8;
-				Log("%i: I/O Interrupt Assignment", i);
-				Log("\t.IntType = %i", ents->IOInt.IntType);
-				Log("\t.Flags = 0x%04x", ents->IOInt.Flags);
-				Log("\t.SourceBusID = 0x%02x", ents->IOInt.SourceBusID);
-				Log("\t.SourceBusIRQ = 0x%02x", ents->IOInt.SourceBusIRQ);
-				Log("\t.DestAPICID = 0x%02x", ents->IOInt.DestAPICID);
-				Log("\t.DestAPICIRQ = 0x%02x", ents->IOInt.DestAPICIRQ);
-				break;
-			case 4:	// Local Interrupt Assignment
-				entSize = 8;
-				Log("%i: Local Interrupt Assignment", i);
-				Log("\t.IntType = %i", ents->LocalInt.IntType);
-				Log("\t.Flags = 0x%04x", ents->LocalInt.Flags);
-				Log("\t.SourceBusID = 0x%02x", ents->LocalInt.SourceBusID);
-				Log("\t.SourceBusIRQ = 0x%02x", ents->LocalInt.SourceBusIRQ);
-				Log("\t.DestLocalAPICID = 0x%02x", ents->LocalInt.DestLocalAPICID);
-				Log("\t.DestLocalAPICIRQ = 0x%02x", ents->LocalInt.DestLocalAPICIRQ);
-				break;
-			default:
-				Log("%i: Unknown (%i)", i, ents->Type);
-				break;
-			#endif
-			}
-			ents = (void*)( (Uint)ents + entSize );
-		}
-		
-		if( giNumCPUs > MAX_CPUS ) {
-			Warning("Too many CPUs detected (%i), only using %i of them", giNumCPUs, MAX_CPUS);
-			giNumCPUs = MAX_CPUS;
+			// TODO: Determine if there's an overlap
+			gaAPIC_to_CPU[gaCPUs[i].APICID] = i;
 		}
 		gTSSs = gaTSSs;
 	}
-	else {
-		Log("No MP Table was found, assuming uniprocessor\n");
+	else
+	{
+		Log("No MP Table was found, assuming uniprocessor");
 		giNumCPUs = 1;
 		gTSSs = &gTSS0;
 	}
@@ -353,10 +178,10 @@ void ArchThreads_Init(void)
 	}
 	
 	// Initialise Normal TSS(s)
-	for(pos=0;pos<giNumCPUs;pos++)
+	for(int pos=0;pos<giNumCPUs;pos++)
 	{
 	#else
-	pos = 0;
+	const int pos = 0;
 	#endif
 		gTSSs[pos].SS0 = 0x10;
 		gTSSs[pos].ESP0 = 0;	// Set properly by scheduler
@@ -412,7 +237,7 @@ void MP_StartAP(int CPU)
 	// - MM_AllocDMA mabye?
 	// Create a far jump
 	*(Uint8*)(KERNEL_BASE|0x11000) = 0xEA;	// Far JMP
-	*(Uint16*)(KERNEL_BASE|0x11001) = (Uint16)&APStartup + 0x10;	// IP
+	*(Uint16*)(KERNEL_BASE|0x11001) = (Uint16)(tVAddr)&APStartup + 0x10;	// IP
 	*(Uint16*)(KERNEL_BASE|0x11003) = 0xFFFF;	// CS
 	
 	giNumInitingCPUs ++;
@@ -475,7 +300,7 @@ void Proc_IdleThread(void *Ptr)
 	cpu->Current->Quantum = 1;	// 1 slice quantum
 	for(;;) {
 		__asm__ __volatile__ ("sti");	// Make sure interrupts are enabled
-		__asm__ __volatile__ ("hlt");	// Make sure interrupts are enabled
+		__asm__ __volatile__ ("hlt");
 		Proc_Reschedule();
 	}
 }
@@ -808,40 +633,6 @@ void Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP)
 	for(;;);
 }
 
-/**
- * \fn int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
- * \brief Demotes a process to a lower permission level
- * \param Err	Pointer to user's errno
- * \param Dest	New Permission Level
- * \param Regs	Pointer to user's register structure
- */
-int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
-{
-	 int	cpl = Regs->cs & 3;
-	// Sanity Check
-	if(Dest > 3 || Dest < 0) {
-		*Err = -EINVAL;
-		return -1;
-	}
-	
-	// Permission Check
-	if(cpl > Dest) {
-		*Err = -EACCES;
-		return -1;
-	}
-	
-	// Change the Segment Registers
-	Regs->cs = (((Dest+1)<<4) | Dest) - 8;
-	Regs->ss = ((Dest+1)<<4) | Dest;
-	// Check if the GP Segs are GDT, then change them
-	if(!(Regs->ds & 4))	Regs->ds = ((Dest+1)<<4) | Dest;
-	if(!(Regs->es & 4))	Regs->es = ((Dest+1)<<4) | Dest;
-	if(!(Regs->fs & 4))	Regs->fs = ((Dest+1)<<4) | Dest;
-	if(!(Regs->gs & 4))	Regs->gs = ((Dest+1)<<4) | Dest;
-	
-	return 0;
-}
-
 /**
  * \brief Calls a signal handler in user mode
  * \note Used for signals