From b02f4cc11f03d3425e3ce83daaa1d903c937b5b3 Mon Sep 17 00:00:00 2001
From: Till Straumann <strauman@slac.stanford.edu>
Date: Tue, 10 Nov 2009 06:55:28 +0000
Subject: 2009-11-09	Till Straumann <strauman@slac.stanford.edu>

	* cpu.c, cpu_asm.S, rtems/score/cpu.h, sse_test.c:
	Added experimental SSE support.
---
 cpukit/score/cpu/i386/cpu_asm.S | 34 ++++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

(limited to 'cpukit/score/cpu/i386/cpu_asm.S')

diff --git a/cpukit/score/cpu/i386/cpu_asm.S b/cpukit/score/cpu/i386/cpu_asm.S
index 4084e03fb8..20b9949a06 100644
--- a/cpukit/score/cpu/i386/cpu_asm.S
+++ b/cpukit/score/cpu/i386/cpu_asm.S
@@ -97,6 +97,7 @@ SYM (_CPU_Context_restore):
 
 .set FPCONTEXT_ARG,   4                    /* FP context argument */
 
+#ifndef __SSE__
         .p2align  1
         PUBLIC (_CPU_Context_save_fp)
 SYM (_CPU_Context_save_fp):
@@ -112,18 +113,44 @@ SYM (_CPU_Context_restore_fp):
         movl      (eax),eax                /* eax = FP context area */
         frstor    (eax)                    /* restore FP context */
         ret
+#endif
+
+#ifdef __SSE__
+#define SSE_OFF  16
+#endif
 
         PUBLIC (_Exception_Handler)
 SYM (_Exception_Handler):
 	pusha                  /* Push general purpose registers    */
+	pushl   $0             /* Null pointer to SSE area          */
 	movl    esp,  ebp      /* Save original SP                  */
+#ifndef __SSE__
 	subl    $4,   esp      /* Reserve space for argument        */
                            /* Align stack (courtesy for C/gcc)  */
 	andl    $ - CPU_STACK_ALIGNMENT, esp
+#else
+	subl    $512, esp      /* Space for SSE area                */
+                           /* Align stack (courtesy for C/gcc)  */
+	andl    $ - CPU_STACK_ALIGNMENT, esp
+/* Doing fwait here will re-throw an already pending FP exception!
+	fwait
+ */
+	fxsave  0(esp)
+	fninit                 /* Clean-slate FPU                   */
+	movl    $0x1f80, 0(ebp)
+	ldmxcsr 0(ebp)         /* Clean-slate MXCSR                 */
+	movl    esp, 0(ebp)    /* Store pointer to SSE area         */
+	subl    $SSE_OFF, esp  /* Aligned space for argument        */
+#endif
 	movl    ebp, (esp)     /* Store argument                    */
 	movl	_currentExcHandler, eax	   /* Call function stored in _currentExcHandler */
 	call	* eax
+#ifdef __SSE__
+	fwait
+	fxrstor 16(esp)
+#endif
 	movl    ebp,  esp      /* Restore original SP               */
+	addl    $4,   esp      /* Skill pointer to SSE area         */
 	popa                   /* Restore general purpose registers */
 	addl	$8, esp        /* Skill vector number and faultCode */
 	iret
@@ -216,6 +243,13 @@ DISTINCT_EXCEPTION_WITH_FAULTCODE_ENTRY (17)
  */
 DISTINCT_EXCEPTION_WITH_FAULTCODE_ENTRY (18)
 
+#ifdef __SSE__
+/*
+ * SIMD FP Exception
+ */
+DISTINCT_EXCEPTION_WITHOUT_FAULTCODE_ENTRY (19)
+#endif
+
 
 /*
  *  void *i386_Logical_to_physical(
-- 
cgit v1.2.3