Official mbed Real Time Operating System based on the RTX implementation of the CMSIS-RTOS API open standard.
Fork of mbed-rtos by
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); }