score: Rework _Thread_Change_priority()

Move the writes to Thread_Control::current_priority and
Thread_Control::real_priority into _Thread_Change_priority() under the
protection of the thread lock.  Add a filter function to
_Thread_Change_priority() to enable specialized variants.

Avoid race conditions during a thread priority restore with the new
Thread_Control::priority_restore_hint for an important average case
optimizations used by priority inheritance mutexes.

Update #2273.

20 files changed, 452 insertions, 227 deletions

diff --git a/cpukit/posix/src/pthread.c b/cpukit/posix/src/pthread.c
index a3a73f117a..6395ec0ebd 100644
--- a/cpukit/posix/src/pthread.c
+++ b/cpukit/posix/src/pthread.c
@@ -83,6 +83,23 @@ pthread_attr_t _POSIX_Threads_Default_attributes = {
   #endif
 };
 
+static bool _POSIX_Threads_Sporadic_budget_TSR_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority,
+  void             *arg
+)
+{
+  the_thread->real_priority = *new_priority;
+
+  /*
+   * If holding a resource, then do not change it.
+   *
+   * If this would make them less important, then do not change it.
+   */
+  return !_Thread_Owns_resources( the_thread ) &&
+    _Thread_Priority_less_than( the_thread->current_priority, *new_priority );
+}
+
 /*
  *  _POSIX_Threads_Sporadic_budget_TSR
  */
