score: Add simple affinity support to EDF SMP

Update #3059.

2 files changed, 205 insertions, 12 deletions

diff --git a/cpukit/score/include/rtems/score/scheduleredfsmp.h b/cpukit/score/include/rtems/score/scheduleredfsmp.h
index 8f6e85777a..68f01d2c4b 100644
--- a/cpukit/score/include/rtems/score/scheduleredfsmp.h
+++ b/cpukit/score/include/rtems/score/scheduleredfsmp.h
@@ -34,14 +34,64 @@ extern "C" {
  */
 
 typedef struct {
-  Scheduler_SMP_Context Base;
-  RBTree_Control        Ready;
-} Scheduler_EDF_SMP_Context;
-
-typedef struct {
   Scheduler_SMP_Node Base;
+
+  /**
+   * @brief Generation number to ensure FIFO/LIFO order for threads of the same
+   * priority across different ready queues.
+   */
+  int64_t generation;
+
+  /**
+   * @brief The ready queue index depending on the processor affinity of the thread.
+   *
+   * The ready queue index zero is used for threads with a one-to-all thread
+   * processor affinity.  Threads with a one-to-one processor affinity use the
+   * processor index plus one as the ready queue index.
+   */
+  uint32_t ready_queue_index;
 } Scheduler_EDF_SMP_Node;
 
+typedef struct {
+  /**
+   * @brief Chain node for Scheduler_SMP_Context::Affine_queues.
+   */
+  Chain_Node Node;
+
+  /**
+   * @brief The ready threads of the corresponding affinity.
+   */
+  RBTree_Control Queue;
+
+  /**
+   * @brief The scheduled thread of the corresponding processor.
+   */
+  Scheduler_EDF_SMP_Node *scheduled;
+} Scheduler_EDF_SMP_Ready_queue;
+
+typedef struct {
+  Scheduler_SMP_Context Base;
+
+  /**
+   * @brief Current generation for FIFO/LIFO ordering.
+   */
+  int64_t generations[ 2 ];
+
+  /**
+   * @brief Chain of ready queues with affine threads to determine the highest
+   * priority ready thread.
+   */
+  Chain_Control Affine_queues;
+
+  /**
+   * @brief A table with ready queues.
+   *
+   * The index zero queue is used for threads with a one-to-all processor
+   * affinity.  Index one corresponds to processor index zero, and so on.
+   */
+  Scheduler_EDF_SMP_Ready_queue Ready[ RTEMS_ZERO_LENGTH_ARRAY ];
+} Scheduler_EDF_SMP_Context;
+
 #define SCHEDULER_EDF_SMP_ENTRY_POINTS \
   { \
     _Scheduler_EDF_SMP_Initialize, \
@@ -62,8 +112,8 @@ typedef struct {
     _Scheduler_EDF_Release_job, \
     _Scheduler_EDF_Cancel_job, \
     _Scheduler_default_Tick, \
-    _Scheduler_SMP_Start_idle \
-    SCHEDULER_OPERATION_DEFAULT_GET_SET_AFFINITY \
+    _Scheduler_EDF_SMP_Start_idle, \
+    _Scheduler_EDF_SMP_Set_affinity \
   }
 
 void _Scheduler_EDF_SMP_Initialize( const Scheduler_Control *scheduler );
@@ -128,6 +178,19 @@ void _Scheduler_EDF_SMP_Yield(
   Scheduler_Node          *node
 );
 
+void _Scheduler_EDF_SMP_Start_idle(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *idle,
+  struct Per_CPU_Control  *cpu
+);
+
+bool _Scheduler_EDF_SMP_Set_affinity(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *thread,
+  Scheduler_Node          *node,
+  const Processor_mask    *affinity
+);
+
 /** @} */
 
 #ifdef __cplusplus
diff --git a/cpukit/score/include/rtems/score/schedulersmpimpl.h b/cpukit/score/include/rtems/score/schedulersmpimpl.h
index 620a42f863..3afa6b2a0c 100644
--- a/cpukit/score/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulersmpimpl.h
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2013, 2016 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2013, 2017 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -317,6 +317,12 @@ typedef void ( *Scheduler_SMP_Update )(
   Priority_Control   new_priority
 );
 
+typedef void ( *Scheduler_SMP_Set_affinity )(
+  Scheduler_Context *context,
+  Scheduler_Node    *node,
+  void              *arg
+);
+
 typedef bool ( *Scheduler_SMP_Enqueue )(
   Scheduler_Context *context,
   Scheduler_Node    *node_to_enqueue
@@ -329,6 +335,23 @@ typedef void ( *Scheduler_SMP_Allocate_processor )(
   Per_CPU_Control   *victim_cpu
 );
 
+typedef void ( *Scheduler_SMP_Register_idle )(
+  Scheduler_Context *context,
+  Scheduler_Node    *idle,
+  Per_CPU_Control   *cpu
+);
+
+static inline void _Scheduler_SMP_Do_nothing_register_idle(
+  Scheduler_Context *context,
+  Scheduler_Node    *idle,
+  Per_CPU_Control   *cpu
+)
+{
+  (void) context;
+  (void) idle;
+  (void) cpu;
+}
+
 static inline bool _Scheduler_SMP_Insert_priority_lifo_order(
   const Chain_Node *to_insert,
   const Chain_Node *next
@@ -903,6 +926,50 @@ static inline void _Scheduler_SMP_Schedule_highest_ready(
   } while ( action == SCHEDULER_TRY_TO_SCHEDULE_DO_BLOCK );
 }
 
+static inline void _Scheduler_SMP_Preempt_and_schedule_highest_ready(
+  Scheduler_Context                *context,
+  Scheduler_Node                   *victim,
+  Per_CPU_Control                  *victim_cpu,
+  Scheduler_SMP_Extract             extract_from_ready,
+  Scheduler_SMP_Get_highest_ready   get_highest_ready,
+  Scheduler_SMP_Move                move_from_ready_to_scheduled,
+  Scheduler_SMP_Allocate_processor  allocate_processor
+)
+{
+  Scheduler_Try_to_schedule_action action;
+
+  do {
+    Scheduler_Node *highest_ready = ( *get_highest_ready )( context, victim );
+
+    action = _Scheduler_Try_to_schedule_node(
+      context,
+      highest_ready,
+      NULL,
+      _Scheduler_SMP_Get_idle_thread
+    );
+
+    if ( action == SCHEDULER_TRY_TO_SCHEDULE_DO_SCHEDULE ) {
+      _Scheduler_SMP_Preempt(
+        context,
+        highest_ready,
+        victim,
+        allocate_processor
+      );
+
+      ( *move_from_ready_to_scheduled )( context, highest_ready );
+    } else {
+      _Assert( action == SCHEDULER_TRY_TO_SCHEDULE_DO_BLOCK );
+
+      _Scheduler_SMP_Node_change_state(
+        highest_ready,
+        SCHEDULER_SMP_NODE_BLOCKED
+      );
+
+      ( *extract_from_ready )( context, highest_ready );
+    }
+  } while ( action == SCHEDULER_TRY_TO_SCHEDULE_DO_BLOCK );
+}
+
 /**
  * @brief Blocks a thread.
  *
@@ -1274,11 +1341,34 @@ static inline void _Scheduler_SMP_Withdraw_node(
   }
 }
 
+static inline void _Scheduler_SMP_Do_start_idle(
+  Scheduler_Context           *context,
+  Thread_Control              *idle,
+  Per_CPU_Control             *cpu,
+  Scheduler_SMP_Register_idle  register_idle
+)
+{
+  Scheduler_SMP_Context *self;
+  Scheduler_SMP_Node    *node;
+
+  self = _Scheduler_SMP_Get_self( context );
+  node = _Scheduler_SMP_Thread_get_node( idle );
+
+  _Scheduler_Thread_change_state( idle, THREAD_SCHEDULER_SCHEDULED );
+  node->state = SCHEDULER_SMP_NODE_SCHEDULED;
+
+  _Thread_Set_CPU( idle, cpu );
+  ( *register_idle )( context, &node->Base, cpu );
+  _Chain_Append_unprotected( &self->Scheduled, &node->Base.Node.Chain );
+  _Scheduler_SMP_Release_idle_thread( &self->Base, idle );
+}
+
 static inline void _Scheduler_SMP_Add_processor(
-  Scheduler_Context       *context,
-  Thread_Control          *idle,
-  Scheduler_SMP_Has_ready  has_ready,
-  Scheduler_SMP_Enqueue    enqueue_scheduled_fifo
+  Scheduler_Context           *context,
+  Thread_Control              *idle,
+  Scheduler_SMP_Has_ready      has_ready,
+  Scheduler_SMP_Enqueue        enqueue_scheduled_fifo,
+  Scheduler_SMP_Register_idle  register_idle
 )
 {
   Scheduler_SMP_Context *self;
@@ -1289,6 +1379,7 @@ static inline void _Scheduler_SMP_Add_processor(
   _Scheduler_SMP_Release_idle_thread( &self->Base, idle );
   node = _Thread_Scheduler_get_home_node( idle );
   _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_SCHEDULED );
+  ( *register_idle )( context, node, _Thread_Get_CPU( idle ) );
 
   if ( ( *has_ready )( &self->Base ) ) {
     ( *enqueue_scheduled_fifo )( &self->Base, node );
@@ -1355,6 +1446,45 @@ static inline Thread_Control *_Scheduler_SMP_Remove_processor(
   return idle;
 }
 
+static inline void _Scheduler_SMP_Set_affinity(
+  Scheduler_Context               *context,
+  Thread_Control                  *thread,
+  Scheduler_Node                  *node,
+  void                            *arg,
+  Scheduler_SMP_Set_affinity       set_affinity,
+  Scheduler_SMP_Extract            extract_from_ready,
+  Scheduler_SMP_Get_highest_ready  get_highest_ready,
+  Scheduler_SMP_Move               move_from_ready_to_scheduled,
+  Scheduler_SMP_Enqueue            enqueue_fifo,
+  Scheduler_SMP_Allocate_processor allocate_processor
+)
+{
+  Scheduler_SMP_Node_state node_state;
+
+  node_state = _Scheduler_SMP_Node_state( node );
+
+  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
+    _Scheduler_SMP_Extract_from_scheduled( node );
+    _Scheduler_SMP_Preempt_and_schedule_highest_ready(
+      context,
+      node,
+      _Thread_Get_CPU( thread ),
+      extract_from_ready,
+      get_highest_ready,
+      move_from_ready_to_scheduled,
+      allocate_processor
+    );
+    ( *set_affinity )( context, node, arg );
+    ( *enqueue_fifo )( context, node );
+  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
+    ( *extract_from_ready )( context, node );
+    ( *set_affinity )( context, node, arg );
+    ( *enqueue_fifo )( context, node );
+  } else {
+    ( *set_affinity )( context, node, arg );
+  }
+}
+
 /** @} */
 
 #ifdef __cplusplus