1 files changed, 88 insertions, 49 deletions

diff --git a/sys/kern/kern_sched.c b/sys/kern/kern_sched.c
index 16daae2..774ba71 100644
--- a/sys/kern/kern_sched.c
+++ b/sys/kern/kern_sched.c
@@ -33,8 +33,11 @@
 #include <sys/cdefs.h>
 #include <sys/param.h>
 #include <sys/syslog.h>
+#include <sys/atomic.h>
+#include <dev/cons/cons.h>
 #include <machine/frame.h>
 #include <machine/cpu.h>
+#include <machine/cdefs.h>
 #include <vm/pmap.h>
 #include <dev/timer.h>
 #include <assert.h>
@@ -44,7 +47,7 @@
 
 void sched_switch(struct trapframe *tf);
 
-static sched_policy_t policy = SCHED_POLICY_RR;
+static sched_policy_t policy = SCHED_POLICY_MLFQ;
 
 /*
  * Thread ready queues - all threads ready to be
@@ -102,15 +105,34 @@ sched_dequeue_td(void)
 
     for (size_t i = 0; i < SCHED_NQUEUE; ++i) {
         queue = &qlist[i];
-        if (!TAILQ_EMPTY(&queue->q)) {
-            td = TAILQ_FIRST(&queue->q);
-            TAILQ_REMOVE(&queue->q, td, link);
-            break;
+        if (TAILQ_EMPTY(&queue->q)) {
+            continue;
         }
+
+        td = TAILQ_FIRST(&queue->q);
+        if (td == NULL) {
+            continue;
+        }
+
+        while (ISSET(td->flags, PROC_SLEEP)) {
+            td = TAILQ_NEXT(td, link);
+            if (td == NULL) {
+                break;
+            }
+        }
+
+        if (td == NULL) {
+            continue;
+        }
+
+        TAILQ_REMOVE(&queue->q, td, link);
+        spinlock_release(&tdq_lock);
+        return td;
     }
 
+    /* We got nothing */
     spinlock_release(&tdq_lock);
-    return td;
+    return NULL;
 }
 
 /*
@@ -172,62 +194,51 @@ td_pri_update(struct proc *td)
     }
 }
 
+void
+sched_switch_to(struct trapframe *tf, struct proc *td)
+{
+    struct cpu_info *ci;
+    struct pcb *pcbp;
+
+    ci = this_cpu();
+
+    if (tf != NULL) {
+        memcpy(tf, &td->tf, sizeof(*tf));
+    }
+
+    ci->curtd = td;
+    pcbp = &td->pcb;
+    pmap_switch_vas(pcbp->addrsp);
+}
+
 /*
  * Perform a context switch.
  */
 void
 sched_switch(struct trapframe *tf)
 {
-    struct cpu_info *ci;
-    struct pcb *pcbp;
     struct proc *next_td, *td;
-    bool use_current = true;
+    struct cpu_info *ci;
 
     ci = this_cpu();
     td = ci->curtd;
+    cons_detach();
 
     if (td != NULL) {
-        dispatch_signals(td);
-        td_pri_update(td);
-    }
-
-    /*
-     * Get the next thread and use it only if it isn't
-     * in the middle of an exit, exec, or whatever.
-     */
-    do {
-        if ((next_td = sched_dequeue_td()) == NULL) {
-            sched_oneshot(false);
+        if (td->pid == 0)
             return;
-        }
-
-        /*
-         * If we are in the middle of an exec, don't use this
-         * thread.
-         */
-        if (ISSET(next_td->flags, PROC_EXEC)) {
-            use_current = false;
-        }
 
-        /*
-         * Don't use this thread if we are currently
-         * exiting.
-         */
-        if (ISSET(next_td->flags, PROC_EXITING)) {
-            use_current = false;
-        }
-    } while (!use_current);
-
-    /* Save the previous thread */
-    if (td != NULL) {
+        dispatch_signals(td);
+        td_pri_update(td);
         sched_save_td(td, tf);
     }
 
-    memcpy(tf, &next_td->tf, sizeof(*tf));
-    ci->curtd = next_td;
-    pcbp = &next_td->pcb;
+    if ((next_td = sched_dequeue_td()) == NULL) {
+        sched_oneshot(false);
+        return;
+    }
 
-    pmap_switch_vas(pcbp->addrsp);
+    sched_switch_to(tf, next_td);
     sched_oneshot(false);
 }
 
@@ -237,21 +248,49 @@ sched_switch(struct trapframe *tf)
 void
 sched_enter(void)
 {
+    md_inton();
     sched_oneshot(false);
-    for (;;);
+    for (;;) {
+        md_pause();
+    }
 }
 
 void
 sched_yield(void)
 {
-    struct proc *td = this_td();
+    struct proc *td;
+    struct cpu_info *ci = this_cpu();
 
-    if (td != NULL) {
-        td->rested = true;
+    if ((td = ci->curtd) == NULL) {
+        return;
     }
 
+    td->rested = true;
+
+    /* FIXME: Hang yielding when waited on */
+    if (ISSET(td->flags, PROC_WAITED)) {
+        return;
+    }
+
+    ci->curtd = NULL;
+    md_inton();
     sched_oneshot(false);
-    while (td->rested);
+
+    md_hlt();
+    md_intoff();
+    ci->curtd = td;
+}
+
+void
+sched_detach(struct proc *td)
+{
+    struct sched_queue *queue;
+
+    spinlock_acquire(&tdq_lock);
+    queue = &qlist[td->priority];
+
+    TAILQ_REMOVE(&queue->q, td, link);
+    spinlock_release(&tdq_lock);
 }
 
 void