diff --git a/readme.txt b/readme.txt
index 5b06953fa..e81a07358 100644
--- a/readme.txt
+++ b/readme.txt
@@ -50,6 +50,9 @@ AVR-AT90CANx-GCC       - Port on AVR AT90CAN128, not complete yet.
   various mechanisms and the correct use cases.
 - RT Semaphores subsystem removed, the Mutexes implements a better solution
   for the same problem.
+- More performance improvements to the scheduler see the remarks into the
+  chSchWakeupS() function source. The benchmark suite reports a 6% increase
+  in the context switch performance.
 - Added mutexes test cases to the test suite.
 - Modified the test suite in order to have each test case to have the same
   alignment enforced on functions. This is done to reduce MAM/Cache alignment
diff --git a/src/chmtx.c b/src/chmtx.c
index 6f53a615f..822789270 100644
--- a/src/chmtx.c
+++ b/src/chmtx.c
@@ -189,8 +189,15 @@ void chMtxUnlockS(void) {
         newprio = mp->m_queue.p_next->p_prio;
       mp = mp->m_next;
     }
-    currp->p_prio = newprio;
-    chSchWakeupS(tp, RDY_OK);
+    if (currp->p_prio == newprio)
+      chSchWakeupS(tp, RDY_OK);
+    else {
+      /* Note, changing priority and use chSchWakeupS() is wrong because the
+         internal optimization, see the chSchWakeupS() notes.*/
+      currp->p_prio = newprio;
+      chSchReadyI(tp, RDY_OK);
+      chSchRescheduleS();
+    }
   }
 }
 
diff --git a/src/chschd.c b/src/chschd.c
index e091bb0a0..78efa57f7 100644
--- a/src/chschd.c
+++ b/src/chschd.c
@@ -104,6 +104,8 @@ void chSchGoSleepS(t_tstate newstate) {
  * @note The function is not meant to be used in the user code directly.
  * @note It is equivalent to a \p chSchReadyI() followed by a
  *       \p chSchRescheduleS() but much more efficient.
+ * @note The function assumes that the invoking thread is the highest priority
+ *       thread, so you can't use it to change priority and reschedule.
  */
 void chSchWakeupS(Thread *ntp, t_msg msg) {
 
@@ -111,7 +113,15 @@ void chSchWakeupS(Thread *ntp, t_msg msg) {
     chSchReadyI(ntp, msg);
   else {
     Thread *otp = currp;
-    chSchReadyI(otp, RDY_OK);
+    /* Optimization, assumes that the invoking thread has the highest priority
+       which is always true unless the priority was willingly changed.
+       This assumption allows us to place the thread always on top of the
+       ready list without have to scan it, free lunch.*/
+/*    chSchReadyI(otp, RDY_OK);*/
+    otp->p_state = PRREADY;
+    otp->p_rdymsg = RDY_OK;
+    otp->p_next = (otp->p_prev = (Thread *)&rlist.r_queue)->p_next;
+    otp->p_next->p_prev = rlist.r_queue.p_next = otp;
     (currp = ntp)->p_state = PRCURR;
     ntp->p_rdymsg = msg;
     rlist.r_preempt = CH_TIME_QUANTUM;