rtems: Add scheduler processor add/remove

Update #2797.

8 files changed, 181 insertions, 1 deletions

diff --git a/cpukit/score/include/rtems/score/percpu.h b/cpukit/score/include/rtems/score/percpu.h
index 94aef1de22..ae789b85f5 100644
--- a/cpukit/score/include/rtems/score/percpu.h
+++ b/cpukit/score/include/rtems/score/percpu.h
@@ -426,6 +426,12 @@ typedef struct Per_CPU_Control {
        * scheduler instance.
        */
       const struct Scheduler_Context *context;
+
+      /**
+       * @brief The idle thread for this processor in case it is online and
+       * currently not used by a scheduler instance.
+       */
+      struct _Thread_Control *idle_if_online_and_unused;
     } Scheduler;
 
     /**
diff --git a/cpukit/score/include/rtems/score/scheduler.h b/cpukit/score/include/rtems/score/scheduler.h
index 2e2f5f43c0..7d461f81ff 100644
--- a/cpukit/score/include/rtems/score/scheduler.h
+++ b/cpukit/score/include/rtems/score/scheduler.h
@@ -148,6 +148,30 @@ typedef struct {
     Scheduler_Node          *node,
     Thread_Scheduler_state   next_state
   );
+
+  /**
+   * @brief Add processor operation.
+   *
+   * @param[in] scheduler The scheduler instance to add the processor.
+   * @param[in] idle The idle thread of the processor to add.
+   */
+  void ( *add_processor )(
+    const Scheduler_Control *scheduler,
+    Thread_Control          *idle
+  );
+
+  /**
+   * @brief Remove processor operation.
+   *
+   * @param[in] scheduler The scheduler instance to remove the processor.
+   * @param[in] cpu The processor to remove.
+   *
+   * @return The idle thread of the removed processor.
+   */
+  Thread_Control *( *remove_processor )(
+    const Scheduler_Control *scheduler,
+    struct Per_CPU_Control  *cpu
+  );
 #endif
 
   /** @see _Scheduler_Node_initialize() */
