Maintain legacy RTOS behavior before mbed-5
Fork of mbed-rtos by
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); }