cpu.c, cpu_asm.S, rtems/score/cpu.h: Cleanup of the floating point context initialization, save and restore code.

4 files changed, 293 insertions, 296 deletions

diff --git a/cpukit/score/cpu/m68k/ChangeLog b/cpukit/score/cpu/m68k/ChangeLog
index b344cb7774..faed3ff357 100644
--- a/cpukit/score/cpu/m68k/ChangeLog
+++ b/cpukit/score/cpu/m68k/ChangeLog
@@ -1,3 +1,8 @@
+2009-05-11	Sebastian Huber <sebastian.huber@embedded-brains.de>
+
+	* cpu.c, cpu_asm.S, rtems/score/cpu.h: Cleanup of the floating point
+	context initialization, save and restore code.
+
 2009-03-12	Joel Sherrill <joel.sherrill@OARcorp.com>
 
 	PR 1385/cpukit
diff --git a/cpukit/score/cpu/m68k/cpu.c b/cpukit/score/cpu/m68k/cpu.c
index dd1db6488c..38327dee12 100644
--- a/cpukit/score/cpu/m68k/cpu.c
+++ b/cpukit/score/cpu/m68k/cpu.c
@@ -14,6 +14,10 @@
 #include <rtems/system.h>
 #include <rtems/score/isr.h>
 
+#if defined( __mcoldfire__ ) && ( M68K_HAS_FPU == 1 )
+  uint32_t _CPU_cacr_shadow;
+#endif
+
 /*  _CPU_Initialize
  *
  *  This routine performs processor dependent initialization.
@@ -185,7 +189,7 @@ const unsigned char _CPU_m68k_BFFFO_table[256] = {
 #if (CPU_SOFTWARE_FP == TRUE)
 extern Context_Control_fp _fpCCR;
 
-void CPU_Context_save_fp (Context_Control_fp **fp_context_ptr)
+void _CPU_Context_save_fp (Context_Control_fp **fp_context_ptr)
 {
   Context_Control_fp *fp;
 
@@ -194,7 +198,7 @@ void CPU_Context_save_fp (Context_Control_fp **fp_context_ptr)
   *fp = _fpCCR;
 }
 
-void CPU_Context_restore_fp (Context_Control_fp **fp_context_ptr)
+void _CPU_Context_restore_fp (Context_Control_fp **fp_context_ptr)
 {
   Context_Control_fp *fp;
 
diff --git a/cpukit/score/cpu/m68k/cpu_asm.S b/cpukit/score/cpu/m68k/cpu_asm.S
index 216686bbfd..641e945d7f 100644
--- a/cpukit/score/cpu/m68k/cpu_asm.S
+++ b/cpukit/score/cpu/m68k/cpu_asm.S
@@ -15,17 +15,6 @@
 
 
 #include <rtems/asm.h>
-        .data
-
-#if (defined(__mcoldfire__))
-#if ( M68K_HAS_FPU == 1 )
-PUBLIC (_CPU_cacr_shadow)     
-SYM (_CPU_cacr_shadow):    
-         .long 1
-#endif
-#endif
-
-        .text
 
 /*  void _CPU_Context_switch( run_context, heir_context )
  *
@@ -45,188 +34,167 @@ SYM (_CPU_Context_switch):
 
           moval    a7@(HEIRCONTEXT_ARG),a0| a0 = heir thread context
 
-#if (defined(__mcoldfire__))
-#if ( M68K_HAS_FPU == 1 )          
-          moveb	   a0@(13*4),d0                 | get context specific DF bit info in d0   
-          btstb    #4,d0			| test context specific DF bit info
+#if defined( __mcoldfire__ ) && ( M68K_HAS_FPU == 1 )
+          moveb    a0@(13*4),d0                 | get context specific DF bit info in d0
+          btstb    #4,d0                        | test context specific DF bit info
           beq      fpu_on                       | branch if FPU needs to be switched on
-          
-fpu_off:  movl     _CPU_cacr_shadow,d0          | get content of _CPU_cacr_shadow in d0     
+
+fpu_off:  movl     _CPU_cacr_shadow,d0          | get content of _CPU_cacr_shadow in d0
           btstl    #4,d0                        | test DF bit info in d0
-          bne      restore                      | branch if FPU is already switched off     
+          bne      restore                      | branch if FPU is already switched off
           bsetl    #4,d0                        | set DF bit in d0
-          bra      cacr_set                     | branch to set the new FPU setting in cacr and _CPU_cacr_shadow    
-          
-fpu_on:   movl     _CPU_cacr_shadow,d0          | get content of _CPU_cacr_shadow in d1        
-          btstl    #4,d0			| test context specific DF bit info
-          beq      restore                      | branch if FPU is already switched on   
+          bra      cacr_set                     | branch to set the new FPU setting in cacr and _CPU_cacr_shadow
+
+fpu_on:   movl     _CPU_cacr_shadow,d0          | get content of _CPU_cacr_shadow in d1
+          btstl    #4,d0                        | test context specific DF bit info
+          beq      restore                      | branch if FPU is already switched on
           bclrl    #4,d0                        | clear DF bit info in d0
-         
+
 cacr_set: movew    sr,d1                        | get content of sr in d1
-          oril     #0x00000700,d1               | mask d1 
+          oril     #0x00000700,d1               | mask d1
           movew    d1,sr                        | disable all interrupts
           movl     d0,_CPU_cacr_shadow          | move _CPU_cacr_shadow to d1
-	  movec    d0,cacr                      | enable FPU in cacr    	  
-#endif
+          movec    d0,cacr                      | enable FPU in cacr
 #endif
 
 restore:  movml    a0@,d1-d7/a2-a7       | restore context
           movw     d1,sr                 | restore status register
           rts
 
-/*PAGE
- *  void __CPU_Context_save_fp_context( &fp_context_ptr )
- *  void __CPU_Context_restore_fp_context( &fp_context_ptr )
- *
- *  These routines are used to context switch a MC68881 or MC68882.
- *
- *  NOTE:  Context save and restore code is based upon the code shown
- *         on page 6-38 of the MC68881/68882 Users Manual (rev 1).
- *
- *         CPU_FP_CONTEXT_SIZE is higher than expected to account for the
- *         -1 pushed at end of this sequence.
+/*
+ * Floating point context save and restore.
  *
- *         Neither of these entries is required if we have software FPU
- *         emulation.  But if we don't have an FPU or emulation, then
- *         we need the stub versions of these routines.
+ * The code for the MC68881 or MC68882 is based upon the code shown on pages
+ * 6-38 of the MC68881/68882 Users Manual (rev 1).  CPU_FP_CONTEXT_SIZE is
+ * higher than expected to account for the -1 pushed at end of this sequence.
  */
 
