From fe292af06d48b5d42b0dd8fcb594b5a9c4668144 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Mon, 21 Jun 2010 16:52:51 +0000
Subject: Fixed bug 3019099.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@2027 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/kernel/src/chmtx.c | 58 ++++++++++++++++++++++++++++++++++++++-------------
 1 file changed, 43 insertions(+), 15 deletions(-)

(limited to 'os/kernel')

diff --git a/os/kernel/src/chmtx.c b/os/kernel/src/chmtx.c
index c3076163a..88b6f4ecd 100644
--- a/os/kernel/src/chmtx.c
+++ b/os/kernel/src/chmtx.c
@@ -145,12 +145,17 @@ void chMtxLockS(Mutex *mp) {
     prio_insert(ctp, &mp->m_queue);
     ctp->p_u.wtobjp = mp;
     chSchGoSleepS(THD_STATE_WTMTX);
-    chDbgAssert(mp->m_owner == NULL, "chMtxLockS(), #1", "still owned");
+    /* It is assumed that the thread performing the unlock operation assigns
+       the mutex to this thread.*/
+    chDbgAssert(mp->m_owner == ctp, "chMtxLockS(), #1", "not owner");
+    chDbgAssert(ctp->p_mtxlist == mp, "chMtxLockS(), #2", "not owned");
+  }
+  else {
+    /* It was not owned, inserted in the owned mutexes list.*/
+    mp->m_owner = ctp;
+    mp->m_next = ctp->p_mtxlist;
+    ctp->p_mtxlist = mp;
   }
-  /* The mutex is now inserted in the owned mutexes list.*/
-  mp->m_owner = ctp;
-  mp->m_next = ctp->p_mtxlist;
-  ctp->p_mtxlist = mp;
 }
 
 /**
@@ -216,9 +221,10 @@ Mutex *chMtxUnlock(void) {
      as not owned.*/
   ump = ctp->p_mtxlist;
   ctp->p_mtxlist = ump->m_next;
-  ump->m_owner = NULL;
   /* If a thread is waiting on the mutex then the fun part begins.*/
   if (chMtxQueueNotEmptyS(ump)) {
+    Thread *tp;
+
     /* Recalculates the optimal thread priority by scanning the owned
        mutexes list.*/
     tprio_t newprio = ctp->p_realprio;
@@ -234,9 +240,16 @@ Mutex *chMtxUnlock(void) {
     /* Assigns to the current thread the highest priority among all the
        waiting threads.*/
     ctp->p_prio = newprio;
-    /* Awakens the highest priority thread waiting for the unlocked mutex.*/
-    chSchWakeupS(fifo_remove(&ump->m_queue), RDY_OK);
+    /* Awakens the highest priority thread waiting for the unlocked mutex and
+       assigns the mutex to it.*/
+    tp = fifo_remove(&ump->m_queue);
+    ump->m_owner = tp;
+    ump->m_next = tp->p_mtxlist;
+    tp->p_mtxlist = ump;
+    chSchWakeupS(tp, RDY_OK);
   }
+  else
+    ump->m_owner = NULL;
   chSysUnlock();
   return ump;
 }
@@ -262,9 +275,10 @@ Mutex *chMtxUnlockS(void) {
      owned.*/
   ump = ctp->p_mtxlist;
   ctp->p_mtxlist = ump->m_next;
-  ump->m_owner = NULL;
   /* If a thread is waiting on the mutex then the fun part begins.*/
   if (chMtxQueueNotEmptyS(ump)) {
+    Thread *tp;
+
     /* Recalculates the optimal thread priority by scanning the owned
        mutexes list.*/
     tprio_t newprio = ctp->p_realprio;
@@ -278,8 +292,16 @@ Mutex *chMtxUnlockS(void) {
       mp = mp->m_next;
     }
     ctp->p_prio = newprio;
-    chSchReadyI(fifo_remove(&ump->m_queue));
+    /* Awakens the highest priority thread waiting for the unlocked mutex and
+       assigns the mutex to it.*/
+    tp = fifo_remove(&ump->m_queue);
+    ump->m_owner = tp;
+    ump->m_next = tp->p_mtxlist;
+    tp->p_mtxlist = ump;
+    chSchReadyI(tp);
   }
+  else
+    ump->m_owner = NULL;
   return ump;
 }
 
@@ -296,11 +318,17 @@ void chMtxUnlockAll(void) {
   chSysLock();
   if (ctp->p_mtxlist != NULL) {
     do {
-      Mutex *mp = ctp->p_mtxlist;
-      ctp->p_mtxlist = mp->m_next;
-      mp->m_owner = NULL;
-      if (chMtxQueueNotEmptyS(mp))
-        chSchReadyI(fifo_remove(&mp->m_queue));
+      Mutex *ump = ctp->p_mtxlist;
+      ctp->p_mtxlist = ump->m_next;
+      if (chMtxQueueNotEmptyS(ump)) {
+        Thread *tp = fifo_remove(&ump->m_queue);
+        ump->m_owner = tp;
+        ump->m_next = tp->p_mtxlist;
+        tp->p_mtxlist = ump;
+        chSchReadyI(tp);
+      }
+      else
+        ump->m_owner = NULL;
     } while (ctp->p_mtxlist != NULL);
     ctp->p_prio = ctp->p_realprio;
     chSchRescheduleS();
-- 
cgit v1.2.3