static inline CORE_mutex_Status _CORE_mutex_Pop_priority(
CORE_mutex_Control *mutex,
Thread_Control *holder
)
{
/*
* Check whether the holder release the mutex in LIFO order if not return
* error code.
*/
if ( _Chain_First( holder->lock_mutex ) != &mutex->queue.lock_queue ) {
mutex->nest_count++;
return CORE_MUTEX_RELEASE_NOT_ORDER;
}
/*
* This pops the first node from the list.
*/
_Chain_Get_first_unprotected( &holder->lock_mutex );
if ( mutex->queue.priority_before != holder->current_priority )
_Thread_Change_priority( holder, mutex->queue.priority_before, true );
return CORE_MUTEX_STATUS_SUCCESSFUL;
}
Thread_Control *_Thread_queue_Dequeue_fifo(
Thread_queue_Control *the_thread_queue
)
{
ISR_Level level;
Thread_Control *the_thread;
_ISR_Disable( level );
if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) ) {
the_thread = (Thread_Control *)
_Chain_Get_first_unprotected( &the_thread_queue->Queues.Fifo );
the_thread->Wait.queue = NULL;
if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
_ISR_Enable( level );
_Thread_Unblock( the_thread );
} else {
_Watchdog_Deactivate( &the_thread->Timer );
_ISR_Enable( level );
(void) _Watchdog_Remove( &the_thread->Timer );
_Thread_Unblock( the_thread );
}
#if defined(RTEMS_MULTIPROCESSING)
if ( !_Objects_Is_local_id( the_thread->Object.id ) )
_Thread_MP_Free_proxy( the_thread );
#endif
return the_thread;
}
_ISR_Enable( level );
return NULL;
}
Thread_Control *_Thread_queue_Dequeue(
Thread_queue_Control *the_thread_queue
)
{
Thread_Control *the_thread;
ISR_lock_Context lock_context;
Thread_blocking_operation_States sync_state;
the_thread = NULL;
_Thread_queue_Acquire( &lock_context );
/*
* Invoke the discipline specific dequeue method.
*/
if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) ) {
the_thread = (Thread_Control *)
_Chain_Get_first_unprotected( &the_thread_queue->Queues.Fifo );
}
} else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
RBTree_Node *first;
first = _RBTree_Get( &the_thread_queue->Queues.Priority, RBT_LEFT );
if ( first ) {
the_thread = THREAD_RBTREE_NODE_TO_THREAD( first );
_Thread_Priority_restore_default_change_handler( the_thread );
_Thread_Lock_restore_default( the_thread );
}
}
if ( the_thread == NULL ) {
/*
* We did not find a thread to unblock in the queue. Maybe the executing
* thread is about to block on this thread queue.
*/
sync_state = the_thread_queue->sync_state;
if ( (sync_state == THREAD_BLOCKING_OPERATION_TIMEOUT) ||
(sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) ) {
the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SATISFIED;
the_thread = _Thread_Executing;
} else {
_Thread_queue_Release( &lock_context );
return NULL;
}
}
/*
* We found a thread to unblock.
*
* NOTE: This is invoked with interrupts still disabled.
*/
_Thread_blocking_operation_Finalize( the_thread, &lock_context );
return the_thread;
}
Chain_Node *_Chain_Get(
Chain_Control *the_chain
)
{
Chain_Node *return_node;
return_node = NULL;
if ( !_Chain_Is_empty( the_chain ) )
return_node = _Chain_Get_first_unprotected( the_chain );
return return_node;
}
Thread_Control *_Thread_queue_Dequeue(
Thread_queue_Control *the_thread_queue
)
{
Thread_Control *the_thread;
ISR_Level level;
Thread_blocking_operation_States sync_state;
the_thread = NULL;
_ISR_Disable( level );
/*
* Invoke the discipline specific dequeue method.
*/
if ( the_thread_queue->discipline == THREAD_QUEUE_DISCIPLINE_FIFO ) {
if ( !_Chain_Is_empty( &the_thread_queue->Queues.Fifo ) ) {
the_thread = (Thread_Control *)
_Chain_Get_first_unprotected( &the_thread_queue->Queues.Fifo );
}
} else { /* must be THREAD_QUEUE_DISCIPLINE_PRIORITY */
RBTree_Node *first;
first = _RBTree_Get( &the_thread_queue->Queues.Priority, RBT_LEFT );
if ( first ) {
the_thread = THREAD_RBTREE_NODE_TO_THREAD( first );
}
}
/*
* We did not find a thread to unblock.
*/
if ( !the_thread ) {
sync_state = the_thread_queue->sync_state;
if ( (sync_state == THREAD_BLOCKING_OPERATION_TIMEOUT) ||
(sync_state == THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED) ) {
the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SATISFIED;
the_thread = _Thread_Executing;
}
_ISR_Enable( level );
return NULL;
}
/*
* We found a thread to unblock.
*
* NOTE: This is invoked with interrupts still disabled.
*/
_Thread_blocking_operation_Finalize( the_thread, level );
return the_thread;
}
Chain_Node *_Chain_Get(
Chain_Control *the_chain
)
{
ISR_Level level;
Chain_Node *return_node;
return_node = NULL;
_ISR_Disable( level );
if ( !_Chain_Is_empty( the_chain ) )
return_node = _Chain_Get_first_unprotected( the_chain );
_ISR_Enable( level );
return return_node;
}
static void _Thread_queue_Queue_extract(
Thread_queue_Queue *queue,
Thread_queue_Heads *heads,
Thread_Control *the_thread,
void ( *extract )( Thread_queue_Heads *, Thread_Control * )
)
{
_Assert( heads != NULL );
the_thread->Wait.spare_heads = RTEMS_CONTAINER_OF(
_Chain_Get_first_unprotected( &heads->Free_chain ),
Thread_queue_Heads,
Free_node
);
if ( _Chain_Is_empty( &heads->Free_chain ) ) {
queue->heads = NULL;
}
( *extract )( heads, the_thread );
}
CORE_mutex_Status _CORE_mutex_Surrender(
CORE_mutex_Control *the_mutex,
#if defined(RTEMS_MULTIPROCESSING)
Objects_Id id,
CORE_mutex_API_mp_support_callout api_mutex_mp_support
#else
Objects_Id id __attribute__((unused)),
CORE_mutex_API_mp_support_callout api_mutex_mp_support __attribute__((unused))
#endif
)
{
Thread_Control *the_thread;
Thread_Control *holder;
#ifdef __RTEMS_STRICT_ORDER_MUTEX__
Chain_Node *first_node;
#endif
holder = the_mutex->holder;
/*
* The following code allows a thread (or ISR) other than the thread
* which acquired the mutex to release that mutex. This is only
* allowed when the mutex in quetion is FIFO or simple Priority
* discipline. But Priority Ceiling or Priority Inheritance mutexes
* must be released by the thread which acquired them.
*/
if ( the_mutex->Attributes.only_owner_release ) {
if ( !_Thread_Is_executing( holder ) )
return CORE_MUTEX_STATUS_NOT_OWNER_OF_RESOURCE;
}
/* XXX already unlocked -- not right status */
if ( !the_mutex->nest_count )
return CORE_MUTEX_STATUS_SUCCESSFUL;
the_mutex->nest_count--;
if ( the_mutex->nest_count != 0 ) {
/*
* All error checking is on the locking side, so if the lock was
* allowed to acquired multiple times, then we should just deal with
* that. The RTEMS_DEBUG is just a validation.
*/
#if defined(RTEMS_DEBUG)
switch ( the_mutex->Attributes.lock_nesting_behavior ) {
case CORE_MUTEX_NESTING_ACQUIRES:
return CORE_MUTEX_STATUS_SUCCESSFUL;
case CORE_MUTEX_NESTING_IS_ERROR:
/* should never occur */
return CORE_MUTEX_STATUS_NESTING_NOT_ALLOWED;
case CORE_MUTEX_NESTING_BLOCKS:
/* Currently no API exercises this behavior. */
break;
}
#else
/* must be CORE_MUTEX_NESTING_ACQUIRES or we wouldn't be here */
return CORE_MUTEX_STATUS_SUCCESSFUL;
#endif
}
/*
* Formally release the mutex before possibly transferring it to a
* blocked thread.
*/
if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ||
_CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) ){
#ifdef __RTEMS_STRICT_ORDER_MUTEX__
/*Check whether the holder release the mutex in LIFO order
if not return error code*/
if(holder->lock_mutex.first != &the_mutex->queue.lock_queue){
the_mutex->nest_count++;
return CORE_MUTEX_RELEASE_NOT_ORDER;
}
first_node = _Chain_Get_first_unprotected(&holder->lock_mutex);
#endif
holder->resource_count--;
}
the_mutex->holder = NULL;
the_mutex->holder_id = 0;
/*
* Whether or not someone is waiting for the mutex, an
* inherited priority must be lowered if this is the last
* mutex (i.e. resource) this task has.
*/
if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ||
_CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) ) {
#ifdef __RTEMS_STRICT_ORDER_MUTEX__
if(the_mutex->queue.priority_before != holder->current_priority)
_Thread_Change_priority(holder,the_mutex->queue.priority_before,true);
#endif
if ( holder->resource_count == 0 &&
holder->real_priority != holder->current_priority ) {
_Thread_Change_priority( holder, holder->real_priority, true );
}
}
/*
* Now we check if another thread was waiting for this mutex. If so,
* transfer the mutex to that thread.
*/
if ( ( the_thread = _Thread_queue_Dequeue( &the_mutex->Wait_queue ) ) ) {
#if defined(RTEMS_MULTIPROCESSING)
if ( !_Objects_Is_local_id( the_thread->Object.id ) ) {
the_mutex->holder = NULL;
the_mutex->holder_id = the_thread->Object.id;
the_mutex->nest_count = 1;
( *api_mutex_mp_support)( the_thread, id );
} else
#endif
{
the_mutex->holder = the_thread;
the_mutex->holder_id = the_thread->Object.id;
the_mutex->nest_count = 1;
switch ( the_mutex->Attributes.discipline ) {
case CORE_MUTEX_DISCIPLINES_FIFO:
case CORE_MUTEX_DISCIPLINES_PRIORITY:
break;
case CORE_MUTEX_DISCIPLINES_PRIORITY_INHERIT:
#ifdef __RTEMS_STRICT_ORDER_MUTEX__
_Chain_Prepend_unprotected(&the_thread->lock_mutex,&the_mutex->queue.lock_queue);
the_mutex->queue.priority_before = the_thread->current_priority;
#endif
the_thread->resource_count++;
break;
case CORE_MUTEX_DISCIPLINES_PRIORITY_CEILING:
#ifdef __RTEMS_STRICT_ORDER_MUTEX__
_Chain_Prepend_unprotected(&the_thread->lock_mutex,&the_mutex->queue.lock_queue);
the_mutex->queue.priority_before = the_thread->current_priority;
#endif
the_thread->resource_count++;
if (the_mutex->Attributes.priority_ceiling <
the_thread->current_priority){
_Thread_Change_priority(
the_thread,
the_mutex->Attributes.priority_ceiling,
false
);
}
break;
}
}
} else
the_mutex->lock = CORE_MUTEX_UNLOCKED;
return CORE_MUTEX_STATUS_SUCCESSFUL;
}