-#if (CPU_SOFTWARE_FP == FALSE)
+#if ( CPU_HARDWARE_FP == TRUE )
 
 .set FPCONTEXT_ARG,   4                   | save FP context argument
-.set FP_STATE_SAVED,  (4*4)               | FPU state is 4 longwords
-.set FP_REGS_SAVED,   (8*8)               | FPU regs is 8 64bit values
 
         .align  4
         .global SYM (_CPU_Context_save_fp)
 SYM (_CPU_Context_save_fp):
-#if ( M68K_HAS_FPU == 1 )
-#if (defined(__mcoldfire__))
-        
-        moval    a7@(FPCONTEXT_ARG),a1    | a1 = &ptr to context area
-        moval    a1@,a0                   | a0 = Save context area
-        leal     a0@(-16),a0              | open context frame for coldfire state frame
-        fsave    a0@                      | save coldfire state frame   
-        tstb     a0@                      | check for a null frame
-        beq.b    nosave                   | Yes, skip save of user model
-        leal     a0@(-64),a0              | open context frame for coldfire data registers (fp0-fp7)
-        fmovem   fp0-fp7,a0@              | save coldfire data registers (fp0-fp7)                      
-        movl     #-1,a0@-                 | place not-null flag on stack
-nosave: movl     a0,a1@                   | save pointer to saved context
-
-#if ( M68K_HAS_EMAC == 1 )
-
-        movel    macsr,d0                 | store content of macsr in d0
-        clrl     d1                       | clear d1
-        movl     d1,macsr                 | disable rounding in macsr
-        movl     acc0,d1                  | store content of acc0 in d1
-        moveml   d0-d1,a0@(-8)            | save EMAC macsr/acc0
-        movl     acc1,d0                  | store acc1 in d0 
-        movl     acc2,d1                  | store acc2 in d1 
-        moveml   d0-d1,a0@(-16)           | save EMAC acc1/acc2 with offset
-        movl     acc3,d0                  | store acc3 in d0 
-	movl     accext01,d1              | store acc2 in d1
-        moveml   d0-d1,a0@(-24)           | save EMAC acc3/accext01 with offset
-        movl     accext23,d0              | store accext23 in d0
-        movl     mask,d1                  | store mask in d1
-        moveml   d0-d1,a0@(-32)           | save EMAC accext23/mask with offset
-        leal     a0@(-32),a0              | set a0 to the begin of coldfire data registers frame (fp0-fp7)
-        movl     a0,a1@                   | save pointer to saved context
 
