LXR 6.1/​cpukit/​score/​cpu/​i386/​sse_test.c	Source navigation Diff markup Identifier search General search
	Version: 6.1 Revert   Change

File indexing completed on 2025-05-11 08:24:24

 /**
  *  @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;
 }

This page was automatically generated by the 2.3.7 LXR engine. The LXR team