[PATCH 17/30] score: Yield support for new SMP helping protocol

[Sebastian Huber]

Update #2556.
---
 cpukit/score/include/rtems/score/schedulerimpl.h   | 58 ++++++++++++++++++----
 .../score/include/rtems/score/schedulersmpimpl.h   | 11 ++--
 testsuites/smptests/smpmutex01/init.c              | 52 +++++++++++++++++++
 3 files changed, 107 insertions(+), 14 deletions(-)

diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index dbb17a8..ea32e00 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -288,29 +288,65 @@ RTEMS_INLINE_ROUTINE void _Scheduler_Ask_for_help_if_necessary(
  */
 RTEMS_INLINE_ROUTINE void _Scheduler_Yield( Thread_Control *the_thread )
 {
+#if defined(RTEMS_SMP)
+  Chain_Node              *node;
+  const Chain_Node        *tail;
+  Scheduler_Node          *scheduler_node;
   const Scheduler_Control *scheduler;
   ISR_lock_Context         lock_context;
-#if defined(RTEMS_SMP)
   Thread_Control          *needs_help;
-#endif
 
-  scheduler = _Scheduler_Get( the_thread );
+  node = _Chain_First( &the_thread->Scheduler.Scheduler_nodes );
+  tail = _Chain_Immutable_tail( &the_thread->Scheduler.Scheduler_nodes );
+
+  scheduler_node = SCHEDULER_NODE_OF_THREAD_SCHEDULER_NODE( node );
+  scheduler = _Scheduler_Node_get_scheduler( scheduler_node );
+
   _Scheduler_Acquire_critical( scheduler, &lock_context );
+  needs_help = ( *scheduler->Operations.yield )(
+    scheduler,
+    the_thread,
+    _Thread_Scheduler_get_home_node( the_thread )
+  );
+  _Scheduler_Ask_for_help_if_necessary( needs_help );
+  _Scheduler_Release_critical( scheduler, &lock_context );
 
-#if defined(RTEMS_SMP)
-  needs_help =
-#endif
+  if ( needs_help != the_thread ) {
+    return;
+  }
+
+  node = _Chain_Next( node );
+
+  while ( node != tail ) {
+    bool success;
+
+    scheduler_node = SCHEDULER_NODE_OF_THREAD_SCHEDULER_NODE( node );
+    scheduler = _Scheduler_Node_get_scheduler( scheduler_node );
+
+    _Scheduler_Acquire_critical( scheduler, &lock_context );
+    success = ( *scheduler->Operations.ask_for_help )(
+      scheduler,
+      the_thread,
+      scheduler_node
+    );
+    _Scheduler_Release_critical( scheduler, &lock_context );
+
+    if ( success ) {
+      break;
+    }
+
+    node = _Chain_Next( node );
+  }
+#else
+  const Scheduler_Control *scheduler;
+
+  scheduler = _Scheduler_Get( the_thread );
   ( *scheduler->Operations.yield )(
     scheduler,
     the_thread,
     _Thread_Scheduler_get_home_node( the_thread )
   );
-
-#if defined(RTEMS_SMP)
-  _Scheduler_Ask_for_help_if_necessary( needs_help );
 #endif
-
-  _Scheduler_Release_critical( scheduler, &lock_context );
 }
 
 /**
diff --git a/cpukit/score/include/rtems/score/schedulersmpimpl.h b/cpukit/score/include/rtems/score/schedulersmpimpl.h
index 8f9bf2f..0e7d358 100644
--- a/cpukit/score/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulersmpimpl.h
@@ -1138,16 +1138,21 @@ static inline Thread_Control *_Scheduler_SMP_Yield(
   Scheduler_SMP_Enqueue_scheduled  enqueue_scheduled_fifo
 )
 {
-  Thread_Control *needs_help;
+  Thread_Control           *needs_help;
+  Scheduler_SMP_Node_state  node_state;
+
+  node_state = _Scheduler_SMP_Node_state( node );
 
-  if ( _Scheduler_SMP_Node_state( node ) == SCHEDULER_SMP_NODE_SCHEDULED ) {
+  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
     _Scheduler_SMP_Extract_from_scheduled( node );
 
     needs_help = ( *enqueue_scheduled_fifo )( context, node );
-  } else {
+  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
     ( *extract_from_ready )( context, node );
 
     needs_help = ( *enqueue_fifo )( context, node, NULL );
+  } else {
+    needs_help = thread;
   }
 
   return needs_help;
diff --git a/testsuites/smptests/smpmutex01/init.c b/testsuites/smptests/smpmutex01/init.c
index 8e499b4..33b7e93 100644
--- a/testsuites/smptests/smpmutex01/init.c
+++ b/testsuites/smptests/smpmutex01/init.c
@@ -313,6 +313,14 @@ static void check_generations(test_context *ctx, task_id a, task_id b)
   }
 }
 
+static void yield(void)
+{
+  rtems_status_code sc;
+
+  sc = rtems_task_wake_after(RTEMS_YIELD_PROCESSOR);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+}
+
 static void set_prio(test_context *ctx, task_id id, rtems_task_priority prio)
 {
   rtems_status_code sc;
@@ -858,6 +866,49 @@ static void test_omip_timeout(test_context *ctx)
   release(ctx);
 }
 
+static void test_omip_yield(test_context *ctx)
+{
+  assert_cpu(0);
+  obtain(ctx);
+  assert_prio_by_scheduler(ctx, M, SCHED_A, 3);
+  assert_prio_by_scheduler(ctx, M, SCHED_B, PRIO_NONE);
+
+  request(ctx, B_5_0, REQ_MTX_OBTAIN);
+  assert_prio_by_scheduler(ctx, M, SCHED_A, 3);
+  assert_prio_by_scheduler(ctx, M, SCHED_B, 5);
+  check_generations(ctx, NONE, NONE);
+
+  clear_done(ctx);
+  send_event(ctx, H_A, REQ_SET_DONE);
+  yield();
+  assert_cpu(1);
+  wait_for_done(ctx);
+
+  clear_done(ctx);
+  send_event(ctx, H_B, REQ_SET_DONE);
+  set_prio(ctx, H_B, 5);
+  yield();
+  assert_cpu(1);
+  rtems_test_assert(!is_done(ctx));
+
+  set_prio(ctx, H_B, 4);
+  assert_cpu(0);
+
+  wait_for_done(ctx);
+  set_prio(ctx, H_B, 6);
+
+  release(ctx);
+  sync_with_helper(ctx);
+  assert_prio_by_scheduler(ctx, M, SCHED_A, 3);
+  assert_prio_by_scheduler(ctx, M, SCHED_B, PRIO_NONE);
+  check_generations(ctx, B_5_0, NONE);
+
+  request(ctx, B_5_0, REQ_MTX_RELEASE);
+  assert_prio_by_scheduler(ctx, B_5_0, SCHED_A, PRIO_NONE);
+  assert_prio_by_scheduler(ctx, B_5_0, SCHED_B, 5);
+  check_generations(ctx, B_5_0, NONE);
+}
+
 static void test(void)
 {
   test_context *ctx = &test_instance;
@@ -874,6 +925,7 @@ static void test(void)
   test_omip_pre_emption(ctx);
   test_omip_rescue(ctx);
   test_omip_timeout(ctx);
+  test_omip_yield(ctx);
 }
 
 static void Init(rtems_task_argument arg)
-- 
1.8.4.5