/**
 *  @file
 *
 *  @brief Test FPU/SSE Context Save and Restore
 */

/*
 * Authorship
 * ----------
 * This software was created by
 *     Till Straumann <strauman@slac.stanford.edu>, 2009,
 *     Stanford Linear Accelerator Center, Stanford University.
 *
 * Acknowledgement of sponsorship
 * ------------------------------
 * This software was produced by
 *     the Stanford Linear Accelerator Center, Stanford University,
 *     under Contract DE-AC03-76SFO0515 with the Department of Energy.
 *
 * Government disclaimer of liability
 * ----------------------------------
 * Neither the United States nor the United States Department of Energy,
 * nor any of their employees, makes any warranty, express or implied, or
 * assumes any legal liability or responsibility for the accuracy,
 * completeness, or usefulness of any data, apparatus, product, or process
 * disclosed, or represents that its use would not infringe privately owned
 * rights.
 *
 * Stanford disclaimer of liability
 * --------------------------------
 * Stanford University makes no representations or warranties, express or
 * implied, nor assumes any liability for the use of this software.
 *
 * Stanford disclaimer of copyright
 * --------------------------------
 * Stanford University, owner of the copyright, hereby disclaims its
 * copyright and all other rights in this software.  Hence, anyone may
 * freely use it for any purpose without restriction.
 *
 * Maintenance of notices
 * ----------------------
 * In the interest of clarity regarding the origin and status of this
 * SLAC software, this and all the preceding Stanford University notices
 * are to remain affixed to any copy or derivative of this software made
 * or distributed by the recipient and are to be affixed to any copy of
 * software made or distributed by the recipient that contains a copy or
 * derivative of this software.
 *
 * ------------------ SLAC Software Notices, Set 4 OTT.002a, 2004 FEB 03
 */


/* Code for testing FPU/SSE context save/restore across exceptions
 * (including interrupts).
 *
 * There are two tasks and an IRQ/EXC handler involved. One task (LP)
 * is of lower priority than the other (HP) task.
 *
 * 1) LP task sets up a context area in memory (known contents; every
 *    register is loaded with different values)
 *
 * 2) LP task
 *       2a saves original FP/SSE context
 *       2b loads context from 1) into FPU/SSE
 *       2c raises an exception or interrupt
 *
 *   *  (2d save FPU/SSE context after irq/exception returns to
 *          separate area for verification
 *       2e reload original FP/SSE context.)
 *
 *   * All these five steps are coded in assembly to prevent
 *     gcc from manipulating the FP/SSE state. The last two steps,
 *     however, are effectively executed during 6 when control is
 *     returned to the LP task.
 *
 * 3) IRQ/EXC handler OS wrapper saves context, initializes FPU and
 *    MXCSR.
 *
 * 4) user (our) irq/exc handler clears exception condition, clobbers
 *    FPU and XMM regs and finally releases a semaphore on which HP
 *    task is waiting.
 *
 * 5) context switch to HP task. HP task clobbers FPU and XMM regs.
 *    Then it tries to re-acquire the synchronization semaphore and
 *    blocks.
 *
 * 6) task switch back to (interrupted) LP task. Original context is
 *    restored and verified against the context that was setup in 1).
 *
 *
 * Three methods for interrupting the LP task are tested
 *
 *  a) FP exception (by setting an exception status in the context from 1)
 *  b) SSE exception (by computing the square root of a vector of negative
 *     numbers.
 *  c) IRQ (software IRQ via 'INT xx' instruction)
 *
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#ifdef __rtems__
#include <rtems.h>
#include <rtems/score/cpu.h>
#include <rtems/irq.h>
#include <rtems/error.h>
#endif

#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

/* This is currently hardcoded (int xx opcode requires immediate operand) */
#define SSE_TEST_IRQ  10

typedef uint8_t   __v8  __attribute__((vector_size(16)));
typedef uint32_t __v32  __attribute__((vector_size(16)));
typedef float     __vf  __attribute__((vector_size(16)));

#ifndef __rtems__
/* Clone of what is defined in rtems/score/cpu.h (for testing under linux) */
typedef struct Context_Control_sse {
  uint16_t  fcw;
  uint16_t  fsw;
  uint8_t   ftw;
  uint8_t   res_1;
  uint16_t  fop;
  uint32_t  fpu_ip;
  uint16_t  cs;
  uint16_t  res_2;
  uint32_t  fpu_dp;
  uint16_t  ds;
  uint16_t  res_3;
  uint32_t  mxcsr;
  uint32_t  mxcsr_mask;
  struct {
    uint8_t fpreg[10];
    uint8_t res_4[ 6];
  } fp_mmregs[8];
  uint8_t   xmmregs[8][16];
  uint8_t   res_5[224];
} Context_Control_sse
__attribute__((aligned(16)))
;
#endif

