score: Rework thread priority management

Add priority nodes which contribute to the overall thread priority.

The actual priority of a thread is now an aggregation of priority nodes.
The thread priority aggregation for the home scheduler instance of a
thread consists of at least one priority node, which is normally the
real priority of the thread.  The locking protocols (e.g. priority
ceiling and priority inheritance), rate-monotonic period objects and the
POSIX sporadic server add, change and remove priority nodes.

A thread changes its priority now immediately, e.g. priority changes are
not deferred until the thread releases its last resource.

Replace the _Thread_Change_priority() function with

 * _Thread_Priority_perform_actions(),
 * _Thread_Priority_add(),
 * _Thread_Priority_remove(),
 * _Thread_Priority_change(), and
 * _Thread_Priority_update().

Update #2412.
Update #2556.

7 files changed, 177 insertions, 210 deletions

diff --git a/cpukit/posix/src/killinfo.c b/cpukit/posix/src/killinfo.c
index 7cf74eb0bf..6f45b29ec6 100644
--- a/cpukit/posix/src/killinfo.c
+++ b/cpukit/posix/src/killinfo.c
@@ -153,7 +153,7 @@ int _POSIX_signals_Send(
           the_node = the_node->next ) {
       Scheduler_Node *scheduler_node;
 
-      scheduler_node = SCHEDULER_NODE_OF_WAIT_CHAIN_NODE( the_node );
+      scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY_NODE( the_node );
       the_thread = _Scheduler_Node_get_owner( scheduler_node );
       api = the_thread->API_Extensions[ THREAD_API_POSIX ];
 
diff --git a/cpukit/posix/src/mutexsetprioceiling.c b/cpukit/posix/src/mutexsetprioceiling.c
index 31e54c5fc3..aaea57ddfb 100644
--- a/cpukit/posix/src/mutexsetprioceiling.c
+++ b/cpukit/posix/src/mutexsetprioceiling.c
@@ -64,7 +64,18 @@ int pthread_mutex_setprioceiling(
 
     new_priority = _POSIX_Priority_To_core( scheduler, prioceiling, &valid );
     if ( valid ) {
-      _CORE_ceiling_mutex_Set_priority( &the_mutex->Mutex, new_priority );
+      Thread_queue_Context  queue_context;
+      Per_CPU_Control      *cpu_self;
+
+      _Thread_queue_Context_clear_priority_updates( &queue_context );
+      _CORE_ceiling_mutex_Set_priority(
+        &the_mutex->Mutex,
+        new_priority,
+        &queue_context
+      );
+      cpu_self = _Thread_Dispatch_disable();
+      _Thread_Priority_update( &queue_context );
+      _Thread_Dispatch_enable( cpu_self );
       error = 0;
     } else {
       error = EINVAL;
diff --git a/cpukit/posix/src/pthread.c b/cpukit/posix/src/pthread.c
index a1394a5dee..9cd730b59f 100644
--- a/cpukit/posix/src/pthread.c
+++ b/cpukit/posix/src/pthread.c
@@ -82,85 +82,70 @@ pthread_attr_t _POSIX_Threads_Default_attributes = {
   #endif
 };
 
-static bool _POSIX_Threads_Sporadic_timer_filter(
-  Thread_Control   *the_thread,
-  Priority_Control *new_priority_p,
-  void             *arg
-)
+void _POSIX_Threads_Sporadic_timer( Watchdog_Control *watchdog )
 {
-  POSIX_API_Control *api;
-  Priority_Control   current_priority;
-  Priority_Control   new_priority;
+  POSIX_API_Control    *api;
+  Thread_Control       *the_thread;
+  Thread_queue_Context  queue_context;
 
-  api = arg;
-
-  new_priority = api->Sporadic.high_priority;
-  *new_priority_p = new_priority;
+  api = RTEMS_CONTAINER_OF( watchdog, POSIX_API_Control, Sporadic.Timer );
+  the_thread = api->thread;
 
-  current_priority = _Thread_Get_priority( the_thread );
-  the_thread->real_priority = new_priority;
+  _Thread_queue_Context_clear_priority_updates( &queue_context );
+  _Thread_Wait_acquire( the_thread, &queue_context );
+
+  if ( _Priority_Node_is_active( &api->Sporadic.Low_priority ) ) {
+    _Thread_Priority_add(
+      the_thread,
+      &the_thread->Real_priority,
+      &queue_context
+    );
+    _Thread_Priority_remove(
+      the_thread,
+      &api->Sporadic.Low_priority,
+      &queue_context
+    );
+    _Priority_Node_set_inactive( &api->Sporadic.Low_priority );
+  }
 
   _Watchdog_Per_CPU_remove_relative( &api->Sporadic.Timer );
   _POSIX_Threads_Sporadic_timer_insert( the_thread, api );
 
-  return _Thread_Priority_less_than( current_priority, new_priority )
-    || !_Thread_Owns_resources( the_thread );
-}
-
-static void _POSIX_Threads_Sporadic_timer( Watchdog_Control *watchdog )
-{
-  POSIX_API_Control *api;
-  Thread_Control    *the_thread;
-
-  api = RTEMS_CONTAINER_OF( watchdog, POSIX_API_Control, Sporadic.Timer );
-  the_thread = api->thread;
-
-  _Thread_Change_priority(
-    the_thread,
-    0,
-    api,
-    _POSIX_Threads_Sporadic_timer_filter,
-    true
-  );
+  _Thread_Wait_release( the_thread, &queue_context );
+  _Thread_Priority_update( &queue_context );
 }
 
-static bool _POSIX_Threads_Sporadic_budget_callout_filter(
-  Thread_Control   *the_thread,
-  Priority_Control *new_priority_p,
-  void             *arg
-)
+void _POSIX_Threads_Sporadic_budget_callout( Thread_Control *the_thread )
 {
-  POSIX_API_Control *api;
-  Priority_Control   current_priority;
-  Priority_Control   new_priority;
+  POSIX_API_Control    *api;
+  Thread_queue_Context  queue_context;
 
   api = the_thread->API_Extensions[ THREAD_API_POSIX ];
 
+  _Thread_queue_Context_clear_priority_updates( &queue_context );
+  _Thread_Wait_acquire( the_thread, &queue_context );
+
   /*
    *  This will prevent the thread from consuming its entire "budget"
    *  while at low priority.
    */
   the_thread->cpu_time_budget = UINT32_MAX;
 
-  new_priority = api->Sporadic.low_priority;
-  *new_priority_p = new_priority;
-
-  current_priority = _Thread_Get_priority( the_thread );
-  the_thread->real_priority = new_priority;
-
-  return _Thread_Priority_less_than( current_priority, new_priority )
-    || !_Thread_Owns_resources( the_thread );
-}
+  if ( !_Priority_Node_is_active( &api->Sporadic.Low_priority ) ) {
+    _Thread_Priority_add(
+      the_thread,
+      &api->Sporadic.Low_priority,
+      &queue_context
+    );
+    _Thread_Priority_remove(
+      the_thread,
+      &the_thread->Real_priority,
+      &queue_context
+    );
+  }
 
-void _POSIX_Threads_Sporadic_budget_callout( Thread_Control *the_thread )
-{
-  _Thread_Change_priority(
-    the_thread,
-    0,
-    NULL,
-    _POSIX_Threads_Sporadic_budget_callout_filter,
-    true
-  );
+  _Thread_Wait_release( the_thread, &queue_context );
+  _Thread_Priority_update( &queue_context );
 }
 
 /*
diff --git a/cpukit/posix/src/pthreadcreate.c b/cpukit/posix/src/pthreadcreate.c
index 411882d532..f1983ac128 100644
--- a/cpukit/posix/src/pthreadcreate.c
+++ b/cpukit/posix/src/pthreadcreate.c
@@ -61,11 +61,11 @@ int pthread_create(
     }
   };
   const pthread_attr_t               *the_attr;
+  int                                 normal_prio;
   int                                 low_prio;
-  int                                 high_prio;
   bool                                valid;
+  Priority_Control                    core_normal_prio;
   Priority_Control                    core_low_prio;
-  Priority_Control                    core_high_prio;
   Thread_CPU_budget_algorithms        budget_algorithm;
   Thread_CPU_budget_algorithm_callout budget_callout;
   bool                                is_fp;
@@ -149,22 +149,22 @@ int pthread_create(
     return error;
   }
 
-  if ( schedpolicy == SCHED_SPORADIC ) {
-    low_prio = schedparam.sched_ss_low_priority;
-    high_prio = schedparam.sched_priority;
-  } else {
-    low_prio = schedparam.sched_priority;
-    high_prio = low_prio;
-  }
+  normal_prio = schedparam.sched_priority;
 
   scheduler = _Scheduler_Get_own( executing );
 
-  core_low_prio = _POSIX_Priority_To_core( scheduler, low_prio, &valid );
+  core_normal_prio = _POSIX_Priority_To_core( scheduler, normal_prio, &valid );
   if ( !valid ) {
     return EINVAL;
   }
 
-  core_high_prio = _POSIX_Priority_To_core( scheduler, high_prio, &valid );
+  if ( schedpolicy == SCHED_SPORADIC ) {
+    low_prio = schedparam.sched_ss_low_priority;
+  } else {
+    low_prio = normal_prio;
+  }
+
+  core_low_prio = _POSIX_Priority_To_core( scheduler, low_prio, &valid );
   if ( !valid ) {
     return EINVAL;
   }
@@ -205,7 +205,7 @@ int pthread_create(
     the_attr->stackaddr,
     _POSIX_Threads_Ensure_minimum_stack(the_attr->stacksize),
     is_fp,
-    core_high_prio,
+    core_normal_prio,
     true,                 /* preemptible */
     budget_algorithm,
     budget_callout,
@@ -245,13 +245,11 @@ int pthread_create(
   api = the_thread->API_Extensions[ THREAD_API_POSIX ];
 
   _POSIX_Threads_Copy_attributes( &api->Attributes, the_attr );
-  api->Sporadic.low_priority = core_low_prio;
-  api->Sporadic.high_priority = core_high_prio;
+  _Priority_Node_initialize( &api->Sporadic.Low_priority, core_low_prio );
+  _Priority_Node_set_inactive( &api->Sporadic.Low_priority );
 
   if ( schedpolicy == SCHED_SPORADIC ) {
-    _ISR_lock_ISR_disable( &lock_context );
-    _POSIX_Threads_Sporadic_timer_insert( the_thread, api );
-    _ISR_lock_ISR_enable( &lock_context );
+    _POSIX_Threads_Sporadic_timer( &api->Sporadic.Timer );
   }
 
   /*
diff --git a/cpukit/posix/src/pthreadgetschedparam.c b/cpukit/posix/src/pthreadgetschedparam.c
index 38e0b4f9b5..1d4812c067 100644
--- a/cpukit/posix/src/pthreadgetschedparam.c
+++ b/cpukit/posix/src/pthreadgetschedparam.c
@@ -59,7 +59,7 @@ int pthread_getschedparam(
   *param  = api->Attributes.schedparam;
 
   scheduler = _Scheduler_Get_own( the_thread );
-  priority = the_thread->real_priority;
+  priority = the_thread->Real_priority.priority;
 
   _Thread_Wait_release( the_thread, &queue_context );
 
diff --git a/cpukit/posix/src/pthreadsetschedparam.c b/cpukit/posix/src/pthreadsetschedparam.c
index e3711e368c..9aa2122ab8 100644
--- a/cpukit/posix/src/pthreadsetschedparam.c
+++ b/cpukit/posix/src/pthreadsetschedparam.c
@@ -30,36 +30,24 @@
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/schedulerimpl.h>
 
-typedef struct {
-  int                                  policy;
-  const struct sched_param            *param;
-  Thread_CPU_budget_algorithms         budget_algorithm;
-  Thread_CPU_budget_algorithm_callout  budget_callout;
-  int                                  error;
-} POSIX_Set_sched_param_context;
-
-static bool _POSIX_Set_sched_param_filter(
-  Thread_Control   *the_thread,
-  Priority_Control *new_priority_p,
-  void             *arg
+static int _POSIX_Set_sched_param(
+  Thread_Control                       *the_thread,
+  int                                   policy,
+  struct sched_param                   *param,
+  Thread_CPU_budget_algorithms          budget_algorithm,
+  Thread_CPU_budget_algorithm_callout   budget_callout,
+  Thread_queue_Context                 *queue_context
 )
 {
-  POSIX_Set_sched_param_context *context;
-  const struct sched_param      *param;
-  const Scheduler_Control       *scheduler;
-  POSIX_API_Control             *api;
-  int                            low_prio;
-  int                            high_prio;
-  bool                           valid;
-  Priority_Control               core_low_prio;
-  Priority_Control               core_high_prio;
-  Priority_Control               current_priority;
-
-  context = arg;
-  param = context->param;
-  scheduler = _Scheduler_Get_own( the_thread );
-
-  if ( context->policy == SCHED_SPORADIC ) {
+  const Scheduler_Control *scheduler;
+  POSIX_API_Control       *api;
+  int                      low_prio;
+  int                      high_prio;
+  bool                     valid;
+  Priority_Control         core_normal_prio;
+  Priority_Control         core_low_prio;
+
+  if ( policy == SCHED_SPORADIC ) {
     low_prio = param->sched_ss_low_priority;
     high_prio = param->sched_priority;
   } else {
@@ -67,45 +55,60 @@ static bool _POSIX_Set_sched_param_filter(
     high_prio = low_prio;
   }
 
-  core_low_prio = _POSIX_Priority_To_core( scheduler, low_prio, &valid );
+  scheduler = _Scheduler_Get_own( the_thread );
+
+  core_normal_prio = _POSIX_Priority_To_core( scheduler, low_prio, &valid );
   if ( !valid ) {
-    context->error = EINVAL;
-    return false;
+    return EINVAL;
   }
 
-  core_high_prio = _POSIX_Priority_To_core( scheduler, high_prio, &valid );
+  core_low_prio = _POSIX_Priority_To_core( scheduler, high_prio, &valid );
   if ( !valid ) {
-    context->error = EINVAL;
-    return false;
+    return EINVAL;
   }
 
-  *new_priority_p = core_high_prio;
-
-  current_priority = _Thread_Get_priority( the_thread );
-  the_thread->real_priority = core_high_prio;
-
   api = the_thread->API_Extensions[ THREAD_API_POSIX ];
 
   _Watchdog_Per_CPU_remove_relative( &api->Sporadic.Timer );
 
-  api->Attributes.schedpolicy = context->policy;
+  _Priority_Node_set_priority( &the_thread->Real_priority, core_normal_prio );
+
+  if ( _Priority_Node_is_active( &api->Sporadic.Low_priority ) ) {
+    _Thread_Priority_add(
+      the_thread,
+      &the_thread->Real_priority,
+      queue_context
+    );
+    _Thread_Priority_remove(
+      the_thread,
+      &api->Sporadic.Low_priority,
+      queue_context
+    );
+    _Priority_Node_set_inactive( &api->Sporadic.Low_priority );
+  } else {
+    _Thread_Priority_changed(
+      the_thread,
+      &the_thread->Real_priority,
+      false,
+      queue_context
+    );
+  }
+
+  api->Attributes.schedpolicy = policy;
   api->Attributes.schedparam  = *param;
-  api->Sporadic.low_priority  = core_low_prio;
-  api->Sporadic.high_priority = core_high_prio;
 
-  the_thread->budget_algorithm = context->budget_algorithm;
-  the_thread->budget_callout   = context->budget_callout;
+  the_thread->budget_algorithm = budget_algorithm;
+  the_thread->budget_callout   = budget_callout;
 
-  if ( context->policy == SCHED_SPORADIC ) {
+  if ( policy == SCHED_SPORADIC ) {
+    _Priority_Node_set_priority( &api->Sporadic.Low_priority, core_low_prio );
     _POSIX_Threads_Sporadic_timer_insert( the_thread, api );
   } else {
     the_thread->cpu_time_budget =
       rtems_configuration_get_ticks_per_timeslice();
   }
 
-  context->error = 0;
-  return _Thread_Priority_less_than( current_priority, core_high_prio )
-    || !_Thread_Owns_resources( the_thread );
+  return 0;
 }
 
 int pthread_setschedparam(
@@ -114,11 +117,12 @@ int pthread_setschedparam(
   struct sched_param *param
 )
 {
-  Thread_Control                *the_thread;
-  Per_CPU_Control               *cpu_self;
-  POSIX_Set_sched_param_context  context;
-  ISR_lock_Context               lock_context;
-  int                            error;
+  Thread_CPU_budget_algorithms         budget_algorithm;
+  Thread_CPU_budget_algorithm_callout  budget_callout;
+  Thread_Control                      *the_thread;
+  Per_CPU_Control                     *cpu_self;
+  Thread_queue_Context                 queue_context;
+  int                                  error;
 
   if ( param == NULL ) {
     return EINVAL;
@@ -127,33 +131,34 @@ int pthread_setschedparam(
   error = _POSIX_Thread_Translate_sched_param(
     policy,
     param,
-    &context.budget_algorithm,
-    &context.budget_callout
+    &budget_algorithm,
+    &budget_callout
   );
   if ( error != 0 ) {
     return error;
   }
 
-  context.policy = policy;
-  context.param = param;
-
-  the_thread = _Thread_Get( thread, &lock_context );
+  the_thread = _Thread_Get( thread, &queue_context.Lock_context.Lock_context );
 
   if ( the_thread == NULL ) {
     return ESRCH;
   }
 
-  cpu_self = _Thread_Dispatch_disable_critical( &lock_context );
-  _ISR_lock_ISR_enable( &lock_context );
-
-  _Thread_Change_priority(
+  _Thread_queue_Context_clear_priority_updates( &queue_context );
+  _Thread_Wait_acquire_critical( the_thread, &queue_context );
+  error = _POSIX_Set_sched_param(
     the_thread,
-    0,
-    &context,
-    _POSIX_Set_sched_param_filter,
-    false
+    policy,
+    param,
+    budget_algorithm,
+    budget_callout,
+    &queue_context
   );
-
+  cpu_self = _Thread_Dispatch_disable_critical(
+    &queue_context.Lock_context.Lock_context
+  );
+  _Thread_Wait_release( the_thread, &queue_context );
+  _Thread_Priority_update( &queue_context );
   _Thread_Dispatch_enable( cpu_self );
-  return context.error;
+  return error;
 }
diff --git a/cpukit/posix/src/pthreadsetschedprio.c b/cpukit/posix/src/pthreadsetschedprio.c
index fac86d2609..e3d9de0136 100644
--- a/cpukit/posix/src/pthreadsetschedprio.c
+++ b/cpukit/posix/src/pthreadsetschedprio.c
@@ -18,79 +18,47 @@
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/schedulerimpl.h>
 
-typedef struct {
-  int prio;
-  int error;
-} POSIX_Set_sched_prio_context;
-
-static bool _POSIX_Set_sched_prio_filter(
-  Thread_Control   *the_thread,
-  Priority_Control *new_priority_p,
-  void             *arg
-)
-{
-  POSIX_Set_sched_prio_context *context;
-  int                           prio;
-  const Scheduler_Control      *scheduler;
-  POSIX_API_Control            *api;
-  bool                          valid;
-  Priority_Control              current_priority;
-  Priority_Control              new_priority;
-
-  context = arg;
-  prio = context->prio;
-  scheduler = _Scheduler_Get_own( the_thread );
-
-  new_priority = _POSIX_Priority_To_core( scheduler, prio, &valid );
-  if ( !valid ) {
-    context->error = EINVAL;
-    return false;
-  }
-
-  *new_priority_p = new_priority;
-
-  current_priority = _Thread_Get_priority( the_thread );
-  the_thread->real_priority = new_priority;
-
-  api = the_thread->API_Extensions[ THREAD_API_POSIX ];
-
-  api->Sporadic.high_priority = new_priority;
-
-  if ( api->Sporadic.low_priority < new_priority ) {
-    api->Sporadic.low_priority  = new_priority;
-  }
-
-  context->error = 0;
-  return _Thread_Priority_less_than( current_priority, new_priority )
-    || !_Thread_Owns_resources( the_thread );
-}
-
 int pthread_setschedprio( pthread_t thread, int prio )
 {
-  Thread_Control               *the_thread;
-  Per_CPU_Control              *cpu_self;
-  POSIX_Set_sched_prio_context  context;
-  ISR_lock_Context              lock_context;
-
-  context.prio = prio;
+  Thread_Control          *the_thread;
+  Per_CPU_Control         *cpu_self;
+  Thread_queue_Context     queue_context;
+  const Scheduler_Control *scheduler;
+  Priority_Control         new_priority;
+  bool                     valid;
 
-  the_thread = _Thread_Get( thread, &lock_context );
+  the_thread = _Thread_Get( thread, &queue_context.Lock_context.Lock_context );
 
   if ( the_thread == NULL ) {
     return ESRCH;
   }
 
-  cpu_self = _Thread_Dispatch_disable_critical( &lock_context );
-  _ISR_lock_ISR_enable( &lock_context );
+  _Thread_queue_Context_clear_priority_updates( &queue_context );
+  _Thread_Wait_acquire_critical( the_thread, &queue_context );
 
-  _Thread_Change_priority(
+  scheduler = _Scheduler_Get_own( the_thread );
+
+  new_priority = _POSIX_Priority_To_core( scheduler, prio, &valid );
+  if ( !valid ) {
+    _Thread_Wait_release( the_thread, &queue_context );
+    return EINVAL;
+  }
+
+  _Thread_Priority_change(
     the_thread,
-    0,
-    &context,
-    _POSIX_Set_sched_prio_filter,
-    true
+    &the_thread->Real_priority,
+    new_priority,
+    true,
+    &queue_context
   );
 
+  cpu_self = _Thread_Dispatch_disable_critical(
+    &queue_context.Lock_context.Lock_context
+  );
+  _Thread_Wait_release( the_thread, &queue_context );
+
+  _Thread_Priority_update( &queue_context );
+
   _Thread_Dispatch_enable( cpu_self );
-  return context.error;
+  return 0;
 }