1 files changed, 7 insertions, 1 deletions
diff --git a/os/rt/src/chmtx.c b/os/rt/src/chmtx.c
index 03f78a3ce..42c2836ae 100644
--- a/os/rt/src/chmtx.c
+++ b/os/rt/src/chmtx.c
@@ -375,7 +375,13 @@ void chMtxUnlock(mutex_t *mp) {
       mp->m_owner = tp;
       mp->m_next = tp->p_mtxlist;
       tp->p_mtxlist = mp;
-      chSchWakeupS(tp, MSG_OK);
+
+      /* Note, not using chSchWakeupS() becuase that function expects the
+         current thread to have the higher or equal priority than the ones
+         in the ready list. This is not necessarily true here because we
+         just changed priority.*/
+      (void) chSchReadyI(tp);
+      chSchRescheduleS();
     }
     else {
       mp->m_owner = NULL;