#define MXCSR_FZ          (1<<15)   /* Flush to zero */
#define MXCSR_RC(x) (((x)&3)<<13)   /* Rounding ctrl */
#define MXCSR_PM          (1<<12)   /* Precision msk */
#define MXCSR_UM          (1<<11)   /* Underflow msk */
#define MXCSR_OM          (1<<10)   /* Overflow  msk */
#define MXCSR_ZM          (1<< 9)   /* Divbyzero msk */
#define MXCSR_DM          (1<< 8)   /* Denormal  msk */
#define MXCSR_IM          (1<< 7)   /* Invalidop msk */
#define MXCSR_DAZ         (1<< 6)   /* Denorml are 0 */
#define MXCSR_PE          (1<< 5)   /* Precision flg */
#define MXCSR_UE          (1<< 4)   /* Underflow flg */
#define MXCSR_OE          (1<< 3)   /* Overflow  flg */
#define MXCSR_ZE          (1<< 2)   /* Divbyzero flg */
#define MXCSR_DE          (1<< 1)   /* Denormal  flg */
#define MXCSR_IE          (1<< 0)   /* Invalidop flg */

#define MXCSR_ALLM (MXCSR_PM | MXCSR_UM | MXCSR_OM | MXCSR_ZM | MXCSR_DM | MXCSR_IM)
#define MXCSR_ALLE (MXCSR_PE | MXCSR_UE | MXCSR_OE | MXCSR_ZE | MXCSR_DE | MXCSR_IE)

#define FPSR_B            (1<<15)   /* FPU busy      */
#define FPSR_C3           (1<<14)   /* Cond code C3  */
#define FPSR_TOP(x) (((x)&7)<<11)   /* TOP           */
#define FPSR_C2           (1<<10)   /* Cond code C2  */
#define FPSR_C1           (1<< 9)   /* Cond code C1  */
#define FPSR_C0           (1<< 8)   /* Cond code C0  */
#define FPSR_ES           (1<< 7)   /* Error summary */
#define FPSR_SF           (1<< 6)   /* Stack fault   */
#define FPSR_PE           (1<< 5)   /* Precision flg */
#define FPSR_UE           (1<< 4)   /* Underflow flg */
#define FPSR_OE           (1<< 3)   /* Overflow  flg */
#define FPSR_ZE           (1<< 2)   /* Divbyzero flg */
#define FPSR_DE           (1<< 1)   /* Denormal  flg */
#define FPSR_IE           (1<< 0)   /* Invalidop flg */

#define FPCW_X            (1<<12)   /* Infinity ctrl */
#define FPCW_RC(x)  (((x)&3)<<10)   /* Rounding ctrl */
#define FPCW_PC(x)  (((x)&3)<< 8)   /* Precision ctl */
#define FPCW_PM           (1<< 5)   /* Precision msk */
#define FPCW_UM           (1<< 4)   /* Underflow msk */
#define FPCW_OM           (1<< 3)   /* Overflow  msk */
#define FPCW_ZM           (1<< 2)   /* Divbyzero msk */
#define FPCW_DM           (1<< 1)   /* Denormal  msk */
#define FPCW_IM           (1<< 0)   /* Invalidop msk */

#define FPCW_ALLM (FPCW_PM | FPCW_UM | FPCW_OM | FPCW_ZM | FPCW_DM | FPCW_IM)
#define FPSR_ALLE (FPSR_ES | FPSR_SF | FPSR_PE | FPSR_UE | FPSR_OE | FPSR_ZE | FPSR_DE | FPSR_IE)

/* Store 'double' into 80-bit register image */
void
fp_st1(uint8_t (*p_dst)[10], double v)
{
	asm volatile("fstpt %0":"=m"(*p_dst):"t"(v):"st");
}

/* Store 'double' into 80-bit register image #i in context */
void
fp_st(Context_Control_sse *p_ctxt, int i, double v)
{
	fp_st1(&p_ctxt->fp_mmregs[i].fpreg,v);
}

