1 files changed, 78 insertions, 49 deletions

diff --git a/sys/kern/kern_sched.c b/sys/kern/kern_sched.c
index 4bbe5a0..ec5592e 100644
--- a/sys/kern/kern_sched.c
+++ b/sys/kern/kern_sched.c
@@ -104,12 +104,29 @@ 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);
-            spinlock_release(&tdq_lock);
-            return td;
+        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 */
@@ -176,62 +193,50 @@ 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;
 
     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);
 }
 
@@ -242,9 +247,8 @@ void
 sched_enter(void)
 {
     md_inton();
-    md_sync_all();
+    sched_oneshot(false);
     for (;;) {
-        sched_oneshot(false);
         md_pause();
     }
 }
@@ -252,14 +256,39 @@ sched_enter(void)
 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