Diff: rtx/TARGET_CORTEX_M/rt_Mutex.c

Revision:

112:53ace74b190c

Parent:

49:77c8e4604045

--- a/rtx/TARGET_CORTEX_M/rt_Mutex.c	Tue May 03 00:15:52 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/rt_Mutex.c	Thu May 05 20:45:13 2016 +0100
@@ -1,12 +1,12 @@
 /*----------------------------------------------------------------------------
- *      RL-ARM - RTX
+ *      CMSIS-RTOS  -  RTX
  *----------------------------------------------------------------------------
  *      Name:    RT_MUTEX.C
  *      Purpose: Implements mutex synchronization objects
- *      Rev.:    V4.60
+ *      Rev.:    V4.79
  *----------------------------------------------------------------------------
  *
- * Copyright (c) 1999-2009 KEIL, 2009-2012 ARM Germany GmbH
+ * Copyright (c) 1999-2009 KEIL, 2009-2015 ARM Germany GmbH
  * All rights reserved.
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -33,7 +33,7 @@
  *---------------------------------------------------------------------------*/
 
 #include "rt_TypeDef.h"
-#include "RTX_Conf.h"
+#include "RTX_Config.h"
 #include "rt_List.h"
 #include "rt_Task.h"
 #include "rt_Mutex.h"
@@ -52,10 +52,10 @@
   P_MUCB p_MCB = mutex;
 
   p_MCB->cb_type = MUCB;
-  p_MCB->prio    = 0;
-  p_MCB->level   = 0;
+  p_MCB->level   = 0U;
   p_MCB->p_lnk   = NULL;
   p_MCB->owner   = NULL;
+  p_MCB->p_mlnk  = NULL;
 }
 
 
@@ -66,32 +66,64 @@
   /* Delete a mutex object */
   P_MUCB p_MCB = mutex;
   P_TCB  p_TCB;
+  P_MUCB p_mlnk;
+  U8     prio;
 
