2 files changed, 16 insertions, 1 deletions
diff --git a/os/kernel/src/chschd.c b/os/kernel/src/chschd.c
index af24f5cf4..6183180bc 100644
--- a/os/kernel/src/chschd.c
+++ b/os/kernel/src/chschd.c
@@ -211,7 +211,7 @@ void chSchDoRescheduleI(void) {
 void chSchRescheduleS(void) {
   /* first thread in the runnable queue has higher priority than the running
    * thread? */
-  if (firstprio(&rlist.r_queue) > currp->p_prio)
+  if (chSchMustRescheduleS())
     chSchDoRescheduleI();
 }
 
diff --git a/os/kernel/src/chthreads.c b/os/kernel/src/chthreads.c
index 583bc7c04..c18a4f8cd 100644
--- a/os/kernel/src/chthreads.c
+++ b/os/kernel/src/chthreads.c
@@ -290,6 +290,21 @@ void chThdSleepUntil(systime_t time) {
   chSysUnlock();
 }
 
+#if CH_USE_ROUNDROBIN
+/**
+ * @brief Yields the time slot.
+ * @details Yields the CPU control to the next thread in the ready list with
+ *          equal priority, if any.
+ */
+void chThdYield(void) {
+
+  chSysLock();
+  if (chSchCanYieldS())
+    chSchDoRescheduleI();
+  chSysUnlock();
+}
+#endif /* CH_USE_ROUNDROBIN */
+
 /**
  * @brief Terminates the current thread by specifying an exit status code.
  *