/* Load 'double' from 80-bit register image */
double
fp_ld1(uint8_t (*p_src)[10])
{
double v;

	asm volatile("fldt %1":"=t"(v):"m"((*p_src)[0]),"m"(*p_src));
	return v;
}

/* Load 'double' from 80-bit register image #i in context */
double
fp_ld(Context_Control_sse *p_ctxt, int i)
{
	return fp_ld1(&p_ctxt->fp_mmregs[i].fpreg);
}

#define FPUCLOBBER \
	"st","st(1)","st(2)","st(3)",   \
	"st(4)","st(5)","st(6)","st(7)",\
	"fpsr","memory"

/* There seems to be no way to say that mxcsr was clobbered */

#define SSECLOBBER \
	"xmm0","xmm1","xmm2","xmm3",    \
	"xmm4","xmm5","xmm6","xmm7"

static void
sse_clobber(uint32_t x)
{
__v32 v = { x, x, x, x };
	asm volatile (
		"	movdqa %0,     %%xmm0      \n"
		"	movdqa %%xmm0, %%xmm1      \n"
		"	movdqa %%xmm0, %%xmm2      \n"
		"	movdqa %%xmm0, %%xmm3      \n"
		"	movdqa %%xmm0, %%xmm4      \n"
		"	movdqa %%xmm0, %%xmm5      \n"
		"	movdqa %%xmm0, %%xmm6      \n"
		"	movdqa %%xmm0, %%xmm7      \n"
		:
		:"m"(v)
		:SSECLOBBER
	);
}

void
all_clobber(uint32_t v1, uint32_t v2);

__asm__ (
"all_clobber:               \n"
"   finit                   \n"
"	movq  0(%esp), %xmm0    \n"
"   punpcklqdq %xmm0, %xmm0 \n"
"	movdqa %xmm0, %xmm1     \n"
"	movdqa %xmm0, %xmm2     \n"
"	movdqa %xmm0, %xmm3     \n"
"	movdqa %xmm0, %xmm4     \n"
"	movdqa %xmm0, %xmm5     \n"
"	movdqa %xmm0, %xmm6     \n"
"	movdqa %xmm0, %xmm7     \n"
"	ret                     \n"
);

/* Clear FPU and save FPU/SSE registers to context area */

void
init_ctxt(Context_Control_sse *p_ctxt);

__asm__ (
"init_ctxt:            \n"
"	finit              \n"
"   mov    4(%esp), %eax\n"
"   fxsave (%eax)      \n"
"   fwait              \n"
"   ret                \n"
);

/* Save FPU/SSE registers to context area */

static void
stor_ctxt(Context_Control_sse *p_ctxt)
{
	memset(p_ctxt, 0, sizeof(*p_ctxt));
	asm volatile(
/*		"	finit                \n" */
		"	fxsave %0            \n"
		"   fwait                \n"
		: "=m"(*p_ctxt)
		:
		: FPUCLOBBER
	);
}

#define H08 "0x%02"PRIx8
#define H16 "0x%04"PRIx16
#define H32 "0x%08"PRIx32

#define F16 "mismatch ("H16" != "H16")\n"

