[PATCH 2/2] smp: Generalize _Thread_Start_multitasking()

[Sebastian Huber]

Add context parameter to _Thread_Start_multitasking() and use this
function in rtems_smp_secondary_cpu_initialize().  This avoids
duplication of code.

Fix missing floating point context initialization in
rtems_smp_secondary_cpu_initialize().  Now performed via
_Thread_Start_multitasking().
---
 cpukit/sapi/src/exinit.c                      |    6 ++-
 cpukit/score/include/rtems/score/threadimpl.h |    2 +-
 cpukit/score/src/smp.c                        |   27 +-------------
 cpukit/score/src/threadstartmultitasking.c    |   49 +++++++++++++++----------
 4 files changed, 37 insertions(+), 47 deletions(-)

diff --git a/cpukit/sapi/src/exinit.c b/cpukit/sapi/src/exinit.c
index 7dad241..bfcfa7c 100644
--- a/cpukit/sapi/src/exinit.c
+++ b/cpukit/sapi/src/exinit.c
@@ -219,11 +219,13 @@ void rtems_initialize_start_multitasking(void)
 {
   uint32_t status;
 
-#ifdef RTEMS_SMP
+  _System_state_Set( SYSTEM_STATE_UP );
+
+#if defined(RTEMS_SMP)
   _SMP_Request_other_cores_to_perform_first_context_switch();
 #endif
 
-  _Thread_Start_multitasking();
+  _Thread_Start_multitasking( &_Thread_BSP_context );
 
   /*******************************************************************
    *******************************************************************
diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index fcd3f07..128820b 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -118,7 +118,7 @@ void _Thread_Create_idle(void);
  *    + ready chain
  *    + select heir
  */
-void _Thread_Start_multitasking( void );
+void _Thread_Start_multitasking( Context_Control *context );
 
 /**
  *  @brief Allocate the requested stack space for the thread.
diff --git a/cpukit/score/src/smp.c b/cpukit/score/src/smp.c
index 5b299b4..4f72290 100644
--- a/cpukit/score/src/smp.c
+++ b/cpukit/score/src/smp.c
@@ -19,8 +19,8 @@
 #endif
 
 #include <rtems/bspsmp.h>
-#include <rtems/score/thread.h>
 #include <rtems/score/threaddispatch.h>
+#include <rtems/score/threadimpl.h>
 #include <rtems/score/smp.h>
 #include <rtems/score/sysstate.h>
 
@@ -41,27 +41,7 @@ void rtems_smp_secondary_cpu_initialize( void )
 
   _Per_CPU_Wait_for_state( self_cpu, PER_CPU_STATE_BEGIN_MULTITASKING );
 
-  _Per_CPU_Change_state( self_cpu, PER_CPU_STATE_UP );
-
-  /*
-   *  The Scheduler will have selected the heir thread for each CPU core.
-   *  Now we have been requested to perform the first context switch.  So
-   *  force a switch to the designated heir and make it executing on
-   *  THIS core.
-   */
-  heir = self_cpu->heir;
-  heir->is_executing = true;
-  self_cpu->executing->is_executing = false;
-  self_cpu->executing = heir;
-  self_cpu->dispatch_necessary = false;
-
-  /*
-   * Threads begin execution in the _Thread_Handler() function.   This function
-   * will call _Thread_Enable_dispatch().
-   */
-  _Thread_Disable_dispatch();
-
-  _CPU_Context_switch_to_first_task_smp( &heir->Registers );
+  _Thread_Start_multitasking( NULL );
 }
 
 void rtems_smp_process_interrupt( void )
@@ -158,9 +138,6 @@ void _SMP_Request_other_cores_to_perform_first_context_switch( void )
 
     if ( cpu != self ) {
       _Per_CPU_Change_state( per_cpu, PER_CPU_STATE_BEGIN_MULTITASKING );
-    } else {
-
-      _Per_CPU_Change_state( per_cpu, PER_CPU_STATE_UP );
     }
   }
 }
diff --git a/cpukit/score/src/threadstartmultitasking.c b/cpukit/score/src/threadstartmultitasking.c
index 459d0d6..b6ff073 100644
--- a/cpukit/score/src/threadstartmultitasking.c
+++ b/cpukit/score/src/threadstartmultitasking.c
@@ -19,26 +19,21 @@
 #endif
 
 #include <rtems/score/threadimpl.h>
-#include <rtems/score/sysstate.h>
 
-void _Thread_Start_multitasking( void )
+void _Thread_Start_multitasking( Context_Control *context )
 {
-  /*
-   *  The system is now multitasking and completely initialized.
-   *  This system thread now "hides" in a single processor until
-   *  the system is shut down.
-   */
+  Per_CPU_Control *self_cpu = _Per_CPU_Get();
+  Thread_Control  *heir = self_cpu->heir;
 
-  _System_state_Set( SYSTEM_STATE_UP );
+#if defined(RTEMS_SMP)
+  _Per_CPU_Change_state( self_cpu, PER_CPU_STATE_UP );
 
-  _Thread_Dispatch_necessary = false;
-
-  #if defined(RTEMS_SMP)
-    _Thread_Executing->is_executing = false;
-    _Thread_Heir->is_executing = true;
-  #endif
+  self_cpu->executing->is_executing = false;
+  heir->is_executing = true;
+#endif
 
-  _Thread_Executing = _Thread_Heir;
+  self_cpu->dispatch_necessary = false;
+  self_cpu->executing = heir;
 
    /*
     * Get the init task(s) running.
@@ -58,13 +53,29 @@ void _Thread_Start_multitasking( void )
     *  don't need to worry about saving BSP's floating point state
     */
 
-   if ( _Thread_Heir->fp_context != NULL )
-     _Context_Restore_fp( &_Thread_Heir->fp_context );
+   if ( heir->fp_context != NULL )
+     _Context_Restore_fp( &heir->fp_context );
+#endif
+
+#if defined(RTEMS_SMP)
+  if ( context != NULL ) {
 #endif
 
 #if defined(_CPU_Start_multitasking)
-  _CPU_Start_multitasking( &_Thread_BSP_context, &_Thread_Heir->Registers );
+    _CPU_Start_multitasking( context, &heir->Registers );
 #else
-  _Context_Switch( &_Thread_BSP_context, &_Thread_Heir->Registers );
+    _Context_Switch( context, &heir->Registers );
+#endif
+
+#if defined(RTEMS_SMP)
+  } else {
+    /*
+     * Threads begin execution in the _Thread_Handler() function.   This function
+     * will call _Thread_Enable_dispatch().
+     */
+    _Thread_Disable_dispatch();
+
+    _CPU_Context_switch_to_first_task_smp( &heir->Registers );
+  }
 #endif
 }
-- 
1.7.7