score: Add simple affinity support to EDF SMP

Update #3059.

1 files changed, 135 insertions, 5 deletions

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