diff --git a/bsps/x86_64/amd64/interrupts/isr_handler.S b/bsps/x86_64/amd64/interrupts/isr_handler.S index fc2c9a9e76..fd8c77683c 100644 --- a/bsps/x86_64/amd64/interrupts/isr_handler.S +++ b/bsps/x86_64/amd64/interrupts/isr_handler.S @@ -123,6 +123,19 @@ SYM(_ISR_Handler): /* Save the initial rsp */ movq SAVED_RSP, (0 * CPU_SIZEOF_POINTER)(rsp) + /* Save x87 FPU, MMX and SSE state */ +.set FXSAVE_SIZE, 512 + /* Make space for FXSAVE */ + subq $FXSAVE_SIZE, rsp + fwait + fxsave64 (rsp) + /* Reset to a clean state */ + fninit + subq $4, rsp + movl $0x1F80, (rsp) + ldmxcsr (rsp) + addq $4, rsp + .switch_stack_if_needed: /* Save current aligned rsp so we can find CPU_Interrupt_frame again later */ movq rsp, SAVED_RSP @@ -166,6 +179,12 @@ SYM(_ISR_Handler): call _Thread_Dispatch .restore_cpu_interrupt_frame: + /* Restore x87 FPU, MMX and SSE state */ + fwait + fxrstor64 (rsp) + /* Restore rsp to CPU_Interrupt_frame */ + addq $FXSAVE_SIZE, rsp + /* Restore registers from CPU_Interrupt_frame */ movq (8 * CPU_SIZEOF_POINTER)(rsp), rax movq (7 * CPU_SIZEOF_POINTER)(rsp), rcx diff --git a/cpukit/score/cpu/x86_64/cpu.c b/cpukit/score/cpu/x86_64/cpu.c index 0ad2d1ff3c..44fe39482a 100644 --- a/cpukit/score/cpu/x86_64/cpu.c +++ b/cpukit/score/cpu/x86_64/cpu.c @@ -46,8 +46,19 @@ void _CPU_Exception_frame_print(const CPU_Exception_frame *ctx) { } +Context_Control_fp _CPU_Null_fp_context; + void _CPU_Initialize(void) { + /* + * Save the FP context intialized by the UEFI firmware in "_CPU_Null_fp_context" + * which is given to each task at start and restart time. + * According to the UEFI specification this should mean that: + * _CPU_Null_fp_context.mxcsr = 0x1F80 + * _CPU_Null_fp_context.fpucw = 0x37F + */ + asm volatile( "stmxcsr %0" : "=m"(_CPU_Null_fp_context.mxcsr) ); + asm volatile( "fstcw %0" : "=m"(_CPU_Null_fp_context.fpucw) ); } void _CPU_Fatal_halt( uint32_t source, CPU_Uint32ptr error ) diff --git a/cpukit/score/cpu/x86_64/include/rtems/score/cpu.h b/cpukit/score/cpu/x86_64/include/rtems/score/cpu.h index a6c91c70f8..db50443a17 100644 --- a/cpukit/score/cpu/x86_64/include/rtems/score/cpu.h +++ b/cpukit/score/cpu/x86_64/include/rtems/score/cpu.h @@ -45,9 +45,9 @@ extern "C" { #define CPU_SIMPLE_VECTORED_INTERRUPTS FALSE #define CPU_ISR_PASSES_FRAME_POINTER FALSE -#define CPU_HARDWARE_FP FALSE +#define CPU_HARDWARE_FP TRUE #define CPU_SOFTWARE_FP FALSE -#define CPU_ALL_TASKS_ARE_FP FALSE +#define CPU_ALL_TASKS_ARE_FP TRUE #define CPU_IDLE_TASK_IS_FP FALSE #define CPU_USE_DEFERRED_FP_SWITCH FALSE #define CPU_ENABLE_ROBUST_THREAD_DISPATCH FALSE @@ -68,7 +68,7 @@ typedef struct { /** * Callee-saved registers as listed in the SysV ABI document: - * https://github.com/hjl-tools/x86-psABI/wiki/X86-psABI + * https://gitlab.com/x86-psABIs/x86-64-ABI */ uint64_t rbx; void *rsp; @@ -85,6 +85,15 @@ typedef struct { #endif } Context_Control; +typedef struct { + /** + * Callee-saved FP registers as listed in the SysV ABI document: + * https://gitlab.com/x86-psABIs/x86-64-ABI + */ + uint32_t mxcsr; + uint16_t fpucw; +} Context_Control_fp; + #define _CPU_Context_Get_SP( _context ) \ (_context)->rsp @@ -125,6 +134,10 @@ typedef struct { */ } CPU_Interrupt_frame; +extern Context_Control_fp _CPU_Null_fp_context; + +#define CPU_CONTEXT_FP_SIZE sizeof( Context_Control_fp ) + #endif /* !ASM */ #define CPU_INTERRUPT_FRAME_SIZE 72 @@ -278,6 +291,14 @@ typedef struct { double float_registers [1]; } CPU_Exception_frame; +void _CPU_Context_save_fp( + Context_Control_fp **fp_context_ptr +); + +void _CPU_Context_restore_fp( + Context_Control_fp **fp_context_ptr +); + void _CPU_Exception_frame_print( const CPU_Exception_frame *frame ); static inline uint32_t CPU_swap_u32( @@ -295,6 +316,30 @@ static inline uint32_t CPU_swap_u32( return swapped; } +#define _CPU_Context_save_fp(fp_context_pp) \ + do { \ + __asm__ __volatile__( \ + "fstcw %0" \ + :"=m"((*(fp_context_pp))->fpucw) \ + ); \ + __asm__ __volatile__( \ + "stmxcsr %0" \ + :"=m"((*(fp_context_pp))->mxcsr) \ + ); \ + } while (0) + +#define _CPU_Context_restore_fp(fp_context_pp) \ + do { \ + __asm__ __volatile__( \ + "fldcw %0" \ + :"=m"((*(fp_context_pp))->fpucw) \ + ); \ + __asm__ __volatile__( \ + "ldmxcsr %0" \ + :"=m"((*(fp_context_pp))->mxcsr) \ + ); \ + } while (0) + #define CPU_swap_u16( value ) \ (((value&0xff) << 8) | ((value >> 8)&0xff))