1 files changed, 37 insertions, 13 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;
 }