#define FLDCMP(fld, fmt) \
	if ( a->fld != b->fld ) { \
		rval = 1;             \
		if ( !quiet )         \
           fprintf(stderr,#fld" mismatch ("fmt" != "fmt")\n",a->fld, b->fld); \
	}

#define FLTCMP(i)                                   \
	do {                                            \
		if (   ( (a->ftw ^ b->ftw) & (1<<i))        \
			|| ( (a->ftw & b->ftw  & (1<<i)) &&     \
                 memcmp(a->fp_mmregs[i].fpreg,      \
                    b->fp_mmregs[i].fpreg,          \
                    sizeof(a->fp_mmregs[i].fpreg))  \
               )                                    \
           ) {                                      \
            rval = 1;                               \
            if ( !quiet ) {                         \
              double fa = fp_ld(a, i);              \
              double fb = fp_ld(b, i);              \
			  if ( ((a->ftw ^ b->ftw) & (1<<i)) )   \
                fprintf(stderr,"fpreg[%u] TAG mismatch (%u != %u)\n",i,(a->ftw & (1<<i)) ? 1 : 0,(b->ftw & (1<<i)) ? 1 : 0); \
			  else                                  \
                fprintf(stderr,"fpreg[%u] mismatch (%g != %g)\n",i,fa,fb); \
	        }                                       \
	    }                                           \
    } while (0)	

#define XMMCMP(i)                                   \
	do {                                            \
		if ( memcmp(&a->xmmregs[i],                 \
                    &b->xmmregs[i],                 \
                    sizeof(a->xmmregs[i]))          \
           ) {                                      \
            rval = 1;                               \
            if ( !quiet ) {                         \
              int _jj;                              \
              fprintf(stderr,"xmmreg[%u] mismatch:\n", i); \
              fprintf(stderr,"    ");               \
              for (_jj=0; _jj<16; _jj++)            \
                fprintf(stderr,"%02x ",a->xmmregs[i][_jj]); \
              fprintf(stderr,"\n !=\n");            \
              fprintf(stderr,"    ");               \
              for (_jj=0; _jj<16; _jj++)            \
                fprintf(stderr,"%02x ",b->xmmregs[i][_jj]); \
              fprintf(stderr,"\n");                 \
	        }                                       \
	    }                                           \
    } while (0)	


/* Compare two FPU/SSE context areas and flag differences;
 * RETURNS: zero if the contexts match and nonzero otherwise
 */
static int
cmp_ctxt(Context_Control_sse *a, Context_Control_sse *b, int quiet)
{
int rval = 0;
int i;
	FLDCMP(fcw,H16);
	FLDCMP(fsw,H16);
	FLDCMP(ftw,H08);
	FLDCMP(fop,H16);
	FLDCMP(fpu_ip,H32);
	FLDCMP(cs,H16);
	FLDCMP(fpu_dp,H32);
	FLDCMP(ds,H16);
	FLDCMP(mxcsr,H32);
	FLDCMP(mxcsr_mask,H32);
	for ( i=0; i<8; i++ ) {
		FLTCMP(i);
	}
	for ( i=0; i<8; i++ ) {
		XMMCMP(i);
	}
	return rval;
}

/* Possible arguments to exc_raise() */

#define FP_EXC   0
#define IRQ_EXC  1
#define SSE_EXC -1

/* Check stack alignment by raising the interrupt from a
 * non-16-byte aligned section of code. The exception/IRQ
 * handler must align the stack and SSE context area
 * properly or it will crash.
 */
#define __INTRAISE(x) "	int  $32+"#x" \n"
#define INTRAISE(x)   __INTRAISE(x)

__asm__ (
"do_raise:               \n"
"   fwait                \n"
"	test    %eax, %eax   \n"
"   je      2f           \n"
"   jl      1f           \n"
INTRAISE(SSE_TEST_IRQ)
"   jmp     2f           \n"
"1: sqrtps  %xmm0, %xmm0 \n"
"2:                      \n"
"   ret                  \n"
);

#define SSE_TEST_HP_FAILED       1
#define SSE_TEST_FSPR_FAILED     2
#define SSE_TEST_CTXTCMP_FAILED  4

static const char *fail_msgs[] = {
	"Seems that HP task was not executing",
	"FPSR 'Invalid-operation' flag should be clear",
	"Restored context does NOT match the saved one",
};

static void prstat(int st, const char *where)
{
int i,msk;
	for ( i=0, msk=1; i<sizeof(fail_msgs)/sizeof(fail_msgs[0]); i++, msk<<=1 ) {
		if ( (st & msk) ) {
			fprintf(stderr,"sse_test ERROR: %s (testing: %s)\n", fail_msgs[i], where);
		}
	}
}

int                 sse_test_debug   = 0;

static int
exc_raise(int kind)
{
Context_Control_sse nctxt;
Context_Control_sse octxt;
Context_Control_sse orig_ctxt;
int                 i,j,rval;
double              s2;
uint16_t            fsw;
__vf                f4  = { -1., -2., -3., -4. };
__vf                tmp;
__v32               sgn = { (1<<31), (1<<31), (1<<31), (1<<31) };

	stor_ctxt(&octxt);

	octxt.fsw   &= ~FPSR_ALLE;
	octxt.mxcsr &= ~MXCSR_ALLE;

	for ( i=0; i<8; i++ ) {
		fp_st(&octxt, i, (double)i+0.1);
		for (j=0; j<16; j++) {
			octxt.xmmregs[i][j]=(i<<4)+j;
		}
	}


	if ( SSE_EXC == kind ) {
		memcpy(octxt.xmmregs[0], &f4, sizeof(f4));
		octxt.mxcsr &= ~MXCSR_IM;
	}

	/* set tags to 'valid'            */
	octxt.ftw = 0xff;

	/* enable 'invalid arg' exception */
	octxt.fcw &= ~ ( FPCW_IM );
	
	if ( FP_EXC == kind ) {
		octxt.fsw |=   ( FPSR_IE | FPSR_ES );
	}

	if ( sse_test_debug )
		printk("RAISE (fsw was 0x%04x)\n", orig_ctxt.fsw);
	asm volatile(
		"	fxsave  %2           \n"
#ifdef __rtems__
		"   movl    %4, sse_test_check\n"
#endif
		"	fxrstor %3           \n"
		"   call    do_raise     \n"
#ifdef __rtems__
		"   movl    sse_test_check, %1\n"
#else
		"   movl    $0, %1       \n"
#endif
#ifdef TEST_MISMATCH
		"	pxor %%xmm0, %%xmm0  \n"
#endif
		"	fxsave  %0           \n"
		"	fxrstor %2           \n"
	: "=m"(nctxt),"=&r"(rval),"=m"(orig_ctxt)
	: "m"(octxt), "i"(SSE_TEST_HP_FAILED),"a"(kind)
	: "xmm0"
	);

	if ( ( FPSR_IE & nctxt.fsw ) ) {
		rval |= SSE_TEST_FSPR_FAILED;
	}
	if ( FP_EXC == kind )
		nctxt.fsw |= (FPSR_IE | FPSR_ES);
	else if ( SSE_EXC == kind ) {
		tmp = __builtin_ia32_sqrtps( (__vf)(~sgn & (__v32)f4) );
		/* sqrt raises PE; just clear it */
		nctxt.mxcsr &= ~MXCSR_PE;
		memcpy( octxt.xmmregs[0], &tmp, sizeof(tmp) );
	}

	if ( cmp_ctxt(&nctxt, &octxt, 0) ) {
		rval |= SSE_TEST_CTXTCMP_FAILED;
	}

	s2 = sqrt(2.0);

	asm volatile("fstsw %0":"=m"(fsw));

	if ( sse_test_debug )
		printf("sqrt(2): %f (FSTW: 0x%02"PRIx16")\n", sqrt(2.0), fsw);

	return rval;
}

#ifdef __rtems__
static void
sse_test_ehdl(CPU_Exception_frame *p_f);

rtems_id            sse_test_sync    = 0;
cpuExcHandlerType   sse_test_ohdl    = 0;

CPU_Exception_frame *sse_test_frame  = 0;
volatile int        sse_test_check   = SSE_TEST_HP_FAILED;
unsigned            sse_tests        = 0;

rtems_task
sse_test_hp_task(rtems_task_argument arg)
{
rtems_id sync = (rtems_id)arg;

uint16_t 			fp_cw;
uint32_t 			mxcsr;
rtems_status_code   sc;
const char *        msgs[] = {"FPU_EXC", "SSE_EXC", "IRQ_EXC"};
int                 i;

	/* verify that FPU control word is default value */
	asm volatile("fstcw %0":"=m"(fp_cw));
	if ( fp_cw != _CPU_Null_fp_context.fpucw ) {
		fprintf(
			stderr,
			"ERROR: FPU CW initialization mismatch: got 0x%04"PRIx16"; expected 0x%04"PRIx16"\n",
			fp_cw,
			_CPU_Null_fp_context.fpucw
		);
	}

	/* check MXCSR default value                     */
	asm volatile("stmxcsr %0":"=m"(mxcsr));
	if ( mxcsr != _CPU_Null_fp_context.mxcsr ) {
		fprintf(
			stderr,
			"ERROR: MXCSR initialization mismatch: got 0x%08"PRIx32"; expected 0x%08"PRIx32"\n",
			mxcsr,
			_CPU_Null_fp_context.mxcsr
		);
	}


	for (i=0; i<sizeof(msgs)/sizeof(msgs[0]); i++ ) {
		if ( ( sse_tests & (1<<i) ) ) {
			if ( sse_test_debug )
				printk("HP task will now block for %s\n",msgs[i]);

			/* Blocking here lets the low-priority task continue */
			sc = rtems_semaphore_obtain(sync, RTEMS_WAIT, 500);

			all_clobber(0xaffeaffe, 0xcafecafe);

			if ( RTEMS_SUCCESSFUL != sc ) {
				rtems_error(sc,"ERROR: sse_test hp task wasn't notified of exception\n");
				goto bail;
			}

			/* set flag indicating that we executed until here */
			sse_test_check = 0;
		}
	}

bail:
	rtems_task_suspend(RTEMS_SELF);
}

/* Flags to skip individual tests */
#define SSE_TEST_FPU_EXC  (1<<0)
#define SSE_TEST_SSE_EXC  (1<<1)
#define SSE_TEST_IRQ_EXC  (1<<2)

#define SSE_TEST_ALL      7

/* If this flag is given the executing task is not deleted
 * when the test finishes. This is useful if you want to
 * execute from a shell or similar.
 */
#define SSE_TEST_NO_DEL    (1<<0)

/* Task arg is bitmask of these flags */
rtems_task
sse_test_lp_task(rtems_task_argument arg)
{
rtems_id                hp_task = 0;
rtems_status_code       sc;
rtems_task_priority     pri;
uint16_t                fp_cw,fp_cw_set;
uint32_t                mxcsr, mxcsr_set;
rtems_irq_connect_data  irqd;
int                     flags = (int)arg;
int                     st;
int                     errs = 0;

	sse_tests = SSE_TEST_ALL & ~(flags>>1);

	sse_test_ohdl = 0;

	fp_cw_set = _CPU_Null_fp_context.fpucw | FPCW_RC(3) ;
	mxcsr_set = _CPU_Null_fp_context.mxcsr | MXCSR_RC(3) ;
	asm volatile("ldmxcsr %0"::"m"(mxcsr_set));
	asm volatile("fldcw   %0"::"m"(fp_cw_set));

	sc = rtems_semaphore_create(
			rtems_build_name('s','s','e','S'),
			0,
			RTEMS_SIMPLE_BINARY_SEMAPHORE,
			0,
			&sse_test_sync
		);
	if ( RTEMS_SUCCESSFUL != sc ) {
		rtems_error(sc, "sse_test ERROR: creation of 'sync' semaphore failed");
		errs++;
		goto bail;
	}

	rtems_task_set_priority( RTEMS_SELF, RTEMS_CURRENT_PRIORITY, &pri );

	sc = rtems_task_create(
			rtems_build_name('s','s','e','H'),
			pri - 2,
			20000,
			RTEMS_DEFAULT_MODES,
			RTEMS_FLOATING_POINT,
			&hp_task
		);
	if ( RTEMS_SUCCESSFUL != sc ) {
		hp_task = 0;
		rtems_error( sc, "sse_test ERROR: creation of high-priority task failed");
		errs++;
		goto bail;
	}

	sc = rtems_task_start( hp_task, sse_test_hp_task, (rtems_task_argument)sse_test_sync );
	if ( RTEMS_SUCCESSFUL != sc ) {
		rtems_error( sc, "sse_test ERROR: start of high-priority task failed");
		goto bail;
	}

	/* Test if FP/SSE context is saved/restored across an exception */
	sse_test_ohdl      = _currentExcHandler;
	_currentExcHandler = sse_test_ehdl;

	if ( (sse_tests & SSE_TEST_FPU_EXC) ) {
		if ( (st = exc_raise(FP_EXC)) ) {
			prstat(st,"FP_EXC");
			errs++;
		}

		/* Test modified FPCW/MXCSR */
		asm volatile("fstcw   %0":"=m"(fp_cw));
		asm volatile("stmxcsr %0":"=m"(mxcsr));
		mxcsr &= ~(MXCSR_ALLE);
		if ( fp_cw != fp_cw_set ) {
			fprintf(stderr,"sse_test ERROR: FPCW mismatch (after FP_EXC): expected 0x%04"PRIx16", got 0x%04"PRIx16"\n", fp_cw_set, fp_cw);
			errs++;
		}
		if ( mxcsr != mxcsr_set ) {
			fprintf(stderr,"sse_test ERROR: MXCSR mismatch (after FP_EXC): expected 0x%08"PRIx32", got 0x%08"PRIx32"\n", mxcsr_set, mxcsr);
			errs++;
		}
	}

	if ( (sse_tests & SSE_TEST_SSE_EXC) ) {
		if ( (st = exc_raise(SSE_EXC)) ) {
			prstat(st, "SSE_EXC");
			errs++;
		}

		/* Test modified FPCW/MXCSR */
		asm volatile("fstcw   %0":"=m"(fp_cw));
		asm volatile("stmxcsr %0":"=m"(mxcsr));
		mxcsr &= ~(MXCSR_ALLE);
		if ( fp_cw != fp_cw_set ) {
			fprintf(stderr,"sse_test ERROR: FPCW mismatch (after SSE_EXC): expected 0x%04"PRIx16", got 0x%04"PRIx16"\n", fp_cw_set, fp_cw);
			errs++;
		}
		if ( mxcsr != mxcsr_set ) {
			fprintf(stderr,"sse_test ERROR: MXCSR mismatch (after SSE_EXC): expected 0x%08"PRIx32", got 0x%08"PRIx32"\n", mxcsr_set, mxcsr);
			errs++;
		}
	}


	if ( (sse_tests & SSE_TEST_IRQ_EXC) ) {
		memset( &irqd, 0, sizeof(irqd) );
		irqd.name   = SSE_TEST_IRQ;
		irqd.hdl    = (void*)sse_test_ehdl;
		irqd.handle = 0;

		if ( ! BSP_install_rtems_irq_handler( &irqd ) ) {
			fprintf(stderr, "sse_test ERROR: Unable to install ISR\n");
			errs++;
			goto bail;
		}

		/* Test if FP/SSE context is saved/restored across an interrupt */
		if ( (st = exc_raise(IRQ_EXC)) ) {
			prstat(st, "IRQ");
			errs++;
		}

		if ( ! BSP_remove_rtems_irq_handler( &irqd ) ) {
			fprintf(stderr, "sse_test ERROR: Unable to uninstall ISR\n");
		}

		/* Test modified FPCW/MXCSR */
		asm volatile("fstcw   %0":"=m"(fp_cw));
		asm volatile("stmxcsr %0":"=m"(mxcsr));
		mxcsr &= ~(MXCSR_ALLE);
		if ( fp_cw != fp_cw_set ) {
			fprintf(stderr,"sse_test ERROR: FPCW mismatch (after IRQ): expected 0x%04"PRIx16", got 0x%04"PRIx16"\n", fp_cw_set, fp_cw);
			errs++;
		}
		if ( mxcsr != mxcsr_set ) {
			fprintf(stderr,"sse_test ERROR: MXCSR mismatch (after IRQ): expected 0x%08"PRIx32", got 0x%08"PRIx32"\n", mxcsr_set, mxcsr);
			errs++;
		}
	}


bail:
	/* Wait for console to calm down... */
	rtems_task_wake_after(5);
	fprintf(stderr,"SSE/FPU Test %s (%u errors)\n", errs ? "FAILED":"PASSED", errs);
	if ( sse_test_ohdl ) {
		_currentExcHandler = sse_test_ohdl;
		sse_test_ohdl      = 0;
	}
	if ( sse_test_sync )
		rtems_semaphore_delete( sse_test_sync );
	sse_test_sync = 0;
	if ( hp_task )
		rtems_task_delete( hp_task );

	if ( ! (flags & SSE_TEST_NO_DEL) )
		rtems_task_exit();
}

static void
sse_test_ehdl(CPU_Exception_frame *p_f)
{
int i,j,start = 0;
int mismatch;
__vf    f4;

	if ( p_f ) {
		printk("Got exception #%u\n",        p_f->idtIndex);
		printk("EIP: 0x%08x, ESP: 0x%08x\n", p_f->eip, p_f->esp0);
		printk("TID: 0x%08x\n",              _Thread_Executing->Object.id);

		if ( ! p_f->fp_ctxt ) {
			printk("ERROR: NO FP/SSE CONTEXT ATTACHED ??\n");
			sse_test_ohdl(p_f);
		}
		if ( 16 == p_f->idtIndex ) {
			printk("Resetting FP status (0x%04"PRIx16")\n", p_f->fp_ctxt->fsw);
			p_f->fp_ctxt->fsw = 0;
		} else if ( 19 == p_f->idtIndex ) {
			start = 1;
			memcpy(&f4, p_f->fp_ctxt->xmmregs[0], sizeof(f4));
			f4 = -f4;
			memcpy(p_f->fp_ctxt->xmmregs[0], &f4, sizeof(f4));
			p_f->fp_ctxt->mxcsr &= ~MXCSR_ALLE;
		} else {
			printk("(skipping non-FP exception)\n");
			sse_test_ohdl(p_f);
		}

		printk("Checking XMM regs -- ");
		for ( mismatch=0, i=start; i<8; i++ ) {
			for ( j=0; j<16; j++ ) {
				if ( p_f->fp_ctxt->xmmregs[i][j] != ((i<<4) | j) )
					mismatch++;
			}
		}
		if ( mismatch ) {
			printk("%u mismatches; dump:\n", mismatch);
			for ( i=0; i<8; i++ ) {
				for ( j=0; j<16; j++ ) {
					printk("0x%02x ", p_f->fp_ctxt->xmmregs[i][j]);
				}
				printk("\n");
			}
		} else {
			printk("OK\n");
		}
	} else {
		printk("IRQ %u\n", SSE_TEST_IRQ);
	}
	printk("Clobbering FPU/SSE state\n");
	asm volatile("finit");
	sse_clobber(0xdeadbeef);
	printk("Notifying task\n");
	rtems_semaphore_release( sse_test_sync );	
}

#else

/* Code using signals for testing under linux; unfortunately, 32-bit
 * linux seems to pass no SSE context info to the sigaction...
 */

#include <signal.h>
#include <ucontext.h>

#define MKCASE(X) case FPE_##X: msg="FPE_"#X; break;

#define CLRXMM(i) __asm__ volatile("pxor %%xmm"#i", %%xmm"#i:::"xmm"#i)

static void
fpe_act(int signum, siginfo_t *p_info, void *arg3)
{
ucontext_t *p_ctxt = arg3;
const char *msg    = "FPE_UNKNOWN";
uint16_t   *p_fst;

	if ( SIGFPE != signum ) {
		fprintf(stderr,"WARNING: fpe_act handles SIGFPE\n");
		return;
	}
	switch ( p_info->si_code ) {
		default:
			fprintf(stderr,"WARNING: fpe_act got unkown code %u\n", p_info->si_code);
			return;
		MKCASE(INTDIV);
		MKCASE(INTOVF);
		MKCASE(FLTDIV);
		MKCASE(FLTOVF);
		MKCASE(FLTUND);
		MKCASE(FLTRES);
		MKCASE(FLTINV);
		MKCASE(FLTSUB);
	}
	fprintf(stderr,"Got SIGFPE (%s) @%p\n", msg, p_info->si_addr);
#ifdef __linux__
	fprintf(stderr,"Resetting FP status 0x%02lx\n", p_ctxt->uc_mcontext.fpregs->sw);
	p_ctxt->uc_mcontext.fpregs->sw = 0;
#ifdef TEST_MISMATCH
	fp_st1((void*)&p_ctxt->uc_mcontext.fpregs->_st[3],2.345);
#endif
#endif

	/* Clear FPU; if context is properly saved/restored around exception
	 * then this shouldn't disturb the register contents of the interrupted
	 * task/process.
	 */
	asm volatile("finit");
	sse_clobber(0xdeadbeef);
}

static void
test(void)
{
Context_Control_sse ctxt;

	stor_ctxt(&ctxt);
	printf("FPCW: 0x%"PRIx16"\nFPSW: 0x%"PRIx16"\n", ctxt.fcw, ctxt.fsw);
	printf("FTAG: 0x%"PRIx8"\n",ctxt.ftw);
}

int
main(int argc, char **argv)
{
struct sigaction a1, a2;
uint32_t         mxcsr;

	memset(&a1, 0, sizeof(a1));

	a1.sa_sigaction = fpe_act;
	a1.sa_flags     = SA_SIGINFO;	

	if ( sigaction(SIGFPE, &a1, &a2) ) {
		perror("sigaction");
		return 1;
	}

	asm volatile("stmxcsr %0":"=m"(mxcsr));
	printf("MXCSR: 0x%08"PRIx32"\n", mxcsr);

	test();
	exc_raise(0);
	return 0;
}
#endif

/* Helpers to access CR4 and MXCSR */

uint32_t
mfcr4()
{
uint32_t rval;
	asm volatile("mov %%cr4, %0":"=r"(rval));
	return rval;
}

void
mtcr4(uint32_t rval)
{
	asm volatile("mov %0, %%cr4"::"r"(rval));
}

uint32_t
mfmxcsr()
{
uint32_t rval;
	asm volatile("stmxcsr %0":"=m"(rval));
	return rval;
}

void
mtmxcsr(uint32_t rval)
{
	asm volatile("ldmxcsr %0"::"m"(rval));
}


float
sseraise()
{
__vf f4={-2., -2., -2. -2.};
float f;
     f4 = __builtin_ia32_sqrtps( f4 );
	memcpy(&f,&f4,sizeof(f));
	return f;
}