2 files changed, 10 insertions, 17 deletions

diff --git a/os/kernel/src/chmsg.c b/os/kernel/src/chmsg.c
index 577e82693..2f5a7a127 100644
--- a/os/kernel/src/chmsg.c
+++ b/os/kernel/src/chmsg.c
@@ -48,9 +48,9 @@ msg_t chMsgSend(Thread *tp, msg_t msg) {
   chDbgCheck(tp != NULL, "chMsgSend");

 

   chSysLock();

-  msg_insert(currp, &tp->p_msgqueue);

   currp->p_msg = msg;

-  currp->p_u.wtobjp = tp;

+  currp->p_u.wtobjp = &tp->p_msgqueue;

+  msg_insert(currp, &tp->p_msgqueue);

   if (tp->p_state == THD_STATE_WTMSG)

     chSchReadyI(tp);

   chSchGoSleepS(THD_STATE_SNDMSG);

diff --git a/os/kernel/src/chmtx.c b/os/kernel/src/chmtx.c
index d319b9316..c14db4ad0 100644
--- a/os/kernel/src/chmtx.c
+++ b/os/kernel/src/chmtx.c
@@ -77,37 +77,30 @@ void chMtxLockS(Mutex *mp) {
      * of the running thread requesting the mutex.
      */
     Thread *tp = mp->m_owner;
-    /* {tp is the thread currently owning the mutex} */
-    /* Has the running thread higher priority than tp? */
+    /* Does the running thread have higher priority than the mutex
+     * ownning thread? */
     while (tp->p_prio < currp->p_prio) {
       /* Make priority of thread tp match the running thread's priority.*/
       tp->p_prio = currp->p_prio;
-      /*
-       * The following states need priority queues reordering.
-       */
+      /* The following states need priority queues reordering.*/
       switch (tp->p_state) {
       case THD_STATE_WTMTX:
-        /* Re-enqueues tp with its new priority on the mutex wait queue.*/
+        /* Re-enqueues the mutex owner with its new priority.*/
         prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
-        /* Boost the owner of this mutex if needed.*/
         tp = ((Mutex *)tp->p_u.wtobjp)->m_owner;
         continue;
+#if CH_USE_CONDVARS | CH_USE_SEMAPHORES_PRIORITY | CH_USE_MESSAGES_PRIORITY
 #if CH_USE_CONDVARS
       case THD_STATE_WTCOND:
-        /* Re-enqueues tp with its new priority on the condvar queue.*/
-        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
-        break;
 #endif
 #if CH_USE_SEMAPHORES_PRIORITY
       case THD_STATE_WTSEM:
-        /* Re-enqueues tp with its new priority on the semaphore queue.*/
-        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
-        break;
 #endif
 #if CH_USE_MESSAGES_PRIORITY
       case THD_STATE_SNDMSG:
-        /* Re-enqueues  tp with its new priority on the server thread queue.*/
-        prio_insert(dequeue(tp), &((Thread *)tp->p_u.wtobjp)->p_msgqueue);
+#endif
+        /* Re-enqueues tp with its new priority on the queue.*/
+        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
         break;
 #endif
       case THD_STATE_READY: