Nucleus

diff --git a/task/scheduler.c b/task/scheduler.c
index 95e3990..37e8345 100644
--- a/task/scheduler.c
+++ b/task/scheduler.c
@@ -9,7 +9,9 @@
 #include <nucleus/panic.h>
 #include <nucleus/task.h>
 
-TaskQueue *readyQueue[HIGHEST];
+#define PRIORITY_COUNT 6
+
+TaskQueue *readyQueue[PRIORITY_COUNT];
 
 /* Read the EIP */
 static uintptr_t
@@ -51,15 +53,28 @@ switch_to_task(Task *task)
 end:
 }
 
+/* Find the next schedulable ready queue */
+static TaskQueue *
+highest_priority_queue(void)
+{
+	enum Priority p;
+	for (p = PRIORITY_COUNT - 1; p > 0; p--) {
+		if (readyQueue[p]->start)
+			return readyQueue[p];
+	}
+	return NULL;
+}
+
 /* Schedule the next task */
 void
 schedule(void)
 {
 	Task *task = current;
+	TaskQueue *queue = highest_priority_queue();
 
 	/* Next schedulable task */
-	if (readyQueue[NORMAL]->start) {
-		task = pop_from_queue(readyQueue[NORMAL]);
+	if (queue) {
+		task = pop_from_queue(queue);
 		task->state = RUNNING;
 		if (current->state == RUNNING) {
 			current->state = READY;
@@ -70,11 +85,11 @@ schedule(void)
 	} else if (current->state != RUNNING) {
 		current = NULL;
 		asm volatile("sti");
-		while (!readyQueue[NORMAL]->start)
+		while (!(queue = highest_priority_queue()))
 			asm volatile("hlt");
 		asm volatile("cli");
 		current = task;
-		task = pop_from_queue(readyQueue[NORMAL]);
+		task = pop_from_queue(queue);
 		task->state = RUNNING;
 		switch_to_task(task);
 	}
@@ -85,6 +100,6 @@ void
 init_scheduler(void)
 {
 	enum Priority p;
-	for (p = NONE; p <= HIGHEST; p++)
+	for (p = 0; p < PRIORITY_COUNT; p++)
 		readyQueue[p] = new(&taskQueueType);
 }