score: Fix EDF SMP scheduler

Fix a special case: block a one-to-one scheduled thread while having a
non-empty affine ready queue on the same processor.

2 files changed, 48 insertions, 1 deletions

diff --git a/cpukit/score/src/scheduleredfsmp.c b/cpukit/score/src/scheduleredfsmp.c
index 66e9a8ef36..874ec3e58b 100644
--- a/cpukit/score/src/scheduleredfsmp.c
+++ b/cpukit/score/src/scheduleredfsmp.c
@@ -276,6 +276,29 @@ static inline void _Scheduler_EDF_SMP_Insert_ready(
   }
 }
 
+static inline void _Scheduler_EDF_SMP_Extract_from_scheduled(
+  Scheduler_Context *context,
+  Scheduler_Node    *node_to_extract
+)
+{
+  Scheduler_EDF_SMP_Context     *self;
+  Scheduler_EDF_SMP_Node        *node;
+  uint8_t                        rqi;
+  Scheduler_EDF_SMP_Ready_queue *ready_queue;
+
+  self = _Scheduler_EDF_SMP_Get_self( context );
+  node = _Scheduler_EDF_SMP_Node_downcast( node_to_extract );
+
+  _Scheduler_SMP_Extract_from_scheduled( &self->Base.Base, &node->Base.Base );
+
+  rqi = node->ready_queue_index;
+  ready_queue = &self->Ready[ rqi ];
+
+  if ( rqi != 0 && !_RBTree_Is_empty( &ready_queue->Queue ) ) {
+    _Chain_Append_unprotected( &self->Affine_queues, &ready_queue->Node );
+  }
+}
+
 static inline void _Scheduler_EDF_SMP_Extract_from_ready(
   Scheduler_Context *context,
   Scheduler_Node    *node_to_extract
@@ -403,7 +426,7 @@ void _Scheduler_EDF_SMP_Block(
     context,
     thread,
     node,
-    _Scheduler_SMP_Extract_from_scheduled,
+    _Scheduler_EDF_SMP_Extract_from_scheduled,
     _Scheduler_EDF_SMP_Extract_from_ready,
     _Scheduler_EDF_SMP_Get_highest_ready,
     _Scheduler_EDF_SMP_Move_from_ready_to_scheduled,
diff --git a/testsuites/smptests/smpschededf02/init.c b/testsuites/smptests/smpschededf02/init.c
index e9a521caf5..c045a860e7 100644
--- a/testsuites/smptests/smpschededf02/init.c
+++ b/testsuites/smptests/smpschededf02/init.c
@@ -156,6 +156,30 @@ static const test_action test_actions[] = {
   UNBLOCK(      0,              0, IDLE),
   SET_AFFINITY( 1,  A(1, 0),    0, IDLE),
   UNBLOCK(      1,              1,    0),
+  /*
+   * Block a one-to-one thread while having a non-empty affine ready queue on
+   * the same processor.
+   */
+  RESET,
+  SET_AFFINITY( 1,  A(1, 0), IDLE, IDLE),
+  SET_AFFINITY( 3,  A(1, 0), IDLE, IDLE),
+  UNBLOCK(      0,              0, IDLE),
+  UNBLOCK(      1,              1,    0),
+  UNBLOCK(      2,              1,    0),
+  UNBLOCK(      3,              1,    0),
+  BLOCK(        1,              2,    0),
+  BLOCK(        0,              3,    2),
+  /*
+   * Make sure that a one-to-one thread does not get the wrong processor
+   * allocated after selecting the highest ready thread.
+   */
+  RESET,
+  SET_AFFINITY( 1,  A(1, 0), IDLE, IDLE),
+  SET_AFFINITY( 2,  A(1, 0), IDLE, IDLE),
+  UNBLOCK(      0,              0, IDLE),
+  UNBLOCK(      1,              1,    0),
+  UNBLOCK(      2,              1,    0),
+  BLOCK(        0,              1, IDLE),
   RESET
 };