-#endif
-      
-#else
-
-        moval    a7@(FPCONTEXT_ARG),a1    | a1 = &ptr to context area
-        moval    a1@,a0                   | a0 = Save context area
-#if ( !defined(__mcoldfire__) && !__mc68060__ )
+        /* Get context save area pointer argument from the stack */
+        moval    a7@(FPCONTEXT_ARG), a1
+        moval    a1@, a0
+
+  #if defined( __mcoldfire__ )
+        /* Move MACSR to data register and disable rounding */
+        movel    macsr, d0
+        clrl     d1
+        movl     d1, macsr
+
+        /* Save MACSR and ACC0 */
+        movl     acc0, d1
+        moveml   d0-d1, a0@(0)
+
+        /* Save ACC1 and ACC2 */
+        movl     acc1, d0
+        movl     acc2, d1
+        moveml   d0-d1, a0@(8)
+
+        /* Save ACC3 and ACCEXT01 */
+        movl     acc3, d0
+        movl     accext01, d1
+        moveml   d0-d1, a0@(16)
+
+        /* Save ACCEXT23 and MASK */
+        movl     accext23, d0
+        movl     mask, d1
+        moveml   d0-d1, a0@(24)
+
+    #if ( M68K_HAS_FPU == 1 )
+        /* Save FP state */
+        fsave    a0@(32)
+
+        /* Save FP instruction address */
+        fmovel   fpi, a0@(48)
+
+        /* Save FP data */
+        fmovem   fp0-fp7, a0@(52)
+    #endif
+  #else
+    #if defined( __mc68060__ )
+        lea      a0@(-M68K_FP_STATE_SIZE), a0
+        fsave    a0@                      | save 68060 state frame
+    #else
         fsave    a0@-                     | save 68881/68882 state frame
-#else
-        lea      a0@(-FP_STATE_SAVED),a0  | save the state of the FPU
-        fsave    a0@                      | on a Coldfire and 68060.
-#endif
+    #endif
         tstb     a0@                      | check for a null frame
         beq.b    nosv                     | Yes, skip save of user model
-#if ( !defined(__mcoldfire__) )
-        fmovem   fp0-fp7,a0@-             | save data registers (fp0-fp7)
-        fmovem   fpc/fps/fpi,a0@-         | and save control registers
-#else
-        lea      a0@(-FP_REGS_SAVED),a0
-        fmovem   fp0-fp7,a0@              | save data registers (fp0-fp7)
-        fmove.l  fpc,a0@-                 | and save control registers
-        fmove.l  fps,a0@-
-        fmove.l  fpi,a0@-
-#endif
-        movl     #-1,a0@-                 | place not-null flag on stack
-nosv:   movl     a0,a1@                   | save pointer to saved context
+        fmovem   fp0-fp7, a0@-            | save data registers (fp0-fp7)
+        fmovem   fpc/fps/fpi, a0@-        | and save control registers
+        movl     #-1, a0@-                | place not-null flag on stack
+nosv:
+        movl     a0, a1@                  | save pointer to saved context
+  #endif
 
-#endif
-#endif
+        /* Return */
         rts
 
         .align  4
         .global SYM (_CPU_Context_restore_fp)
 SYM (_CPU_Context_restore_fp):
