Nucleus

diff --git a/kernel/acpi/apic.c b/kernel/acpi/apic.c
index 4d969f5..8bdfbf8 100644
--- a/kernel/acpi/apic.c
+++ b/kernel/acpi/apic.c
@@ -10,6 +10,7 @@
 #include <string.h>
 #include <nucleus/memory.h>
 #include <nucleus/cpu.h>
+#include <nucleus/task.h>
 #include <io.h>
 #include "acpi.h"
 
@@ -219,13 +220,12 @@ init_apic(struct SDTHeader *header)
 _Noreturn void
 ap_startup(void)
 {
+	/* Set up CPU environment */
 	enable_apic();
 	cpu_load();
-	apic_start_timer();
 	cpu_load_paging();
+	apic_start_timer();
 
-	/* Do nothing */
-	asm volatile("cli");
-	while (1)
-		asm volatile("hlt");
+	/* Start running tasks */
+	schedule();
 }
diff --git a/kernel/main.c b/kernel/main.c
index 34bb397..0b45511 100644
--- a/kernel/main.c
+++ b/kernel/main.c
@@ -46,8 +46,6 @@ cpu_load(void)
 	asm volatile("sti");
 }
 
-#include <io.h>
-
 /* Kernel main function */
 _Noreturn void
 kmain(struct MultibootInfo *mbinfo)
diff --git a/memory/fault.c b/memory/fault.c
index 0ed452d..08a4252 100644
--- a/memory/fault.c
+++ b/memory/fault.c
@@ -20,9 +20,10 @@ early_page_fault_handler(struct InterruptFrame *frame, uint32_t err)
 	uint8_t write   = err & (1 << 1);
 	uint8_t user    = err & (1 << 2);
 	if (!PAGE_ADDR(addr))
-		panic("Null dereference @ %#.8x", frame->eip);
+		panic("Null dereference @ %#.8x (CPU#%d)", frame->eip, CPUID);
 	ASSERT(!present);
-	kprintf("Allocating frame for %#.8x [%#.8x]", addr, frame->eip);
+	kprintf("Allocating frame for %#.8x [%#.8x] CPU#%d",
+	        addr, frame->eip,CPUID);
 	/* Allocate a page */
 	set_page(addr, alloc_frame() | PTE_PRESENT | PTE_WRITE | PTE_GLOBAL);
 }
diff --git a/task/clone.c b/task/clone.c
index 22d737b..c9a6dc0 100644
--- a/task/clone.c
+++ b/task/clone.c
@@ -46,7 +46,8 @@ clone(int flags)
 end:
 	if (!tid) {
 		outb(0x20, 0x20);
-		LAPIC(0xB0) = 0;
+		if (apic)
+			LAPIC(0xB0) = 0;
 	}
 
 	return tid;
diff --git a/task/scheduler.c b/task/scheduler.c
index 1bc0f55..5cb1440 100644
--- a/task/scheduler.c
+++ b/task/scheduler.c
@@ -17,15 +17,18 @@ ObjectList *readyQueue[PRIORITY_COUNT];
 static void
 switch_to_task(Task *task)
 {
-	lock(current);
-	asm volatile("mov %%esp, %0" : "=r" (current->esp));
-	asm volatile("mov %%ebp, %0" : "=r" (current->ebp));
-	current->eip = (uintptr_t) &&end;
-	unlock(current);
+	/* Save current task state */
+	if (__builtin_expect(!!current, 1)) {
+		lock(current);
+		asm volatile("mov %%esp, %0" : "=r" (current->esp));
+		asm volatile("mov %%ebp, %0" : "=r" (current->ebp));
+		current->eip = (uintptr_t) &&end;
+		unlock(current);
+		put(current);
+	}
 
-	put(current);
+	/* Switch to new context */
 	current = task; /* Use the passed reference */
-
 	asm volatile (
 		"cli;"
 		"movl %0, %%ecx;"
@@ -39,7 +42,7 @@ switch_to_task(Task *task)
 	);
 end:
 	/* This prevents GCC from optimising the jump to be after the return */
-	__atomic_thread_fence(__ATOMIC_ACQ_REL);
+	asm volatile("":::"memory");
 }
 
 /* Find the next schedulable ready queue */
@@ -58,33 +61,32 @@ highest_priority_queue(void)
 void
 schedule(void)
 {
-	if (current->inCriticalSection)
+	if (current && current->inCriticalSection)
 		return;
 
 	Task *task = current;
 	ObjectList *queue = highest_priority_queue();
 
-	/* Next schedulable task */
-	if (queue) {
-		task = pop_from_start(queue);
-		task->state = RUNNING;
-		if (current->state == RUNNING) {
-			current->state = READY;
-			add(readyQueue[current->priority], current);
-		}
-		switch_to_task(task);
-	/* Idle */
-	} else if (current->state != RUNNING) {
+	/* Idle if necessary */
+	if (!queue) {
+		if (current && current->state == RUNNING)
+			return;
 		current = NULL;
 		asm volatile("sti");
 		while (!(queue = highest_priority_queue()))
 			asm volatile("hlt");
 		asm volatile("cli");
 		current = task;
-		task = pop_from_start(queue);
-		task->state = RUNNING;
-		switch_to_task(task);
 	}
+
+	/* Schedule next task */
+	task = pop_from_start(queue);
+	task->state = RUNNING;
+	if (current && current->state == RUNNING) {
+		current->state = READY;
+		add(readyQueue[current->priority], current);
+	}
+	switch_to_task(task);
 }
 
 /* Initialise the scheduler */
diff --git a/task/time.c b/task/time.c
index 345b491..3f5a50a 100644
--- a/task/time.c
+++ b/task/time.c
@@ -15,14 +15,15 @@ uint8_t slice[MAX_CPUS] = {0};
 void
 timer_handler(struct InterruptFrame *frame)
 {
-	monotonic++;
+	if (CPUID == 0)
+		monotonic++;
 
 	if (!current)
 		return;
 	slice[CPUID]++;
 
 	/* Call scheduler */
-	if (slice[CPUID] < current->priority)
+	if (slice[CPUID] < (current->priority * 10))
 		return;
 	slice[CPUID] = 0;
 	schedule();