diff --git a/os/kernel/include/scheduler.h b/os/kernel/include/scheduler.h
index 00294e190..8aa692e58 100644
--- a/os/kernel/include/scheduler.h
+++ b/os/kernel/include/scheduler.h
@@ -98,6 +98,9 @@ extern "C" {
   void chSchDoRescheduleI(void);
   void chSchRescheduleS(void);
   bool_t chSchRescRequiredI(void);
+#if CH_USE_ROUNDROBIN
+  void chSchDoYieldS(void);
+#endif
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/kernel/src/chschd.c b/os/kernel/src/chschd.c
index 6183180bc..929f114e5 100644
--- a/os/kernel/src/chschd.c
+++ b/os/kernel/src/chschd.c
@@ -61,7 +61,7 @@ Thread *chSchReadyI(Thread *tp) {
   Thread *cp;
 
   tp->p_state = PRREADY;
-  cp = (Thread *)&rlist;
+  cp = (Thread *)&rlist.r_queue;
   do {
     cp = cp->p_next;
   } while (cp->p_prio >= tp->p_prio);
@@ -71,6 +71,22 @@ Thread *chSchReadyI(Thread *tp) {
   return tp;
 }
 
+#if 0
+INLINE Thread *chSchReadyReverseI(Thread *tp) {
+  Thread *cp;
+
+  tp->p_state = PRREADY;
+  cp = (Thread *)&rlist.r_queue;
+  do {
+    cp = cp->p_prev;
+  } while ((cp->p_prio < tp->p_prio) && (cp->p_prio < tp->p_prio));
+  /* Insertion on p_next.*/
+  tp->p_next = (tp->p_prev = cp)->p_next;
+  tp->p_next->p_prev = cp->p_next = tp;
+  return tp;
+}
+#endif
+
 /**
  * @brief Puts the current thread to sleep into the specified state.
  * @details The thread goes into a sleeping state. The @ref thread_states are
@@ -109,7 +125,7 @@ static void wakeup(void *p) {
 #if CH_USE_CONDVARS
   case PRWTCOND:
 #endif
-    /* States requiring dequeuing. */
+    /* States requiring dequeuing.*/
     dequeue(tp);
   }
 #endif
@@ -193,7 +209,7 @@ void chSchWakeupS(Thread *ntp, msg_t msg) {
 void chSchDoRescheduleI(void) {
 
   Thread *otp = currp;
-  /* pick the first thread from the ready queue and makes it current */
+  /* Pick the first thread from the ready queue and makes it current.*/
   (currp = fifo_remove(&rlist.r_queue))->p_state = PRCURR;
   chSchReadyI(otp);
 #if CH_USE_ROUNDROBIN
@@ -209,8 +225,8 @@ void chSchDoRescheduleI(void) {
  *          the ready list then make the higher priority thread running.
  */
 void chSchRescheduleS(void) {
-  /* first thread in the runnable queue has higher priority than the running
-   * thread? */
+  /* First thread in the runnable queue has higher priority than the running
+   * thread?.*/
   if (chSchMustRescheduleS())
     chSchDoRescheduleI();
 }
@@ -239,4 +255,33 @@ bool_t chSchRescRequiredI(void) {
 #endif
 }
 
+#if CH_USE_ROUNDROBIN
+void chSchDoYieldS(void) {
+
+  if (chSchCanYieldS()) {
+    Thread *cp = (Thread *)&rlist.r_queue;
+    Thread *otp = currp;
+
+    /*
+     * Note, the following insertion code works because we know that on the
+     * ready list there is at least one thread with priority equal or higher
+     * than the current one.
+     */
+    otp->p_state = PRREADY;
+    do {
+      cp = cp->p_prev;
+    } while (cp->p_prio < otp->p_prio);
+    /* Insertion on p_next.*/
+    otp->p_next = (otp->p_prev = cp)->p_next;
+    otp->p_next->p_prev = cp->p_next = otp;
+
+    /* Pick the first thread from the ready queue and makes it current.*/
+    (currp = fifo_remove(&rlist.r_queue))->p_state = PRCURR;
+    rlist.r_preempt = CH_TIME_QUANTUM;
+    chDbgTrace(otp, currp);
+    chSysSwitchI(otp, currp);
+  }
+}
+#endif /* CH_USE_ROUNDROBIN */
+
 /** @} */
diff --git a/os/kernel/src/chthreads.c b/os/kernel/src/chthreads.c
index c18a4f8cd..a3028867a 100644
--- a/os/kernel/src/chthreads.c
+++ b/os/kernel/src/chthreads.c
@@ -299,8 +299,7 @@ void chThdSleepUntil(systime_t time) {
 void chThdYield(void) {
 
   chSysLock();
-  if (chSchCanYieldS())
-    chSchDoRescheduleI();
+  chSchDoYieldS();
   chSysUnlock();
 }
 #endif /* CH_USE_ROUNDROBIN */
diff --git a/readme.txt b/readme.txt
index 8a90b6f15..7db4b1b91 100644
--- a/readme.txt
+++ b/readme.txt
@@ -12,7 +12,7 @@
 - FIX: Fixed GCC 4.4.x related problems in CM3 port (bug 2846162).(backported
   in stable branch)
 - FIX: Fixed LPC214x UART problem (bug 2841088)(backported in stable branch).
-- NEW: Added new API chThdYield().
+- NEW: Added new APIs chSchDoYieldS() abd chThdYield().
 - MEW: Added new benchmark about round robin rescheduling.
 - NEW: Reorganized and rationalized the distribution tree and the
   documentation.