Enhanced priority dis-inheritance functionality in the case where a task that caused...

[freertos] / FreeRTOS / Source / queue.c

diff --git a/FreeRTOS/Source/queue.c b/FreeRTOS/Source/queue.c
index 7325148b48d546121750b0c874029af2aaa5526a..7b2df093b6a116266cb5af6f939f77d362c6a4a0 100644 (file)
--- a/FreeRTOS/Source/queue.c
+++ b/FreeRTOS/Source/queue.c
@@ -255,6 +255,16 @@ static void prvInitialiseNewQueue( const UBaseType_t uxQueueLength, const UBaseT
        static void prvInitialiseMutex( Queue_t *pxNewQueue ) PRIVILEGED_FUNCTION;\r
 #endif\r
 \r
+#if( configUSE_MUTEXES == 1 )\r
+       /*\r
+        * If a task waiting for a mutex causes the mutex holder to inherit a\r
+        * priority, but the waiting task times out, then the holder should\r
+        * disinherit the priority - but only down to the highest priority of any\r
+        * other tasks that are waiting for the same mutex.  This function returns\r
+        * that priority.\r
+        */\r
+       static UBaseType_t prvGetDisinheritPriorityAfterTimeout( const Queue_t * const pxQueue ) PRIVILEGED_FUNCTION;\r
+#endif\r
 /*-----------------------------------------------------------*/\r
 \r
 /*\r
@@ -1408,6 +1418,10 @@ BaseType_t xEntryTimeSet = pdFALSE;
 TimeOut_t xTimeOut;\r
 Queue_t * const pxQueue = ( Queue_t * ) xQueue;\r
 \r
+#if( configUSE_MUTEXES == 1 )\r
+       BaseType_t xInheritanceOccurred = pdFALSE;\r
+#endif\r
+\r
        /* Check the queue pointer is not NULL. */\r
        configASSERT( ( pxQueue ) );\r
 \r
@@ -1485,6 +1499,11 @@ Queue_t * const pxQueue = ( Queue_t * ) xQueue;
                        {\r
                                if( xTicksToWait == ( TickType_t ) 0 )\r
                                {\r
+                                       /* For inheritance to have occurred there must have been an\r
+                                       initial timeout, and an adjusted timeout cannot become 0, as\r
+                                       if it were 0 the function would have exited. */\r
+                                       configASSERT( xInheritanceOccurred == pdFALSE );\r
+\r
                                        /* The semaphore count was 0 and no block time is specified\r
                                        (or the block time has expired) so exit now. */\r
                                        taskEXIT_CRITICAL();\r
@@ -1530,7 +1549,7 @@ Queue_t * const pxQueue = ( Queue_t * ) xQueue;
                                        {\r
                                                taskENTER_CRITICAL();\r
                                                {\r
-                                                       xTaskPriorityInherit( ( void * ) pxQueue->pxMutexHolder );\r
+                                                       xInheritanceOccurred = xTaskPriorityInherit( ( void * ) pxQueue->pxMutexHolder );\r
                                                }\r
                                                taskEXIT_CRITICAL();\r
                                        }\r
@@ -1572,6 +1591,30 @@ Queue_t * const pxQueue = ( Queue_t * ) xQueue;
                        queue being empty is equivalent to the semaphore count being 0. */\r
                        if( prvIsQueueEmpty( pxQueue ) != pdFALSE )\r
                        {\r
+                               #if ( configUSE_MUTEXES == 1 )\r
+                               {\r
+                                       /* xInheritanceOccurred could only have be set if\r
+                                       pxQueue->uxQueueType == queueQUEUE_IS_MUTEX so no need to\r
+                                       test the mutex type again to check it is actually a mutex. */\r
+                                       if( xInheritanceOccurred != pdFALSE )\r
+                                       {\r
+                                               taskENTER_CRITICAL();\r
+                                               {\r
+                                                       UBaseType_t uxHighestWaitingPriority;\r
+\r
+                                                       /* This task blocking on the mutex caused another\r
+                                                       task to inherit this task's priority.  Now this task\r
+                                                       has timed out the priority should be disinherited\r
+                                                       again, but only as low as the next highest priority\r
+                                                       task that is waiting for the same mutex. */\r
+                                                       uxHighestWaitingPriority = prvGetDisinheritPriorityAfterTimeout( pxQueue );\r
+                                                       vTaskPriorityDisinheritAfterTimeout( ( void * ) pxQueue->pxMutexHolder, uxHighestWaitingPriority );\r
+                                               }\r
+                                               taskEXIT_CRITICAL();\r
+                                       }\r
+                               }\r
+                               #endif /* configUSE_MUTEXES */\r
+\r
                                traceQUEUE_RECEIVE_FAILED( pxQueue );\r
                                return errQUEUE_EMPTY;\r
                        }\r
@@ -1997,6 +2040,33 @@ Queue_t * const pxQueue = ( Queue_t * ) xQueue;
 #endif /* configUSE_TRACE_FACILITY */\r
 /*-----------------------------------------------------------*/\r
 \r
+#if( configUSE_MUTEXES == 1 )\r
+\r
+       static UBaseType_t prvGetDisinheritPriorityAfterTimeout( const Queue_t * const pxQueue )\r
+       {\r
+       UBaseType_t uxHighestPriorityOfWaitingTasks;\r
+\r
+               /* If a task waiting for a mutex causes the mutex holder to inherit a\r
+               priority, but the waiting task times out, then the holder should\r
+               disinherit the priority - but only down to the highest priority of any\r
+               other tasks that are waiting for the same mutex.  For this purpose,\r
+               return the priority of the highest priority task that is waiting for the\r
+               mutex. */\r
+               if( listCURRENT_LIST_LENGTH( &( pxQueue->xTasksWaitingToReceive ) ) > 0 )\r
+               {\r
+                       uxHighestPriorityOfWaitingTasks = configMAX_PRIORITIES - listGET_ITEM_VALUE_OF_HEAD_ENTRY( &( pxQueue->xTasksWaitingToReceive ) );\r
+               }\r
+               else\r
+               {\r
+                       uxHighestPriorityOfWaitingTasks = tskIDLE_PRIORITY;\r
+               }\r
+\r
+               return uxHighestPriorityOfWaitingTasks;\r
+       }\r
+\r
+#endif /* configUSE_MUTEXES */\r
+/*-----------------------------------------------------------*/\r
+\r
 static BaseType_t prvCopyDataToQueue( Queue_t * const pxQueue, const void *pvItemToQueue, const BaseType_t xPosition )\r
 {\r
 BaseType_t xReturn = pdFALSE;\r