diff options
Diffstat (limited to 'cpukit/score/src/threadqops.c')
| -rw-r--r-- | cpukit/score/src/threadqops.c | 847 |
1 files changed, 661 insertions, 186 deletions
diff --git a/cpukit/score/src/threadqops.c b/cpukit/score/src/threadqops.c index e20241d494..602d6bbffb 100644 --- a/cpukit/score/src/threadqops.c +++ b/cpukit/score/src/threadqops.c @@ -22,31 +22,58 @@ #include <rtems/score/rbtreeimpl.h> #include <rtems/score/schedulerimpl.h> -static void _Thread_queue_Do_nothing_priority_change( +#define THREAD_QUEUE_CONTEXT_OF_PRIORITY_ACTIONS( priority_actions ) \ + RTEMS_CONTAINER_OF( \ + priority_actions, \ + Thread_queue_Context, \ + Priority.Actions \ + ) + +#define THREAD_QUEUE_PRIORITY_QUEUE_OF_PRIORITY_AGGREGATION( \ + priority_aggregation \ +) \ + RTEMS_CONTAINER_OF( \ + priority_aggregation, \ + Thread_queue_Priority_queue, \ + Queue \ + ) + +static void _Thread_queue_Do_nothing_priority_actions( Thread_queue_Queue *queue, - Thread_Control *the_thread, - Priority_Control new_priority + Priority_Actions *priority_actions ) { (void) queue; - (void) the_thread; - (void) new_priority; + _Priority_Actions_initialize_empty( priority_actions ); } static void _Thread_queue_Do_nothing_extract( - Thread_queue_Queue *queue, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context ) { (void) queue; (void) the_thread; + (void) queue_context; } -static Thread_queue_Heads *_Thread_queue_Queue_enqueue( - Thread_queue_Queue *queue, - Thread_Control *the_thread, - void ( *initialize )( Thread_queue_Heads *, Thread_Control * ), - void ( *enqueue )( Thread_queue_Heads *, Thread_Control * ) +static void _Thread_queue_Queue_enqueue( + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + void ( *initialize )( + Thread_queue_Queue *, + Thread_Control *, + Thread_queue_Context *, + Thread_queue_Heads * + ), + void ( *enqueue )( + Thread_queue_Queue *, + Thread_Control *, + Thread_queue_Context *, + Thread_queue_Heads * + ) ) { Thread_queue_Heads *heads; @@ -63,20 +90,26 @@ static Thread_queue_Heads *_Thread_queue_Queue_enqueue( heads = spare_heads; queue->heads = heads; _Chain_Prepend_unprotected( &heads->Free_chain, &spare_heads->Free_node ); - ( *initialize )( heads, the_thread ); + ( *initialize )( queue, the_thread, queue_context, heads ); } else { _Chain_Prepend_unprotected( &heads->Free_chain, &spare_heads->Free_node ); - ( *enqueue )( heads, the_thread ); + ( *enqueue )( queue, the_thread, queue_context, heads ); } - - return heads; } 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 * ) + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *current_or_previous_owner, + Thread_queue_Context *queue_context, + Thread_Control *the_thread, + void ( *extract )( + Thread_queue_Queue *, + Thread_queue_Heads *, + Thread_Control *, + Thread_queue_Context *, + Thread_Control * + ) ) { _Assert( heads != NULL ); @@ -91,76 +124,95 @@ static void _Thread_queue_Queue_extract( queue->heads = NULL; } - ( *extract )( heads, the_thread ); + ( *extract )( + queue, + heads, + current_or_previous_owner, + queue_context, + the_thread + ); } static void _Thread_queue_FIFO_do_initialize( - Thread_queue_Heads *heads, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + Thread_queue_Heads *heads ) { Scheduler_Node *scheduler_node; - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); - _Chain_Initialize_node( &scheduler_node->Wait.Node.Chain ); + _Chain_Initialize_node( &scheduler_node->Wait.Priority.Node.Node.Chain ); _Chain_Initialize_one( &heads->Heads.Fifo, - &scheduler_node->Wait.Node.Chain + &scheduler_node->Wait.Priority.Node.Node.Chain ); } static void _Thread_queue_FIFO_do_enqueue( - Thread_queue_Heads *heads, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + Thread_queue_Heads *heads ) { Scheduler_Node *scheduler_node; - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); - _Chain_Initialize_node( &scheduler_node->Wait.Node.Chain ); + _Chain_Initialize_node( &scheduler_node->Wait.Priority.Node.Node.Chain ); _Chain_Append_unprotected( &heads->Heads.Fifo, - &scheduler_node->Wait.Node.Chain + &scheduler_node->Wait.Priority.Node.Node.Chain ); } static void _Thread_queue_FIFO_do_extract( - Thread_queue_Heads *heads, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *current_or_previous_owner, + Thread_queue_Context *queue_context, + Thread_Control *the_thread ) { Scheduler_Node *scheduler_node; - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); - _Chain_Extract_unprotected( &scheduler_node->Wait.Node.Chain ); + (void) current_or_previous_owner; + (void) queue_context; + + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + + _Chain_Extract_unprotected( &scheduler_node->Wait.Priority.Node.Node.Chain ); } static void _Thread_queue_FIFO_enqueue( - Thread_queue_Queue *queue, - Thread_Control *the_thread, - Thread_queue_Path *path + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context ) { - path->update_priority = NULL; - _Thread_queue_Queue_enqueue( queue, the_thread, + queue_context, _Thread_queue_FIFO_do_initialize, _Thread_queue_FIFO_do_enqueue ); } static void _Thread_queue_FIFO_extract( - Thread_queue_Queue *queue, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context ) { _Thread_queue_Queue_extract( queue, queue->heads, + NULL, + queue_context, the_thread, _Thread_queue_FIFO_do_extract ); @@ -177,15 +229,16 @@ static Thread_Control *_Thread_queue_FIFO_first( fifo = &heads->Heads.Fifo; _Assert( !_Chain_Is_empty( fifo ) ); first = _Chain_First( fifo ); - scheduler_node = SCHEDULER_NODE_OF_WAIT_CHAIN_NODE( first ); + scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY_NODE( first ); return _Scheduler_Node_get_owner( scheduler_node ); } static Thread_Control *_Thread_queue_FIFO_surrender( - Thread_queue_Queue *queue, - Thread_queue_Heads *heads, - Thread_Control *previous_owner + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *previous_owner, + Thread_queue_Context *queue_context ) { Thread_Control *first; @@ -194,6 +247,8 @@ static Thread_Control *_Thread_queue_FIFO_surrender( _Thread_queue_Queue_extract( queue, heads, + NULL, + queue_context, first, _Thread_queue_FIFO_do_extract ); @@ -203,165 +258,162 @@ static Thread_Control *_Thread_queue_FIFO_surrender( static Thread_queue_Priority_queue *_Thread_queue_Priority_queue( Thread_queue_Heads *heads, - const Thread_Control *the_thread + const Scheduler_Node *scheduler_node ) { #if defined(RTEMS_SMP) - return &heads->Priority[ - _Scheduler_Get_index( _Scheduler_Get_own( the_thread ) ) - ]; -#else - (void) the_thread; + const Scheduler_Control *scheduler; + scheduler = _Priority_Get_scheduler( &scheduler_node->Wait.Priority ); + return &heads->Priority[ _Scheduler_Get_index( scheduler ) ]; +#else + (void) scheduler_node; return &heads->Heads.Priority; #endif } -static bool _Thread_queue_Priority_less( - const void *left, - const RBTree_Node *right -) -{ - const Priority_Control *the_left; - const Scheduler_Node *scheduler_node; - const Thread_Control *the_right; - - the_left = left; - scheduler_node = SCHEDULER_NODE_OF_WAIT_RBTREE_NODE( right ); - the_right = _Scheduler_Node_get_owner( scheduler_node ); - - return *the_left < _Thread_Get_priority( the_right ); -} - -static void _Thread_queue_Priority_priority_change( +static void _Thread_queue_Priority_priority_actions( Thread_queue_Queue *queue, - Thread_Control *the_thread, - Priority_Control new_priority + Priority_Actions *priority_actions ) { - Thread_queue_Heads *heads; - Thread_queue_Priority_queue *priority_queue; - Scheduler_Node *scheduler_node; + Thread_queue_Heads *heads; + Priority_Aggregation *priority_aggregation; heads = queue->heads; _Assert( heads != NULL ); - priority_queue = _Thread_queue_Priority_queue( heads, the_thread ); - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); + _Assert( !_Priority_Actions_is_empty( priority_actions ) ); + priority_aggregation = _Priority_Actions_move( priority_actions ); - _RBTree_Extract( - &priority_queue->Queue, - &scheduler_node->Wait.Node.RBTree - ); - _RBTree_Insert_inline( - &priority_queue->Queue, - &scheduler_node->Wait.Node.RBTree, - &new_priority, - _Thread_queue_Priority_less - ); + do { + Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; + + scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY( priority_aggregation ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); + + _Assert( priority_aggregation->Action.type == PRIORITY_ACTION_CHANGE ); + _Priority_Plain_changed( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node + ); + + priority_aggregation = _Priority_Get_next_action( priority_aggregation ); + } while ( _Priority_Actions_is_valid( priority_aggregation ) ); } static void _Thread_queue_Priority_do_initialize( - Thread_queue_Heads *heads, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + Thread_queue_Heads *heads ) { - Thread_queue_Priority_queue *priority_queue; Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; - priority_queue = _Thread_queue_Priority_queue( heads, the_thread ); + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); #if defined(RTEMS_SMP) _Chain_Initialize_one( &heads->Heads.Fifo, &priority_queue->Node ); #endif - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); - - _RBTree_Initialize_node( &scheduler_node->Wait.Node.RBTree ); - _RBTree_Initialize_one( + _Priority_Initialize_one( &priority_queue->Queue, - &scheduler_node->Wait.Node.RBTree + &scheduler_node->Wait.Priority.Node ); } static void _Thread_queue_Priority_do_enqueue( - Thread_queue_Heads *heads, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + Thread_queue_Heads *heads ) { - Thread_queue_Priority_queue *priority_queue; Scheduler_Node *scheduler_node; - Priority_Control current_priority; + Thread_queue_Priority_queue *priority_queue; - priority_queue = _Thread_queue_Priority_queue( heads, the_thread ); + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); #if defined(RTEMS_SMP) - if ( _RBTree_Is_empty( &priority_queue->Queue ) ) { + if ( _Priority_Is_empty( &priority_queue->Queue ) ) { _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node ); + _Priority_Initialize_one( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node + ); + return; } #endif - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); - current_priority = _Thread_Get_priority( the_thread ); - - _RBTree_Initialize_node( &scheduler_node->Wait.Node.RBTree ); - _RBTree_Insert_inline( + _Priority_Plain_insert( &priority_queue->Queue, - &scheduler_node->Wait.Node.RBTree, - ¤t_priority, - _Thread_queue_Priority_less + &scheduler_node->Wait.Priority.Node, + _Priority_Get_priority( &scheduler_node->Wait.Priority ) ); } static void _Thread_queue_Priority_do_extract( - Thread_queue_Heads *heads, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *current_or_previous_owner, + Thread_queue_Context *queue_context, + Thread_Control *the_thread ) { - Thread_queue_Priority_queue *priority_queue; Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; + + (void) current_or_previous_owner; + (void) queue_context; - priority_queue = _Thread_queue_Priority_queue( heads, the_thread ); - scheduler_node = _Scheduler_Thread_get_own_node( the_thread ); + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); - _RBTree_Extract( + _Priority_Plain_extract( &priority_queue->Queue, - &scheduler_node->Wait.Node.RBTree + &scheduler_node->Wait.Priority.Node ); #if defined(RTEMS_SMP) _Chain_Extract_unprotected( &priority_queue->Node ); - if ( !_RBTree_Is_empty( &priority_queue->Queue ) ) { + if ( !_Priority_Is_empty( &priority_queue->Queue ) ) { _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node ); } #endif } static void _Thread_queue_Priority_enqueue( - Thread_queue_Queue *queue, - Thread_Control *the_thread, - Thread_queue_Path *path + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context ) { - path->update_priority = NULL; - _Thread_queue_Queue_enqueue( queue, the_thread, + queue_context, _Thread_queue_Priority_do_initialize, _Thread_queue_Priority_do_enqueue ); } static void _Thread_queue_Priority_extract( - Thread_queue_Queue *queue, - Thread_Control *the_thread + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context ) { _Thread_queue_Queue_extract( queue, queue->heads, + NULL, + queue_context, the_thread, _Thread_queue_Priority_do_extract ); @@ -372,7 +424,7 @@ static Thread_Control *_Thread_queue_Priority_first( ) { Thread_queue_Priority_queue *priority_queue; - RBTree_Node *first; + Priority_Node *first; Scheduler_Node *scheduler_node; #if defined(RTEMS_SMP) @@ -383,17 +435,18 @@ static Thread_Control *_Thread_queue_Priority_first( priority_queue = &heads->Heads.Priority; #endif - _Assert( !_RBTree_Is_empty( &priority_queue->Queue ) ); - first = _RBTree_Minimum( &priority_queue->Queue ); - scheduler_node = SCHEDULER_NODE_OF_WAIT_RBTREE_NODE( first ); + _Assert( !_Priority_Is_empty( &priority_queue->Queue ) ); + first = _Priority_Get_minimum_node( &priority_queue->Queue ); + scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY_NODE( first ); return _Scheduler_Node_get_owner( scheduler_node ); } static Thread_Control *_Thread_queue_Priority_surrender( - Thread_queue_Queue *queue, - Thread_queue_Heads *heads, - Thread_Control *previous_owner + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *previous_owner, + Thread_queue_Context *queue_context ) { Thread_Control *first; @@ -402,6 +455,8 @@ static Thread_Control *_Thread_queue_Priority_surrender( _Thread_queue_Queue_extract( queue, heads, + NULL, + queue_context, first, _Thread_queue_Priority_do_extract ); @@ -409,104 +464,524 @@ static Thread_Control *_Thread_queue_Priority_surrender( return first; } -static void _Thread_queue_Priority_inherit_enqueue( - Thread_queue_Queue *queue, - Thread_Control *the_thread, - Thread_queue_Path *path +static void _Thread_queue_Priority_inherit_do_actions_change( + Priority_Aggregation *priority_aggregation, + bool prepend_it, + Priority_Actions *priority_actions, + void *arg ) { - Thread_queue_Heads *heads; - Thread_Control *owner; - Priority_Control priority; + Thread_queue_Priority_queue *priority_queue; + Scheduler_Node *scheduler_node; - heads = _Thread_queue_Queue_enqueue( - queue, - the_thread, - _Thread_queue_Priority_do_initialize, - _Thread_queue_Priority_do_enqueue + priority_queue = THREAD_QUEUE_PRIORITY_QUEUE_OF_PRIORITY_AGGREGATION( + priority_aggregation ); + scheduler_node = priority_queue->scheduler_node; - owner = queue->owner; + _Priority_Set_action( + &scheduler_node->Wait.Priority, + &priority_aggregation->Node, + PRIORITY_ACTION_CHANGE + ); + _Priority_Actions_add( priority_actions, &scheduler_node->Wait.Priority ); +} -#if defined(RTEMS_SMP) - if ( _Chain_Has_only_one_node( &heads->Heads.Fifo ) ) { - priority = the_thread->current_priority; - } else { - priority = _Scheduler_Map_priority( - _Scheduler_Get_own( the_thread ), - PRIORITY_PSEUDO_ISR +static void _Thread_queue_Priority_inherit_priority_actions( + Thread_queue_Queue *queue, + Priority_Actions *priority_actions +) +{ + Thread_queue_Heads *heads; + Priority_Aggregation *priority_aggregation; + + heads = queue->heads; + _Assert( heads != NULL ); + + _Assert( !_Priority_Actions_is_empty( priority_actions ) ); + priority_aggregation = _Priority_Actions_move( priority_actions ); + + do { + Priority_Aggregation *next_aggregation; + Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; + + next_aggregation = _Priority_Get_next_action( priority_aggregation ); + + scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY( priority_aggregation ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); + + _Assert( priority_aggregation->Action.type == PRIORITY_ACTION_CHANGE ); + _Priority_Changed( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node, + false, + priority_actions, + _Thread_queue_Priority_inherit_do_actions_change, + NULL ); + + priority_aggregation = next_aggregation; + } while ( _Priority_Actions_is_valid( priority_aggregation ) ); +} + +static void _Thread_queue_Boost_priority( + Thread_queue_Heads *heads, + Thread_Control *the_thread, + Thread_Control *owner, + Thread_queue_Context *queue_context +) +{ +#if defined(RTEMS_SMP) + const Scheduler_Control *scheduler; + const Scheduler_Control *scheduler_of_owner; + Scheduler_Node *scheduler_node_of_owner; + Priority_Control boost_priority; + + if ( _Priority_Node_is_active( &heads->Boost_priority ) ) { + return; } + + scheduler = _Scheduler_Get_own( the_thread ); + scheduler_of_owner = _Scheduler_Get_own( owner ); + + if ( scheduler == scheduler_of_owner ) { + return; + } + + scheduler_node_of_owner = _Thread_Scheduler_get_own_node( owner ); + + boost_priority = _Scheduler_Map_priority( + scheduler_of_owner, + PRIORITY_PSEUDO_ISR + ); + + _Priority_Node_initialize( &heads->Boost_priority, boost_priority ); + _Priority_Actions_initialize_one( + &queue_context->Priority.Actions, + &scheduler_node_of_owner->Wait.Priority, + &heads->Boost_priority, + PRIORITY_ACTION_ADD + ); + _Thread_Priority_perform_actions( owner, queue_context ); #else (void) heads; + (void) the_thread; + (void) owner; + (void) queue_context; +#endif +} + +static void _Thread_queue_Priority_inherit_do_initialize( + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + Thread_queue_Heads *heads +) +{ + Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; + Thread_Control *owner; + Scheduler_Node *scheduler_node_of_owner; + + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); - priority = the_thread->current_priority; +#if defined(RTEMS_SMP) + _Chain_Initialize_one( &heads->Heads.Fifo, &priority_queue->Node ); #endif - if ( priority < owner->current_priority ) { - path->update_priority = owner; + _Priority_Initialize_one( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node + ); - owner->priority_restore_hint = true; - _Atomic_Fence( ATOMIC_ORDER_ACQ_REL ); + owner = queue->owner; + scheduler_node_of_owner = _Thread_Scheduler_get_own_node( owner ); + priority_queue->scheduler_node = scheduler_node_of_owner; + + _Priority_Actions_initialize_one( + &queue_context->Priority.Actions, + &scheduler_node_of_owner->Wait.Priority, + &priority_queue->Queue.Node, + PRIORITY_ACTION_ADD + ); + _Thread_Priority_perform_actions( owner, queue_context ); + _Thread_queue_Boost_priority( heads, the_thread, owner, queue_context ); +} + +static void _Thread_queue_Priority_inherit_do_enqueue_change( + Priority_Aggregation *priority_aggregation, + bool prepend_it, + Priority_Actions *priority_actions, + void *arg +) +{ + Thread_queue_Queue *queue; + Thread_Control *owner; + Scheduler_Node *scheduler_node_of_owner; + Thread_queue_Context *queue_context; + + queue = arg; + owner = queue->owner; + scheduler_node_of_owner = _Thread_Scheduler_get_own_node( owner ); + queue_context = THREAD_QUEUE_CONTEXT_OF_PRIORITY_ACTIONS( priority_actions ); + + _Priority_Actions_initialize_one( + &queue_context->Priority.Actions, + &scheduler_node_of_owner->Wait.Priority, + &priority_aggregation->Node, + PRIORITY_ACTION_CHANGE + ); + _Thread_Priority_perform_actions( owner, queue_context ); +} + +static void _Thread_queue_Priority_inherit_do_enqueue( + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context, + Thread_queue_Heads *heads +) +{ + Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; - _Scheduler_Thread_set_priority( owner, priority, false ); + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); - ( *owner->Wait.operations->priority_change )( - owner->Wait.queue, +#if defined(RTEMS_SMP) + if ( _Priority_Is_empty( &priority_queue->Queue ) ) { + Thread_Control *owner; + Scheduler_Node *scheduler_node_of_owner; + + _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node ); + _Priority_Initialize_one( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node + ); + + owner = queue->owner; + scheduler_node_of_owner = _Thread_Scheduler_get_own_node( owner ); + priority_queue->scheduler_node = scheduler_node_of_owner; + + _Priority_Actions_initialize_one( + &queue_context->Priority.Actions, + &scheduler_node_of_owner->Wait.Priority, + &priority_queue->Queue.Node, + PRIORITY_ACTION_ADD + ); + _Thread_Priority_perform_actions( owner, queue_context ); + _Thread_queue_Boost_priority( heads, the_thread, owner, queue_context ); + return; + } +#endif + + _Priority_Non_empty_insert( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node, + &queue_context->Priority.Actions, + _Thread_queue_Priority_inherit_do_enqueue_change, + queue + ); + _Thread_queue_Boost_priority( + heads, + the_thread, + queue->owner, + queue_context + ); +} + +static void _Thread_queue_Priority_inherit_enqueue( + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context +) +{ + _Thread_queue_Queue_enqueue( + queue, + the_thread, + queue_context, + _Thread_queue_Priority_inherit_do_initialize, + _Thread_queue_Priority_inherit_do_enqueue + ); +} + +static void _Thread_queue_Priority_inherit_do_extract_action( + Priority_Actions *priority_actions, + Thread_Control *owner, + Priority_Node *priority_action_node, + Priority_Action_type priority_action_type +) +{ + Thread_queue_Context *queue_context; + Scheduler_Node *scheduler_node_of_owner; + + queue_context = THREAD_QUEUE_CONTEXT_OF_PRIORITY_ACTIONS( priority_actions ); + scheduler_node_of_owner = _Thread_Scheduler_get_own_node( owner ); + + _Priority_Actions_initialize_one( + &queue_context->Priority.Actions, + &scheduler_node_of_owner->Wait.Priority, + priority_action_node, + priority_action_type + ); + _Thread_Priority_perform_actions( owner, queue_context ); +} + +static void _Thread_queue_Priority_inherit_do_extract_remove( + Priority_Aggregation *priority_aggregation, + Priority_Actions *priority_actions, + void *arg +) +{ + _Thread_queue_Priority_inherit_do_extract_action( + priority_actions, + arg, + &priority_aggregation->Node, + PRIORITY_ACTION_REMOVE + ); +} + +static void _Thread_queue_Priority_inherit_do_extract_change( + Priority_Aggregation *priority_aggregation, + bool prepend_it, + Priority_Actions *priority_actions, + void *arg +) +{ + _Thread_queue_Priority_inherit_do_extract_action( + priority_actions, + arg, + &priority_aggregation->Node, + PRIORITY_ACTION_CHANGE + ); +} + +static void _Thread_queue_Priority_inherit_do_extract( + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *owner, + Thread_queue_Context *queue_context, + Thread_Control *the_thread +) +{ + Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; + + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); + + _Thread_queue_Path_acquire_critical( queue, the_thread, queue_context ); + + _Priority_Extract( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node, + &queue_context->Priority.Actions, + _Thread_queue_Priority_inherit_do_extract_remove, + _Thread_queue_Priority_inherit_do_extract_change, + owner + ); + +#if defined(RTEMS_SMP) + _Chain_Extract_unprotected( &priority_queue->Node ); + + if ( !_Priority_Is_empty( &priority_queue->Queue ) ) { + _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node ); + } + + if ( + _Priority_Node_is_active( &heads->Boost_priority ) + && ( _Chain_Is_empty( &heads->Heads.Fifo ) + || _Chain_Has_only_one_node( &heads->Heads.Fifo ) ) + ) { + _Thread_queue_Priority_inherit_do_extract_action( + &queue_context->Priority.Actions, owner, - priority + &heads->Boost_priority, + PRIORITY_ACTION_REMOVE ); - } else { - path->update_priority = NULL; + _Priority_Node_set_inactive( &heads->Boost_priority ); } +#endif + + _Thread_queue_Path_release_critical( queue_context ); } -static void _Thread_queue_Boost_priority( - Thread_queue_Heads *heads, - Thread_Control *the_thread +static void _Thread_queue_Priority_inherit_extract( + Thread_queue_Queue *queue, + Thread_Control *the_thread, + Thread_queue_Context *queue_context +) +{ + _Thread_queue_Queue_extract( + queue, + queue->heads, + queue->owner, + queue_context, + the_thread, + _Thread_queue_Priority_inherit_do_extract + ); +} + +static void _Thread_queue_Priority_inherit_do_surrender_change( + Priority_Aggregation *priority_aggregation, + bool prepend_it, + Priority_Actions *priority_actions, + void *arg ) { + _Thread_queue_Context_add_priority_update( + THREAD_QUEUE_CONTEXT_OF_PRIORITY_ACTIONS( priority_actions ), + arg + ); + _Scheduler_Node_set_priority( + SCHEDULER_NODE_OF_WAIT_PRIORITY( priority_aggregation ), + _Priority_Get_priority( priority_aggregation ), + prepend_it + ); +} + +static void _Thread_queue_Priority_add( + Thread_Control *the_thread, + Priority_Aggregation *priority_aggregation, + Priority_Node *priority_node, + Thread_queue_Context *queue_context +) +{ + _Priority_Non_empty_insert( + priority_aggregation, + priority_node, + &queue_context->Priority.Actions, + _Thread_queue_Priority_inherit_do_surrender_change, + the_thread + ); +} + +static void _Thread_queue_Priority_remove( + Thread_Control *the_thread, + Scheduler_Node *scheduler_node, + Priority_Node *priority_node, + Thread_queue_Context *queue_context +) +{ + _Priority_Extract_non_empty( + &scheduler_node->Wait.Priority, + priority_node, + &queue_context->Priority.Actions, + _Thread_queue_Priority_inherit_do_surrender_change, + the_thread + ); +} + +static void _Thread_queue_Priority_inherit_do_surrender( + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *previous_owner, + Thread_queue_Context *queue_context, + Thread_Control *the_thread +) +{ + Scheduler_Node *scheduler_node; + Thread_queue_Priority_queue *priority_queue; + ISR_lock_Context lock_context; + + scheduler_node = _Thread_Scheduler_get_own_node( the_thread ); + priority_queue = _Thread_queue_Priority_queue( heads, scheduler_node ); + + _Thread_Wait_acquire_default_critical( previous_owner, &lock_context ); + #if defined(RTEMS_SMP) - if ( !_Chain_Has_only_one_node( &heads->Heads.Fifo ) ) { - const Scheduler_Control *scheduler; - Priority_Control boost_priority; + if ( _Priority_Node_is_active( &heads->Boost_priority ) ) { + _Thread_queue_Priority_remove( + previous_owner, + _Thread_Scheduler_get_own_node( previous_owner ), + &heads->Boost_priority, + queue_context + ); + _Priority_Node_set_inactive( &heads->Boost_priority ); + } +#endif - the_thread->priority_restore_hint = true; - _Atomic_Fence( ATOMIC_ORDER_ACQ_REL ); + _Thread_queue_Priority_remove( + previous_owner, + priority_queue->scheduler_node, + &priority_queue->Queue.Node, + queue_context + ); - scheduler = _Scheduler_Get_own( the_thread ); - boost_priority = _Scheduler_Map_priority( scheduler, PRIORITY_PSEUDO_ISR ); + _Thread_Wait_release_default_critical( previous_owner, &lock_context ); - _Scheduler_Thread_set_priority( the_thread, boost_priority, false ); + _Priority_Extract( + &priority_queue->Queue, + &scheduler_node->Wait.Priority.Node, + NULL, + _Priority_Remove_nothing, + _Priority_Change_nothing, + previous_owner + ); + + if ( !_Priority_Is_empty( &priority_queue->Queue ) ) { + priority_queue->scheduler_node = scheduler_node; + _Thread_queue_Priority_add( + the_thread, + &scheduler_node->Wait.Priority, + &priority_queue->Queue.Node, + queue_context + ); + } + +#if defined(RTEMS_SMP) + _Chain_Extract_unprotected( &priority_queue->Node ); + + if ( !_Priority_Is_empty( &priority_queue->Queue ) ) { + _Chain_Append_unprotected( &heads->Heads.Fifo, &priority_queue->Node ); + } + + if ( + !_Chain_Is_empty( &heads->Heads.Fifo) + && !_Chain_Has_only_one_node( &heads->Heads.Fifo) + ) { + Priority_Control boost_priority; + + boost_priority = _Scheduler_Map_priority( + _Scheduler_Get_own( the_thread ), + PRIORITY_PSEUDO_ISR + ); + _Priority_Node_initialize( &heads->Boost_priority, boost_priority ); + _Thread_queue_Priority_add( + the_thread, + &scheduler_node->Wait.Priority, + &heads->Boost_priority, + queue_context + ); } -#else - (void) heads; - (void) the_thread; #endif } static Thread_Control *_Thread_queue_Priority_inherit_surrender( - Thread_queue_Queue *queue, - Thread_queue_Heads *heads, - Thread_Control *previous_owner + Thread_queue_Queue *queue, + Thread_queue_Heads *heads, + Thread_Control *previous_owner, + Thread_queue_Context *queue_context ) { Thread_Control *first; first = _Thread_queue_Priority_first( heads ); - _Thread_queue_Boost_priority( heads, first ); _Thread_queue_Queue_extract( queue, heads, + previous_owner, + queue_context, first, - _Thread_queue_Priority_do_extract + _Thread_queue_Priority_inherit_do_surrender ); return first; } const Thread_queue_Operations _Thread_queue_Operations_default = { - .priority_change = _Thread_queue_Do_nothing_priority_change, + .priority_actions = _Thread_queue_Do_nothing_priority_actions, .extract = _Thread_queue_Do_nothing_extract /* * The default operations are only used in _Thread_Change_priority() and @@ -516,7 +991,7 @@ const Thread_queue_Operations _Thread_queue_Operations_default = { }; const Thread_queue_Operations _Thread_queue_Operations_FIFO = { - .priority_change = _Thread_queue_Do_nothing_priority_change, + .priority_actions = _Thread_queue_Do_nothing_priority_actions, .enqueue = _Thread_queue_FIFO_enqueue, .extract = _Thread_queue_FIFO_extract, .surrender = _Thread_queue_FIFO_surrender, @@ -524,7 +999,7 @@ const Thread_queue_Operations _Thread_queue_Operations_FIFO = { }; const Thread_queue_Operations _Thread_queue_Operations_priority = { - .priority_change = _Thread_queue_Priority_priority_change, + .priority_actions = _Thread_queue_Priority_priority_actions, .enqueue = _Thread_queue_Priority_enqueue, .extract = _Thread_queue_Priority_extract, .surrender = _Thread_queue_Priority_surrender, @@ -532,9 +1007,9 @@ const Thread_queue_Operations _Thread_queue_Operations_priority = { }; const Thread_queue_Operations _Thread_queue_Operations_priority_inherit = { - .priority_change = _Thread_queue_Priority_priority_change, + .priority_actions = _Thread_queue_Priority_inherit_priority_actions, .enqueue = _Thread_queue_Priority_inherit_enqueue, - .extract = _Thread_queue_Priority_extract, + .extract = _Thread_queue_Priority_inherit_extract, .surrender = _Thread_queue_Priority_inherit_surrender, .first = _Thread_queue_Priority_first }; |