Nucleus

diff --git a/task/scheduler.c b/task/scheduler.c
index 9ad1405..95e3990 100644
--- a/task/scheduler.c
+++ b/task/scheduler.c
@@ -9,6 +9,8 @@
 #include <nucleus/panic.h>
 #include <nucleus/task.h>
 
+TaskQueue *readyQueue[HIGHEST];
+
 /* Read the EIP */
 static uintptr_t
 read_eip(void)
@@ -25,14 +27,13 @@ switch_to_task(Task *task)
 	uintptr_t esp, ebp, eip;
 	asm volatile("mov %%esp, %0" : "=r" (esp));
 	asm volatile("mov %%ebp, %0" : "=r" (ebp));
-	eip = read_eip();
-	if (eip == 0x10032004) /* Magic number */
-		return;
+	eip = (uintptr_t) &&end;
 
 	current->esp = esp;
 	current->ebp = ebp;
 	current->eip = eip;
-	current = task;
+	put(current);
+	current = task; /* Use the passed reference */
 	esp = current->esp;
 	ebp = current->ebp;
 	eip = current->eip;
@@ -43,11 +44,11 @@ switch_to_task(Task *task)
 		"movl %1, %%esp;"
 		"movl %2, %%ebp;"
 		"movl %3, %%cr3;"
-		"movl $0x10032004, %%eax;"
 		"sti;"
 		"jmp *%%ecx"
 		:: "g" (eip), "g" (esp), "g" (ebp), "g" (current->pageDir)
 	);
+end:
 }
 
 /* Schedule the next task */
@@ -55,7 +56,35 @@ void
 schedule(void)
 {
 	Task *task = current;
-	if (!task->next)
-		return;
-	switch_to_task(task->next);
+
+	/* Next schedulable task */
+	if (readyQueue[NORMAL]->start) {
+		task = pop_from_queue(readyQueue[NORMAL]);
+		task->state = RUNNING;
+		if (current->state == RUNNING) {
+			current->state = READY;
+			add_to_queue(readyQueue[current->priority], current);
+		}
+		switch_to_task(task);
+	/* Idle */
+	} else if (current->state != RUNNING) {
+		current = NULL;
+		asm volatile("sti");
+		while (!readyQueue[NORMAL]->start)
+			asm volatile("hlt");
+		asm volatile("cli");
+		current = task;
+		task = pop_from_queue(readyQueue[NORMAL]);
+		task->state = RUNNING;
+		switch_to_task(task);
+	}
+}
+
+/* Initialise the scheduler */
+void
+init_scheduler(void)
+{
+	enum Priority p;
+	for (p = NONE; p <= HIGHEST; p++)
+		readyQueue[p] = new(&taskQueueType);
 }