4 files changed, 38 insertions, 6 deletions
diff --git a/os/kernel/include/scheduler.h b/os/kernel/include/scheduler.h
index 235761216..00294e190 100644
--- a/os/kernel/include/scheduler.h
+++ b/os/kernel/include/scheduler.h
@@ -78,6 +78,12 @@ typedef struct {
 
 extern ReadyList rlist;
 
+#ifdef CH_CURRP_REGISTER_CACHE
+register Thread *currp asm(CH_CURRP_REGISTER_CACHE);
+#else
+#define currp rlist.r_current
+#endif
+
 /*
  * Scheduler APIs.
  */
@@ -96,11 +102,19 @@ extern "C" {
 }
 #endif
 
-#ifdef CH_CURRP_REGISTER_CACHE
-register Thread *currp asm(CH_CURRP_REGISTER_CACHE);
-#else
-#define currp rlist.r_current
-#endif
+/**
+ * @brief Determines if yielding is possible.
+ * @details This function returns @p TRUE if there is a ready thread with
+ *          equal or higher priority.
+ */
+#define chSchCanYieldS() (firstprio(&rlist.r_queue) >= currp->p_prio)
+
+/**
+ * @brief Determines if the current thread must reschedule.
+ * @details This function returns @p TRUE if there is a ready thread with
+ *          higher priority.
+ */
+#define chSchMustRescheduleS() (firstprio(&rlist.r_queue) >= currp->p_prio)
 
 #endif /* _SCHEDULER_H_ */
 
diff --git a/os/kernel/include/threads.h b/os/kernel/include/threads.h
index 7155e9b32..da7dfc152 100644
--- a/os/kernel/include/threads.h
+++ b/os/kernel/include/threads.h
@@ -183,6 +183,9 @@ extern "C" {
 #if CH_USE_WAITEXIT
   msg_t chThdWait(Thread *tp);
 #endif
+#if CH_USE_ROUNDROBIN
+  void chThdYield(void);
+#endif
 #ifdef __cplusplus
 }
 #endif
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.
  *