Nucleus

diff --git a/Makefile b/Makefile
index 5d47233..2461903 100644
--- a/Makefile
+++ b/Makefile
@@ -45,7 +45,7 @@ build/%.o: %.S
 	@$(AS) -c $< -o $@ $(AFLAGS)
 
 # Overrides
-build/kernel/idt.o: kernel/idt.c
+build/object/%.o: object/%.c
 	$(info CC $<)
 	@mkdir -p $(@D)
-	@$(CC) -c $< -o $@ $(CFLAGS) -mgeneral-regs-only
+	@$(CC) -c $< -o $@ $(CFLAGS) -O3
diff --git a/include/nucleus/cpu.h b/include/nucleus/cpu.h
index f4d7175..2a8c52b 100644
--- a/include/nucleus/cpu.h
+++ b/include/nucleus/cpu.h
@@ -2,14 +2,20 @@
 #define _NUCLEUS_CPU_H
 
 #include <stdint.h>
+#include <stddef.h>
 #include <nucleus/types.h>
 
 typedef unsigned int cpu_t;
+typedef struct Processor Processor;
 
 /* Structure for CPU specific data */
-struct CPUData {
+struct Processor {
+	Processor *self;
 	cpu_t id;
-	Task *task;
+	uint32_t inCriticalSection;
+	uint32_t critFlags;
+	Scheduler *scheduler;
+	struct IPIQueue *ipiq;
 };
 
 /* Structure for an Interrupt Frame */
@@ -20,12 +26,22 @@ struct InterruptFrame {
 	uint32_t eip, cs, eflags, useresp, ss;
 };
 
-typedef void (*exc_handler_t)(struct InterruptFrame *);
-typedef void (*int_handler_t)(struct InterruptFrame *);
+/* Flags for IPI Queue Messages */
+enum IPIQFlag {
+	IPIQ_ASYNC,
+	IPIQ_SYNC,
+	IPIQ_DONE,
+};
 
-extern struct CPUData __seg_gs *cpu;
+typedef void (*int_handler_t)(struct InterruptFrame *);
+typedef void (*ipiq_func_t)(void *);
 
+extern Processor __seg_gs *const cpu;
+extern Processor *cpus[];
+extern size_t ncpus;
 extern int apic;
+#define for_each_cpu(c) for (cpu_t __i_ ## c = 0; ((c) = cpus[__i_ ## c]) && \
+                             __i_ ## c < ncpus; __i_ ## c++)
 
 extern uintptr_t lapicPtr, ioapicPtr;
 #define LAPIC(off)  (*((uint32_t *) ((uint32_t)  lapicPtr + (off))))
@@ -35,8 +51,38 @@ extern cpu_t lapicNums[];
 #define CPUID (apic ? lapicNums[(cpu_t) (LAPIC(0x20) >> 24)] : 0)
 #define MAX_CPUS 2
 
-void register_exception(int num, exc_handler_t addr);
-void register_interrupt(int num, int_handler_t addr);
+/* Enter a critical section */
+static inline void
+enter_critical_section(void)
+{
+	uint32_t *ics = &cpu->self->inCriticalSection;
+	if (__atomic_fetch_add(ics, 1, __ATOMIC_RELAXED) == 0)
+		asm volatile (
+			"pushf;"
+			"cli;"
+			"pop %0"
+			: "=r" (cpu->critFlags)
+			: : "memory"
+		);
+}
+/* Exit a critical section */
+static inline void
+exit_critical_section(void)
+{
+	uint32_t *ics = &cpu->self->inCriticalSection;
+	if (__atomic_sub_fetch(ics, 1, __ATOMIC_RELAXED) == 0)
+		asm volatile (
+			"push %0;"
+			"popf"
+			: : "rm" (cpu->critFlags)
+			: "memory","cc"
+		);
+}
+
+void register_exception(uint8_t num, int_handler_t addr);
+void register_interrupt(uint8_t num, int_handler_t addr);
+void register_ipi(uint8_t num, int_handler_t addr);
+void send_ipiq(cpu_t targid, ipiq_func_t func, void *arg, enum IPIQFlag flags);
 
 void set_fs_base(uintptr_t base);
 void set_gs_base(uintptr_t base);
diff --git a/include/nucleus/object.h b/include/nucleus/object.h
index 9683410..70e7bc1 100644
--- a/include/nucleus/object.h
+++ b/include/nucleus/object.h
@@ -49,6 +49,7 @@ struct Object {
 enum ListMode {
 	LIST_NORMAL,
 	LIST_ORDERED,
+	LIST_LOCKLESS,
 };
 
 int check(void *addr);
@@ -80,6 +81,6 @@ void *prev(Iterator *iter);
 int done_iterating(Iterator *iter);
 #define foreach(l,i) for (Iterator *(__l_ ## i) = iterate(l); (__l_ ## i); \
                           done_iterating(__l_ ## i), (__l_ ## i) = NULL) \
-                     for (i = first(__l_ ## i); i; i = next(__l_ ## i))
+                     for ((i) = first(__l_ ## i); (i); (i) = next(__l_ ## i))
 
 #endif
diff --git a/include/nucleus/task.h b/include/nucleus/task.h
index 80a2a71..6212be0 100644
--- a/include/nucleus/task.h
+++ b/include/nucleus/task.h
@@ -18,6 +18,7 @@ enum Priority {
 	HIGH,
 	HIGHEST,
 };
+#define PRIORITY_COUNT 6
 
 /* Task states */
 enum State {
@@ -34,11 +35,11 @@ struct Task {
 	uid_t uid, euid, suid;
 	gid_t gid, egid, sgid;
 	int status;
-	enum Priority priority;
-	enum State state;
-	uint32_t inCriticalSection;
 	uint32_t inSyscall;
 
+	Scheduler *scheduler;
+	enum Priority priority;
+	enum State state;
 	uintptr_t esi, edi, ebx;
 	uintptr_t esp, ebp, eip;
 	page_dir_t pageDir;
@@ -55,9 +56,20 @@ struct Task {
 	Signals *signals;
 };
 
-#define current cpu->task
+/* Structure for a Scheduler */
+struct Scheduler {
+	Object obj;
+	Processor *cpu;
+	Task *task;
+	ObjectList *queue[PRIORITY_COUNT];
+	uint32_t timeslice;
+	size_t tasks;
+};
+
+#define current cpu->scheduler->task
 
 extern ObjectType taskType;
+extern ObjectType schedulerType;
 extern ObjectType signalsType;
 
 /* Check if super-user */
@@ -67,24 +79,11 @@ super_user(void)
 	return (current->euid == 0);
 }
 
-/* Enter a critical section */
-static inline void
-enter_critical_section(void)
-{
-	__atomic_add_fetch(&current->inCriticalSection, 1, __ATOMIC_RELAXED);
-}
-/* Exit a critical section */
-static inline void
-exit_critical_section(void)
-{
-	__atomic_sub_fetch(&current->inCriticalSection, 1, __ATOMIC_RELAXED);
-}
-
 /* Enter system call context */
 static inline void
-enter_syscall_context(uint32_t syscall)
+enter_syscall_context(void)
 {
-	current->inSyscall = syscall;
+	current->inSyscall = 1;
 }
 /* Exit system call context */
 static inline uint32_t
@@ -96,6 +95,7 @@ exit_syscall_context(void)
 }
 
 void init_tasking(void);
+void enqueue_task(Task *task);
 void block_task(enum State reason, ObjectList *list);
 void unblock_task(Task *task);
 Task *find_task(pid_t tid);
diff --git a/include/nucleus/types.h b/include/nucleus/types.h
index d64d543..35132d8 100644
--- a/include/nucleus/types.h
+++ b/include/nucleus/types.h
@@ -11,6 +11,7 @@ typedef struct VMRegion VMRegion;
 
 /* Task */
 typedef struct Task Task;
+typedef struct Scheduler Scheduler;
 typedef struct Signals Signals;
 
 /* VFS */
diff --git a/kernel/acpi/apic.c b/kernel/acpi/apic.c
index c7f8259..5c2297d 100644
--- a/kernel/acpi/apic.c
+++ b/kernel/acpi/apic.c
@@ -55,7 +55,7 @@ struct ISOEntry {
 };
 
 int apic = 0;
-size_t numCores = 1;
+size_t ncpus = 1;
 uintptr_t lapicPtr, ioapicPtr;
 cpu_t lapicIds[MAX_CPUS], lapicNums[MAX_CPUS];
 
@@ -128,7 +128,7 @@ void
 init_apic(struct SDTHeader *header)
 {
 	apic = 1;
-	numCores = 0;
+	ncpus = 0;
 
 	uint8_t overrides[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
 
@@ -144,14 +144,14 @@ init_apic(struct SDTHeader *header)
 	while ((void *) entry < (((void *) header) + header->length)) {
 		switch (entry->type) {
 		case ENTRY_LAPIC:
-			if (numCores >= MAX_CPUS)
+			if (ncpus >= MAX_CPUS)
 				break;
 			lapic = (void *) entry;
 			if (!(lapic->flags & 1))
 				break;
-			lapicNums[lapic->id] = numCores;
-			lapicIds[numCores] = lapic->id;
-			numCores++;
+			lapicNums[lapic->id] = ncpus;
+			lapicIds[ncpus] = lapic->id;
+			ncpus++;
 			break;
 		case ENTRY_IOAPIC:
 			ioapic = (void *) entry;
@@ -176,7 +176,7 @@ init_apic(struct SDTHeader *header)
 	uint32_t i, j;
 	uintptr_t apTrampoline = 0x1000, stack;
 	memcpy((void *) apTrampoline, &ap_trampoline, PAGE_SIZE);
-	for (i = 1; i < numCores; i++) {
+	for (i = 1; i < ncpus; i++) {
 		/* Give each processor a separate stack */
 		stack = alloc_frame() + PAGE_SIZE - sizeof(uintptr_t);
 		*((uint32_t *) (apTrampoline + 0xF00 + i)) = stack;
@@ -230,3 +230,14 @@ ap_startup(void)
 	schedule();
 	__builtin_unreachable();
 }
+
+/* Send an IPI */
+void
+send_ipi(cpu_t target, uint8_t num)
+{
+	LAPIC(0x310) = (LAPIC(0x310) & 0x00FFFFFF)
+	             | (lapicIds[target] << 24);
+	LAPIC(0x300) = (LAPIC(0x300) & 0xFFF32000)
+	             | (0x5000 + num + 48);
+	while ((LAPIC(0x300) >> 12) & 1);
+}
diff --git a/kernel/cpu.c b/kernel/cpu.c
new file mode 100644
index 0000000..be56047
--- /dev/null
+++ b/kernel/cpu.c
@@ -0,0 +1,154 @@
+/*
+ * This file implements the CPU setup and control functions, in particular the
+ * IPI messaging system.  Each CPU has a message queue (IPIQ) which can be
+ * written by other CPUs.  Each message describes a function that shoudl be run
+ * on the target CPU. When signalled via an IPI, a CPU will check it's queue for
+ * work and execute anything outstanding.  Sending a message to an IPIQ will
+ * wait for the IPI to be received.  If the message is synchronous (IPIQ_SYNC)
+ * then the call will also wait for the function to return on the target.  If
+ * the message is asynchronous (IPIQ_ASYNC) then it will return immediately and
+ * the call may be completed at any time, with the target performing any
+ * necessary clean-up.
+ */
+
+#include <nucleus/cpu.h>
+#include <nucleus/memory.h>
+#include <nucleus/task.h>
+#include "desc.h"
+
+/* Structure for a IPI Message Queue */
+struct IPIQueue {
+	struct IPIMessage *msg[32];
+	uint32_t slots;
+	int working;
+};
+
+/* Structure for an IPI Message */
+struct IPIMessage {
+	enum IPIQFlag flags;
+	ipiq_func_t func;
+	void *arg;
+};
+
+void send_ipi(cpu_t target, uint8_t num);
+
+Processor __seg_gs *const cpu = 0;
+Processor *cpus[MAX_CPUS];
+
+/* Per-CPU setup */
+void
+cpu_load(void)
+{
+	/* Initialise SSE */
+	asm volatile(
+		"mov %cr0, %eax;"
+		"andl $0xfffffffb, %eax;"
+		"orl $0x2, %eax;"
+		"mov %eax, %cr0;"
+		"mov %cr4, %eax;"
+		"orl $0x600, %eax;"
+		"mov %eax, %cr4;"
+	);
+
+	/* Initialise CR4.PGE */
+	asm volatile(
+		"mov %cr4, %eax;"
+		"orl $0x10, %eax;"
+		"mov %eax, %cr4;"
+	);
+
+	/* Tables */
+	cpu_load_idt();
+	cpu_load_gdt();
+
+	/* Processor structure */
+	cpu_t id = CPUID;
+	cpus[id] = kmalloc(sizeof(Processor));
+	set_gs_base((uintptr_t) cpus[id]);
+	cpu->self = cpus[id];
+	cpu->id = id;
+	cpu->scheduler = new(&schedulerType);
+	cpu->ipiq = kmalloc(sizeof(struct IPIQueue));
+
+	asm volatile("sti");
+}
+
+/* IPI Queue handler */
+void
+ipiq_handler(struct InterruptFrame *frame)
+{
+	struct IPIMessage *msg;
+	struct IPIQueue *queue = cpu->ipiq;
+
+	if (queue->working || !queue->slots)
+		return;
+
+	enter_critical_section();
+	queue->working++;
+
+	/* Dispatch all messages that are pending */
+	uint8_t slot;
+	for (slot = 0; slot < 32; slot++) {
+		msg = queue->msg[slot];
+		/* Check the bitset for used slots */
+		if (!(queue->slots & (1 << slot)))
+			continue;
+		queue->slots &= ~(1 << slot);
+		msg->func(msg->arg);
+		if (msg->flags & IPIQ_SYNC)
+			msg->flags |= IPIQ_DONE;
+		else
+			kfree(msg);
+	}
+
+	queue->working--;
+	exit_critical_section();
+}
+
+/* Request a CPU run a function */
+void
+send_ipiq(cpu_t targid, ipiq_func_t func, void *arg, enum IPIQFlag flags)
+{
+	enter_critical_section();
+	if (__builtin_expect((targid == cpu->id), 0)) {
+		func(arg);
+		goto end;
+	}
+
+	/* Construct IPI message */
+	struct IPIMessage *msg = kmalloc(sizeof(struct IPIMessage));
+	msg->func = func;
+	msg->arg = arg;
+	msg->flags = flags & ~IPIQ_DONE;
+
+	/* Find slot and send */
+	uint8_t slot;
+	struct IPIQueue *queue = cpus[targid]->ipiq;
+find_slot:
+	for (slot = 0; slot < 32; slot++) {
+		/*
+		 * Search for a free slot by testing and setting each bit in the
+		 * bitset until one is changed.  One bit per slot minimises the
+		 * contention and guarantees synchronisation when acted on
+		 * atomically. Only try to set one bit at a time, then test that
+		 * bit, none of the others.
+		 */
+		if (!((__atomic_fetch_xor(&queue->slots, 1 << slot,
+		    __ATOMIC_ACQUIRE) >> slot) & 1))
+			break;
+	}
+	if (slot == 32)
+		goto find_slot;
+	queue->msg[slot] = msg;
+	if (!queue->working)
+		send_ipi(targid, 0);
+
+	/* Wait for completion */
+	if (flags & IPIQ_SYNC) {
+		while (!(msg->flags & IPIQ_DONE));
+		kfree(msg);
+	}
+
+end:
+	exit_critical_section();
+}
diff --git a/kernel/idt.c b/kernel/idt.c
index 6aceb88..37d38d6 100644
--- a/kernel/idt.c
+++ b/kernel/idt.c
@@ -21,7 +21,6 @@ static struct IDTEntry {
 	uint16_t offsetHigher;
 } __attribute__((packed)) *IDT;
 
-void (**exceptions)(struct InterruptFrame *);
 void (**interrupts)(struct InterruptFrame *);
 
 /* Install an IDT Entry */
@@ -43,12 +42,12 @@ install_idt_entry(uint8_t num, void *addr)
 void
 isr_handler(struct InterruptFrame frame)
 {
-	if (!exceptions[frame.intnum] && frame.intnum < 32)
+	if (!interrupts[frame.intnum] && frame.intnum < 32)
 		panic("[CPU#%d] Failed to handle exception %d (%#.8x) @ %#.8x",
 		      cpu->id, frame.intnum, frame.err, frame.eip);
 
 	/* Run registered handler */
-	exc_handler_t handler = exceptions[frame.intnum];
+	int_handler_t handler = interrupts[frame.intnum];
 	if (handler)
 		handler(&frame);
 
@@ -65,18 +64,25 @@ isr_handler(struct InterruptFrame frame)
 
 /* Register an exception handler */
 void
-register_exception(int num, exc_handler_t addr)
+register_exception(uint8_t num, int_handler_t addr)
 {
-	if (num >= 0 && num < 256)
-		exceptions[num] = addr;
+	interrupts[num] = addr;
 }
 
 /* Register an interrupt handler */
 void
-register_interrupt(int num, int_handler_t addr)
+register_interrupt(uint8_t num, int_handler_t addr)
 {
 	if (num >= 0 && num < 16)
-		interrupts[num] = addr;
+		register_exception(num + 32, addr);
+}
+
+/* Register an IPI handler */
+void
+register_ipi(uint8_t num, int_handler_t addr)
+{
+	if (num >= 0 && num < 16)
+		register_exception(num + 48, addr);
 }
 
 /* Exceptions */
@@ -88,6 +94,10 @@ extern char exc0[],  exc1[],  exc2[],  exc3[],  exc4[],  exc5[],  exc6[],
 extern char exc32[], exc33[], exc34[], exc35[], exc36[], exc37[], exc38[],
             exc39[], exc40[], exc41[], exc42[], exc43[], exc44[], exc45[],
             exc46[], exc47[];
+/* IPIs */
+extern char exc48[], exc49[], exc50[], exc51[], exc52[], exc53[], exc54[],
+            exc55[], exc56[], exc57[], exc58[], exc59[], exc60[], exc61[],
+            exc62[], exc63[];
 /* System Call */
 extern char exc128[];
 
@@ -139,12 +149,29 @@ init_idt(void)
 	install_idt_entry(46, exc46);
 	install_idt_entry(47, exc47);
 
+	/* Install IPIs */
+	install_idt_entry(48, exc48);
+	install_idt_entry(49, exc49);
+	install_idt_entry(50, exc50);
+	install_idt_entry(51, exc51);
+	install_idt_entry(52, exc52);
+	install_idt_entry(53, exc53);
+	install_idt_entry(54, exc54);
+	install_idt_entry(55, exc55);
+	install_idt_entry(56, exc56);
+	install_idt_entry(57, exc57);
+	install_idt_entry(58, exc58);
+	install_idt_entry(59, exc59);
+	install_idt_entry(60, exc60);
+	install_idt_entry(61, exc61);
+	install_idt_entry(62, exc62);
+	install_idt_entry(63, exc63);
+
 	/* Install system call handler */
 	install_idt_entry(128, exc128);
 
-	exceptions = (void *) (IDT + 256);
-	interrupts = (void *) (exceptions + 32);
-	memset(exceptions, 0, 1024);
+	interrupts = (void *) (IDT + 256);
+	memset(interrupts, 0, 1024);
 }
 
 /* Load the IDT */
diff --git a/kernel/isr.S b/kernel/isr.S
index 914d855..b1b849e 100644
--- a/kernel/isr.S
+++ b/kernel/isr.S
@@ -63,6 +63,23 @@ EXC_NOERR 44
 EXC_NOERR 45
 EXC_NOERR 46
 EXC_NOERR 47
+/* IPIs */
+EXC_NOERR 48
+EXC_NOERR 49
+EXC_NOERR 50
+EXC_NOERR 51
+EXC_NOERR 52
+EXC_NOERR 53
+EXC_NOERR 54
+EXC_NOERR 55
+EXC_NOERR 56
+EXC_NOERR 57
+EXC_NOERR 58
+EXC_NOERR 59
+EXC_NOERR 60
+EXC_NOERR 61
+EXC_NOERR 62
+EXC_NOERR 63
 /* System Call */
 EXC_NOERR 128
 
diff --git a/kernel/main.c b/kernel/main.c
index 76d5ef1..b7d8eb8 100644
--- a/kernel/main.c
+++ b/kernel/main.c
@@ -17,41 +17,10 @@
 #include "desc.h"
 #include "acpi/acpi.h"
 
-struct CPUData __seg_gs *cpu = 0;
-
 extern char _bss[], _end[];
 void page_fault_handler(struct InterruptFrame *frame);
-
-/* Per-CPU Setup */
-void
-cpu_load(void)
-{
-	/* Initialise SSE */
-	asm volatile(
-		"mov %cr0, %eax;"
-		"andl $0xfffffffb, %eax;"
-		"orl $0x2, %eax;"
-		"mov %eax, %cr0;"
-		"mov %cr4, %eax;"
-		"orl $0x600, %eax;"
-		"mov %eax, %cr4;"
-	);
-
-	/* Initialise CR4.PGE */
-	asm volatile(
-		"mov %cr4, %eax;"
-		"orl $0x10, %eax;"
-		"mov %eax, %cr4;"
-	);
-
-	/* Tables */
-	cpu_load_idt();
-	cpu_load_gdt();
-	set_gs_base((uintptr_t) kmalloc(sizeof(struct CPUData)));
-	cpu->id = CPUID;
-
-	asm volatile("sti");
-}
+void ipiq_handler(struct InterruptFrame *frame);
+void cpu_load(void);
 
 /* Kernel main function */
 _Noreturn void
@@ -67,6 +36,7 @@ kmain(struct MultibootInfo *mbinfo)
 	init_idt();
 	init_pic();
 	cpu_load();
+	register_ipi(0, ipiq_handler);
 
 	/* Initialise paging */
 	init_paging();
diff --git a/kernel/panic.c b/kernel/panic.c
index 2c07d24..68b68b5 100644
--- a/kernel/panic.c
+++ b/kernel/panic.c
@@ -5,6 +5,8 @@
  * be compiled in.
  */
 
+#include <string.h>
+#include <nucleus/cpu.h>
 #include <nucleus/kernel.h>
 #include <io.h>
 
@@ -12,6 +14,15 @@
 _Noreturn void
 panic(char *fmt, ...)
 {
+	if (!fmt)
+		goto halt;
+	Processor *proc;
+	for_each_cpu(proc) {
+		if (proc == cpu->self)
+			continue;
+		send_ipiq(proc->id, (ipiq_func_t) panic, NULL, IPIQ_ASYNC);
+	}
+
 	outb(0xE9, '\033');
 	outb(0xE9, '[');
 	outb(0xE9, '3');
@@ -19,6 +30,13 @@ panic(char *fmt, ...)
 	outb(0xE9, 'm');
 
 	char buf[1024], *p = buf;
+	memset(buf, 0, 1024);
+
+	sprintf(buf, "[CPU#%d] ", cpu->id);
+	while (*p)
+		outb(0xE9, *p++);
+	memset(buf, 0, 1024);
+	p = buf;
 
 	/* Print error to serial port */
 	va_list args;
@@ -29,6 +47,7 @@ panic(char *fmt, ...)
 		outb(0xE9, *p++);
 	outb(0xE9, '\n');
 
+halt:
 	/* Halt processor */
 	while (1) asm volatile("cli; hlt");
 	__builtin_unreachable();
diff --git a/kernel/printf.c b/kernel/printf.c
index d0d55fa..858f26e 100644
--- a/kernel/printf.c
+++ b/kernel/printf.c
@@ -21,6 +21,13 @@ kprintf(char *fmt, ...)
 	outb(0xE9, 'm');
 
 	char buf[1024], *p = buf;
+	memset(buf, 0, 1024);
+
+	sprintf(buf, "[CPU#%d] ", cpu->id);
+	while (*p)
+		outb(0xE9, *p++);
+	memset(buf, 0, 1024);
+	p = buf;
 
 	/* Print message to debug port */
 	va_list args;
diff --git a/kernel/uname.c b/kernel/uname.c
index cf0a931..10e52c0 100644
--- a/kernel/uname.c
+++ b/kernel/uname.c
@@ -9,7 +9,7 @@
 #include <nucleus/memory.h>
 
 static const char *SYSNAME = "Nucleus";
-static const char *RELEASE = "0.9.1";
+static const char *RELEASE = "0.9.2";
 static const char *VERSION = "SMP PREEMPT "__DATE__" "__TIME__;
 static const char *MACHINE = "x86";
 
diff --git a/memory/copy.S b/memory/copy.S
index 1d1c6a3..65aa576 100644
--- a/memory/copy.S
+++ b/memory/copy.S
@@ -29,7 +29,6 @@ copy_page_frame:
 	mov %cr0, %edx
 	or $0x80000000, %edx
 	mov %edx, %cr0
-	sti
 	popf
 	pop %ebx
 	ret
diff --git a/memory/fault.c b/memory/fault.c
index efc8b66..1de4939 100644
--- a/memory/fault.c
+++ b/memory/fault.c
@@ -213,7 +213,7 @@ early_page_fault_handler(struct InterruptFrame *frame)
 	uint8_t write   = frame->err & (1 << 1);
 	uint8_t user    = frame->err & (1 << 2);
 	if (!PAGE_ADDR(addr))
-		panic("Null dereference @ %#.8x (CPU#%d)", frame->eip, cpu->id);
+		panic("Null dereference @ %#.8x -> %#.8x", frame->eip, addr);
 	ASSERT(!present);
 	kprintf("Allocating frame for %#.8x [%#.8x] CPU#%d",
 	        addr, frame->eip, cpu->id);
diff --git a/object/list.c b/object/list.c
index 6745f28..a4e2f17 100644
--- a/object/list.c
+++ b/object/list.c
@@ -72,7 +72,8 @@ add(ObjectList *list, void *addr)
 	if (list->type && obj->type != list->type)
 		return;
 
-	acquire(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		acquire(&list->lock);
 
 	struct ListEntry *entry, *next;
 	entry = kmalloc(sizeof(struct ListEntry));
@@ -106,7 +107,8 @@ end:
 		list->end = entry;
 	}
 
-	release(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		release(&list->lock);
 }
 
 /* Remove an Object from a List */
@@ -119,7 +121,8 @@ remove(ObjectList *list, void *addr)
 		return;
 	if (list->type && obj->type != list->type)
 		return;
-	acquire(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		acquire(&list->lock);
 
 	/* Search for object */
 	struct ListEntry *entry;
@@ -148,7 +151,8 @@ remove(ObjectList *list, void *addr)
 		put(obj);
 	list->entries--;
 	kfree(entry);
-	release(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		release(&list->lock);
 }
 
 /* Pop the first Object in a List */
@@ -157,10 +161,12 @@ pop_from_start(ObjectList *list)
 {
 	if (!list->start)
 		return NULL;
-	acquire(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		acquire(&list->lock);
 	Object *head = get(list->start->obj);
 	remove(list, head);
-	release(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		release(&list->lock);
 	return head;
 }
 
@@ -170,10 +176,12 @@ pop_from_end(ObjectList *list)
 {
 	if (!list->end)
 		return NULL;
-	acquire(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		acquire(&list->lock);
 	Object *tail = get(list->end->obj);
 	remove(list, tail);
-	release(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		release(&list->lock);
 	return tail;
 }
 
@@ -211,11 +219,13 @@ copy_list(ObjectList *list)
 {
 	ObjectList *newlist = create_list(list->type, list->mode,
 	                                  list->compare);
-	acquire(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		acquire(&list->lock);
 	struct ListEntry *entry;
 	for (entry = list->start; entry; entry = entry->next)
 		add(newlist, entry->obj);
-	release(&list->lock);
+	if (list->mode != LIST_LOCKLESS)
+		release(&list->lock);
 	return newlist;
 }
 
@@ -223,11 +233,13 @@ copy_list(ObjectList *list)
 void
 concat_list(ObjectList *src, ObjectList *dest)
 {
-	acquire(&src->lock);
+	if (src->mode != LIST_LOCKLESS)
+		acquire(&src->lock);
 	struct ListEntry *entry;
 	for (entry = src->start; entry; entry = entry->next)
 		add(dest, entry->obj);
-	release(&src->lock);
+	if (src->mode != LIST_LOCKLESS)
+		release(&src->lock);
 }
 
 /* Iterate a List */
diff --git a/task/clone.c b/task/clone.c
index 7f23de3..3a9f579 100644
--- a/task/clone.c
+++ b/task/clone.c
@@ -9,8 +9,6 @@
 #include <nucleus/vfs.h>
 #include <io.h>
 
-extern ObjectList *readyQueue[];
-
 /* Clone a task */
 pid_t
 clone(int flags)
@@ -68,7 +66,7 @@ clone(int flags)
 	asm volatile("mov %%esp, %0" : "=r" (child->esp));
 	asm volatile("mov %%ebp, %0" : "=r" (child->ebp));
 	child->eip = (uintptr_t) &&end;
-	add(readyQueue[child->priority], child);
+	enqueue_task(child);
 	tid = child->tid;
 	put(child);
 	exit_critical_section();
diff --git a/task/exit.c b/task/exit.c
index 4069775..b352fd4 100644
--- a/task/exit.c
+++ b/task/exit.c
@@ -12,7 +12,6 @@ _Noreturn void
 terminate(void)
 {
 	current->state = TERMINATED;
-	current->inCriticalSection = 0;
 
 	/* Unblock waiting tasks */
 	Task *tmp;
diff --git a/task/scheduler.c b/task/scheduler.c
index 02413bd..d83fe0c 100644
--- a/task/scheduler.c
+++ b/task/scheduler.c
@@ -3,21 +3,52 @@
  * routine, as well as the schedule() function.  The scheduler can be called
  * from anywhere, and will switch to the next task decided by the scheduler
  * rules.  If it cannot find a task to schedule, it just idles until one becomes
- * available.  This avoids the need for an idle task.
+ * available.  This avoids the need for an idle task.  Each processor has a
+ * scheduler, which improves performance.  Tasks will run on the same processor,
+ * but will migrate if they must, to keep the run queues balanced.
  */
 
+#include <nucleus/cpu.h>
 #include <nucleus/kernel.h>
 #include <nucleus/task.h>
 
-#define PRIORITY_COUNT 6
-
+static void scheduler_new(Object *);
+static void scheduler_delete(Object *);
 void context_switch(uintptr_t eip, page_dir_t pagedir,
                     uintptr_t esi, uintptr_t edi, uintptr_t ebx,
                     uintptr_t ebp, uintptr_t esp);
 
-extern uint8_t slice[];
+static _Atomic size_t tasks = 0;
+
+/* Scheduler object type */
+ObjectType schedulerType = {
+	.name = "SCHEDULER",
+	.size = sizeof(Scheduler),
+	.new = scheduler_new,
+	.delete = scheduler_delete,
+};
 
-ObjectList *readyQueue[PRIORITY_COUNT];
+/* Create a new scheduler object */
+static void
+scheduler_new(Object *obj)
+{
+	enum Priority p;
+	Scheduler *s = (void *) obj;
+	s->cpu = cpu->self;
+	s->task = NULL;
+	for (p = 0; p < PRIORITY_COUNT; p++)
+		s->queue[p] = create_list(&taskType, LIST_NORMAL);
+}
+
+/* Destroy a scheduler object */
+static void
+scheduler_delete(Object *obj)
+{
+	enum Priority p;
+	Scheduler *s = (void *) obj;
+	for (p = 0; p < PRIORITY_COUNT; p++)
+		destroy_list(s->queue[p]);
+}
 
 /* Switch to a task */
 static void
@@ -47,12 +78,12 @@ end:
 
 /* Find the next schedulable ready queue */
 static ObjectList *
-highest_priority_queue(void)
+highest_priority_queue(Scheduler *s)
 {
 	enum Priority p;
 	for (p = PRIORITY_COUNT - 1; p > 0; p--) {
-		if (count(readyQueue[p]))
-			return readyQueue[p];
+		if (count(s->queue[p]))
+			return s->queue[p];
 	}
 	return NULL;
 }
@@ -61,42 +92,97 @@ highest_priority_queue(void)
 void
 schedule(void)
 {
-	if (current && current->inCriticalSection)
-		return;
-
+	enter_critical_section();
 	Task *task = current;
-	ObjectList *queue = highest_priority_queue();
-	slice[cpu->id] = 0;
+	Scheduler *s = cpu->scheduler;
+	ObjectList *queue = highest_priority_queue(s);
+	s->timeslice = 0;
 
 	/* Idle if necessary */
 	if (!queue) {
 		if (current && current->state == RUNNING)
 			return;
 		current = NULL;
+		if (task)
+			s->tasks--;
 		asm volatile("sti");
-		while (!(queue = highest_priority_queue()))
+		while (!(queue = highest_priority_queue(s)))
 			asm volatile("hlt");
 		asm volatile("cli");
+		if (task)
+			s->tasks++;
 		current = task;
 	}
 
 	/* Schedule next task */
 	task = pop_from_start(queue);
 	task->state = RUNNING;
-	if (task == current)
+	if (task == current) {
+		exit_critical_section();
 		return;
+	}
 	if (current && current->state == RUNNING) {
 		current->state = READY;
-		add(readyQueue[current->priority], current);
+		add(s->queue[current->priority], current);
+	} else if (current) {
+		tasks--;
+		s->tasks--;
 	}
+	exit_critical_section();
 	switch_to_task(task);
 }
 
-/* Initialise the scheduler */
+/* Find the scheduler with the least tasks */
+Scheduler *
+least_used_scheduler(void)
+{
+	Processor *proc;
+	Scheduler *best = cpu->scheduler;
+	for_each_cpu(proc) {
+		if (proc->scheduler->tasks < best->tasks)
+			best = proc->scheduler;
+	}
+	return best;
+}
+
+/* Add a task to a scheduler */
 void
-init_scheduler(void)
+enqueue_task(Task *task)
 {
-	enum Priority p;
-	for (p = 0; p < PRIORITY_COUNT; p++)
-		readyQueue[p] = create_list(&taskType, LIST_NORMAL);
+	Processor *target;
+	Scheduler *s = task->scheduler;
+	if (__builtin_expect(!s, 0))
+		s = task->scheduler = least_used_scheduler();
+	if (s != cpu->scheduler && 0) {
+		send_ipiq(s->cpu->id, (ipiq_func_t) enqueue_task,
+		          task, IPIQ_SYNC);
+	} else {
+		enter_critical_section();
+		tasks++;
+		s->tasks++;
+		add(s->queue[task->priority], task);
+		exit_critical_section();
+	}
+}
+
+/* Balance the schedulers */
+void
+balance_scheduler(void)
+{
+	Task *t;
+	Scheduler *s = cpu->scheduler;
+	size_t max = (tasks / ncpus) + 1;
+	if (s->tasks <= max)
+		return;
+
+	/* Redistribute tasks on overloaded processors */
+	enter_critical_section();
+	while (s->tasks > max) {
+		s->tasks--;
+		t = pop_from_start(highest_priority_queue(s));
+		t->scheduler = NULL;
+		send_ipiq(least_used_scheduler()->cpu->id,
+		          (ipiq_func_t) enqueue_task, t, IPIQ_SYNC);
+	}
+	exit_critical_section();
 }
diff --git a/task/switch.S b/task/switch.S
index 5ef5437..54769af 100644
--- a/task/switch.S
+++ b/task/switch.S
@@ -5,9 +5,12 @@
  * must be taken to read and set the registers in the correct order.
  */
 
-/* Perform a context switch */
-.globl context_switch
+.section .text
+.global context_switch
+.type copy_page_frame, @function
+.align 4
 context_switch:
+.code32
 	cli
 	mov 4(%esp), %ecx
 	mov 8(%esp), %eax
diff --git a/task/syscall.c b/task/syscall.c
index effe6c2..ba7937e 100644
--- a/task/syscall.c
+++ b/task/syscall.c
@@ -93,7 +93,7 @@ syscall_handler(struct InterruptFrame *frame)
 	int num = frame->eax;
 	uintptr_t args = frame->esi;
 	int ret = -EINVAL;
-	enter_syscall_context(num);
+	enter_syscall_context();
 
 	/* Find syscall entry */
 	if (num >= sizeof(syscalls) / sizeof(syscalls[0]))
diff --git a/task/task.c b/task/task.c
index 69af014..b0268e2 100644
--- a/task/task.c
+++ b/task/task.c
@@ -12,7 +12,6 @@
 #include <nucleus/object.h>
 #include <nucleus/vfs.h>
 
-void init_scheduler(void);
 void timer_handler(struct InterruptFrame *frame);
 void syscall_handler(struct InterruptFrame *frame);
 
@@ -27,17 +26,15 @@ ObjectType taskType = {
 	.delete = task_delete,
 };
 
-Task *currentTask[MAX_CPUS];
-pid_t nextTid = 1;
 extern char stackTop[];
-extern ObjectList *readyQueue[];
 
 /* Create a new Task */
 static void
 task_new(Object *obj)
 {
+	static pid_t tid = 1;
 	Task *task = (void *) obj;
-	task->tid = nextTid++;
+	task->tid = tid++;
 	task->tgid = task->tid;
 	task->priority = NORMAL;
 	task->state = READY;
@@ -48,16 +45,22 @@ static void
 task_delete(Object *obj)
 {
 	Task *task = (void *) obj;
-	put(task->executable);
-	put(task->stack);
+	if (task->executable)
+		put(task->executable);
+	if (task->stack)
+		put(task->stack);
 	if (task->target)
 		put(task->target);
 	if (task->wait)
 		destroy_list(task->wait);
-	put(task->fs);
-	put(task->files);
-	put(task->vm);
-	put(task->signals);
+	if (task->fs)
+		put(task->fs);
+	if (task->files)
+		put(task->files);
+	if (task->vm)
+		put(task->vm);
+	if (task->signals)
+		put(task->signals);
 }
 
 /* Move the stack */
@@ -109,7 +112,7 @@ init_tasking(void)
 	/* Signals namespace */
 	current->signals = new(&signalsType);
 
-	init_scheduler();
+	cpu->scheduler->tasks = 1;
 	register_interrupt(0, timer_handler);
 	register_exception(128, syscall_handler);
 }
@@ -139,7 +142,7 @@ unblock_task(Task *task)
 {
 	lock(task);
 	task->state = READY;
-	add(readyQueue[task->priority], task);
+	enqueue_task(task);
 	unlock(task);
 }
 
diff --git a/task/time.c b/task/time.c
index d6a47d9..654795c 100644
--- a/task/time.c
+++ b/task/time.c
@@ -8,8 +8,9 @@
 #include <nucleus/cpu.h>
 #include <nucleus/task.h>
 
+void balance_scheduler(void);
+
 uint32_t monotonic = 0;
-uint8_t slice[MAX_CPUS] = {0};
 
 /* Timer interrupt */
 void
@@ -17,14 +18,17 @@ timer_handler(struct InterruptFrame *frame)
 {
 	if (cpu->id == 0)
 		monotonic++;
-
 	if (!current)
 		return;
-	slice[cpu->id]++;
+
+	/* Book-keep the scheduler */
+	Scheduler *s = cpu->scheduler;
+	if (monotonic % 300000 == 0)
+		balance_scheduler();
+	s->timeslice++;
 
 	/* Call scheduler */
-	if (slice[cpu->id] < (current->priority * 10))
+	if (s->timeslice < (current->priority * 10))
 		return;
-	slice[cpu->id] = 0;
 	schedule();
 }