score: Scheduler node awareness for thread queues

Maintain the priority of a thread for each scheduler instance via the
thread queue enqueue, extract, priority actions and surrender
operations.  This replaces the primitive priority boosting.

Update #2556.

cpukit/score/include/rtems/score/schedulernode.h

@@ -209,6 +209,11 @@ typedef struct Scheduler_Node {
 extern const size_t _Scheduler_Node_size;
 #endif
 
+#if defined(RTEMS_SMP)
+#define SCHEDULER_NODE_OF_THREAD_WAIT_NODE( node ) \
+  RTEMS_CONTAINER_OF( node, Scheduler_Node, Thread.Wait_node )
+#endif
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */

cpukit/score/include/rtems/score/threadimpl.h

@@ -997,6 +997,20 @@ RTEMS_INLINE_ROUTINE Scheduler_Node *_Thread_Scheduler_get_own_node(
 #endif
 }
 
+RTEMS_INLINE_ROUTINE Scheduler_Node *_Thread_Scheduler_get_home_node(
+  const Thread_Control *the_thread
+)
+{
+#if defined(RTEMS_SMP)
+  _Assert( !_Chain_Is_empty( &the_thread->Scheduler.Wait_nodes ) );
+  return SCHEDULER_NODE_OF_THREAD_WAIT_NODE(
+    _Chain_First( &the_thread->Scheduler.Wait_nodes )
+  );
+#else
+  return the_thread->Scheduler.nodes;
+#endif
+}
+
 RTEMS_INLINE_ROUTINE Scheduler_Node *_Thread_Scheduler_get_node_by_index(
   const Thread_Control *the_thread,
   size_t                scheduler_index
@@ -1308,21 +1322,22 @@ RTEMS_INLINE_ROUTINE void _Thread_Wait_release(
 }
 
 /**
- * @brief Claims the thread wait queue and operations.
+ * @brief Claims the thread wait queue.
  *
  * The caller must not be the owner of the default thread wait lock.  The
- * caller must be the owner of the corresponding thread queue lock.
+ * caller must be the owner of the corresponding thread queue lock.  The
+ * registration of the corresponding thread queue operations is deferred and
+ * done after the deadlock detection.  This is crucial to support timeouts on
+ * SMP configurations.
  *
  * @param[in] the_thread The thread.
  * @param[in] queue The new thread queue.
- * @param[in] operations The new thread operations.
  *
- * @see _Thread_Wait_restore_default().
+ * @see _Thread_Wait_claim_finalize() and _Thread_Wait_restore_default().
  */
 RTEMS_INLINE_ROUTINE void _Thread_Wait_claim(
-  Thread_Control                *the_thread,
-  Thread_queue_Queue            *queue,
-  const Thread_queue_Operations *operations
+  Thread_Control     *the_thread,
+  Thread_queue_Queue *queue
 )
 {
   ISR_lock_Context lock_context;
@@ -1338,11 +1353,25 @@ RTEMS_INLINE_ROUTINE void _Thread_Wait_claim(
 #endif
 
   the_thread->Wait.queue = queue;
-  the_thread->Wait.operations = operations;
 
   _Thread_Wait_release_default_critical( the_thread, &lock_context );
 }
 
+/**
+ * @brief Finalizes the thread wait queue claim via registration of the
+ * corresponding thread queue operations.
+ *
+ * @param[in] the_thread The thread.
+ * @param[in] operations The corresponding thread queue operations.
+ */
+RTEMS_INLINE_ROUTINE void _Thread_Wait_claim_finalize(
+  Thread_Control                *the_thread,
+  const Thread_queue_Operations *operations
+)
+{
+  the_thread->Wait.operations = operations;
+}
+
 /**
  * @brief Removes a thread wait lock request.
  *

cpukit/score/include/rtems/score/threadq.h

@@ -216,6 +216,12 @@ typedef struct {
      * @brief The start of a thread queue path.
      */
     Thread_queue_Link Start;
+
+    /**
+     * @brief In case of a deadlock, a link for the first thread on the path
+     * that tries to enqueue on a thread queue.
+     */
+    Thread_queue_Link Deadlock;
   } Path;
 #endif
 
@@ -344,11 +350,6 @@ typedef struct _Thread_queue_Heads {
   Chain_Node Free_node;
 
 #if defined(RTEMS_SMP)
-  /**
-   * @brief Boost priority.
-   */
-  Priority_Node Boost_priority;
-
   /**
    * @brief One priority queue per scheduler instance.
    */

cpukit/score/include/rtems/score/threadqimpl.h

@@ -280,12 +280,10 @@ RTEMS_INLINE_ROUTINE void _Thread_queue_Heads_initialize(
 #if defined(RTEMS_SMP)
   size_t i;
 
-  _Priority_Node_initialize( &heads->Boost_priority, 0 );
-  _Priority_Node_set_inactive( &heads->Boost_priority );
-
   for ( i = 0; i < _Scheduler_Count; ++i ) {
     _Chain_Initialize_node( &heads->Priority[ i ].Node );
     _Priority_Initialize_empty( &heads->Priority[ i ].Queue );
+    heads->Priority[ i ].Queue.scheduler = &_Scheduler_Table[ i ];
   }
 #endif
 
@@ -955,6 +953,7 @@ void _Thread_queue_Unblock_proxy(
 );
 #endif
 
+#if defined(RTEMS_SMP)
 bool _Thread_queue_Path_acquire_critical(
   Thread_queue_Queue   *queue,
   Thread_Control       *the_thread,
@@ -964,6 +963,7 @@ bool _Thread_queue_Path_acquire_critical(
 void _Thread_queue_Path_release_critical(
   Thread_queue_Context *queue_context
 );
+#endif
 
 /**
  * @brief Helper structure to ensure that all objects containing a thread queue

cpukit/score/src/threadchangepriority.c

@@ -44,6 +44,16 @@ static void _Thread_Priority_action_add(
   void                 *arg
 )
 {
+  Scheduler_Node *scheduler_node;
+  Thread_Control *the_thread;
+
+  scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY( priority_aggregation );
+  the_thread = arg;
+
+  _Chain_Append_unprotected(
+    &the_thread->Scheduler.Wait_nodes,
+    &scheduler_node->Thread.Wait_node
+  );
   _Thread_Set_scheduler_node_priority( priority_aggregation, false );
   _Priority_Set_action_type( priority_aggregation, PRIORITY_ACTION_ADD );
   _Priority_Actions_add( priority_actions, priority_aggregation );
@@ -55,6 +65,11 @@ static void _Thread_Priority_action_remove(
   void                 *arg
 )
 {
+  Scheduler_Node *scheduler_node;
+
+  scheduler_node = SCHEDULER_NODE_OF_WAIT_PRIORITY( priority_aggregation );
+
+  _Chain_Extract_unprotected( &scheduler_node->Thread.Wait_node );
   _Thread_Set_scheduler_node_priority( priority_aggregation, true );
   _Priority_Set_action_type( priority_aggregation, PRIORITY_ACTION_REMOVE );
   _Priority_Actions_add( priority_actions, priority_aggregation );
@@ -107,7 +122,7 @@ static void _Thread_Priority_do_perform_actions(
           &queue_context->Priority.Actions,
           _Thread_Priority_action_add,
           _Thread_Priority_action_change,
-          NULL
+          the_thread
         );
 #else
         _Priority_Non_empty_insert(
@@ -157,6 +172,7 @@ static void _Thread_Priority_do_perform_actions(
 
   if ( !_Priority_Actions_is_empty( &queue_context->Priority.Actions ) ) {
     _Thread_queue_Context_add_priority_update( queue_context, the_thread );
+
     ( *operations->priority_actions )(
       queue,
       &queue_context->Priority.Actions
@@ -169,29 +185,27 @@ void _Thread_Priority_perform_actions(
   Thread_queue_Context *queue_context
 )
 {
-#if defined(RTEMS_SMP)
-  Thread_queue_Link *link;
-#endif
-  Thread_Control    *the_thread;
-  size_t             update_count;
+  Thread_Control *the_thread;
+  size_t          update_count;
 
   _Assert( start_of_path != NULL );
 
-#if defined(RTEMS_SMP)
-  link = &queue_context->Path.Start;
-#endif
+  /*
+   * This function is tricky on SMP configurations.  Please note that we do not
+   * use the thread queue path available via the thread queue context.  Instead
+   * we directly use the thread wait information to traverse the thread queue
+   * path.  Thus, we do not necessarily acquire all thread queue locks on our
+   * own.  In case of a deadlock, we use locks acquired by other processors
+   * along the path.
+   */
+
   the_thread = start_of_path;
   update_count = _Thread_queue_Context_save_priority_updates( queue_context );
 
   while ( true ) {
     Thread_queue_Queue *queue;
 
-#if defined(RTEMS_SMP)
-    _Assert( link->owner == the_thread );
-    queue = link->Lock_context.Wait.queue;
-#else
     queue = the_thread->Wait.queue;
-#endif
 
     _Thread_Priority_do_perform_actions(
       the_thread,
@@ -209,10 +223,6 @@ void _Thread_Priority_perform_actions(
     the_thread = queue->owner;
     _Assert( the_thread != NULL );
 
-#if defined(RTEMS_SMP)
-    link = THREAD_QUEUE_LINK_OF_PATH_NODE( _Chain_Next( &link->Path_node ) );
-#endif
-
     /*
      * In case the priority action list is non-empty, then the current thread
      * is enqueued on a thread queue.  There is no need to notify the scheduler
@@ -255,9 +265,13 @@ static void _Thread_Priority_apply(
   );
 
   if ( !_Priority_Actions_is_empty( &queue_context->Priority.Actions ) ) {
+#if defined(RTEMS_SMP)
     _Thread_queue_Path_acquire_critical( queue, the_thread, queue_context );
+#endif
     _Thread_Priority_perform_actions( queue->owner, queue_context );
+#if defined(RTEMS_SMP)
     _Thread_queue_Path_release_critical( queue_context );
+#endif
   }
 }
 

cpukit/score/src/threadqenqueue.c

@@ -114,6 +114,9 @@ static bool _Thread_queue_Link_add(
   Thread_queue_Queue *recursive_target;
   ISR_lock_Context    lock_context;
 
+  link->source = source;
+  link->target = target;
+
   links = &_Thread_queue_Links;
   recursive_target = target;
 
@@ -136,8 +139,6 @@ static bool _Thread_queue_Link_add(
     }
   }
 
-  link->source = source;
-  link->target = target;
   _RBTree_Insert_inline(
     &links->Links,
     &link->Registry_node,
@@ -162,6 +163,9 @@ static void _Thread_queue_Link_remove( Thread_queue_Link *link )
 }
 #endif
 
+#if !defined(RTEMS_SMP)
+static
+#endif
 void _Thread_queue_Path_release_critical(
   Thread_queue_Context *queue_context
 )
@@ -173,51 +177,80 @@ void _Thread_queue_Path_release_critical(
   head = _Chain_Head( &queue_context->Path.Links );
   node = _Chain_Last( &queue_context->Path.Links );
 
-  if ( head != node ) {
+  while ( head != node ) {
     Thread_queue_Link *link;
 
-    /*
-     * The terminal link may have an owner which does not wait on a thread
-     * queue.
-     */
-
     link = THREAD_QUEUE_LINK_OF_PATH_NODE( node );
 
-    if ( link->Lock_context.Wait.queue == NULL ) {
-      _Thread_Wait_release_default_critical(
-        link->owner,
-        &link->Lock_context.Lock_context
-      );
-
-      node = _Chain_Previous( node );
-#if defined(RTEMS_DEBUG)
-      _Chain_Set_off_chain( &link->Path_node );
-#endif
-    }
-
-    while ( head != node ) {
-      /* The other links have an owner which waits on a thread queue */
-      link = THREAD_QUEUE_LINK_OF_PATH_NODE( node );
-      _Assert( link->Lock_context.Wait.queue != NULL );
-
+    if ( link->Lock_context.Wait.queue != NULL ) {
       _Thread_queue_Link_remove( link );
       _Thread_Wait_release_queue_critical(
         link->Lock_context.Wait.queue,
         &link->Lock_context
       );
       _Thread_Wait_remove_request( link->owner, &link->Lock_context );
-
-      node = _Chain_Previous( node );
-#if defined(RTEMS_DEBUG)
-      _Chain_Set_off_chain( &link->Path_node );
-#endif
+    } else {
+      _Thread_Wait_release_default_critical(
+        link->owner,
+        &link->Lock_context.Lock_context
+      );
     }
+
+    node = _Chain_Previous( node );
+#if defined(RTEMS_DEBUG)
+    _Chain_Set_off_chain( &link->Path_node );
+#endif
   }
 #else
   (void) queue_context;
 #endif
 }
 
+#if defined(RTEMS_SMP)
+static void _Thread_queue_Path_append_deadlock_thread(
+  Thread_Control       *the_thread,
+  Thread_queue_Context *queue_context
+)
+{
+  Thread_Control *deadlock;
+
+  /*
+   * In case of a deadlock, we must obtain the thread wait default lock for the
+   * first thread on the path that tries to enqueue on a thread queue.  This
+   * thread can be identified by the thread wait operations.  This lock acquire
+   * is necessary for the timeout and explicit thread priority changes, see
+   * _Thread_Priority_perform_actions().
+   */
+
+  deadlock = NULL;
+
+  while ( the_thread->Wait.operations != &_Thread_queue_Operations_default ) {
+    the_thread = the_thread->Wait.queue->owner;
+    deadlock = the_thread;
+  }
+
+  if ( deadlock != NULL ) {
+    Thread_queue_Link *link;
+
+    link = &queue_context->Path.Deadlock;
+    _Chain_Initialize_node( &link->Path_node );
+    _Chain_Append_unprotected(
+      &queue_context->Path.Links,
+      &link->Path_node
+    );
+    link->owner = deadlock;
+    link->Lock_context.Wait.queue = NULL;
+    _Thread_Wait_acquire_default_critical(
+      deadlock,
+      &link->Lock_context.Lock_context
+    );
+  }
+}
+#endif
+
+#if !defined(RTEMS_SMP)
+static
+#endif
 bool _Thread_queue_Path_acquire_critical(
   Thread_queue_Queue   *queue,
   Thread_Control       *the_thread,
@@ -249,12 +282,12 @@ bool _Thread_queue_Path_acquire_critical(
     return false;
   }
 
-  _RBTree_Initialize_node( &queue_context->Path.Start.Registry_node );
-  _Chain_Initialize_node( &queue_context->Path.Start.Path_node );
   _Chain_Initialize_node(
     &queue_context->Path.Start.Lock_context.Wait.Gate.Node
   );
   link = &queue_context->Path.Start;
+  _RBTree_Initialize_node( &link->Registry_node );
+  _Chain_Initialize_node( &link->Path_node );
 
   do {
     _Chain_Append_unprotected( &queue_context->Path.Links, &link->Path_node );
@@ -293,6 +326,7 @@ bool _Thread_queue_Path_acquire_critical(
         }
       } else {
         link->Lock_context.Wait.queue = NULL;
+        _Thread_queue_Path_append_deadlock_thread( owner, queue_context );
         return false;
       }
     } else {
@@ -353,7 +387,7 @@ void _Thread_queue_Enqueue_critical(
   }
 #endif
 
-  _Thread_Wait_claim( the_thread, queue, operations );
+  _Thread_Wait_claim( the_thread, queue );
 
   if ( !_Thread_queue_Path_acquire_critical( queue, the_thread, queue_context ) ) {
     _Thread_queue_Path_release_critical( queue_context );
@@ -365,6 +399,7 @@ void _Thread_queue_Enqueue_critical(
   }
 
   _Thread_queue_Context_clear_priority_updates( queue_context );
+  _Thread_Wait_claim_finalize( the_thread, operations );
   ( *operations->enqueue )( queue, the_thread, queue_context );
 
   _Thread_queue_Path_release_critical( queue_context );