2 files changed, 40 insertions, 16 deletions

diff --git a/src/kernel/task/sched.c b/src/kernel/task/sched.c
index 85329ea..2b0ef8a 100644
--- a/src/kernel/task/sched.c
+++ b/src/kernel/task/sched.c
@@ -2,25 +2,49 @@
 #include <core/sys.h>
 #include <mem/mem.h>
 
-static struct thread *queue = 0, *last = 0;
+// Lower priority numbers have high priority. Priorities must start at 0.
+#define PRIORITIES 4		// we have 4 priority levels
+#define PRIORITY(t) (t->process->privilege)		//get priority for a thread
 
-/*	Used by task.c. Enqueus a thread in the queue of threads waiting to run. */
-void sched_enqueue(struct thread *t) {
+extern struct thread *idle_thread;
+
+static struct thread *queue[PRIORITIES] = {0}, *last[PRIORITIES] = {0};
+
+/*	For internal use only. Enqueues specified thread in specified priority queue. */
+static void sched_enqueueIn(struct thread *t, int qid) {
 	t->queue_next = 0;
-	if (queue == 0) {
-		queue = last = t;
+	if (queue[qid] == 0) {
+		queue[qid] = last[qid] = t;
 	} else {
-		last->queue_next = t;
-		last = t;
+		last[qid]->queue_next = t;
+		last[qid] = t;
 	}
 }
 
-/*	Used by task.c. Pops a thread from the queue to be run. */
+/*	For internal use only. Pops a thread from specified queue, if available. */
+static struct thread *sched_dequeueFrom(int qid) {
+	if (queue[qid] == 0) return 0;
+	struct thread *it = queue[qid];
+	ASSERT((it->queue_next == 0 && it == last[qid]) || it != last[qid]);
+	queue[qid] = it->queue_next;
+	if (queue[qid] == 0) last[qid] = 0;
+	return it;
+}
+
+/*	Used by task.c. Enqueus a thread in the corresponding priority queue. */
+void sched_enqueue(struct thread *t) {
+	if (t == idle_thread) return;
+	sched_enqueueIn(t, PRIORITY(t));
+}
+
+/*	Used by task.c. Pops a thread from the lowest priority non-empty queue. */
 struct thread *sched_dequeue() {
-	if (queue == 0) return 0;
-	struct thread *it = queue;
-	ASSERT((it->queue_next == 0 && it == last) || it != last);
-	queue = it->queue_next;
-	if (queue == 0) last = 0;
+	struct thread *it = 0;
+	int i;
+	for (i = 0; i < PRIORITIES; i++) {
+		it = sched_dequeueFrom(i);
+		if (it != 0) break;
+	}
+	if (it == 0) return idle_thread;
 	return it;
 }
diff --git a/src/kernel/task/task.c b/src/kernel/task/task.c
index 9d64b9e..8dcf713 100644
--- a/src/kernel/task/task.c
+++ b/src/kernel/task/task.c
@@ -24,7 +24,7 @@ extern void task_idle(void*);
 
 static uint32_t nextpid = 1;
 struct process *processes = 0, *kernel_process;
-struct thread *current_thread = 0, *idle_thread;
+struct thread *current_thread = 0, *idle_thread = 0;
 
 uint32_t tasking_tmpStack[KSTACKSIZE];
 
@@ -76,11 +76,11 @@ void tasking_switch() {
 		current_thread->esp = esp;
 		current_thread->ebp = ebp;
 		current_thread->eip = eip;
-		if (current_thread->state == TS_RUNNING && current_thread != idle_thread) sched_enqueue(current_thread);
+		if (current_thread->state == TS_RUNNING) sched_enqueue(current_thread);
 	}
 
 	current_thread = sched_dequeue();
-	if (current_thread == 0) current_thread = idle_thread;
+	ASSERT(current_thread != 0);
 
 	pagedir_switch(current_thread->process->pagedir);