1 files changed, 51 insertions, 16 deletions

diff --git a/sys/kern/kern_sched.c b/sys/kern/kern_sched.c
index 2fe0f32..8e456d2 100644
--- a/sys/kern/kern_sched.c
+++ b/sys/kern/kern_sched.c
@@ -104,12 +104,22 @@ 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;
+        }
+
+        TAILQ_REMOVE(&queue->q, td, link);
+        if (ISSET(td->flags, PROC_SLEEP)) {
+            continue;
+        }
+
+        spinlock_release(&tdq_lock);
+        return td;
     }
 
     /* We got nothing */
@@ -125,6 +135,10 @@ sched_enqueue_td(struct proc *td)
 {
     struct sched_queue *queue;
 
+    if (ISSET(td->flags, PROC_SLEEP)) {
+        return;
+    }
+
     spinlock_acquire(&tdq_lock);
     queue = &qlist[td->priority];
 
@@ -176,15 +190,31 @@ 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;
+    struct cpu_info *ci;
 
     ci = this_cpu();
     td = ci->curtd;
@@ -203,11 +233,7 @@ sched_switch(struct trapframe *tf)
         return;
     }
 
-    memcpy(tf, &next_td->tf, sizeof(*tf));
-    ci->curtd = next_td;
-    pcbp = &next_td->pcb;
-
-    pmap_switch_vas(pcbp->addrsp);
+    sched_switch_to(tf, next_td);
     sched_oneshot(false);
 }
 
@@ -217,6 +243,7 @@ sched_switch(struct trapframe *tf)
 void
 sched_enter(void)
 {
+    md_inton();
     sched_oneshot(false);
     for (;;) {
         md_pause();
@@ -226,12 +253,20 @@ 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;
-        sched_switch(&td->tf);
+    if ((td = ci->curtd) == NULL) {
+        return;
     }
+
+    td->rested = true;
+    ci->curtd = NULL;
+
+    md_inton();
+    sched_oneshot(false);
+    md_hlt();
+    ci->curtd = td;
 }
 
 void