-#if ( M68K_HAS_FPU == 1 )
-
-#if (defined(__mcoldfire__))
-
-        moval    a7@(FPCONTEXT_ARG),a1    | a1 = &ptr to context area
-        moval    a1@,a0                   | a0 = address of saved context
-        
-#if ( M68K_HAS_EMAC == 1 )
-
-        clrl     d0                       | clear d0
-        movl     d0,macsr                 | disable roundrounding in macsr
-        moveml   a0@(0),d0-d1		  | get mask/accext23 in d0/d1
-        movl     d0,mask                  | restore mask
-        movl     d1,accext23              | restore accext23
-        moveml   a0@(8),d0-d1		  | get accext01/acc3 in d0/d1
-	movl     d0,accext01              | restore accext01
-        movl     d1,acc3                  | restore acc3
-        moveml   a0@(16),d0-d1		  | get acc2/acc1 in d0/d1
-	movl     d0,acc2                  | restore acc2 
-        movl     d1,acc1                  | restore acc1
-        moveml   a0@(24),d0-d1		  | get acc0/macsr in d0/d1
-        movl     d0,acc0                  | restore acc0
-        movl     d1,macsr                 | restore macsr
-        leal     a0@(32),a0               | set a0 to the begin of coldfire FPU frame
-  
-#endif
-                
-        tstb     a0@                      | Null context frame?
-        beq.b    norest                   | Yes, skip fp restore
-        addql    #4,a0                    | throwaway non-null flag
-        fmovem   a0@,fp0-fp7              | restore data regs (fp0-fp7)
-        leal     a0@(+64),a0              | close context frame for coldfire data registers (fp0-fp7)
-norest: frestore a0@                      | restore the fp state frame
-        leal     a0@(+16),a0              | close context frame for coldfire state frame
-        movl     a0,a1@                   | save pointer to saved context
-
-#else
 
-        moval    a7@(FPCONTEXT_ARG),a1    | a1 = &ptr to context area
-        moval    a1@,a0                   | a0 = address of saved context
+        /* Get context save area pointer argument from the stack */
+        moval    a7@(FPCONTEXT_ARG), a1
+        moval    a1@, a0
+
+  #if defined( __mcoldfire__ )
+    #if ( M68K_HAS_FPU == 1 )
+        /* Restore FP data */
+        fmovem   a0@(52), fp0-fp7
+
+        /* Restore FP instruction address */
+        fmovel   a0@(48), fpi
+
+        /* Restore FP state */
+        frestore a0@(32)
+    #endif
+
+        /* Disable rounding */
+        clrl     d0
+        movl     d0, macsr
+
+        /* Restore MASK and ACCEXT23 */
+        moveml   a0@(24), d0-d1
+        movl     d0, mask
+        movl     d1, accext23
+
+        /* Restore ACCEXT01 and ACC3 */
+        moveml   a0@(16), d0-d1
+        movl     d0, accext01
+        movl     d1, acc3
+
+        /* Restore ACC2 and ACC1 */
+        moveml   a0@(8), d0-d1
+        movl     d0, acc2
+        movl     d1, acc1
+
+        /* Restore ACC0 and MACSR */
+        moveml   a0@(0), d0-d1
+        movl     d0, acc0
+        movl     d1, macsr
+  #else
         tstb     a0@                      | Null context frame?
         beq.b    norst                    | Yes, skip fp restore
-        addql    #4,a0                    | throwaway non-null flag
-#if ( !defined(__mcoldfire__) )
-        fmovem   a0@+,fpc/fps/fpi         | restore control registers
-        fmovem   a0@+,fp0-fp7             | restore data regs (fp0-fp7)
-norst:  frestore a0@+                     | restore the fp state frame
-#else
-        fmove.l  a0@+,fpc                 | restore control registers
-        fmove.l  a0@+,fps    
-        fmove.l  a0@+,fpi
-        fmovem   a0@,fp0-fp7              | restore data regs (fp0-fp7)
-        lea      a0@(FP_REGS_SAVED),a0
-norst:	frestore a0@                      | restore the fp state frame
-        lea      a0@(FP_STATE_SAVED),a0
-#endif
-        movl     a0,a1@                   | save pointer to saved context
+        addql    #4, a0                   | throwaway non-null flag
+        fmovem   a0@+, fpc/fps/fpi        | restore control registers
+        fmovem   a0@+, fp0-fp7            | restore data regs (fp0-fp7)
+norst:
+    #if defined( __mc68060__ )
+        frestore a0@                      | restore 68060 state frame
+        lea      a0@(M68K_FP_STATE_SIZE), a0
+    #else
+        frestore a0@+                     | restore 68881/68882 state frame
+    #endif
+        movl     a0, a1@                  | save pointer to saved context
+  #endif
 
-#endif
-#endif
+        /* Return */
         rts
 #endif
 
@@ -257,7 +225,7 @@ norst:	frestore a0@                      | restore the fp state frame
 .set PC_OFFSET,    4                     | Program Counter offset
 .set FVO_OFFSET,   0                     | Format/vector offset placed in the stack
 #endif /* M68K_HAS_VBR */
