Diff: rtx/TARGET_CORTEX_A/rt_Mutex.c

Revision:

92:bc9729798a19

Parent:

68:d3d0e710b443

--- a/rtx/TARGET_CORTEX_A/rt_Mutex.c	Wed Sep 16 11:15:38 2015 +0100
+++ b/rtx/TARGET_CORTEX_A/rt_Mutex.c	Fri Sep 25 13:30:34 2015 +0100
@@ -3,10 +3,10 @@
  *----------------------------------------------------------------------------
  *      Name:    RT_MUTEX.C
  *      Purpose: Implements mutex synchronization objects
- *      Rev.:    V4.60
+ *      Rev.:    V4.73
  *----------------------------------------------------------------------------
  *
- * Copyright (c) 1999-2009 KEIL, 2009-2012 ARM Germany GmbH
+ * Copyright (c) 1999-2009 KEIL, 2009-2013 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:
@@ -56,10 +56,10 @@
   P_MUCB p_MCB = mutex;
 
   p_MCB->cb_type = MUCB;
-  p_MCB->prio    = 0;
   p_MCB->level   = 0;
   p_MCB->p_lnk   = NULL;
   p_MCB->owner   = NULL;
+  p_MCB->p_mlnk  = NULL;
 }
 
 
@@ -70,14 +70,47 @@
   /* Delete a mutex object */
   P_MUCB p_MCB = mutex;
   P_TCB  p_TCB;
+  P_MUCB p_mlnk;
+  U8     prio;
 
   __DMB();
   /* 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);
+
+    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 && (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) {
@@ -109,6 +142,8 @@
   /* 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) {
     /* Unbalanced mutex release or task is not the owner */
@@ -118,8 +153,34 @@
   if (--p_MCB->level != 0) {
     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 && (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);
@@ -132,7 +193,8 @@
     /* 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->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);
@@ -147,7 +209,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;
@@ -166,7 +228,8 @@
 
   if (p_MCB->level == 0) {
     p_MCB->owner = os_tsk.run;
-    p_MCB->prio  = os_tsk.run->prio;
+    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) {
@@ -181,7 +244,7 @@
   }
   /* 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);
   }