@@ -392,7 +416,9 @@ Priority_Control _Scheduler_default_Map_priority(
   #define SCHEDULER_OPERATION_DEFAULT_ASK_FOR_HELP \
     _Scheduler_default_Ask_for_help, \
     _Scheduler_default_Reconsider_help_request, \
-    _Scheduler_default_Withdraw_node,
+    _Scheduler_default_Withdraw_node, \
+    NULL, \
+    NULL,
 #else
   #define SCHEDULER_OPERATION_DEFAULT_ASK_FOR_HELP
 #endif
diff --git a/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h b/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h
index 4c5b8bb1a2..d1275bc727 100644
--- a/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h
+++ b/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h
@@ -60,6 +60,8 @@ extern "C" {
     _Scheduler_priority_affinity_SMP_Ask_for_help, \
     _Scheduler_priority_affinity_SMP_Reconsider_help_request, \
     _Scheduler_priority_affinity_SMP_Withdraw_node, \
+    _Scheduler_priority_affinity_SMP_Add_processor, \
+    _Scheduler_priority_affinity_SMP_Remove_processor, \
     _Scheduler_priority_affinity_SMP_Node_initialize, \
     _Scheduler_default_Node_destroy, \
     _Scheduler_default_Release_job, \
@@ -143,6 +145,16 @@ void _Scheduler_priority_affinity_SMP_Withdraw_node(
   Thread_Scheduler_state   next_state
 );
 
+void _Scheduler_priority_affinity_SMP_Add_processor(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *idle
+);
+
+Thread_Control *_Scheduler_priority_affinity_SMP_Remove_processor(
+  const Scheduler_Control *scheduler,
+  struct Per_CPU_Control  *cpu
+);
+
 /** 
  * @brief Set affinity for the priority affinity SMP scheduler.
  *
diff --git a/cpukit/score/include/rtems/score/schedulerprioritysmp.h b/cpukit/score/include/rtems/score/schedulerprioritysmp.h
index b5fdec4021..75cc9b6e67 100644
--- a/cpukit/score/include/rtems/score/schedulerprioritysmp.h
+++ b/cpukit/score/include/rtems/score/schedulerprioritysmp.h
@@ -89,6 +89,8 @@ typedef struct {
     _Scheduler_priority_SMP_Ask_for_help, \
     _Scheduler_priority_SMP_Reconsider_help_request, \
     _Scheduler_priority_SMP_Withdraw_node, \
+    _Scheduler_priority_SMP_Add_processor, \
+    _Scheduler_priority_SMP_Remove_processor, \
     _Scheduler_priority_SMP_Node_initialize, \
     _Scheduler_default_Node_destroy, \
     _Scheduler_default_Release_job, \
@@ -144,6 +146,16 @@ void _Scheduler_priority_SMP_Withdraw_node(
   Thread_Scheduler_state   next_state
 );
 
+void _Scheduler_priority_SMP_Add_processor(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *idle
+);
+
+Thread_Control *_Scheduler_priority_SMP_Remove_processor(
+  const Scheduler_Control *scheduler,
+  struct Per_CPU_Control  *cpu
+);
+
 bool _Scheduler_priority_SMP_Yield(
   const Scheduler_Control *scheduler,
   Thread_Control          *thread,
diff --git a/cpukit/score/include/rtems/score/schedulerprioritysmpimpl.h b/cpukit/score/include/rtems/score/schedulerprioritysmpimpl.h
index 4fe4d29248..5136565bbe 100644
--- a/cpukit/score/include/rtems/score/schedulerprioritysmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerprioritysmpimpl.h
@@ -57,6 +57,14 @@ _Scheduler_priority_SMP_Node_downcast( Scheduler_Node *node )
   return (Scheduler_priority_SMP_Node *) node;
 }
 
+static inline bool _Scheduler_priority_SMP_Has_ready( Scheduler_Context *context )
+{
+  Scheduler_priority_SMP_Context *self =
+    _Scheduler_priority_SMP_Get_self( context );
+
+  return !_Priority_bit_map_Is_empty( &self->Bit_map );
+}
+
 static inline void _Scheduler_priority_SMP_Move_from_scheduled_to_ready(
   Scheduler_Context *context,
   Scheduler_Node    *scheduled_to_ready
diff --git a/cpukit/score/include/rtems/score/schedulersimplesmp.h b/cpukit/score/include/rtems/score/schedulersimplesmp.h
index a242325dbd..0cf3877b43 100644
--- a/cpukit/score/include/rtems/score/schedulersimplesmp.h
+++ b/cpukit/score/include/rtems/score/schedulersimplesmp.h
@@ -72,6 +72,8 @@ typedef struct {
     _Scheduler_simple_SMP_Ask_for_help, \
     _Scheduler_simple_SMP_Reconsider_help_request, \
     _Scheduler_simple_SMP_Withdraw_node, \
+    _Scheduler_simple_SMP_Add_processor, \
+    _Scheduler_simple_SMP_Remove_processor, \
     _Scheduler_simple_SMP_Node_initialize, \
     _Scheduler_default_Node_destroy, \
     _Scheduler_default_Release_job, \
@@ -127,6 +129,16 @@ void _Scheduler_simple_SMP_Withdraw_node(
   Thread_Scheduler_state   next_state
 );
 
+void _Scheduler_simple_SMP_Add_processor(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *idle
+);
+
+Thread_Control *_Scheduler_simple_SMP_Remove_processor(
+  const Scheduler_Control *scheduler,
+  struct Per_CPU_Control  *cpu
+);
+
 bool _Scheduler_simple_SMP_Yield(
   const Scheduler_Control *scheduler,
   Thread_Control          *thread,
diff --git a/cpukit/score/include/rtems/score/schedulersmpimpl.h b/cpukit/score/include/rtems/score/schedulersmpimpl.h
index ece075a931..95a9eaed06 100644
--- a/cpukit/score/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulersmpimpl.h
@@ -275,6 +275,10 @@ extern "C" {
  * @{
  */
 
+typedef bool ( *Scheduler_SMP_Has_ready )(
+  Scheduler_Context *context
+);
+
 typedef Scheduler_Node *( *Scheduler_SMP_Get_highest_ready )(
   Scheduler_Context *context,
   Scheduler_Node    *node
@@ -469,6 +473,13 @@ static inline void _Scheduler_SMP_Release_idle_thread(
   _Chain_Prepend_unprotected( &self->Idle_threads, &idle->Object.Node );
 }
 
+static inline void _Scheduler_SMP_Exctract_idle_thread(
+  Thread_Control *idle
+)
+{
+  _Chain_Extract_unprotected( &idle->Object.Node );
+}
+
 static inline void _Scheduler_SMP_Allocate_processor_lazy(
   Scheduler_Context *context,
   Thread_Control    *scheduled_thread,
@@ -1271,6 +1282,87 @@ static inline void _Scheduler_SMP_Withdraw_node(
   }
 }
 
+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_SMP_Context *self;
+  Scheduler_Node        *node;
+
+  self = _Scheduler_SMP_Get_self( context );
+  idle->Scheduler.state = THREAD_SCHEDULER_SCHEDULED;
+  _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 );
+
+  if ( ( *has_ready )( &self->Base ) ) {
+    ( *enqueue_scheduled_fifo )( &self->Base, node );
+  } else {
+    _Chain_Append_unprotected( &self->Scheduled, &node->Node );
+  }
+}
+
+static inline Thread_Control *_Scheduler_SMP_Remove_processor(
+  Scheduler_Context     *context,
+  Per_CPU_Control       *cpu,
+  Scheduler_SMP_Extract  extract_from_ready,
+  Scheduler_SMP_Enqueue  enqueue_fifo
+)
+{
+  Scheduler_SMP_Context *self;
+  Chain_Node            *chain_node;
+  Scheduler_Node        *victim_node;
+  Thread_Control        *victim_user;
+  Thread_Control        *victim_owner;
+  Thread_Control        *idle;
+
+  self = _Scheduler_SMP_Get_self( context );
+  chain_node = _Chain_First( &self->Scheduled );
+
+  do {
+    _Assert( chain_node != _Chain_Immutable_tail( &self->Scheduled ) );
+    victim_node = (Scheduler_Node *) chain_node;
+    victim_user = _Scheduler_Node_get_user( victim_node );
+    chain_node = _Chain_Next( chain_node );
+  } while ( _Thread_Get_CPU( victim_user ) != cpu );
+
+  _Scheduler_SMP_Extract_from_scheduled( victim_node );
+  victim_owner = _Scheduler_Node_get_owner( victim_node );
+
+  if ( !victim_owner->is_idle ) {
+    Scheduler_Node *idle_node;
+
+    _Scheduler_Release_idle_thread(
+      &self->Base,
+      victim_node,
+      _Scheduler_SMP_Release_idle_thread
+    );
+    idle = _Scheduler_SMP_Get_idle_thread( &self->Base );
+    idle_node = _Thread_Scheduler_get_home_node( idle );
+    ( *extract_from_ready )( &self->Base, idle_node );
+    _Scheduler_SMP_Preempt(
+      &self->Base,
+      idle_node,
+      victim_node,
+      _Scheduler_SMP_Allocate_processor_exact
+    );
+
+    if ( !_Chain_Is_empty( &self->Scheduled ) ) {
+      ( *enqueue_fifo )( context, victim_node );
+    }
+  } else {
+    _Assert( victim_owner == victim_user );
+    _Assert( _Scheduler_Node_get_idle( victim_node ) == NULL );
+    idle = victim_owner;
+    _Scheduler_SMP_Exctract_idle_thread( idle );
+  }
+
+  return idle;
+}
+
 /** @} */
 
 #ifdef __cplusplus
diff --git a/cpukit/score/include/rtems/score/schedulerstrongapa.h b/cpukit/score/include/rtems/score/schedulerstrongapa.h
index 99013f2ef9..29dee66c44 100644
--- a/cpukit/score/include/rtems/score/schedulerstrongapa.h
+++ b/cpukit/score/include/rtems/score/schedulerstrongapa.h
@@ -89,6 +89,8 @@ typedef struct {
     _Scheduler_strong_APA_Ask_for_help, \
     _Scheduler_strong_APA_Reconsider_help_request, \
     _Scheduler_strong_APA_Withdraw_node, \
+    _Scheduler_strong_APA_Add_processor, \
+    _Scheduler_strong_APA_Remove_processor, \
     _Scheduler_strong_APA_Node_initialize, \
     _Scheduler_default_Node_destroy, \
     _Scheduler_default_Release_job, \
@@ -144,6 +146,16 @@ void _Scheduler_strong_APA_Withdraw_node(
   Thread_Scheduler_state   next_state
 );
 
+void _Scheduler_strong_APA_Add_processor(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *idle
+);
+
+Thread_Control *_Scheduler_strong_APA_Remove_processor(
+  const Scheduler_Control *scheduler,
+  struct Per_CPU_Control  *cpu
+);
+
 bool _Scheduler_strong_APA_Yield(
   const Scheduler_Control *scheduler,
   Thread_Control          *the_thread,