- 
+
 .set SAVED,        16                    | space for saved registers
 
         .align  4
@@ -292,7 +260,7 @@ SYM (_ISR_Handler):
 #endif /* CPU_HAS_SOFTWARE_INTERRUPT_STACK == 1 */
 
         addql   #1,SYM(_ISR_Nest_level) | one nest level deeper
-        
+
         movel   SYM (_ISR_Vector_table),a0 | a0= base of RTEMS table
 #if ( M68K_HAS_PREINDEXING == 1 )
         movel   (a0,d0:w:1),a0           | a0 = address of user routine
@@ -334,7 +302,7 @@ SYM (_ISR_Handler):
  */
   #if ( !defined(__mcoldfire__) && !__mc68060__ )
         cmpl    #_ISR_Handler,a7@(SAVED+PC_OFFSET)
-        beq.b   exit        
+        beq.b   exit
   #endif
 #endif
         tstb    SYM (_Context_Switch_necessary)
diff --git a/cpukit/score/cpu/m68k/rtems/score/cpu.h b/cpukit/score/cpu/m68k/rtems/score/cpu.h
index c60faf34d1..85ea4247cd 100644
--- a/cpukit/score/cpu/m68k/rtems/score/cpu.h
+++ b/cpukit/score/cpu/m68k/rtems/score/cpu.h
@@ -25,7 +25,11 @@ extern "C" {
 
 #include <rtems/score/m68k.h>              /* pick up machine definitions */
 #ifndef ASM
-#include <rtems/score/types.h>
+  #include <rtems/score/types.h>
+#else
+  /* FIXME */
+  #define TRUE 1
+  #define FALSE 0
 #endif
 
 /* conditional compilation parameters */
@@ -77,16 +81,16 @@ extern "C" {
  *         must be contexted switched on a per task basis.
  */
 
-#if ( M68K_HAS_FPU == 1 )
-#define CPU_HARDWARE_FP     TRUE
-#define CPU_SOFTWARE_FP     FALSE
-#else
-#define CPU_HARDWARE_FP     FALSE
-#if defined(__GNUC__)
-#define CPU_SOFTWARE_FP     TRUE
+#if ( M68K_HAS_FPU == 1 ) || ( M68K_HAS_EMAC == 1 )
+  #define CPU_HARDWARE_FP TRUE
+  #define CPU_SOFTWARE_FP FALSE
 #else
-#define CPU_SOFTWARE_FP     FALSE
-#endif
+  #define CPU_HARDWARE_FP FALSE
+  #if defined( __GNUC__ )
+    #define CPU_SOFTWARE_FP TRUE
+  #else
+    #define CPU_SOFTWARE_FP FALSE
+  #endif
 #endif
 
 /*
@@ -113,6 +117,7 @@ extern "C" {
 #define CPU_LITTLE_ENDIAN                        FALSE
 
 #ifndef ASM
+
 /* structures */
 
 /*
@@ -133,83 +138,151 @@ typedef struct {
   void       *a5;                /* (a5) address register 5 */
   void       *a6;                /* (a6) address register 6 */
   void       *a7_msp;            /* (a7) master stack pointer */
-
-#if (defined(__mcoldfire__))
-#if ( M68K_HAS_FPU == 1 )
-  uint8_t   fpu_dis;
-#endif
-#endif
-
-}   Context_Control;
+  #if defined( __mcoldfire__ ) && ( M68K_HAS_FPU == 1 )
+    uint8_t   fpu_dis;
+  #endif
+} Context_Control;
 
 #define _CPU_Context_Get_SP( _context ) \
   (_context)->a7_msp
 
 /*
- *  Floating point context ares
- */
-
-#if (CPU_SOFTWARE_FP == TRUE)
-
-/*
- *  This is the same as gcc's view of the software FP condition code
- *  register _fpCCR.  The implementation of the emulation code is
- *  in the gcc-VERSION/config/m68k directory.  This structure is
- *  correct as of gcc 2.7.2.2.
- */
-
-typedef struct {
-  uint16_t     _exception_bits;
-  uint16_t     _trap_enable_bits;
-  uint16_t     _sticky_bits;
-  uint16_t     _rounding_mode;
-  uint16_t     _format;
-  uint16_t     _last_operation;
-  union {
-    float sf;
-    double df;
-  } _operand1;
-  union {
-    float sf;
-    double df;
-  } _operand2;
-} Context_Control_fp;
-
-#elif (defined(__mcoldfire__))
-
-/*
- *  FP context save area for the ColdFire core numeric coprocessors
- */
-typedef struct {
-  uint8_t     fp_save_area[84];     /*    16 bytes for FSAVE/FRESTORE    */
-                                    /*    64 bytes for FMOVEM FP0-7      */
-                                    /*     4 bytes for non-null flag     */
-
-#if (M68K_HAS_EMAC == 1)
-
-/*
- *  EMAC context save area for the ColdFire core
- */
-  uint8_t     emac_save_area[32];   /*  32 bytes for EMAC registers      */
-
-#endif
-
-} Context_Control_fp;
-
-#if ( M68K_HAS_FPU == 1 )
-extern uint32_t _CPU_cacr_shadow;
+ *  Floating point context areas and support routines
+ */
+
+#if ( CPU_SOFTWARE_FP == TRUE )
+  /*
+   *  This is the same as gcc's view of the software FP condition code
+   *  register _fpCCR.  The implementation of the emulation code is
+   *  in the gcc-VERSION/config/m68k directory.  This structure is
+   *  correct as of gcc 2.7.2.2.
+   */
+  typedef struct {
+    uint16_t _exception_bits;
+    uint16_t _trap_enable_bits;
+    uint16_t _sticky_bits;
+    uint16_t _rounding_mode;
+    uint16_t _format;
+    uint16_t _last_operation;
+    union {
+      float sf;
+      double df;
+    } _operand1;
+    union {
+      float sf;
+      double df;
+    } _operand2;
+  } Context_Control_fp;
+
+  /*
+   *  This software FP implementation is only for GCC.
+   */
+  #define _CPU_Context_Fp_start( _base, _offset ) \
+     ((void *) _Addresses_Add_offset( (_base), (_offset) ) )
+
+  #define _CPU_Context_Initialize_fp( _fp_area ) \
+     { \
+       Context_Control_fp *_fp; \
+       _fp = *(Context_Control_fp **)_fp_area; \
+       _fp->_exception_bits = 0; \
+       _fp->_trap_enable_bits = 0; \
+       _fp->_sticky_bits = 0; \
+       _fp->_rounding_mode = 0;  /* ROUND_TO_NEAREST */ \
+       _fp->_format = 0;         /* NIL */ \
+       _fp->_last_operation = 0; /* NOOP */ \
+       _fp->_operand1.df = 0; \
+       _fp->_operand2.df = 0; \
+     }
 #endif
 
