Nucleus

diff --git a/task/clone.c b/task/clone.c
index 0c95083..f7496c9 100644
--- a/task/clone.c
+++ b/task/clone.c
@@ -6,6 +6,8 @@
 #include <sys/types.h>
 #include <nucleus/task.h>
 
+extern TaskQueue *readyQueue[];
+
 /* Read the EIP */
 static uintptr_t
 read_eip(void)
@@ -20,22 +22,23 @@ pid_t
 clone(int flags)
 {
 	Task *parent = current, *child = new(&taskType), *tmp;
+	pid_t tid = 0;
 
+	/* After this, anything on the stack is desynchronised */
 	child->pageDir = clone_dir();
 
-	parent->next = child;
-	child->next = parent;
-
 	/* Split tasks here */
 	uintptr_t esp, ebp, eip;
-	eip = read_eip();
-	if (current == parent) {
-		asm volatile("mov %%esp, %0" : "=r" (esp));
-		asm volatile("mov %%ebp, %0" : "=r" (ebp));
-		child->esp = esp;
-		child->ebp = ebp;
-		child->eip = eip;
-		return child->tid;
-	}
-	return 0;
+	eip = (uintptr_t) &&end;
+
+	asm volatile("mov %%esp, %0" : "=r" (esp));
+	asm volatile("mov %%ebp, %0" : "=r" (ebp));
+	child->esp = esp;
+	child->ebp = ebp;
+	child->eip = eip;
+	add_to_queue(readyQueue[child->priority], child);
+	tid = child->tid;
+	put(child);
+end:
+	return tid;
 }
diff --git a/task/queue.c b/task/queue.c
new file mode 100644
index 0000000..670c26c
--- /dev/null
+++ b/task/queue.c
@@ -0,0 +1,84 @@
+/*
+ * This file implements the Task Queue object and contains all the functions for
+ * dealing with them.  A Task Queue holds a reference to every Task it contains.
+ * There is a single reference per task for the entire queue, and tasks do not
+ * hold references to their neighbour, nor the queue its end.
+ */
+
+#include <nucleus/object.h>
+#include <nucleus/task.h>
+
+static void *task_queue_new(void);
+static void task_queue_delete(Object *);
+
+/* Task Queue object type */
+ObjectType taskQueueType = {
+	.new = task_queue_new,
+	.delete = task_queue_delete,
+};
+
+/* Create a new Task Queue */
+static void *
+task_queue_new(void)
+{
+	TaskQueue *queue = kmalloc(sizeof(TaskQueue));
+}
+
+/* Destroy a Task Queue */
+static void
+task_queue_delete(Object *obj)
+{
+	TaskQueue *queue = (void *) obj;
+	while (queue->start)
+		put(pop_from_queue(queue));
+	kfree(queue);
+}
+
+/* Add a Task to a Task Queue */
+void
+add_to_queue(TaskQueue *queue, Task *task)
+{
+	if (!queue->start)
+		queue->start = get(task);
+	else
+		queue->end->next = get(task);
+	queue->end = task;
+	task->next = NULL;
+}
+
+/* Remove a Task from a Task Queue */
+void
+remove_from_queue(TaskQueue *queue, Task *task)
+{
+	/* Start of queue */
+	if (queue->start == task) {
+		queue->start = task->next;
+		goto found;
+	}
+
+	/* Search */
+	Task *prev;
+	for (prev = queue->start; prev->next; prev = prev->next)
+		if (prev->next == task)
+			break;
+	if (!prev->next)
+		return;
+
+	prev->next = task->next;
+
+found:
+	if (queue->end == task)
+		queue->end = NULL;
+
+	task->next = NULL;
+	put(task);
+}
+
+/* Remove the first Task from a Task Queue */
+Task *
+pop_from_queue(TaskQueue *queue)
+{
+	Task *head = get(queue->start);
+	remove_from_queue(queue, queue->start);
+	return head;
+}
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);
 }
diff --git a/task/task.c b/task/task.c
index 507caae..9fafc6f 100644
--- a/task/task.c
+++ b/task/task.c
@@ -11,6 +11,7 @@
 #include <nucleus/memory.h>
 #include <nucleus/object.h>
 
+void init_scheduler(void);
 void timer_handler(struct InterruptFrame *frame);
 
 static void *task_new(void);
@@ -84,5 +85,6 @@ init_tasking(void)
 	current = new(&taskType);
 	asm volatile("mov %%cr3, %0" : "=r" (current->pageDir));
 
+	init_scheduler();
 	register_interrupt(0, timer_handler);
 }