score: Implement forced thread migration

The current implementation of task migration in RTEMS has some
implications with respect to the interrupt latency. It is crucial to
preserve the system invariant that a task can execute on at most one
processor in the system at a time. This is accomplished with a boolean
indicator in the task context. The processor architecture specific
low-level task context switch code will mark that a task context is no
longer executing and waits that the heir context stopped execution
before it restores the heir context and resumes execution of the heir
task. So there is one point in time in which a processor is without a
task. This is essential to avoid cyclic dependencies in case multiple
tasks migrate at once. Otherwise some supervising entity is necessary to
prevent life-locks. Such a global supervisor would lead to scalability
problems so this approach is not used. Currently the thread dispatch is
performed with interrupts disabled. So in case the heir task is
currently executing on another processor then this prolongs the time of
disabled interrupts since one processor has to wait for another
processor to make progress.

It is difficult to avoid this issue with the interrupt latency since
interrupts normally store the context of the interrupted task on its
stack. In case a task is marked as not executing we must not use its
task stack to store such an interrupt context. We cannot use the heir
stack before it stopped execution on another processor. So if we enable
interrupts during this transition we have to provide an alternative task
independent stack for this time frame. This issue needs further
investigation.

9 files changed, 202 insertions, 73 deletions

diff --git a/cpukit/score/include/rtems/score/percpu.h b/cpukit/score/include/rtems/score/percpu.h
index 7f063eafb8..e232674c30 100644
--- a/cpukit/score/include/rtems/score/percpu.h
+++ b/cpukit/score/include/rtems/score/percpu.h
@@ -56,6 +56,8 @@ extern "C" {
 typedef struct Thread_Control_struct Thread_Control;
 #endif
 
+struct Scheduler_Context;
+
 /**
  *  @defgroup PerCPU RTEMS Per CPU Information
  *
@@ -268,13 +270,46 @@ typedef struct Per_CPU_Control {
    */
   volatile uint32_t thread_dispatch_disable_level;
 
-  /** This is set to true when this CPU needs to run the dispatcher. */
+  /**
+   * @brief This is set to true when this processor needs to run the
+   * dispatcher.
+   *
+   * It is volatile since interrupts may alter this flag.
+   *
+   * This field is not protected by a lock.  There are two writers after
+   * multitasking start.  The scheduler owning this processor sets this
+   * indicator to true, after it updated the heir field.  This processor sets
+   * this indicator to false, before it reads the heir.  This field is used in
+   * combination with the heir field.
+   *
+   * @see _Thread_Get_heir_and_make_it_executing().
+   */
   volatile bool dispatch_necessary;
 
-  /** This is the thread executing on this CPU. */
+  /**
+   * @brief This is the thread executing on this processor.
+   *
+   * This field is not protected by a lock.  The only writer is this processor.
+   *
+   * On SMP configurations a thread may be registered as executing on more than
+   * one processor in case a thread migration is in progress.  On SMP
+   * configurations use _Thread_Is_executing_on_a_processor() to figure out if
+   * a thread context is executing on a processor.
+   */
   Thread_Control *executing;
 
-  /** This is the heir thread for this this CPU. */
+  /**
+   * @brief This is the heir thread for this processor.
+   *
+   * This field is not protected by a lock.  The only writer after multitasking
+   * start is the scheduler owning this processor.  This processor will set the
+   * dispatch necessary indicator to false, before it reads the heir.  This
+   * field is used in combination with the dispatch necessary indicator.
+   *
+   * A thread can be a heir on at most one processor in the system.
+   *
+   * @see _Thread_Get_heir_and_make_it_executing().
+   */
   Thread_Control *heir;
 
   /** This is the time of the last context switch on this CPU. */
@@ -282,11 +317,12 @@ typedef struct Per_CPU_Control {
 
   #if defined( RTEMS_SMP )
     /**
-     * @brief This lock protects the dispatch_necessary, executing, heir and
-     * message fields.
+     * @brief This lock protects some parts of the low-level thread dispatching.
      *
      * We must use a ticket lock here since we cannot transport a local context
      * through the context switch.
+     *
+     * @see _Thread_Dispatch().
      */
     SMP_ticket_lock_Control Lock;
 
@@ -310,6 +346,11 @@ typedef struct Per_CPU_Control {
     Atomic_Ulong message;
 
     /**
+     * @brief The scheduler context of the scheduler owning this processor.
+     */
+    const struct Scheduler_Context *scheduler_context;
+
+    /**
      * @brief Indicates the current state of the CPU.
      *
      * This field is protected by the _Per_CPU_State_lock lock.
diff --git a/cpukit/score/include/rtems/score/scheduler.h b/cpukit/score/include/rtems/score/scheduler.h
index 9002ef85d9..2a1c4338a6 100644
--- a/cpukit/score/include/rtems/score/scheduler.h
+++ b/cpukit/score/include/rtems/score/scheduler.h
@@ -148,7 +148,7 @@ typedef struct {
  * The scheduler context of a particular scheduler implementation must place
  * this structure at the begin of its context structure.
  */
-typedef struct {
+typedef struct Scheduler_Context {
 #if defined(RTEMS_SMP)
   /**
    * @brief Count of processors owned by this scheduler instance.
diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index 6fad4e2190..cb73d5e586 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -390,29 +390,25 @@ RTEMS_INLINE_ROUTINE const Scheduler_Control *_Scheduler_Get(
 #endif
 }
 
-RTEMS_INLINE_ROUTINE bool _Scheduler_Set(
+RTEMS_INLINE_ROUTINE void _Scheduler_Set(
   const Scheduler_Control *scheduler,
   Thread_Control          *the_thread
 )
 {
-  bool ok;
-
-  if ( _States_Is_dormant( the_thread->current_state ) ) {
 #if defined(RTEMS_SMP)
+  const Scheduler_Control *current_scheduler = _Scheduler_Get( the_thread );
+
+  if ( current_scheduler != scheduler ) {
+    _Thread_Set_state( the_thread, STATES_MIGRATING );
     _Scheduler_Free( _Scheduler_Get( the_thread ), the_thread );
     the_thread->scheduler = scheduler;
     _Scheduler_Allocate( scheduler, the_thread );
     _Scheduler_Update( scheduler, the_thread );
+    _Thread_Clear_state( the_thread, STATES_MIGRATING );
+  }
 #else
-    (void) scheduler;
+  (void) scheduler;
 #endif
-
-    ok = true;
-  } else {
-    ok = false;
-  }
-
-  return ok;
 }
 
 RTEMS_INLINE_ROUTINE bool _Scheduler_default_Set_affinity_body(
@@ -448,9 +444,7 @@ RTEMS_INLINE_ROUTINE bool _Scheduler_default_Set_affinity_body(
     ok = ok && !CPU_ISSET_S( (int) cpu_index, cpusetsize, cpuset );
   }
 
-  if ( ok ) {
-    ok = _Scheduler_Set( scheduler, the_thread );
-  }
+  _Scheduler_Set( scheduler, the_thread );
 
   return ok;
 }
diff --git a/cpukit/score/include/rtems/score/schedulersmp.h b/cpukit/score/include/rtems/score/schedulersmp.h
index 8f5a390a26..778a1fb832 100644
--- a/cpukit/score/include/rtems/score/schedulersmp.h
+++ b/cpukit/score/include/rtems/score/schedulersmp.h
@@ -24,9 +24,7 @@
 #define _RTEMS_SCORE_SCHEDULERSMP_H
 
 #include <rtems/score/chain.h>
-#include <rtems/score/percpu.h>
-#include <rtems/score/prioritybitmap.h>
-#include <rtems/score/thread.h>
+#include <rtems/score/scheduler.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/cpukit/score/include/rtems/score/schedulersmpimpl.h b/cpukit/score/include/rtems/score/schedulersmpimpl.h
index c3e11e6dc6..69222c28f8 100644
--- a/cpukit/score/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/score/include/rtems/score/schedulersmpimpl.h
@@ -24,9 +24,9 @@
 #define _RTEMS_SCORE_SCHEDULERSMPIMPL_H
 
 #include <rtems/score/schedulersmp.h>
-#include <rtems/score/schedulersimpleimpl.h>
+#include <rtems/score/assert.h>
 #include <rtems/score/chainimpl.h>
-#include <rtems/score/scheduler.h>
+#include <rtems/score/schedulersimpleimpl.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -64,47 +64,74 @@ static inline void _Scheduler_SMP_Initialize(
   _Chain_Initialize_empty( &self->Scheduled );
 }
 
+static inline bool _Scheduler_SMP_Is_processor_owned_by_us(
+  const Scheduler_SMP_Context *self,
+  const Per_CPU_Control *cpu
+)
+{
+  return cpu->scheduler_context == &self->Base;
+}
+
+static inline void _Scheduler_SMP_Update_heir(
+  Per_CPU_Control *cpu_self,
+  Per_CPU_Control *cpu_for_heir,
+  Thread_Control *heir
+)
+{
+  cpu_for_heir->heir = heir;
+
+  /*
+   * It is critical that we first update the heir and then the dispatch
+   * necessary so that _Thread_Get_heir_and_make_it_executing() cannot miss an
+   * update.
+   */
+  _Atomic_Fence( ATOMIC_ORDER_SEQ_CST );
+
+  /*
+   * Only update the dispatch necessary indicator if not already set to
+   * avoid superfluous inter-processor interrupts.
+   */
+  if ( !cpu_for_heir->dispatch_necessary ) {
+    cpu_for_heir->dispatch_necessary = true;
+
+    if ( cpu_for_heir != cpu_self ) {
+      _Per_CPU_Send_interrupt( cpu_for_heir );
+    }
+  }
+}
+
 static inline void _Scheduler_SMP_Allocate_processor(
+  Scheduler_SMP_Context *self,
   Thread_Control *scheduled,
   Thread_Control *victim
 )
 {
   Per_CPU_Control *cpu_of_scheduled = _Thread_Get_CPU( scheduled );
   Per_CPU_Control *cpu_of_victim = _Thread_Get_CPU( victim );
+  Per_CPU_Control *cpu_self = _Per_CPU_Get();
   Thread_Control *heir;
 
   scheduled->is_scheduled = true;
   victim->is_scheduled = false;
 
-  _Per_CPU_Acquire( cpu_of_scheduled );
+  _Assert( _ISR_Get_level() != 0 );
 
-  if ( scheduled->is_executing ) {
-    heir = cpu_of_scheduled->heir;
-    cpu_of_scheduled->heir = scheduled;
+  if ( _Thread_Is_executing_on_a_processor( scheduled ) ) {
+    if ( _Scheduler_SMP_Is_processor_owned_by_us( self, cpu_of_scheduled ) ) {
+      heir = cpu_of_scheduled->heir;
+      _Scheduler_SMP_Update_heir( cpu_self, cpu_of_scheduled, scheduled );
+    } else {
+      /* We have to force a migration to our processor set */
+      _Assert( scheduled->debug_real_cpu->heir != scheduled );
+      heir = scheduled;
+    }
   } else {
     heir = scheduled;
   }
 
-  _Per_CPU_Release( cpu_of_scheduled );
-
   if ( heir != victim ) {
-    const Per_CPU_Control *cpu_of_executing = _Per_CPU_Get();
-
     _Thread_Set_CPU( heir, cpu_of_victim );
-
-    cpu_of_victim->heir = heir;
-
-    /*
-     * It is critical that we first update the heir and then the dispatch
-     * necessary so that _Thread_Dispatch() cannot miss an update.
-     */
-    _Atomic_Fence( ATOMIC_ORDER_RELEASE );
-
-    cpu_of_victim->dispatch_necessary = true;
-
-    if ( cpu_of_victim != cpu_of_executing ) {
-      _Per_CPU_Send_interrupt( cpu_of_victim );
-    }
+    _Scheduler_SMP_Update_heir( cpu_self, cpu_of_victim, heir );
   }
 }
 
@@ -148,7 +175,7 @@ static inline void _Scheduler_SMP_Enqueue_ordered(
       highest_ready != NULL
         && !( *order )( &thread->Object.Node, &highest_ready->Object.Node )
     ) {
-      _Scheduler_SMP_Allocate_processor( highest_ready, thread );
+      _Scheduler_SMP_Allocate_processor( self, highest_ready, thread );
 
       ( *insert_ready )( self, thread );
       ( *move_from_ready_to_scheduled )( self, highest_ready );
@@ -168,7 +195,7 @@ static inline void _Scheduler_SMP_Enqueue_ordered(
       lowest_scheduled != NULL
         && ( *order )( &thread->Object.Node, &lowest_scheduled->Object.Node )
     ) {
-      _Scheduler_SMP_Allocate_processor( thread, lowest_scheduled );
+      _Scheduler_SMP_Allocate_processor( self, thread, lowest_scheduled );
 
       ( *insert_scheduled )( self, thread );
       ( *move_from_scheduled_to_ready )( self, lowest_scheduled );
@@ -187,7 +214,7 @@ static inline void _Scheduler_SMP_Schedule_highest_ready(
 {
   Thread_Control *highest_ready = ( *get_highest_ready )( self );
 
-  _Scheduler_SMP_Allocate_processor( highest_ready, victim );
+  _Scheduler_SMP_Allocate_processor( self, highest_ready, victim );
 
   ( *move_from_ready_to_scheduled )( self, highest_ready );
 }
diff --git a/cpukit/score/include/rtems/score/statesimpl.h b/cpukit/score/include/rtems/score/statesimpl.h
index 842d108236..0dbf0db71e 100644
--- a/cpukit/score/include/rtems/score/statesimpl.h
+++ b/cpukit/score/include/rtems/score/statesimpl.h
@@ -82,6 +82,8 @@ extern "C" {
 #define STATES_WAITING_FOR_TERMINATION         0x100000
 /** This macro corresponds to a task being a zombie. */
 #define STATES_ZOMBIE                          0x200000
+/** This macro corresponds to a task migration to another scheduler. */
+#define STATES_MIGRATING                       0x400000
 
 /** This macro corresponds to a task which is in an interruptible
  *  blocking state.
diff --git a/cpukit/score/include/rtems/score/thread.h b/cpukit/score/include/rtems/score/thread.h
index 90df3a74dc..248ae96850 100644
--- a/cpukit/score/include/rtems/score/thread.h
+++ b/cpukit/score/include/rtems/score/thread.h
@@ -504,20 +504,6 @@ struct Thread_Control_struct {
   bool                                  is_in_the_air;
 
   /**
-   * @brief This field is true if the thread is executing.
-   *
-   * A thread is executing if it executes on a processor.  An executing thread
-   * executes on exactly one processor.  There are exactly processor count
-   * executing threads in the system.  An executing thread may have a heir
-   * thread and thread dispatching is necessary.  On SMP a thread dispatch on a
-   * remote processor needs help from an inter-processor interrupt, thus it
-   * will take some time to complete the state change.  A lot of things can
-   * happen in the meantime.  This field is volatile since it is polled in
-   * _Thread_Kill_zombies().
-   */
-  volatile bool                         is_executing;
-
-  /**
    * @brief The scheduler of this thread.
    */
   const struct Scheduler_Control       *scheduler;
@@ -548,7 +534,18 @@ struct Thread_Control_struct {
   void                                 *scheduler_info;
 
 #ifdef RTEMS_SMP
+  /**
+   * @brief The processor assigned by the scheduler.
+   */
   Per_CPU_Control                      *cpu;
+
+#ifdef RTEMS_DEBUG
+  /**
+   * @brief The processor on which this thread executed the last time or is
+   * executing.
+   */
+  Per_CPU_Control                      *debug_real_cpu;
+#endif
 #endif
 
   /** This field contains information about the starting state of
diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index 4efc85d8f1..2be5cc56fa 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -454,6 +454,22 @@ RTEMS_INLINE_ROUTINE bool _Thread_Is_executing (
   return ( the_thread == _Thread_Executing );
 }
 
+#if defined(RTEMS_SMP)
+/**
+ * @brief Returns @true in case the thread executes currently on some processor
+ * in the system, otherwise @a false.
+ *
+ * Do not confuse this with _Thread_Is_executing() which checks only the
+ * current processor.
+ */
+RTEMS_INLINE_ROUTINE bool _Thread_Is_executing_on_a_processor(
+  const Thread_Control *the_thread
+)
+{
+  return _CPU_Context_Get_is_executing( &the_thread->Registers );
+}
+#endif
+
 /**
  * This function returns true if the_thread is the heir
  * thread, and false otherwise.
@@ -491,7 +507,7 @@ RTEMS_INLINE_ROUTINE void _Thread_Restart_self( Thread_Control *executing )
 
   _Giant_Release();
 
-  _Per_CPU_ISR_disable_and_acquire( _Per_CPU_Get(), level );
+  _ISR_Disable_without_giant( level );
   ( void ) level;
 #endif
 
@@ -590,7 +606,7 @@ RTEMS_INLINE_ROUTINE void _Thread_Request_dispatch_if_executing(
 )
 {
 #if defined(RTEMS_SMP)
-  if ( thread->is_executing ) {
+  if ( _Thread_Is_executing_on_a_processor( thread ) ) {
     const Per_CPU_Control *cpu_of_executing = _Per_CPU_Get();
     Per_CPU_Control *cpu_of_thread = _Thread_Get_CPU( thread );
 
@@ -611,7 +627,7 @@ RTEMS_INLINE_ROUTINE void _Thread_Signal_notification( Thread_Control *thread )
     _Thread_Dispatch_necessary = true;
   } else {
 #if defined(RTEMS_SMP)
-    if ( thread->is_executing ) {
+    if ( _Thread_Is_executing_on_a_processor( thread ) ) {
       const Per_CPU_Control *cpu_of_executing = _Per_CPU_Get();
       Per_CPU_Control *cpu_of_thread = _Thread_Get_CPU( thread );
 
@@ -624,6 +640,39 @@ RTEMS_INLINE_ROUTINE void _Thread_Signal_notification( Thread_Control *thread )
   }
 }
 
+/**
+ * @brief Gets the heir of the processor and makes it executing.
+ *
+ * The thread dispatch necessary indicator is cleared as a side-effect.
+ *
+ * @return The heir thread.
+ *
+ * @see _Thread_Dispatch(), _Thread_Start_multitasking() and
+ * _Scheduler_SMP_Update_heir().
+ */
+RTEMS_INLINE_ROUTINE Thread_Control *_Thread_Get_heir_and_make_it_executing(
+  Per_CPU_Control *cpu_self
+)
+{
+  Thread_Control *heir;
+
+  cpu_self->dispatch_necessary = false;
+
+#if defined( RTEMS_SMP )
+  /*
+   * It is critical that we first update the dispatch necessary and then the
+   * read the heir so that we don't miss an update by
+   * _Scheduler_SMP_Update_heir().
+   */
+  _Atomic_Fence( ATOMIC_ORDER_SEQ_CST );
+#endif
+
+  heir = cpu_self->heir;
+  cpu_self->executing = heir;
+
+  return heir;
+}
+
 RTEMS_INLINE_ROUTINE void _Thread_Update_cpu_time_used(
   Thread_Control *executing,
   Timestamp_Control *time_of_last_context_switch
@@ -736,6 +785,19 @@ RTEMS_INLINE_ROUTINE bool _Thread_Is_life_changing(
   return ( life_state & THREAD_LIFE_RESTARTING_TERMINTING ) != 0;
 }
 
+RTEMS_INLINE_ROUTINE void _Thread_Debug_set_real_processor(
+  Thread_Control  *the_thread,
+  Per_CPU_Control *cpu
+)
+{
+#if defined(RTEMS_SMP) && defined(RTEMS_DEBUG)
+  the_thread->debug_real_cpu = cpu;
+#else
+  (void) the_thread;
+  (void) cpu;
+#endif
+}
+
 #if !defined(__DYNAMIC_REENT__)
 /**
  * This routine returns the C library re-enterant pointer.
diff --git a/cpukit/score/include/rtems/score/userextimpl.h b/cpukit/score/include/rtems/score/userextimpl.h
index 04808e1f17..19055f976b 100644
--- a/cpukit/score/include/rtems/score/userextimpl.h
+++ b/cpukit/score/include/rtems/score/userextimpl.h
@@ -216,13 +216,21 @@ static inline void _User_extensions_Thread_switch(
   const Chain_Node    *tail = _Chain_Immutable_tail( chain );
   const Chain_Node    *node = _Chain_Immutable_first( chain );
 
-  while ( node != tail ) {
-    const User_extensions_Switch_control *extension =
-      (const User_extensions_Switch_control *) node;
+  if ( node != tail ) {
+    Per_CPU_Control *cpu_self = _Per_CPU_Get();
 
-    (*extension->thread_switch)( executing, heir );
+    _Per_CPU_Acquire( cpu_self );
 
-    node = _Chain_Immutable_next( node );
+    while ( node != tail ) {
+      const User_extensions_Switch_control *extension =
+        (const User_extensions_Switch_control *) node;
+
+      (*extension->thread_switch)( executing, heir );
+
+      node = _Chain_Immutable_next( node );
+    }
+
+    _Per_CPU_Release( cpu_self );
   }
 }