-#else
-/*
- *  FP context save area for the M68881/M68882 numeric coprocessors.
- */
-typedef struct {
-  uint8_t     fp_save_area[332];    /*   216 bytes for FSAVE/FRESTORE    */
-                                    /*    96 bytes for FMOVEM FP0-7      */
-                                    /*    12 bytes for FMOVEM CREGS      */
-                                    /*     4 bytes for non-null flag     */
-} Context_Control_fp;
+#if ( CPU_HARDWARE_FP == TRUE )
+  #if defined( __mcoldfire__ )
+    /* We need memset() to initialize the FP context */
+    #include <string.h>
+
+    #if ( M68K_HAS_FPU == 1 )
+      /*
+       * The Cache Control Register (CACR) has write-only access.  It is also
+       * used to enable and disable the FPU.  We need to maintain a copy of
+       * this register to allow per thread values.
+       */
+      extern uint32_t _CPU_cacr_shadow;
+    #endif
+
+    /* We assume that each ColdFire core with a FPU has also an EMAC unit */
+    typedef struct {
+      uint32_t emac_macsr;
+      uint32_t emac_acc0;
+      uint32_t emac_acc1;
+      uint32_t emac_acc2;
+      uint32_t emac_acc3;
+      uint32_t emac_accext01;
+      uint32_t emac_accext23;
+      uint32_t emac_mask;
+      #if ( M68K_HAS_FPU == 1 )
+        uint16_t fp_state_format;
+        uint16_t fp_state_fpcr;
+        double fp_state_op;
+        uint32_t fp_state_fpsr;
+
+        /*
+         * We need to save the FP Instruction Address Register (FPIAR), because
+         * a context switch can occur within a FP exception before the handler
+         * was able to save this register.
+         */
+        uint32_t fp_fpiar;
+
+        double fp_data [8];
+      #endif
+    } Context_Control_fp;
+
+    #define _CPU_Context_Fp_start( _base, _offset ) \
+      ((void *) _Addresses_Add_offset( (_base), (_offset) ))
+
+    /*
+     * The reset value for all context relevant registers except the FP data
+     * registers is zero.  The reset value of the FP data register is NAN.  The
+     * restore of the reset FP state will reset the FP data registers, so the
+     * initial value of them can be arbitrary here.
+     */
+    #define _CPU_Context_Initialize_fp( _fp_area ) \
+      memset( *(_fp_area), 0, sizeof( Context_Control_fp ) )
+  #else
+    /*
+     *  FP context save area for the M68881/M68882 and 68060 numeric coprocessors.
+     */
+
+    #if defined( __mc68060__ )
+      #define M68K_FP_STATE_SIZE 16
+    #else
+      #define M68K_FP_STATE_SIZE 216
+    #endif
+
+    typedef struct {
+      /*
+       * M68K_FP_STATE_SIZE bytes for FSAVE/FRESTORE
+       * 96 bytes for FMOVEM FP0-7
+       * 12 bytes for FMOVEM CREGS
+       * 4 bytes for non-null flag
+       */
+      uint8_t fp_save_area [M68K_FP_STATE_SIZE + 112];
+    } Context_Control_fp;
+
+    #define _CPU_Context_Fp_start( _base, _offset ) \
+       ( \
+         (void *) _Addresses_Add_offset( \
+            (_base), \
+            (_offset) + CPU_CONTEXT_FP_SIZE - 4 \
+         ) \
+       )
+
+    #define _CPU_Context_Initialize_fp( _fp_area ) \
+       { \
+         uint32_t   *_fp_context = (uint32_t *)*(_fp_area); \
+         *(--(_fp_context)) = 0; \
+         *(_fp_area) = (void *)(_fp_context); \
+       }
+    #endif
 #endif
 
 /*
@@ -264,6 +337,7 @@ typedef struct {
 SCORE_EXTERN _CPU_ISR_handler_entry _CPU_ISR_jump_table[256]; 
 
 #endif /* M68K_HAS_VBR */
