score: Fix FP context restore via _Thread_Handler

After a context switch we end up in the second part of
_Thread_Dispatch() or in _Thread_Handler() in case of new threads.  Use
the same function _Thread_Restore_fp() to restore the floating-point
context.  It makes no sense to do this in _Thread_Start_multitasking().
This fixes also a race condition in SMP configurations.

Update #2268.

4 files changed, 44 insertions, 68 deletions

diff --git a/cpukit/score/include/rtems/score/threadimpl.h b/cpukit/score/include/rtems/score/threadimpl.h
index bfaf1fdf67..353093ce3a 100644
--- a/cpukit/score/include/rtems/score/threadimpl.h
+++ b/cpukit/score/include/rtems/score/threadimpl.h
@@ -587,6 +587,47 @@ RTEMS_INLINE_ROUTINE bool _Thread_Is_allocated_fp (
 }
 #endif
 
+/*
+ *  If the CPU has hardware floating point, then we must address saving
+ *  and restoring it as part of the context switch.
+ *
+ *  The second conditional compilation section selects the algorithm used
+ *  to context switch between floating point tasks.  The deferred algorithm
+ *  can be significantly better in a system with few floating point tasks
+ *  because it reduces the total number of save and restore FP context
+ *  operations.  However, this algorithm can not be used on all CPUs due
+ *  to unpredictable use of FP registers by some compilers for integer
+ *  operations.
+ */
+
+RTEMS_INLINE_ROUTINE void _Thread_Save_fp( Thread_Control *executing )
+{
+#if ( CPU_HARDWARE_FP == TRUE ) || ( CPU_SOFTWARE_FP == TRUE )
+#if ( CPU_USE_DEFERRED_FP_SWITCH != TRUE )
+  if ( executing->fp_context != NULL )
+    _Context_Save_fp( &executing->fp_context );
+#endif
+#endif
+}
+
+RTEMS_INLINE_ROUTINE void _Thread_Restore_fp( Thread_Control *executing )
+{
+#if ( CPU_HARDWARE_FP == TRUE ) || ( CPU_SOFTWARE_FP == TRUE )
+#if ( CPU_USE_DEFERRED_FP_SWITCH == TRUE )
+  if ( (executing->fp_context != NULL) &&
+       !_Thread_Is_allocated_fp( executing ) ) {
+    if ( _Thread_Allocated_fp != NULL )
+      _Context_Save_fp( &_Thread_Allocated_fp->fp_context );
+    _Context_Restore_fp( &executing->fp_context );
+    _Thread_Allocated_fp = executing;
+  }
+#else
+  if ( executing->fp_context != NULL )
+    _Context_Restore_fp( &executing->fp_context );
+#endif
+#endif
+}
+
 /**
  * This routine is invoked when the currently loaded floating
  * point context is now longer associated with an active thread.
diff --git a/cpukit/score/src/threaddispatch.c b/cpukit/score/src/threaddispatch.c
index 982bbc476a..cc023fc57f 100644
--- a/cpukit/score/src/threaddispatch.c
+++ b/cpukit/score/src/threaddispatch.c
@@ -142,43 +142,9 @@ void _Thread_Dispatch( void )
 #endif
 
     _User_extensions_Thread_switch( executing, heir );
-
-    /*
-     *  If the CPU has hardware floating point, then we must address saving
-     *  and restoring it as part of the context switch.
-     *
-     *  The second conditional compilation section selects the algorithm used
-     *  to context switch between floating point tasks.  The deferred algorithm
-     *  can be significantly better in a system with few floating point tasks
-     *  because it reduces the total number of save and restore FP context
-     *  operations.  However, this algorithm can not be used on all CPUs due
-     *  to unpredictable use of FP registers by some compilers for integer
-     *  operations.
-     */
-
-#if ( CPU_HARDWARE_FP == TRUE ) || ( CPU_SOFTWARE_FP == TRUE )
-#if ( CPU_USE_DEFERRED_FP_SWITCH != TRUE )
-    if ( executing->fp_context != NULL )
-      _Context_Save_fp( &executing->fp_context );
-#endif
-#endif
-
+    _Thread_Save_fp( executing );
     _Context_Switch( &executing->Registers, &heir->Registers );
-
-#if ( CPU_HARDWARE_FP == TRUE ) || ( CPU_SOFTWARE_FP == TRUE )
-#if ( CPU_USE_DEFERRED_FP_SWITCH == TRUE )
-    if ( (executing->fp_context != NULL) &&
-         !_Thread_Is_allocated_fp( executing ) ) {
-      if ( _Thread_Allocated_fp != NULL )
-        _Context_Save_fp( &_Thread_Allocated_fp->fp_context );
-      _Context_Restore_fp( &executing->fp_context );
-      _Thread_Allocated_fp = executing;
-    }
-#else
-    if ( executing->fp_context != NULL )
-      _Context_Restore_fp( &executing->fp_context );
-#endif
-#endif
+    _Thread_Restore_fp( executing );
 
     /*
      * We have to obtain this value again after the context switch since the
diff --git a/cpukit/score/src/threadhandler.c b/cpukit/score/src/threadhandler.c
index f8a9a62429..db7302807c 100644
--- a/cpukit/score/src/threadhandler.c
+++ b/cpukit/score/src/threadhandler.c
@@ -51,16 +51,7 @@ void _Thread_Handler( void )
    * through _Thread_Dispatch on our first invocation. So the normal
    * code path for performing the FP context switch is not hit.
    */
-  #if ( CPU_HARDWARE_FP == TRUE ) || ( CPU_SOFTWARE_FP == TRUE )
-    #if ( CPU_USE_DEFERRED_FP_SWITCH == TRUE )
-      if ( (executing->fp_context != NULL) &&
-            !_Thread_Is_allocated_fp( executing ) ) {
-        if ( _Thread_Allocated_fp != NULL )
-          _Context_Save_fp( &_Thread_Allocated_fp->fp_context );
-        _Thread_Allocated_fp = executing;
-      }
-    #endif
-  #endif
+  _Thread_Restore_fp( executing );
 
   /*
    * Take care that 'begin' extensions get to complete before
diff --git a/cpukit/score/src/threadstartmultitasking.c b/cpukit/score/src/threadstartmultitasking.c
index 7fbdd84dbc..838cf2352a 100644
--- a/cpukit/score/src/threadstartmultitasking.c
+++ b/cpukit/score/src/threadstartmultitasking.c
@@ -37,28 +37,6 @@ void _Thread_Start_multitasking( void )
 
   heir = _Thread_Get_heir_and_make_it_executing( cpu_self );
 
-   /*
-    * Get the init task(s) running.
-    *
-    * Note: Thread_Dispatch() is normally used to dispatch threads.  As
-    *       part of its work, Thread_Dispatch() restores floating point
-    *       state for the heir task.
-    *
-    *       This code avoids Thread_Dispatch(), and so we have to restore
-    *       (actually initialize) the floating point state "by hand".
-    *
-    *       Ignore the CPU_USE_DEFERRED_FP_SWITCH because we must always
-    *       switch in the first thread if it is FP.
-    */
-#if ( CPU_HARDWARE_FP == TRUE ) || ( CPU_SOFTWARE_FP == TRUE )
-   /*
-    *  don't need to worry about saving BSP's floating point state
-    */
-
-   if ( heir->fp_context != NULL )
-     _Context_Restore_fp( &heir->fp_context );
-#endif
-
   _Profiling_Thread_dispatch_disable( cpu_self, 0 );
 
 #if defined(RTEMS_SMP)