smp: Fix scheduler helping protocol

New test case for smptests/smpmrsp01.

Fix _Scheduler_Block_node() in case the node is in the
SCHEDULER_HELP_ACTIVE_RIVAL helping state.  For example a
rtems_task_suspend() on a task waiting for a MrsP semaphore.

Fix _Scheduler_Unblock_node() in case the node is in the
SCHEDULER_SMP_NODE_READY state.  For example a rtems_task_resume() on a
task owning or waiting for a MrsP semaphore.

2 files changed, 35 insertions, 18 deletions

diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index 45a2f8da9d..b262b91f84 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -1081,6 +1081,7 @@ RTEMS_INLINE_ROUTINE Thread_Control *_Scheduler_Release_idle_thread(
  */
 RTEMS_INLINE_ROUTINE bool _Scheduler_Block_node(
   Scheduler_Context         *context,
+  Thread_Control            *thread,
   Scheduler_Node            *node,
   bool                       is_scheduled,
   Scheduler_Get_idle_thread  get_idle_thread
@@ -1088,25 +1089,24 @@ RTEMS_INLINE_ROUTINE bool _Scheduler_Block_node(
 {
   bool block;
   Thread_Control *old_user = _Scheduler_Node_get_user( node );
-  Thread_Control *new_user;
+  Thread_Control *new_user = NULL;
 
   _Scheduler_Thread_change_state( old_user, THREAD_SCHEDULER_BLOCKED );
 
-  if ( node->help_state == SCHEDULER_HELP_ACTIVE_RIVAL ) {
-    new_user = _Scheduler_Node_get_owner( node );
-
-    _Assert( new_user != old_user );
-    _Scheduler_Node_set_user( node, new_user );
-  } else if (
-    node->help_state == SCHEDULER_HELP_ACTIVE_OWNER
-      && is_scheduled
-  ) {
-    new_user = _Scheduler_Use_idle_thread( context, node, get_idle_thread );
-  } else {
-    new_user = NULL;
+  if ( is_scheduled ) {
+    if ( node->help_state == SCHEDULER_HELP_ACTIVE_OWNER ) {
+      new_user = _Scheduler_Use_idle_thread( context, node, get_idle_thread );
+    } else if ( node->help_state == SCHEDULER_HELP_ACTIVE_RIVAL ) {
+      Thread_Control *owner = _Scheduler_Node_get_owner( node );
+
+      if ( thread == old_user && owner != old_user ) {
+        new_user = owner;
+        _Scheduler_Node_set_user( node, new_user );
+      }
+    }
   }
 
-  if ( new_user != NULL && is_scheduled ) {
+  if ( new_user != NULL ) {
     Per_CPU_Control *cpu = _Thread_Get_CPU( old_user );
 
     _Scheduler_Thread_change_state( new_user, THREAD_SCHEDULER_SCHEDULED );
diff --git a/cpukit/score/include/rtems/score/schedulersmpimpl.h b/cpukit/score/include/rtems/score/schedulersmpimpl.h
index 156307db24..0ddfce09fe 100644
--- a/cpukit/score/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulersmpimpl.h
@@ -793,13 +793,17 @@ static inline void _Scheduler_SMP_Block(
 {
   Scheduler_SMP_Node *node = _Scheduler_SMP_Thread_get_node( thread );
   bool is_scheduled = node->state == SCHEDULER_SMP_NODE_SCHEDULED;
-  bool block = _Scheduler_Block_node(
+  bool block;
+
+  _Assert( is_scheduled || node->state == SCHEDULER_SMP_NODE_READY );
+
+  block = _Scheduler_Block_node(
     context,
+    thread,
     &node->Base,
     is_scheduled,
     _Scheduler_SMP_Get_idle_thread
   );
-
   if ( block ) {
     _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_BLOCKED );
 
@@ -838,9 +842,22 @@ static inline Thread_Control *_Scheduler_SMP_Unblock(
   Thread_Control *needs_help;
 
   if ( unblock ) {
-    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
+    if ( node->state != SCHEDULER_SMP_NODE_READY ) {
+      _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
+
+      needs_help = ( *enqueue_fifo )( context, &node->Base, thread );
+    } else {
+      _Assert( node->state == SCHEDULER_SMP_NODE_READY );
+      _Assert( node->Base.idle == NULL );
 
-    needs_help = ( *enqueue_fifo )( context, &node->Base, thread );
+      if ( node->Base.accepts_help == thread ) {
+        _Assert( node->Base.help_state == SCHEDULER_HELP_ACTIVE_OWNER );
+        needs_help = thread;
+      } else {
+        _Assert( node->Base.help_state == SCHEDULER_HELP_ACTIVE_RIVAL );
+        needs_help = NULL;
+      }
+    }
   } else {
     needs_help = NULL;
   }