+
 #endif /* ASM */
 
 /* constants */
@@ -404,60 +478,6 @@ uint32_t   _CPU_ISR_Get_level( void );
         : "0" ((_the_context)->sr), "1" ((_the_context)->a7_msp) ); \
   }
 
-/*
- *  Floating Point Context Area Support routines
- */
-
-#if (CPU_SOFTWARE_FP == TRUE)
-
-/*
- *  This software FP implementation is only for GCC.
- */
-
-#define _CPU_Context_Fp_start( _base, _offset ) \
-   ((void *) _Addresses_Add_offset( (_base), (_offset) ) )
-
-
-#define _CPU_Context_Initialize_fp( _fp_area ) \
-   { \
-   Context_Control_fp *_fp; \
-   _fp = *(Context_Control_fp **)_fp_area; \
-   _fp->_exception_bits = 0; \
-   _fp->_trap_enable_bits = 0; \
-   _fp->_sticky_bits = 0; \
-   _fp->_rounding_mode = 0;  /* ROUND_TO_NEAREST */ \
-   _fp->_format = 0;         /* NIL */ \
-   _fp->_last_operation = 0;  /* NOOP */ \
-   _fp->_operand1.df = 0; \
-   _fp->_operand2.df = 0; \
-   }
-#else
-#define _CPU_Context_Fp_start( _base, _offset ) \
-   ((void *) \
-     _Addresses_Add_offset( \
-        (_base), \
-        (_offset) + CPU_CONTEXT_FP_SIZE - 4 \
-     ) \
-   )
-
-#if (defined(__mcoldfire__) && ( M68K_HAS_FPU == 1 ))
-#define _CPU_Context_Initialize_fp( _fp_area ) \
-   { uint32_t   *_fp_context = (uint32_t *)*(_fp_area); \
-     \
-     *(--(_fp_context)) = 0; \
-     *(_fp_area) = (void *)(_fp_context); \
-     asm volatile("movl %0,%%macsr": : "d" (0) ); \
-   }
-#else
-#define _CPU_Context_Initialize_fp( _fp_area ) \
-   { uint32_t   *_fp_context = (uint32_t *)*(_fp_area); \
-     \
-     *(--(_fp_context)) = 0; \
-     *(_fp_area) = (void *)(_fp_context); \
-   }
-#endif
-#endif
-
 /* end of Context handler macros */
 
 /*