3 files changed, 51 insertions, 0 deletions
diff --git a/sys/include/sys/proc.h b/sys/include/sys/proc.h
index ed75f71..b9db603 100644
--- a/sys/include/sys/proc.h
+++ b/sys/include/sys/proc.h
@@ -56,6 +56,7 @@ struct proc {
     struct trapframe tf;
     struct pcb pcb;
     size_t priority;
+    bool rested;
     uint32_t flags;
     uintptr_t stack_base;
     TAILQ_ENTRY(proc) link;
diff --git a/sys/include/sys/sched.h b/sys/include/sys/sched.h
index 52ca1ba..829e57d 100644
--- a/sys/include/sys/sched.h
+++ b/sys/include/sys/sched.h
@@ -36,6 +36,7 @@
 #if defined(_KERNEL)
 
 void sched_init(void);
+void sched_yield(void);
 __dead void sched_enter(void);
 void sched_enqueue_td(struct proc *td);
 
diff --git a/sys/kern/kern_sched.c b/sys/kern/kern_sched.c
index 1d79937..6d85330 100644
--- a/sys/kern/kern_sched.c
+++ b/sys/kern/kern_sched.c
@@ -140,6 +140,38 @@ this_td(void)
     return ci->curtd;
 }
 
+static inline void
+td_pri_raise(struct proc *td)
+{
+    if (td->priority > 0) {
+	    td->priority--;
+    }
+}
+
+static inline void
+td_pri_lower(struct proc *td)
+{
+    if (td->priority < SCHED_NQUEUE - 1) {
+        td->priority++;
+    }
+}
+
+static inline void
+td_pri_update(struct proc *td)
+{
+    switch (policy) {
+    case SCHED_POLICY_MLFQ:
+        if (td->rested) {
+            td->rested = false;
+            td_pri_raise(td);
+        } else {
+            td_pri_lower(td);
+        }
+
+        break;
+    }
+}
+
 /*
  * Perform a context switch.
  */
@@ -154,6 +186,10 @@ sched_switch(struct trapframe *tf)
     ci = this_cpu();
     td = ci->curtd;
 
+    if (td != NULL) {
+        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.
@@ -205,6 +241,19 @@ sched_enter(void)
 }
 
 void
+sched_yield(void)
+{
+    struct proc *td = this_td();
+
+    if (td != NULL) {
+        td->rested = true;
+    }
+
+    sched_oneshot(false);
+    while (td->rested);
+}
+
+void
 sched_init(void)
 {
     /* Setup the queues */