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.

5 files changed, 171 insertions, 87 deletions

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