-  /* Restore owner task's priority. */
-  if (p_MCB->level != 0) {
-    p_MCB->owner->prio = p_MCB->prio;
-    if (p_MCB->owner != os_tsk.run) {
-      rt_resort_prio (p_MCB->owner);
+  if (p_MCB->level != 0U) {
+
+    p_TCB = p_MCB->owner;
+
+    /* Remove mutex from task mutex owner list. */
+    p_mlnk = p_TCB->p_mlnk;
+    if (p_mlnk == p_MCB) {
+      p_TCB->p_mlnk = p_MCB->p_mlnk;
+    }
+    else {
+      while (p_mlnk) {
+        if (p_mlnk->p_mlnk == p_MCB) {
+          p_mlnk->p_mlnk = p_MCB->p_mlnk;
+          break;
+        }
+        p_mlnk = p_mlnk->p_mlnk;
+      }
     }
+
+    /* Restore owner task's priority. */
+    prio = p_TCB->prio_base;
+    p_mlnk = p_TCB->p_mlnk;
+    while (p_mlnk) {
+      if ((p_mlnk->p_lnk != NULL) && (p_mlnk->p_lnk->prio > prio)) {
+        /* A task with higher priority is waiting for mutex. */
+        prio = p_mlnk->p_lnk->prio;
+      }
+      p_mlnk = p_mlnk->p_mlnk;
+    }
+    if (p_TCB->prio != prio) {
+      p_TCB->prio = prio;
+      if (p_TCB != os_tsk.run) {
+        rt_resort_prio (p_TCB);
+      }
+    }
+
   }
 
   while (p_MCB->p_lnk != NULL) {
     /* A task is waiting for mutex. */
     p_TCB = rt_get_first ((P_XCB)p_MCB);
-    rt_ret_val(p_TCB, 0/*osOK*/);
+    rt_ret_val(p_TCB, 0U/*osOK*/);
     rt_rmv_dly(p_TCB);
     p_TCB->state = READY;
     rt_put_prio (&os_rdy, p_TCB);
   }
 
-  if (os_rdy.p_lnk && (os_rdy.p_lnk->prio > os_tsk.run->prio)) {
+  if ((os_rdy.p_lnk != NULL) && (os_rdy.p_lnk->prio > os_tsk.run->prio)) {
     /* preempt running task */
     rt_put_prio (&os_rdy, os_tsk.run);
     os_tsk.run->state = READY;
     rt_dispatch (NULL);
   }
 
-  p_MCB->cb_type = 0;
+  p_MCB->cb_type = 0U;
 
   return (OS_R_OK);
 }
@@ -104,29 +136,58 @@
   /* Release a mutex object */
   P_MUCB p_MCB = mutex;
   P_TCB  p_TCB;
+  P_MUCB p_mlnk;
+  U8     prio;
 
-  if (p_MCB->level == 0 || p_MCB->owner != os_tsk.run) {
+  if ((p_MCB->level == 0U) || (p_MCB->owner != os_tsk.run)) {
     /* Unbalanced mutex release or task is not the owner */
     return (OS_R_NOK);
   }
-  if (--p_MCB->level != 0) {
+  if (--p_MCB->level != 0U) {
     return (OS_R_OK);
   }
+
+  /* Remove mutex from task mutex owner list. */
+  p_mlnk = os_tsk.run->p_mlnk;
+  if (p_mlnk == p_MCB) {
+    os_tsk.run->p_mlnk = p_MCB->p_mlnk;
+  }
+  else {
+    while (p_mlnk) {
+      if (p_mlnk->p_mlnk == p_MCB) {
+        p_mlnk->p_mlnk = p_MCB->p_mlnk;
+        break;
+      }
+      p_mlnk = p_mlnk->p_mlnk;
+    }
+  }
+
   /* Restore owner task's priority. */
-  os_tsk.run->prio = p_MCB->prio;
+  prio = os_tsk.run->prio_base;
+  p_mlnk = os_tsk.run->p_mlnk;
+  while (p_mlnk) {
+    if ((p_mlnk->p_lnk != NULL) && (p_mlnk->p_lnk->prio > prio)) {
+      /* A task with higher priority is waiting for mutex. */
+      prio = p_mlnk->p_lnk->prio;
+    }
+    p_mlnk = p_mlnk->p_mlnk;
+  }
+  os_tsk.run->prio = prio;
+
   if (p_MCB->p_lnk != NULL) {
     /* A task is waiting for mutex. */
     p_TCB = rt_get_first ((P_XCB)p_MCB);
 #ifdef __CMSIS_RTOS
-    rt_ret_val(p_TCB, 0/*osOK*/);
+    rt_ret_val(p_TCB, 0U/*osOK*/);
 #else
-    rt_ret_val(p_TCB, OS_R_MUT);
+    rt_ret_val(p_TCB, OS_R_MUT); 
 #endif
     rt_rmv_dly (p_TCB);
     /* A waiting task becomes the owner of this mutex. */
-    p_MCB->level     = 1;
-    p_MCB->owner     = p_TCB;
-    p_MCB->prio      = p_TCB->prio;
+    p_MCB->level  = 1U;
+    p_MCB->owner  = p_TCB;
+    p_MCB->p_mlnk = p_TCB->p_mlnk;
+    p_TCB->p_mlnk = p_MCB; 
     /* Priority inversion, check which task continues. */
     if (os_tsk.run->prio >= rt_rdy_prio()) {
       rt_dispatch (p_TCB);
@@ -141,7 +202,7 @@
     }
   }
   else {
-    /* Check if own priority raised by priority inversion. */
+    /* Check if own priority lowered by priority inversion. */
     if (rt_rdy_prio() > os_tsk.run->prio) {
       rt_put_prio (&os_rdy, os_tsk.run);
       os_tsk.run->state = READY;
@@ -158,9 +219,10 @@
   /* Wait for a mutex, continue when mutex is free. */
   P_MUCB p_MCB = mutex;
 
-  if (p_MCB->level == 0) {
-    p_MCB->owner = os_tsk.run;
-    p_MCB->prio  = os_tsk.run->prio;
+  if (p_MCB->level == 0U) {
+    p_MCB->owner  = os_tsk.run;
+    p_MCB->p_mlnk = os_tsk.run->p_mlnk;
+    os_tsk.run->p_mlnk = p_MCB; 
     goto inc;
   }
   if (p_MCB->owner == os_tsk.run) {
@@ -169,12 +231,12 @@
     return (OS_R_OK);
   }
   /* Mutex owned by another task, wait until released. */
-  if (timeout == 0) {
+  if (timeout == 0U) {
     return (OS_R_TMO);
   }
   /* Raise the owner task priority if lower than current priority. */
   /* This priority inversion is called priority inheritance.       */
-  if (p_MCB->prio < os_tsk.run->prio) {
+  if (p_MCB->owner->prio < os_tsk.run->prio) {
     p_MCB->owner->prio = os_tsk.run->prio;
     rt_resort_prio (p_MCB->owner);
   }