@@ -92,7 +109,6 @@ void _POSIX_Threads_Sporadic_budget_TSR(
 )
 {
   uint32_t            ticks;
-  uint32_t            new_priority;
   Thread_Control     *the_thread;
   POSIX_API_Control  *api;
 
@@ -105,27 +121,13 @@ void _POSIX_Threads_Sporadic_budget_TSR(
 
   the_thread->cpu_time_budget = ticks;
 
-  new_priority = _POSIX_Priority_To_core( api->schedparam.sched_priority );
-  the_thread->real_priority = new_priority;
-
-  /*
-   *  If holding a resource, then do not change it.
-   */
-  #if 0
-    printk( "TSR %d %d %d\n", the_thread->resource_count,
-        the_thread->current_priority, new_priority );
-  #endif
-  if ( !_Thread_Owns_resources( the_thread ) ) {
-    /*
-     *  If this would make them less important, then do not change it.
-     */
-    if ( the_thread->current_priority > new_priority ) {
-      _Thread_Change_priority( the_thread, new_priority, true );
-      #if 0
-        printk( "raise priority\n" );
-      #endif
-    }
-  }
+  _Thread_Change_priority(
+    the_thread,
+    _POSIX_Priority_To_core( api->schedparam.sched_priority ),
+    NULL,
+    _POSIX_Threads_Sporadic_budget_TSR_filter,
+    true
+  );
 
   /* ticks is guaranteed to be at least one */
   ticks = _Timespec_To_ticks( &api->schedparam.sched_ss_repl_period );
@@ -133,6 +135,25 @@ void _POSIX_Threads_Sporadic_budget_TSR(
   _Watchdog_Insert_ticks( &api->Sporadic_timer, ticks );
 }
 
+static bool _POSIX_Threads_Sporadic_budget_callout_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority,
+  void             *arg
+)
+{
+  the_thread->real_priority = *new_priority;
+
+  /*
+   * If holding a resource, then do not change it.
+   *
+   * Make sure we are actually lowering it. If they have lowered it
+   * to logically lower than sched_ss_low_priority, then we do not want to
+   * change it.
+   */
+  return !_Thread_Owns_resources( the_thread ) &&
+    _Thread_Priority_less_than( *new_priority, the_thread->current_priority );
+}
+
 /*
  *  _POSIX_Threads_Sporadic_budget_callout
  */
@@ -141,7 +162,6 @@ void _POSIX_Threads_Sporadic_budget_callout(
 )
 {
   POSIX_API_Control *api;
-  uint32_t           new_priority;
 
   api = the_thread->API_Extensions[ THREAD_API_POSIX ];
 
@@ -151,29 +171,13 @@ void _POSIX_Threads_Sporadic_budget_callout(
    */
   the_thread->cpu_time_budget = UINT32_MAX;
 
-  new_priority = _POSIX_Priority_To_core(api->schedparam.sched_ss_low_priority);
-  the_thread->real_priority = new_priority;
-
-  /*
-   *  If holding a resource, then do not change it.
-   */
-  #if 0
-    printk( "callout %d %d %d\n", the_thread->resource_count,
-	the_thread->current_priority, new_priority );
-  #endif
-  if ( !_Thread_Owns_resources( the_thread ) ) {
-    /*
-     *  Make sure we are actually lowering it. If they have lowered it
-     *  to logically lower than sched_ss_low_priority, then we do not want to
-     *  change it.
-     */
-    if ( the_thread->current_priority < new_priority ) {
-      _Thread_Change_priority( the_thread, new_priority, true );
-      #if 0
-        printk( "lower priority\n" );
-      #endif
-    }
-  }
+  _Thread_Change_priority(
+    the_thread,
+    _POSIX_Priority_To_core( api->schedparam.sched_ss_low_priority ),
+    NULL,
+    _POSIX_Threads_Sporadic_budget_callout_filter,
+    true
+  );
 }
 
 /*
diff --git a/cpukit/posix/src/pthreadsetschedparam.c b/cpukit/posix/src/pthreadsetschedparam.c
index dcb70bdec8..067e6ba509 100644
--- a/cpukit/posix/src/pthreadsetschedparam.c
+++ b/cpukit/posix/src/pthreadsetschedparam.c
@@ -44,6 +44,7 @@ int pthread_setschedparam(
   Thread_CPU_budget_algorithm_callout  budget_callout;
   Objects_Locations                    location;
   int                                  rc;
+  Priority_Control                     unused;
 
   /*
    *  Check all the parameters
@@ -87,13 +88,11 @@ int pthread_setschedparam(
           the_thread->cpu_time_budget =
             rtems_configuration_get_ticks_per_timeslice();
 
-          the_thread->real_priority =
-            _POSIX_Priority_To_core( api->schedparam.sched_priority );
-
-          _Thread_Change_priority(
-             the_thread,
-             the_thread->real_priority,
-             true
+          _Thread_Set_priority(
+            the_thread,
+            _POSIX_Priority_To_core( api->schedparam.sched_priority ),
+            &unused,
+            true
           );
           break;
 
diff --git a/cpukit/rtems/src/tasksetpriority.c b/cpukit/rtems/src/tasksetpriority.c
index 4e4835675d..582c67f9f2 100644
--- a/cpukit/rtems/src/tasksetpriority.c
+++ b/cpukit/rtems/src/tasksetpriority.c
@@ -41,16 +41,18 @@ rtems_status_code rtems_task_set_priority(
   switch ( location ) {
 
     case OBJECTS_LOCAL:
-      *old_priority = _RTEMS_tasks_Priority_from_Core(
-                        the_thread->current_priority
-                      );
       if ( new_priority != RTEMS_CURRENT_PRIORITY ) {
-        the_thread->real_priority = _RTEMS_tasks_Priority_to_Core(
-                                      new_priority
-                                    );
-        if ( !_Thread_Owns_resources( the_thread ) ||
-             the_thread->current_priority > new_priority )
-          _Thread_Change_priority( the_thread, new_priority, false );
+        _Thread_Set_priority(
+          the_thread,
+          _RTEMS_tasks_Priority_to_Core( new_priority ),
+          old_priority,
+          false
+        );
+        *old_priority = _RTEMS_tasks_Priority_from_Core( *old_priority );
+      } else {
+        *old_priority = _RTEMS_tasks_Priority_from_Core(
+          the_thread->current_priority
+        );
       }
       _Objects_Put( &the_thread->Object );
       return RTEMS_SUCCESSFUL;
diff --git a/cpukit/score/include/rtems/score/coremuteximpl.h b/cpukit/score/include/rtems/score/coremuteximpl.h
index 54dadbe7e4..ca9d61df4a 100644
--- a/cpukit/score/include/rtems/score/coremuteximpl.h
+++ b/cpukit/score/include/rtems/score/coremuteximpl.h
@@ -491,11 +491,7 @@ RTEMS_INLINE_ROUTINE int _CORE_mutex_Seize_interrupt_trylock_body(
 
         cpu_self = _Thread_Dispatch_disable_critical();
         _Thread_queue_Release( &the_mutex->Wait_queue, lock_context );
-        _Thread_Change_priority(
-          executing,
-          ceiling,
-          false
-        );
+        _Thread_Raise_priority( executing, ceiling );
         _Thread_Dispatch_enable( cpu_self );
         return 0;
       }
diff --git a/cpukit/score/include/rtems/score/mrspimpl.h b/cpukit/score/include/rtems/score/mrspimpl.h
index 07f78ce7e9..05aee42a22 100644
--- a/cpukit/score/include/rtems/score/mrspimpl.h
+++ b/cpukit/score/include/rtems/score/mrspimpl.h
@@ -36,13 +36,18 @@ extern "C" {
  * @{
  */
 
-RTEMS_INLINE_ROUTINE void _MRSP_Elevate_priority(
-  MRSP_Control     *mrsp,
-  Thread_Control   *new_owner,
-  Priority_Control  ceiling_priority
+RTEMS_INLINE_ROUTINE bool _MRSP_Restore_priority_filter(
+  Thread_Control   *thread,
+  Priority_Control *new_priority,
+  void             *arg
 )
 {
-  _Thread_Change_priority( new_owner, ceiling_priority, false );
+  *new_priority = _Thread_Priority_highest(
+    thread->real_priority,
+    *new_priority
+  );
+
+  return *new_priority != thread->current_priority;
 }
 
 RTEMS_INLINE_ROUTINE void _MRSP_Restore_priority(
@@ -55,13 +60,13 @@ RTEMS_INLINE_ROUTINE void _MRSP_Restore_priority(
    * or priority inheritance semaphores.
    */
   if ( thread->resource_count == 0 ) {
-    Priority_Control new_priority = _Scheduler_Highest_priority_of_two(
-      _Scheduler_Get( thread ),
+    _Thread_Change_priority(
+      thread,
       initial_priority,
-      thread->real_priority
+      NULL,
+      _MRSP_Restore_priority_filter,
+      true
     );
-
-    _Thread_Change_priority( thread, new_priority, true );
   }
 }
 
@@ -75,7 +80,7 @@ RTEMS_INLINE_ROUTINE void _MRSP_Claim_ownership(
   _Resource_Node_add_resource( &new_owner->Resource_node, &mrsp->Resource );
   _Resource_Set_owner( &mrsp->Resource, &new_owner->Resource_node );
   mrsp->initial_priority_of_owner = initial_priority;
-  _MRSP_Elevate_priority( mrsp, new_owner, ceiling_priority );
+  _Thread_Raise_priority( new_owner, ceiling_priority );
   _Scheduler_Thread_change_help_state( new_owner, SCHEDULER_HELP_ACTIVE_OWNER );
 }
 
@@ -177,7 +182,7 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
     _Scheduler_Thread_change_help_state( executing, SCHEDULER_HELP_ACTIVE_RIVAL );
   rival.status = MRSP_WAIT_FOR_OWNERSHIP;
 
-  _MRSP_Elevate_priority( mrsp, executing, ceiling_priority );
+  _Thread_Raise_priority( executing, ceiling_priority );
 
   _ISR_Disable( level );
 
@@ -235,10 +240,9 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Obtain(
   Priority_Control initial_priority = executing->current_priority;
   Priority_Control ceiling_priority =
     _MRSP_Get_ceiling_priority( mrsp, scheduler_index );
-  bool priority_ok = !_Scheduler_Is_priority_higher_than(
-    scheduler,
-    initial_priority,
-    ceiling_priority
+  bool priority_ok = !_Thread_Priority_less_than(
+    ceiling_priority,
+    initial_priority
   );
   Resource_Node *owner;
 
diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index 212bace075..cadebfd02f 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -693,54 +693,6 @@ RTEMS_INLINE_ROUTINE bool _Scheduler_Is_priority_higher_than(
   return _Scheduler_Priority_compare( scheduler, p1,  p2 ) > 0;
 }
 
-/**
- * @brief Returns the priority encoding @a p1 or @a p2 with the higher priority
- * in the intuitive sense of priority.
- */
-RTEMS_INLINE_ROUTINE Priority_Control _Scheduler_Highest_priority_of_two(
-  const Scheduler_Control *scheduler,
-  Priority_Control         p1,
-  Priority_Control         p2
-)
-{
-  return _Scheduler_Is_priority_higher_than( scheduler, p1, p2 ) ? p1 : p2;
-}
-
-/**
- * @brief Sets the thread priority to @a priority if it is higher than the
- * current priority of the thread in the intuitive sense of priority.
- */
-RTEMS_INLINE_ROUTINE void _Scheduler_Set_priority_if_higher(
-  const Scheduler_Control *scheduler,
-  Thread_Control          *the_thread,
-  Priority_Control         priority
-)
-{
-  Priority_Control current = the_thread->current_priority;
-
-  if ( _Scheduler_Is_priority_higher_than( scheduler, priority, current ) ) {
-    _Thread_Set_priority( the_thread, priority );
-  }
-}
-
-/**
- * @brief Changes the thread priority to @a priority if it is higher than the
- * current priority of the thread in the intuitive sense of priority.
- */
-RTEMS_INLINE_ROUTINE void _Scheduler_Change_priority_if_higher(
-  const Scheduler_Control *scheduler,
-  Thread_Control          *the_thread,
-  Priority_Control         priority,
-  bool                     prepend_it
-)
-{
-  Priority_Control current = the_thread->current_priority;
-
-  if ( _Scheduler_Is_priority_higher_than( scheduler, priority, current ) ) {
-    _Thread_Change_priority( the_thread, priority, prepend_it );
-  }
-}
-
 RTEMS_INLINE_ROUTINE uint32_t _Scheduler_Get_processor_count(
   const Scheduler_Control *scheduler
 )
diff --git a/cpukit/score/include/rtems/score/thread.h b/cpukit/score/include/rtems/score/thread.h
index b6662e49af..39fcb1707f 100644
--- a/cpukit/score/include/rtems/score/thread.h
+++ b/cpukit/score/include/rtems/score/thread.h
@@ -366,9 +366,21 @@ typedef struct {
   Objects_Control          Object;
   /** This field is the current execution state of this proxy. */
   States_Control           current_state;
-  /** This field is the current priority state of this proxy. */
+
+  /**
+   * @brief This field is the current priority state of this thread.
+   *
+   * Writes to this field are only allowed in _Thread_Initialize() or via
+   * _Thread_Change_priority().
+   */
   Priority_Control         current_priority;
-  /** This field is the base priority of this proxy. */
+
+  /**
+   * @brief This field is the base priority of this thread.
+   *
+   * Writes to this field are only allowed in _Thread_Initialize() or via
+   * _Thread_Change_priority().
+   */
   Priority_Control         real_priority;
 
   /**
@@ -379,6 +391,17 @@ typedef struct {
    */
   uint32_t                 priority_generation;
 
+  /**
+   * @brief Hints if a priority restore is necessary once the resource count
+   * changes from one to zero.
+   *
+   * This is an optimization to speed up the mutex surrender sequence in case
+   * no attempt to change the priority was made during the mutex ownership.  On
+   * SMP configurations atomic fences must synchronize writes to
+   * Thread_Control::priority_restore_hint and Thread_Control::resource_count.
+   */
+  bool                     priority_restore_hint;
+
   /** This field is the number of mutexes currently held by this proxy. */
   uint32_t                 resource_count;
 
@@ -653,9 +676,21 @@ struct Thread_Control_struct {
   Objects_Control          Object;
   /** This field is the current execution state of this thread. */
   States_Control           current_state;
-  /** This field is the current priority state of this thread. */
+
+  /**
+   * @brief This field is the current priority state of this thread.
+   *
+   * Writes to this field are only allowed in _Thread_Initialize() or via
+   * _Thread_Change_priority().
+   */
   Priority_Control         current_priority;
-  /** This field is the base priority of this thread. */
+
+  /**
+   * @brief This field is the base priority of this thread.
+   *
+   * Writes to this field are only allowed in _Thread_Initialize() or via
+   * _Thread_Change_priority().
+   */
   Priority_Control         real_priority;
 
   /**
@@ -666,6 +701,17 @@ struct Thread_Control_struct {
    */
   uint32_t                 priority_generation;
 
+  /**
+   * @brief Hints if a priority restore is necessary once the resource count
+   * changes from one to zero.
+   *
+   * This is an optimization to speed up the mutex surrender sequence in case
+   * no attempt to change the priority was made during the mutex ownership.  On
+   * SMP configurations atomic fences must synchronize writes to
+   * Thread_Control::priority_restore_hint and Thread_Control::resource_count.
+   */
+  bool                     priority_restore_hint;
+
   /** This field is the number of mutexes currently held by this thread. */
   uint32_t                 resource_count;
   /** This field is the blocking information for this thread. */
diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index b8c235c846..ffff220288 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -333,34 +333,120 @@ void _Thread_Delay_ended(
 );
 
 /**
- *  @brief Change the priority of a thread.
+ * @brief Returns true if the left thread priority is less than the right
+ * thread priority in the intuitive sense of priority and false otherwise.
+ */
+RTEMS_INLINE_ROUTINE bool _Thread_Priority_less_than(
+  Priority_Control left,
+  Priority_Control right
+)
+{
+  return left > right;
+}
+
+/**
+ * @brief Returns the highest priority of the left and right thread priorities
+ * in the intuitive sense of priority.
+ */
+RTEMS_INLINE_ROUTINE Priority_Control _Thread_Priority_highest(
+  Priority_Control left,
+  Priority_Control right
+)
+{
+  return _Thread_Priority_less_than( left, right ) ? right : left;
+}
+
+/**
+ * @brief Filters a thread priority change.
+ *
+ * Called by _Thread_Change_priority() under the protection of the thread lock.
  *
- *  This routine changes the current priority of @a the_thread to
- *  @a new_priority.  It performs any necessary scheduling operations
- *  including the selection of a new heir thread.
+ * @param[in] the_thread The thread.
+ * @param[in, out] new_priority The new priority of the thread.  The filter may
+ * alter this value.
+ * @param[in] arg The argument passed to _Thread_Change_priority().
  *
- *  @param[in] the_thread is the thread to change
- *  @param[in] new_priority is the priority to set @a the_thread to
- *  @param[in] prepend_it is a switch to prepend the thread
+ * @retval true Change the current priority.
+ * @retval false Otherwise.
  */
-void _Thread_Change_priority (
+typedef bool ( *Thread_Change_priority_filter )(
   Thread_Control   *the_thread,
-  Priority_Control  new_priority,
-  bool              prepend_it
+  Priority_Control *new_priority,
+  void             *arg
+);
+
+/**
+ * @brief Changes the priority of a thread if allowed by the filter function.
+ *
+ * It changes current priority of the thread to the new priority in case the
+ * filter function returns true.  In this case the scheduler is notified of the
+ * priority change as well.
+ *
+ * @param[in] the_thread The thread.
+ * @param[in] new_priority The new priority of the thread.
+ * @param[in] arg The argument for the filter function.
+ * @param[in] filter The filter function to determine if a priority change is
+ * allowed and optionally perform other actions under the protection of the
+ * thread lock simultaneously with the update of the current priority.
+ * @param[in] prepend_it In case this is true, then the thread is prepended to
+ * its priority group in its scheduler instance, otherwise it is appended.
+ */
+void _Thread_Change_priority(
+  Thread_Control                *the_thread,
+  Priority_Control               new_priority,
+  void                          *arg,
+  Thread_Change_priority_filter  filter,
+  bool                           prepend_it
 );
 
 /**
- *  @brief Set thread priority.
+ * @brief Raises the priority of a thread.
+ *
+ * It changes the current priority of the thread to the new priority if the new
+ * priority is higher than the current priority.  In this case the thread is
+ * appended to its new priority group in its scheduler instance.
  *
- *  This routine updates the priority related fields in the_thread
- *  control block to indicate the current priority is now new_priority.
+ * @param[in] the_thread The thread.
+ * @param[in] new_priority The new priority of the thread.
+ *
+ * @see _Thread_Change_priority().
  */
-void _Thread_Set_priority(
+void _Thread_Raise_priority(
   Thread_Control   *the_thread,
   Priority_Control  new_priority
 );
 
 /**
+ * @brief Sets the current to the real priority of a thread.
+ *
+ * Sets the priority restore hint to false.
+ */
+void _Thread_Restore_priority( Thread_Control *the_thread );
+
+/**
+ * @brief Sets the priority of a thread.
+ *
+ * It sets the real priority of the thread.  In addition it changes the current
+ * priority of the thread if the new priority is higher than the current
+ * priority or the thread owns no resources.
+ *
+ * @param[in] the_thread The thread.
+ * @param[in] new_priority The new priority of the thread.
+ * @param[out] old_priority The old real priority of the thread.  This pointer
+ * must not be @c NULL.
+ * @param[in] prepend_it In case this is true, then the thread is prepended to
+ * its priority group in its scheduler instance, otherwise it is appended.
+ *
+ * @see _Thread_Change_priority().
+ */
+void _Thread_Set_priority(
+  Thread_Control   *the_thread,
+  Priority_Control  new_priority,
+  Priority_Control *old_priority,
+  bool              prepend_it
+);
+
+/**
  *  @brief Maps thread Id to a TCB pointer.
  *
  *  This function maps thread IDs to thread control
diff --git a/cpukit/score/src/coremutex.c b/cpukit/score/src/coremutex.c
index 58c6c42b06..6bb535a8ec 100644
--- a/cpukit/score/src/coremutex.c
+++ b/cpukit/score/src/coremutex.c
@@ -50,15 +50,20 @@ CORE_mutex_Status _CORE_mutex_Initialize(
       Priority_Control ceiling = the_mutex->Attributes.priority_ceiling;
       Per_CPU_Control *cpu_self;
 
-      /*
-       * The mutex initialization is only protected by the allocator lock in
-       * general.  Disable thread dispatching before the priority check to
-       * prevent interference with priority inheritance.
-       */
+      /* The mutex initialization is only protected by the allocator lock */
       cpu_self = _Thread_Dispatch_disable();
 
+      /*
+       * The test to check for a ceiling violation is a bit arbitrary.  In case
+       * this thread is the owner of a priority inheritance mutex, then it may
+       * get a higher priority later or anytime on SMP configurations.
+       */
       if ( is_priority_ceiling && executing->current_priority < ceiling ) {
-        _Thread_Enable_dispatch();
+        /*
+         * There is no need to undo the previous work since this error aborts
+         * the object creation.
+         */
+        _Thread_Dispatch_enable( cpu_self );
         return CORE_MUTEX_STATUS_CEILING_VIOLATED;
       }
 
@@ -71,7 +76,7 @@ CORE_mutex_Status _CORE_mutex_Initialize(
       executing->resource_count++;
 
       if ( is_priority_ceiling ) {
-        _Thread_Change_priority( executing, ceiling, false );
+        _Thread_Raise_priority( executing, ceiling );
       }
 
       _Thread_Dispatch_enable( cpu_self );
diff --git a/cpukit/score/src/coremutexseize.c b/cpukit/score/src/coremutexseize.c
index 94c480d054..d5f85c5fad 100644
--- a/cpukit/score/src/coremutexseize.c
+++ b/cpukit/score/src/coremutexseize.c
@@ -21,7 +21,6 @@
 #include <rtems/system.h>
 #include <rtems/score/isr.h>
 #include <rtems/score/coremuteximpl.h>
-#include <rtems/score/schedulerimpl.h>
 #include <rtems/score/statesimpl.h>
 #include <rtems/score/thread.h>
 
@@ -76,12 +75,7 @@ void _CORE_mutex_Seize_interrupt_blocking(
     _Thread_queue_Release( &the_mutex->Wait_queue, lock_context );
 #endif
 
-    _Scheduler_Change_priority_if_higher(
-      _Scheduler_Get( holder ),
-      holder,
-      executing->current_priority,
-      false
-    );
+    _Thread_Raise_priority( holder, executing->current_priority );
 
 #if !defined(RTEMS_SMP)
     _Thread_queue_Acquire( &the_mutex->Wait_queue, lock_context );
diff --git a/cpukit/score/src/coremutexsurrender.c b/cpukit/score/src/coremutexsurrender.c
index 3f0cd8619c..d5dde1e8e6 100644
--- a/cpukit/score/src/coremutexsurrender.c
+++ b/cpukit/score/src/coremutexsurrender.c
@@ -207,14 +207,10 @@ CORE_mutex_Status _CORE_mutex_Surrender(
         case CORE_MUTEX_DISCIPLINES_PRIORITY_CEILING:
           _CORE_mutex_Push_priority( the_mutex, the_thread );
           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
-              );
-          }
+          _Thread_Raise_priority(
+            the_thread,
+            the_mutex->Attributes.priority_ceiling
+          );
           break;
       }
     }
@@ -246,13 +242,20 @@ CORE_mutex_Status _CORE_mutex_Surrender(
    *  inherited priority must be lowered if this is the last
    *  mutex (i.e. resource) this task has.
    */
-  if ( !_Thread_Owns_resources( holder ) &&
-       holder->real_priority != holder->current_priority ) {
-    Per_CPU_Control *cpu_self;
+  if ( !_Thread_Owns_resources( holder ) ) {
+    /*
+     * Ensure that the holder resource count is visible to all other processors
+     * and that we read the latest priority restore hint.
+     */
+    _Atomic_Fence( ATOMIC_ORDER_ACQ_REL );
 
-    cpu_self = _Thread_Dispatch_disable();
-    _Thread_Change_priority( holder, holder->real_priority, true );
-    _Thread_Dispatch_enable( cpu_self );
+    if ( holder->priority_restore_hint ) {
+      Per_CPU_Control *cpu_self;
+
+      cpu_self = _Thread_Dispatch_disable();
+      _Thread_Restore_priority( holder );
+      _Thread_Dispatch_enable( cpu_self );
+    }
   }
 
   return CORE_MUTEX_STATUS_SUCCESSFUL;
diff --git a/cpukit/score/src/schedulercbs.c b/cpukit/score/src/schedulercbs.c
index 44221cdbdf..98ec0eb29e 100644
--- a/cpukit/score/src/schedulercbs.c
+++ b/cpukit/score/src/schedulercbs.c
@@ -27,15 +27,13 @@ void _Scheduler_CBS_Budget_callout(
 )
 {
   Priority_Control          new_priority;
+  Priority_Control          unused;
   Scheduler_CBS_Node       *node;
   Scheduler_CBS_Server_id   server_id;
 
   /* Put violating task to background until the end of period. */
   new_priority = the_thread->Start.initial_priority;
-  if ( the_thread->real_priority != new_priority )
-    the_thread->real_priority = new_priority;
-  if ( the_thread->current_priority != new_priority )
-    _Thread_Change_priority(the_thread, new_priority, true);
+  _Thread_Set_priority( the_thread, new_priority, &unused, true );
 
   /* Invoke callback function if any. */
   node = _Scheduler_CBS_Thread_get_node( the_thread );
diff --git a/cpukit/score/src/schedulercbsreleasejob.c b/cpukit/score/src/schedulercbsreleasejob.c
index 36a31551cf..a9f8e33201 100644
--- a/cpukit/score/src/schedulercbsreleasejob.c
+++ b/cpukit/score/src/schedulercbsreleasejob.c
@@ -32,6 +32,7 @@ void _Scheduler_CBS_Release_job(
   Scheduler_CBS_Node   *node = _Scheduler_CBS_Thread_get_node( the_thread );
   Scheduler_CBS_Server *serv_info = node->cbs_server;
   Priority_Control      new_priority;
+  Priority_Control      unused;
 
   if (deadline) {
     /* Initializing or shifting deadline. */
@@ -51,6 +52,5 @@ void _Scheduler_CBS_Release_job(
   if (serv_info)
     the_thread->cpu_time_budget = serv_info->parameters.budget;
 
-  the_thread->real_priority = new_priority;
-  _Thread_Change_priority(the_thread, new_priority, true);
+  _Thread_Set_priority( the_thread, new_priority, &unused, true );
 }
diff --git a/cpukit/score/src/scheduleredfreleasejob.c b/cpukit/score/src/scheduleredfreleasejob.c
index 6c1b642890..2c3db65b64 100644
--- a/cpukit/score/src/scheduleredfreleasejob.c
+++ b/cpukit/score/src/scheduleredfreleasejob.c
@@ -29,6 +29,7 @@ void _Scheduler_EDF_Release_job(
 )
 {
   Priority_Control new_priority;
+  Priority_Control unused;
 
   (void) scheduler;
 
@@ -42,6 +43,5 @@ void _Scheduler_EDF_Release_job(
     new_priority = the_thread->Start.initial_priority;
   }
 
-  the_thread->real_priority = new_priority;
-  _Thread_Change_priority(the_thread, new_priority, true);
+  _Thread_Set_priority( the_thread, new_priority, &unused, true );
 }
diff --git a/cpukit/score/src/thread.c b/cpukit/score/src/thread.c
index 8a34ced684..ef9788ca41 100644
--- a/cpukit/score/src/thread.c
+++ b/cpukit/score/src/thread.c
@@ -32,6 +32,7 @@ THREAD_OFFSET_ASSERT( current_state );
 THREAD_OFFSET_ASSERT( current_priority );
 THREAD_OFFSET_ASSERT( real_priority );
 THREAD_OFFSET_ASSERT( priority_generation );
+THREAD_OFFSET_ASSERT( priority_restore_hint );
 THREAD_OFFSET_ASSERT( resource_count );
 THREAD_OFFSET_ASSERT( Wait );
 THREAD_OFFSET_ASSERT( Timer );
diff --git a/cpukit/score/src/threadchangepriority.c b/cpukit/score/src/threadchangepriority.c
index 3223544fe3..8f5d14f412 100644
--- a/cpukit/score/src/threadchangepriority.c
+++ b/cpukit/score/src/threadchangepriority.c
@@ -21,12 +21,13 @@
 
 #include <rtems/score/threadimpl.h>
 #include <rtems/score/schedulerimpl.h>
-#include <rtems/score/threadqimpl.h>
 
 void _Thread_Change_priority(
-  Thread_Control   *the_thread,
-  Priority_Control  new_priority,
-  bool              prepend_it
+  Thread_Control                *the_thread,
+  Priority_Control               new_priority,
+  void                          *arg,
+  Thread_Change_priority_filter  filter,
+  bool                           prepend_it
 )
 {
   ISR_lock_Context  lock_context;
@@ -35,10 +36,20 @@ void _Thread_Change_priority(
   lock = _Thread_Lock_acquire( the_thread, &lock_context );
 
   /*
+   * For simplicity set the priority restore hint unconditionally since this is
+   * an average case optimization.  Otherwise complicated atomic operations
+   * would be necessary.  Synchronize with a potential read of the resource
+   * count in the filter function.  See also _CORE_mutex_Surrender(),
+   * _Thread_Set_priority_filter() and _Thread_Restore_priority_filter().
+   */
+  the_thread->priority_restore_hint = true;
+  _Atomic_Fence( ATOMIC_ORDER_ACQ_REL );
+
+  /*
    *  Do not bother recomputing all the priority related information if
    *  we are not REALLY changing priority.
    */
-  if ( the_thread->current_priority != new_priority ) {
+  if ( ( *filter )( the_thread, &new_priority, arg ) ) {
     uint32_t my_generation;
 
     my_generation = the_thread->priority_generation + 1;
@@ -72,3 +83,53 @@ void _Thread_Change_priority(
     _Thread_Lock_release( lock, &lock_context );
   }
 }
+
+static bool _Thread_Raise_priority_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority,
+  void             *arg
+)
+{
+  return _Thread_Priority_less_than(
+    the_thread->current_priority,
+    *new_priority
+  );
+}
+
+void _Thread_Raise_priority(
+  Thread_Control   *the_thread,
+  Priority_Control  new_priority
+)
+{
+  _Thread_Change_priority(
+    the_thread,
+    new_priority,
+    NULL,
+    _Thread_Raise_priority_filter,
+    false
+  );
+}
+
+static bool _Thread_Restore_priority_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority,
+  void             *arg
+)
+{
+  *new_priority = the_thread->real_priority;
+
+  the_thread->priority_restore_hint = false;
+
+  return *new_priority != the_thread->current_priority;
+}
+
+void _Thread_Restore_priority( Thread_Control *the_thread )
+{
+  _Thread_Change_priority(
+    the_thread,
+    0,
+    NULL,
+    _Thread_Restore_priority_filter,
+    true
+  );
+}
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index 9f5df257aa..2133d7485b 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -201,6 +201,7 @@ bool _Thread_Initialize(
   the_thread->Wait.queue              = NULL;
   the_thread->Wait.operations         = &_Thread_queue_Operations_default;
   the_thread->resource_count          = 0;
+  the_thread->current_priority        = priority;
   the_thread->real_priority           = priority;
   the_thread->priority_generation     = 0;
   the_thread->Start.initial_priority  = priority;
@@ -210,7 +211,7 @@ bool _Thread_Initialize(
   _Scheduler_Node_initialize( scheduler, the_thread );
   scheduler_node_initialized = true;
 
-  _Thread_Set_priority( the_thread, priority );
+  _Scheduler_Update_priority( the_thread, priority );
 
   /*
    *  Initialize the CPU usage statistics
diff --git a/cpukit/score/src/threadrestart.c b/cpukit/score/src/threadrestart.c
index 8dea518e10..b98b6388f8 100644
--- a/cpukit/score/src/threadrestart.c
+++ b/cpukit/score/src/threadrestart.c
@@ -42,6 +42,28 @@ static Thread_Zombie_control _Thread_Zombies = {
   .Lock = ISR_LOCK_INITIALIZER( "thread zombies" )
 };
 
+static bool _Thread_Raise_real_priority_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority_ptr,
+  void             *arg
+)
+{
+  Priority_Control real_priority;
+  Priority_Control new_priority;
+  Priority_Control current_priority;
+
+  real_priority = the_thread->real_priority;
+  new_priority = *new_priority_ptr;
+  current_priority = the_thread->current_priority;
+
+  new_priority = _Thread_Priority_highest( real_priority, new_priority );
+  *new_priority_ptr = new_priority;
+
+  the_thread->real_priority = new_priority;
+
+  return _Thread_Priority_less_than( current_priority, new_priority );
+}
+
 static void _Thread_Make_zombie( Thread_Control *the_thread )
 {
   ISR_lock_Context lock_context;
@@ -231,12 +253,17 @@ static void _Thread_Start_life_change(
   the_thread->is_preemptible   = the_thread->Start.is_preemptible;
   the_thread->budget_algorithm = the_thread->Start.budget_algorithm;
   the_thread->budget_callout   = the_thread->Start.budget_callout;
-  the_thread->real_priority    = priority;
 
   _Thread_Set_state( the_thread, STATES_RESTARTING );
   _Thread_queue_Extract_with_proxy( the_thread );
   _Watchdog_Remove_ticks( &the_thread->Timer );
-  _Scheduler_Set_priority_if_higher( scheduler, the_thread, priority );
+  _Thread_Change_priority(
+    the_thread,
+    priority,
+    NULL,
+    _Thread_Raise_real_priority_filter,
+    false
+  );
   _Thread_Add_post_switch_action( the_thread, &the_thread->Life.Action );
   _Thread_Ready( the_thread );
 }
@@ -260,9 +287,9 @@ static void _Thread_Request_life_change(
 
   scheduler = _Scheduler_Get( the_thread );
   if ( the_thread == executing ) {
-    executing->real_priority = priority;
+    Priority_Control unused;
 
-    _Scheduler_Set_priority_if_higher( scheduler, the_thread, priority );
+    _Thread_Set_priority( the_thread, priority, &unused, true );
     _Thread_Start_life_change_for_executing( executing );
   } else if ( previous_life_state == THREAD_LIFE_NORMAL ) {
     _Thread_Start_life_change( the_thread, scheduler, priority );
@@ -270,16 +297,11 @@ static void _Thread_Request_life_change(
     _Thread_Clear_state( the_thread, STATES_SUSPENDED );
 
     if ( _Thread_Is_life_terminating( additional_life_state ) ) {
-      the_thread->real_priority = _Scheduler_Highest_priority_of_two(
-        scheduler,
-        the_thread->real_priority,
-        priority
-      );
-
-      _Scheduler_Change_priority_if_higher(
-        scheduler,
+      _Thread_Change_priority(
         the_thread,
         priority,
+        NULL,
+        _Thread_Raise_real_priority_filter,
         false
       );
     }
diff --git a/cpukit/score/src/threadsetpriority.c b/cpukit/score/src/threadsetpriority.c
index e1ff118c7e..f6a061a281 100644
--- a/cpukit/score/src/threadsetpriority.c
+++ b/cpukit/score/src/threadsetpriority.c
@@ -19,14 +19,41 @@
 #endif
 
 #include <rtems/score/threadimpl.h>
-#include <rtems/score/schedulerimpl.h>
 
-void _Thread_Set_priority(
+static bool _Thread_Set_priority_filter(
   Thread_Control   *the_thread,
-  Priority_Control  new_priority
+  Priority_Control *new_priority_ptr,
+  void             *arg
 )
 {
-  the_thread->current_priority = new_priority;
+  Priority_Control  current_priority;
+  Priority_Control  new_priority;
+  Priority_Control *old_priority_ptr;
+
+  current_priority = the_thread->current_priority;
+  new_priority = *new_priority_ptr;
+
+  old_priority_ptr = arg;
+  *old_priority_ptr = current_priority;
+
+  the_thread->real_priority = new_priority;
+
+  return _Thread_Priority_less_than( current_priority, new_priority )
+    || !_Thread_Owns_resources( the_thread );
+}
 
-  _Scheduler_Update_priority( the_thread, new_priority );
+void _Thread_Set_priority(
+  Thread_Control   *the_thread,
+  Priority_Control  new_priority,
+  Priority_Control *old_priority,
+  bool              prepend_it
+)
+{
+  _Thread_Change_priority(
+    the_thread,
+    new_priority,
+    old_priority,
+    _Thread_Set_priority_filter,
+    prepend_it
+  );
 }
diff --git a/testsuites/smptests/smpscheduler03/init.c b/testsuites/smptests/smpscheduler03/init.c
index d919482c8a..d54b6aacc3 100644
--- a/testsuites/smptests/smpscheduler03/init.c
+++ b/testsuites/smptests/smpscheduler03/init.c
@@ -40,6 +40,30 @@ typedef struct {
 
 static test_context test_instance;
 
+static bool change_priority_filter(
+  Thread_Control   *thread,
+  Priority_Control *new_priority,
+  void             *arg
+)
+{
+  return thread->current_priority != *new_priority;
+}
+
+static void change_priority(
+  Thread_Control   *thread,
+  Priority_Control  new_priority,
+  bool              prepend_it
+)
+{
+  _Thread_Change_priority(
+    thread,
+    new_priority,
+    NULL,
+    change_priority_filter,
+    prepend_it
+  );
+}
+
 static void barrier_wait(test_context *ctx)
 {
   rtems_status_code sc;
@@ -97,10 +121,10 @@ static void test_case_change_priority(
 {
   switch (start_state) {
     case SCHEDULER_SMP_NODE_SCHEDULED:
-      _Thread_Change_priority(executing, 1, true);
+      change_priority(executing, 1, true);
       break;
     case SCHEDULER_SMP_NODE_READY:
-      _Thread_Change_priority(executing, 4, true);
+      change_priority(executing, 4, true);
       break;
     default:
       rtems_test_assert(0);
@@ -108,7 +132,7 @@ static void test_case_change_priority(
   }
   rtems_test_assert(node->state == start_state);
 
-  _Thread_Change_priority(executing, prio, prepend_it);
+  change_priority(executing, prio, prepend_it);
   rtems_test_assert(node->state == new_state);
 }
 
@@ -153,7 +177,7 @@ static void test_change_priority(void)
     }
   }
 
-  _Thread_Change_priority(executing, 1, true);
+  change_priority(executing, 1, true);
   rtems_test_assert(node->state == SCHEDULER_SMP_NODE_SCHEDULED);
 
   _Thread_Enable_dispatch();
@@ -199,10 +223,10 @@ static void test_case_change_priority_op(
 
   switch (start_state) {
     case SCHEDULER_SMP_NODE_SCHEDULED:
-      _Thread_Change_priority(executing, 1, true);
+      change_priority(executing, 1, true);
       break;
     case SCHEDULER_SMP_NODE_READY:
-      _Thread_Change_priority(executing, 4, true);
+      change_priority(executing, 4, true);
       break;
     default:
       rtems_test_assert(0);
@@ -266,7 +290,7 @@ static void test_change_priority_op(void)
     }
   }
 
-  _Thread_Change_priority(executing, 1, true);
+  change_priority(executing, 1, true);
   rtems_test_assert(executing_node->state == SCHEDULER_SMP_NODE_SCHEDULED);
 
   _Thread_Enable_dispatch();
@@ -298,19 +322,19 @@ static void test_case_yield_op(
 {
   Thread_Control *needs_help;
 
-  _Thread_Change_priority(executing, 4, false);
-  _Thread_Change_priority(other, 4, false);
+  change_priority(executing, 4, false);
+  change_priority(other, 4, false);
 
   switch (start_state) {
     case SCHEDULER_SMP_NODE_SCHEDULED:
       switch (new_state) {
         case SCHEDULER_SMP_NODE_SCHEDULED:
-          _Thread_Change_priority(executing, 2, false);
-          _Thread_Change_priority(other, 3, false);
+          change_priority(executing, 2, false);
+          change_priority(other, 3, false);
           break;
         case SCHEDULER_SMP_NODE_READY:
-          _Thread_Change_priority(executing, 2, false);
-          _Thread_Change_priority(other, 2, false);
+          change_priority(executing, 2, false);
+          change_priority(other, 2, false);
           break;
         default:
           rtems_test_assert(0);
@@ -323,8 +347,8 @@ static void test_case_yield_op(
           rtems_test_assert(0);
           break;
         case SCHEDULER_SMP_NODE_READY:
-          _Thread_Change_priority(executing, 3, false);
-          _Thread_Change_priority(other, 2, false);
+          change_priority(executing, 3, false);
+          change_priority(other, 2, false);
           break;
         default:
           rtems_test_assert(0);
@@ -393,7 +417,7 @@ static void test_yield_op(void)
     }
   }
 
-  _Thread_Change_priority(executing, 1, true);
+  change_priority(executing, 1, true);
   rtems_test_assert(executing_node->state == SCHEDULER_SMP_NODE_SCHEDULED);
 
   _Thread_Enable_dispatch();
@@ -436,11 +460,11 @@ static void test_case_unblock_op(
 
   switch (new_state) {
     case SCHEDULER_SMP_NODE_SCHEDULED:
-      _Thread_Change_priority(executing, 2, false);
+      change_priority(executing, 2, false);
       rtems_test_assert(executing_node->state == SCHEDULER_SMP_NODE_SCHEDULED);
       break;
     case SCHEDULER_SMP_NODE_READY:
-      _Thread_Change_priority(executing, 4, false);
+      change_priority(executing, 4, false);
       rtems_test_assert(executing_node->state == SCHEDULER_SMP_NODE_READY);
       break;
     default:
@@ -494,7 +518,7 @@ static void test_unblock_op(void)
     );
   }
 
-  _Thread_Change_priority(executing, 1, true);
+  change_priority(executing, 1, true);
   rtems_test_assert(executing_node->state == SCHEDULER_SMP_NODE_SCHEDULED);
 
   _Thread_Enable_dispatch();