Context Navigation

source: rtems/cpukit/score/cpu/i386/sse_test.c @ 199041aa

4.104.115

Last change on this file since 199041aa was 199041aa, checked in by Joel Sherrill <joel.sherrill@…>, on 03/27/10 at 15:01:47

2010-03-27 Joel Sherrill <joel.sherrill@…>

cpu.c, cpu_asm.S, sse_test.c: Add include of config.h

Property mode set to 100644

File size: 25.5 KB

Line
1	/* $Id$ */
2
3	/*
4	* Authorship
5	* ----------
6	* This software was created by
7	* Till Straumann <strauman@slac.stanford.edu>, 2009,
8	* Stanford Linear Accelerator Center, Stanford University.
9	*
10	* Acknowledgement of sponsorship
11	* ------------------------------
12	* This software was produced by
13	* the Stanford Linear Accelerator Center, Stanford University,
14	* under Contract DE-AC03-76SFO0515 with the Department of Energy.
15	*
16	* Government disclaimer of liability
17	* ----------------------------------
18	* Neither the United States nor the United States Department of Energy,
19	* nor any of their employees, makes any warranty, express or implied, or
20	* assumes any legal liability or responsibility for the accuracy,
21	* completeness, or usefulness of any data, apparatus, product, or process
22	* disclosed, or represents that its use would not infringe privately owned
23	* rights.
24	*
25	* Stanford disclaimer of liability
26	* --------------------------------
27	* Stanford University makes no representations or warranties, express or
28	* implied, nor assumes any liability for the use of this software.
29	*
30	* Stanford disclaimer of copyright
31	* --------------------------------
32	* Stanford University, owner of the copyright, hereby disclaims its
33	* copyright and all other rights in this software. Hence, anyone may
34	* freely use it for any purpose without restriction.
35	*
36	* Maintenance of notices
37	* ----------------------
38	* In the interest of clarity regarding the origin and status of this
39	* SLAC software, this and all the preceding Stanford University notices
40	* are to remain affixed to any copy or derivative of this software made
41	* or distributed by the recipient and are to be affixed to any copy of
42	* software made or distributed by the recipient that contains a copy or
43	* derivative of this software.
44	*
45	* ------------------ SLAC Software Notices, Set 4 OTT.002a, 2004 FEB 03
46	*/
47
48
49	/* Code for testing FPU/SSE context save/restore across exceptions
50	* (including interrupts).
51	*
52	* There are two tasks and an IRQ/EXC handler involved. One task (LP)
53	* is of lower priority than the other (HP) task.
54	*
55	* 1) LP task sets up a context area in memory (known contents; every
56	* register is loaded with different values)
57	*
58	* 2) LP task
59	* 2a saves original FP/SSE context
60	* 2b loads context from 1) into FPU/SSE
61	* 2c raises an exception or interrupt
62	*
63	* * (2d save FPU/SSE context after irq/exception returns to
64	* separate area for verification
65	* 2e reload original FP/SSE context.)
66	*
67	* * All these five steps are coded in assembly to prevent
68	* gcc from manipulating the FP/SSE state. The last two steps,
69	* however, are effectively executed during 6 when control is
70	* returned to the LP task.
71	*
72	* 3) IRQ/EXC handler OS wrapper saves context, initializes FPU and
73	* MXCSR.
74	*
75	* 4) user (our) irq/exc handler clears exception condition, clobbers
76	* FPU and XMM regs and finally releases a semaphore on which HP
77	* task is waiting.
78	*
79	* 5) context switch to HP task. HP task clobbers FPU and XMM regs.
80	* Then it tries to re-acquire the synchronization semaphore and
81	* blocks.
82	*
83	* 6) task switch back to (interrupted) LP task. Original context is
84	* restored and verified against the context that was setup in 1).
85	*
86	*
87	* Three methods for interrupting the LP task are tested
88	*
89	* a) FP exception (by setting an exception status in the context from 1)
90	* b) SSE exception (by computing the square root of a vector of negative
91	* numbers.
92	* c) IRQ (software IRQ via 'INT xx' instruction)
93	*
94	*/
95
96	#ifdef HAVE_CONFIG_H
97	#include "config.h"
98	#endif
99
100	#ifdef __rtems__
101	#include <rtems.h>
102	#include <rtems/score/cpu.h>
103	#include <rtems/irq.h>
104	#include <rtems/error.h>
105	#endif
106
107	#include <inttypes.h>
108	#include <stdio.h>
109	#include <stdlib.h>
110	#include <string.h>
111	#include <math.h>
112
113	/* This is currently hardcoded (int xx opcode requires immediate operand) */
114	#define SSE_TEST_IRQ 10
115
116	typedef uint8_t __v8 __attribute__((vector_size(16)));
117	typedef uint32_t __v32 __attribute__((vector_size(16)));
118	typedef float __vf __attribute__((vector_size(16)));
119
120	#ifndef __rtems__
121	/* Clone of what is defined in rtems/score/cpu.h (for testing under linux) */
122	typedef struct Context_Control_sse {
123	uint16_t fcw;
124	uint16_t fsw;
125	uint8_t ftw;
126	uint8_t res_1;
127	uint16_t fop;
128	uint32_t fpu_ip;
129	uint16_t cs;
130	uint16_t res_2;
131	uint32_t fpu_dp;
132	uint16_t ds;
133	uint16_t res_3;
134	uint32_t mxcsr;
135	uint32_t mxcsr_mask;
136	struct {
137	uint8_t fpreg[10];
138	uint8_t res_4[ 6];
139	} fp_mmregs[8];
140	uint8_t xmmregs[8][16];
141	uint8_t res_5[224];
142	} Context_Control_sse
143	__attribute__((aligned(16)))
144	;
145	#endif
146
147	#define MXCSR_FZ (1<<15) /* Flush to zero */
148	#define MXCSR_RC(x) (((x)&3)<<13) /* Rounding ctrl */
149	#define MXCSR_PM (1<<12) /* Precision msk */
150	#define MXCSR_UM (1<<11) /* Underflow msk */
151	#define MXCSR_OM (1<<10) /* Overflow msk */
152	#define MXCSR_ZM (1<< 9) /* Divbyzero msk */
153	#define MXCSR_DM (1<< 8) /* Denormal msk */
154	#define MXCSR_IM (1<< 7) /* Invalidop msk */
155	#define MXCSR_DAZ (1<< 6) /* Denorml are 0 */
156	#define MXCSR_PE (1<< 5) /* Precision flg */
157	#define MXCSR_UE (1<< 4) /* Underflow flg */
158	#define MXCSR_OE (1<< 3) /* Overflow flg */
159	#define MXCSR_ZE (1<< 2) /* Divbyzero flg */
160	#define MXCSR_DE (1<< 1) /* Denormal flg */
161	#define MXCSR_IE (1<< 0) /* Invalidop flg */
162
163	#define MXCSR_ALLM (MXCSR_PM \| MXCSR_UM \| MXCSR_OM \| MXCSR_ZM \| MXCSR_DM \| MXCSR_IM)
164	#define MXCSR_ALLE (MXCSR_PE \| MXCSR_UE \| MXCSR_OE \| MXCSR_ZE \| MXCSR_DE \| MXCSR_IE)
165
166	#define FPSR_B (1<<15) /* FPU busy */
167	#define FPSR_C3 (1<<14) /* Cond code C3 */
168	#define FPSR_TOP(x) (((x)&7)<<11) /* TOP */
169	#define FPSR_C2 (1<<10) /* Cond code C2 */
170	#define FPSR_C1 (1<< 9) /* Cond code C1 */
171	#define FPSR_C0 (1<< 8) /* Cond code C0 */
172	#define FPSR_ES (1<< 7) /* Error summary */
173	#define FPSR_SF (1<< 6) /* Stack fault */
174	#define FPSR_PE (1<< 5) /* Precision flg */
175	#define FPSR_UE (1<< 4) /* Underflow flg */
176	#define FPSR_OE (1<< 3) /* Overflow flg */
177	#define FPSR_ZE (1<< 2) /* Divbyzero flg */
178	#define FPSR_DE (1<< 1) /* Denormal flg */
179	#define FPSR_IE (1<< 0) /* Invalidop flg */
180
181	#define FPCW_X (1<<12) /* Infinity ctrl */
182	#define FPCW_RC(x) (((x)&3)<<10) /* Rounding ctrl */
183	#define FPCW_PC(x) (((x)&3)<< 8) /* Precision ctl */
184	#define FPCW_PM (1<< 5) /* Precision msk */
185	#define FPCW_UM (1<< 4) /* Underflow msk */
186	#define FPCW_OM (1<< 3) /* Overflow msk */
187	#define FPCW_ZM (1<< 2) /* Divbyzero msk */
188	#define FPCW_DM (1<< 1) /* Denormal msk */
189	#define FPCW_IM (1<< 0) /* Invalidop msk */
190
191	#define FPCW_ALLM (FPCW_PM \| FPCW_UM \| FPCW_OM \| FPCW_ZM \| FPCW_DM \| FPCW_IM)
192	#define FPSR_ALLE (FPSR_ES \| FPSR_SF \| FPSR_PE \| FPSR_UE \| FPSR_OE \| FPSR_ZE \| FPSR_DE \| FPSR_IE)
193
194	/* Store 'double' into 80-bit register image */
195	void
196	fp_st1(uint8_t (*p_dst)[10], double v)
197	{
198	asm volatile("fstpt %0":"=m"(*p_dst):"t"(v):"st");
199	}
200
201	/* Store 'double' into 80-bit register image #i in context */
202	void
203	fp_st(Context_Control_sse *p_ctxt, int i, double v)
204	{
205	fp_st1(&p_ctxt->fp_mmregs[i].fpreg,v);
206	}
207
208	/* Load 'double' from 80-bit register image */
209	double
210	fp_ld1(uint8_t (*p_src)[10])
211	{
212	double v;
213
214	asm volatile("fldt %1":"=t"(v):"m"((p_src)[0]),"m"(p_src));
215	return v;
216	}
217
218	/* Load 'double' from 80-bit register image #i in context */
219	double
220	fp_ld(Context_Control_sse *p_ctxt, int i)
221	{
222	return fp_ld1(&p_ctxt->fp_mmregs[i].fpreg);
223	}
224
225	#define FPUCLOBBER \
226	"st","st(1)","st(2)","st(3)", \
227	"st(4)","st(5)","st(6)","st(7)",\
228	"fpsr","fpcr"
229
230	/* There seems to be no way to say that mxcsr was clobbered */
231
232	#define SSECLOBBER \
233	"xmm0","xmm1","xmm2","xmm3", \
234	"xmm4","xmm5","xmm6","xmm7"
235
236	static void
237	sse_clobber(uint32_t x)
238	{
239	__v32 v = { x, x, x, x };
240	asm volatile (
241	" movdqa %0, %%xmm0 \n"
242	" movdqa %%xmm0, %%xmm1 \n"
243	" movdqa %%xmm0, %%xmm2 \n"
244	" movdqa %%xmm0, %%xmm3 \n"
245	" movdqa %%xmm0, %%xmm4 \n"
246	" movdqa %%xmm0, %%xmm5 \n"
247	" movdqa %%xmm0, %%xmm6 \n"
248	" movdqa %%xmm0, %%xmm7 \n"
249	:
250	:"m"(v)
251	:SSECLOBBER
252	);
253	}
254
255	void
256	all_clobber(uint32_t v1, uint32_t v2);
257
258	asm(
259	"all_clobber: \n"
260	" finit \n"
261	" movq 0(%esp), %xmm0 \n"
262	" punpcklqdq %xmm0, %xmm0 \n"
263	" movdqa %xmm0, %xmm1 \n"
264	" movdqa %xmm0, %xmm2 \n"
265	" movdqa %xmm0, %xmm3 \n"
266	" movdqa %xmm0, %xmm4 \n"
267	" movdqa %xmm0, %xmm5 \n"
268	" movdqa %xmm0, %xmm6 \n"
269	" movdqa %xmm0, %xmm7 \n"
270	" ret \n"
271	);
272
273	/* Clear FPU and save FPU/SSE registers to context area */
274
275	void
276	init_ctxt(Context_Control_sse *p_ctxt);
277
278	asm(
279	"init_ctxt: \n"
280	" finit \n"
281	" mov 4(%esp), %eax\n"
282	" fxsave (%eax) \n"
283	" fwait \n"
284	" ret \n"
285	);
286
287	/* Save FPU/SSE registers to context area */
288
289	static void
290	stor_ctxt(Context_Control_sse *p_ctxt)
291	{
292	memset(p_ctxt, 0, sizeof(*p_ctxt));
293	asm volatile(
294	/* " finit \n" */
295	" fxsave %0 \n"
296	" fwait \n"
297	: "=m"(*p_ctxt)
298	:
299	: FPUCLOBBER
300	);
301	}
302
303	#define H08 "0x%02"PRIx8
304	#define H16 "0x%04"PRIx16
305	#define H32 "0x%08"PRIx32
306
307	#define F16 "mismatch ("H16" != "H16")\n"
308
309	#define FLDCMP(fld, fmt) \
310	if ( a->fld != b->fld ) { \
311	rval = 1; \
312	if ( !quiet ) \
313	fprintf(stderr,#fld" mismatch ("fmt" != "fmt")\n",a->fld, b->fld); \
314	}
315
316	#define FLTCMP(i) \
317	do { \
318	if ( ( (a->ftw ^ b->ftw) & (1<<i)) \
319	\|\| ( (a->ftw & b->ftw & (1<<i)) && \
320	memcmp(a->fp_mmregs[i].fpreg, \
321	b->fp_mmregs[i].fpreg, \
322	sizeof(a->fp_mmregs[i].fpreg)) \
323	) \
324	) { \
325	rval = 1; \
326	if ( !quiet ) { \
327	double fa = fp_ld(a, i); \
328	double fb = fp_ld(b, i); \
329	if ( ((a->ftw ^ b->ftw) & (1<<i)) ) \
330	fprintf(stderr,"fpreg[%u] TAG mismatch (%u != %u)\n",i,(a->ftw & (1<<i)) ? 1 : 0,(b->ftw & (1<<i)) ? 1 : 0); \
331	else \
332	fprintf(stderr,"fpreg[%u] mismatch (%g != %g)\n",i,fa,fb); \
333	} \
334	} \
335	} while (0)
336
337	#define XMMCMP(i) \
338	do { \
339	if ( memcmp(&a->xmmregs[i], \
340	&b->xmmregs[i], \
341	sizeof(a->xmmregs[i])) \
342	) { \
343	rval = 1; \
344	if ( !quiet ) { \
345	int _jj; \
346	fprintf(stderr,"xmmreg[%u] mismatch:\n", i); \
347	fprintf(stderr," "); \
348	for (_jj=0; _jj<16; _jj++) \
349	fprintf(stderr,"%02x ",a->xmmregs[i][_jj]); \
350	fprintf(stderr,"\n !=\n"); \
351	fprintf(stderr," "); \
352	for (_jj=0; _jj<16; _jj++) \
353	fprintf(stderr,"%02x ",b->xmmregs[i][_jj]); \
354	fprintf(stderr,"\n"); \
355	} \
356	} \
357	} while (0)
358
359
360	/* Compare two FPU/SSE context areas and flag differences;
361	* RETURNS: zero if the contexts match and nonzero otherwise
362	*/
363	static int
364	cmp_ctxt(Context_Control_sse a, Context_Control_sse b, int quiet)
365	{
366	int rval = 0;
367	int i;
368	FLDCMP(fcw,H16);
369	FLDCMP(fsw,H16);
370	FLDCMP(ftw,H08);
371	FLDCMP(fop,H16);
372	FLDCMP(fpu_ip,H32);
373	FLDCMP(cs,H16);
374	FLDCMP(fpu_dp,H32);
375	FLDCMP(ds,H16);
376	FLDCMP(mxcsr,H32);
377	FLDCMP(mxcsr_mask,H32);
378	for ( i=0; i<8; i++ ) {
379	FLTCMP(i);
380	}
381	for ( i=0; i<8; i++ ) {
382	XMMCMP(i);
383	}
384	return rval;
385	}
386
387	/* Possible arguments to exc_raise() */
388
389	#define FP_EXC 0
390	#define IRQ_EXC 1
391	#define SSE_EXC -1
392
393	/* Check stack alignment by raising the interrupt from a
394	* non-16-byte aligned section of code. The exception/IRQ
395	* handler must align the stack and SSE context area
396	* properly or it will crash.
397	*/
398	#define __INTRAISE(x) " int $32+"#x" \n"
399	#define INTRAISE(x) __INTRAISE(x)
400
401	asm(
402	"do_raise: \n"
403	" fwait \n"
404	" test %eax, %eax \n"
405	" je 2f \n"
406	" jl 1f \n"
407	INTRAISE(SSE_TEST_IRQ)
408	" jmp 2f \n"
409	"1: sqrtps %xmm0, %xmm0 \n"
410	"2: \n"
411	" ret \n"
412	);
413
414	#define SSE_TEST_HP_FAILED 1
415	#define SSE_TEST_FSPR_FAILED 2
416	#define SSE_TEST_CTXTCMP_FAILED 4
417
418	static const char *fail_msgs[] = {
419	"Seems that HP task was not executing",
420	"FPSR 'Invalid-operation' flag should be clear",
421	"Restored context does NOT match the saved one",
422	};
423
424	static void prstat(int st, const char *where)
425	{
426	int i,msk;
427	for ( i=0, msk=1; i<sizeof(fail_msgs)/sizeof(fail_msgs[0]); i++, msk<<=1 ) {
428	if ( (st & msk) ) {
429	fprintf(stderr,"sse_test ERROR: %s (testing: %s)\n", fail_msgs[i], where);
430	}
431	}
432	}
433
434	int sse_test_debug = 0;
435
436	static int
437	exc_raise(int kind)
438	{
439	Context_Control_sse nctxt;
440	Context_Control_sse octxt;
441	Context_Control_sse orig_ctxt;
442	int i,j,rval;
443	double s2;
444	uint16_t fsw;
445	__vf f4 = { -1., -2., -3., -4. };
446	__vf tmp;
447	__v32 sgn = { (1<<31), (1<<31), (1<<31), (1<<31) };
448
449	stor_ctxt(&octxt);
450
451	octxt.fsw &= ~FPSR_ALLE;
452	octxt.mxcsr &= ~MXCSR_ALLE;
453
454	for ( i=0; i<8; i++ ) {
455	fp_st(&octxt, i, (double)i+0.1);
456	for (j=0; j<16; j++) {
457	octxt.xmmregs[i][j]=(i<<4)+j;
458	}
459	}
460
461
462	if ( SSE_EXC == kind ) {
463	memcpy(octxt.xmmregs[0], &f4, sizeof(f4));
464	octxt.mxcsr &= ~MXCSR_IM;
465	}
466
467	/* set tags to 'valid' */
468	octxt.ftw = 0xff;
469
470	/* enable 'invalid arg' exception */
471	octxt.fcw &= ~ ( FPCW_IM );
472
473	if ( FP_EXC == kind ) {
474	octxt.fsw \|= ( FPSR_IE \| FPSR_ES );
475	}
476
477	if ( sse_test_debug )
478	printk("RAISE (fsw was 0x%04x)\n", orig_ctxt.fsw);
479	asm volatile(
480	" fxsave %2 \n"
481	#ifdef __rtems__
482	" movl %4, sse_test_check\n"
483	#endif
484	" fxrstor %3 \n"
485	" call do_raise \n"
486	#ifdef __rtems__
487	" movl sse_test_check, %1\n"
488	#else
489	" movl $0, %1 \n"
490	#endif
491	#ifdef TEST_MISMATCH
492	" pxor %%xmm0, %%xmm0 \n"
493	#endif
494	" fxsave %0 \n"
495	" fxrstor %2 \n"
496	: "=m"(nctxt),"=&r"(rval),"=m"(orig_ctxt)
497	: "m"(octxt), "i"(SSE_TEST_HP_FAILED),"a"(kind)
498	: "xmm0"
499	);
500
501	if ( ( FPSR_IE & nctxt.fsw ) ) {
502	rval \|= SSE_TEST_FSPR_FAILED;
503	}
504	if ( FP_EXC == kind )
505	nctxt.fsw \|= (FPSR_IE \| FPSR_ES);
506	else if ( SSE_EXC == kind ) {
507	tmp = __builtin_ia32_sqrtps( (__vf)(~sgn & (__v32)f4) );
508	/* sqrt raises PE; just clear it */
509	nctxt.mxcsr &= ~MXCSR_PE;
510	memcpy( octxt.xmmregs[0], &tmp, sizeof(tmp) );
511	}
512
513	if ( cmp_ctxt(&nctxt, &octxt, 0) ) {
514	rval \|= SSE_TEST_CTXTCMP_FAILED;
515	}
516
517	s2 = sqrt(2.0);
518
519	asm volatile("fstsw %0":"=m"(fsw));
520
521	if ( sse_test_debug )
522	printf("sqrt(2): %f (FSTW: 0x%02"PRIx16")\n", sqrt(2.0), fsw);
523
524	return rval;
525	}
526
527	#ifdef __rtems__
528	static void
529	sse_test_ehdl(CPU_Exception_frame *p_f);
530
531	rtems_id sse_test_sync = 0;
532	cpuExcHandlerType sse_test_ohdl = 0;
533
534	CPU_Exception_frame *sse_test_frame = 0;
535	volatile int sse_test_check = SSE_TEST_HP_FAILED;
536	unsigned sse_tests = 0;
537
538	rtems_task
539	sse_test_hp_task(rtems_task_argument arg)
540	{
541	rtems_id sync = (rtems_id)arg;
542
543	uint16_t fp_cw;
544	uint32_t mxcsr;
545	rtems_status_code sc;
546	const char * msgs[] = {"FPU_EXC", "SSE_EXC", "IRQ_EXC"};
547	int i;
548
549	/* verify that FPU control word is default value */
550	asm volatile("fstcw %0":"=m"(fp_cw));
551	if ( fp_cw != _CPU_Null_fp_context.fpucw ) {
552	fprintf(
553	stderr,
554	"ERROR: FPU CW initialization mismatch: got 0x%04"PRIx16"; expected 0x%04"PRIx16"\n",
555	fp_cw,
556	_CPU_Null_fp_context.fpucw
557	);
558	}
559
560	/* check MXCSR default value */
561	asm volatile("stmxcsr %0":"=m"(mxcsr));
562	if ( mxcsr != _CPU_Null_fp_context.mxcsr ) {
563	fprintf(
564	stderr,
565	"ERROR: MXCSR initialization mismatch: got 0x%08"PRIx32"; expected 0x%08"PRIx32"\n",
566	mxcsr,
567	_CPU_Null_fp_context.mxcsr
568	);
569	}
570
571
572	for (i=0; i<sizeof(msgs)/sizeof(msgs[0]); i++ ) {
573	if ( ( sse_tests & (1<<i) ) ) {
574	if ( sse_test_debug )
575	printk("HP task will now block for %s\n",msgs[i]);
576
577	/* Blocking here lets the low-priority task continue */
578	sc = rtems_semaphore_obtain(sync, RTEMS_WAIT, 500);
579
580	all_clobber(0xaffeaffe, 0xcafecafe);
581
582	if ( RTEMS_SUCCESSFUL != sc ) {
583	rtems_error(sc,"ERROR: sse_test hp task wasn't notified of exception\n");
584	goto bail;
585	}
586
587	/* set flag indicating that we executed until here */
588	sse_test_check = 0;
589	}
590	}
591
592	bail:
593	rtems_task_suspend(RTEMS_SELF);
594	}
595
596	/* Flags to skip individual tests */
597	#define SSE_TEST_FPU_EXC (1<<0)
598	#define SSE_TEST_SSE_EXC (1<<1)
599	#define SSE_TEST_IRQ_EXC (1<<2)
600
601	#define SSE_TEST_ALL 7
602
603	/* If this flag is given the executing task is not deleted
604	* when the test finishes. This is useful if you want to
605	* execute from a shell or similar.
606	*/
607	#define SSE_TEST_NO_DEL (1<<0)
608
609	/* Task arg is bitmask of these flags */
610	rtems_task
611	sse_test_lp_task(rtems_task_argument arg)
612	{
613	rtems_id hp_task = 0;
614	rtems_status_code sc;
615	rtems_task_priority pri;
616	uint16_t fp_cw,fp_cw_set;
617	uint32_t mxcsr, mxcsr_set;
618	rtems_irq_connect_data irqd;
619	int flags = (int)arg;
620	int st;
621	int errs = 0;
622
623	sse_tests = SSE_TEST_ALL & ~(flags>>1);
624
625	sse_test_ohdl = 0;
626
627	fp_cw_set = _CPU_Null_fp_context.fpucw \| FPCW_RC(3) ;
628	mxcsr_set = _CPU_Null_fp_context.mxcsr \| MXCSR_RC(3) ;
629	asm volatile("ldmxcsr %0"::"m"(mxcsr_set));
630	asm volatile("fldcw %0"::"m"(fp_cw_set));
631
632	sc = rtems_semaphore_create(
633	rtems_build_name('s','s','e','S'),
634	0,
635	RTEMS_SIMPLE_BINARY_SEMAPHORE,
636	0,
637	&sse_test_sync
638	);
639	if ( RTEMS_SUCCESSFUL != sc ) {
640	rtems_error(sc, "sse_test ERROR: creation of 'sync' semaphore failed");
641	errs++;
642	goto bail;
643	}
644
645	rtems_task_set_priority( RTEMS_SELF, RTEMS_CURRENT_PRIORITY, &pri );
646
647	sc = rtems_task_create(
648	rtems_build_name('s','s','e','H'),
649	pri - 2,
650	20000,
651	RTEMS_DEFAULT_MODES,
652	RTEMS_FLOATING_POINT,
653	&hp_task
654	);
655	if ( RTEMS_SUCCESSFUL != sc ) {
656	hp_task = 0;
657	rtems_error( sc, "sse_test ERROR: creation of high-priority task failed");
658	errs++;
659	goto bail;
660	}
661
662	sc = rtems_task_start( hp_task, sse_test_hp_task, (rtems_task_argument)sse_test_sync );
663	if ( RTEMS_SUCCESSFUL != sc ) {
664	rtems_error( sc, "sse_test ERROR: start of high-priority task failed");
665	goto bail;
666	}
667
668	/* Test if FP/SSE context is saved/restored across an exception */
669	sse_test_ohdl = _currentExcHandler;
670	_currentExcHandler = sse_test_ehdl;
671
672	if ( (sse_tests & SSE_TEST_FPU_EXC) ) {
673	if ( (st = exc_raise(FP_EXC)) ) {
674	prstat(st,"FP_EXC");
675	errs++;
676	}
677
678	/* Test modified FPCW/MXCSR */
679	asm volatile("fstcw %0":"=m"(fp_cw));
680	asm volatile("stmxcsr %0":"=m"(mxcsr));
681	mxcsr &= ~(MXCSR_ALLE);
682	if ( fp_cw != fp_cw_set ) {
683	fprintf(stderr,"sse_test ERROR: FPCW mismatch (after FP_EXC): expected 0x%04"PRIx16", got 0x%04"PRIx16"\n", fp_cw_set, fp_cw);
684	errs++;
685	}
686	if ( mxcsr != mxcsr_set ) {
687	fprintf(stderr,"sse_test ERROR: MXCSR mismatch (after FP_EXC): expected 0x%08"PRIx32", got 0x%08"PRIx32"\n", mxcsr_set, mxcsr);
688	errs++;
689	}
690	}
691
692	if ( (sse_tests & SSE_TEST_SSE_EXC) ) {
693	if ( (st = exc_raise(SSE_EXC)) ) {
694	prstat(st, "SSE_EXC");
695	errs++;
696	}
697
698	/* Test modified FPCW/MXCSR */
699	asm volatile("fstcw %0":"=m"(fp_cw));
700	asm volatile("stmxcsr %0":"=m"(mxcsr));
701	mxcsr &= ~(MXCSR_ALLE);
702	if ( fp_cw != fp_cw_set ) {
703	fprintf(stderr,"sse_test ERROR: FPCW mismatch (after SSE_EXC): expected 0x%04"PRIx16", got 0x%04"PRIx16"\n", fp_cw_set, fp_cw);
704	errs++;
705	}
706	if ( mxcsr != mxcsr_set ) {
707	fprintf(stderr,"sse_test ERROR: MXCSR mismatch (after SSE_EXC): expected 0x%08"PRIx32", got 0x%08"PRIx32"\n", mxcsr_set, mxcsr);
708	errs++;
709	}
710	}
711
712
713	if ( (sse_tests & SSE_TEST_IRQ_EXC) ) {
714	memset( &irqd, 0, sizeof(irqd) );
715	irqd.name = SSE_TEST_IRQ;
716	irqd.hdl = (void*)sse_test_ehdl;
717	irqd.handle = 0;
718
719	if ( ! BSP_install_rtems_irq_handler( &irqd ) ) {
720	fprintf(stderr, "sse_test ERROR: Unable to install ISR\n");
721	errs++;
722	goto bail;
723	}
724
725	/* Test if FP/SSE context is saved/restored across an interrupt */
726	if ( (st = exc_raise(IRQ_EXC)) ) {
727	prstat(st, "IRQ");
728	errs++;
729	}
730
731	if ( ! BSP_remove_rtems_irq_handler( &irqd ) ) {
732	fprintf(stderr, "sse_test ERROR: Unable to uninstall ISR\n");
733	}
734
735	/* Test modified FPCW/MXCSR */
736	asm volatile("fstcw %0":"=m"(fp_cw));
737	asm volatile("stmxcsr %0":"=m"(mxcsr));
738	mxcsr &= ~(MXCSR_ALLE);
739	if ( fp_cw != fp_cw_set ) {
740	fprintf(stderr,"sse_test ERROR: FPCW mismatch (after IRQ): expected 0x%04"PRIx16", got 0x%04"PRIx16"\n", fp_cw_set, fp_cw);
741	errs++;
742	}
743	if ( mxcsr != mxcsr_set ) {
744	fprintf(stderr,"sse_test ERROR: MXCSR mismatch (after IRQ): expected 0x%08"PRIx32", got 0x%08"PRIx32"\n", mxcsr_set, mxcsr);
745	errs++;
746	}
747	}
748
749
750	bail:
751	/* Wait for console to calm down... */
752	rtems_task_wake_after(5);
753	fprintf(stderr,"SSE/FPU Test %s (%u errors)\n", errs ? "FAILED":"PASSED", errs);
754	if ( sse_test_ohdl ) {
755	_currentExcHandler = sse_test_ohdl;
756	sse_test_ohdl = 0;
757	}
758	if ( sse_test_sync )
759	rtems_semaphore_delete( sse_test_sync );
760	sse_test_sync = 0;
761	if ( hp_task )
762	rtems_task_delete( hp_task );
763
764	if ( ! (flags & SSE_TEST_NO_DEL) )
765	rtems_task_delete( RTEMS_SELF );
766	}
767
768	static void
769	sse_test_ehdl(CPU_Exception_frame *p_f)
770	{
771	int i,j,start = 0;
772	int mismatch;
773	__vf f4;
774
775	if ( p_f ) {
776	printk("Got exception #%u\n", p_f->idtIndex);
777	printk("EIP: 0x%08x, ESP: 0x%08x\n", p_f->eip, p_f->esp0);
778	printk("TID: 0x%08x\n", _Thread_Executing->Object.id);
779
780	if ( ! p_f->fp_ctxt ) {
781	printk("ERROR: NO FP/SSE CONTEXT ATTACHED ??\n");
782	sse_test_ohdl(p_f);
783	}
784	if ( 16 == p_f->idtIndex ) {
785	printk("Resetting FP status (0x%04"PRIx16")\n", p_f->fp_ctxt->fsw);
786	p_f->fp_ctxt->fsw = 0;
787	} else if ( 19 == p_f->idtIndex ) {
788	start = 1;
789	memcpy(&f4, p_f->fp_ctxt->xmmregs[0], sizeof(f4));
790	f4 = -f4;
791	memcpy(p_f->fp_ctxt->xmmregs[0], &f4, sizeof(f4));
792	p_f->fp_ctxt->mxcsr &= ~MXCSR_ALLE;
793	} else {
794	printk("(skipping non-FP exception)\n");
795	sse_test_ohdl(p_f);
796	}
797
798	printk("Checking XMM regs -- ");
799	for ( mismatch=0, i=start; i<8; i++ ) {
800	for ( j=0; j<16; j++ ) {
801	if ( p_f->fp_ctxt->xmmregs[i][j] != ((i<<4) \| j) )
802	mismatch++;
803	}
804	}
805	if ( mismatch ) {
806	printk("%u mismatches; dump:\n", mismatch);
807	for ( i=0; i<8; i++ ) {
808	for ( j=0; j<16; j++ ) {
809	printk("0x%02x ", p_f->fp_ctxt->xmmregs[i][j]);
810	}
811	printk("\n");
812	}
813	} else {
814	printk("OK\n");
815	}
816	} else {
817	printk("IRQ %u\n", SSE_TEST_IRQ);
818	}
819	printk("Clobbering FPU/SSE state\n");
820	asm volatile("finit");
821	sse_clobber(0xdeadbeef);
822	printk("Notifying task\n");
823	rtems_semaphore_release( sse_test_sync );
824	}
825
826	#else
827
828	/* Code using signals for testing under linux; unfortunately, 32-bit
829	* linux seems to pass no SSE context info to the sigaction...
830	*/
831
832	#include <signal.h>
833	#include <ucontext.h>
834
835	#define MKCASE(X) case FPE_##X: msg="FPE_"#X; break;
836
837	#define CLRXMM(i) asm volatile("pxor %%xmm"#i", %%xmm"#i:::"xmm"#i)
838
839	static void
840	fpe_act(int signum, siginfo_t p_info, void arg3)
841	{
842	ucontext_t *p_ctxt = arg3;
843	const char *msg = "FPE_UNKNOWN";
844	uint16_t *p_fst;
845
846	if ( SIGFPE != signum ) {
847	fprintf(stderr,"WARNING: fpe_act handles SIGFPE\n");
848	return;
849	}
850	switch ( p_info->si_code ) {
851	default:
852	fprintf(stderr,"WARNING: fpe_act got unkown code %u\n", p_info->si_code);
853	return;
854	MKCASE(INTDIV);
855	MKCASE(INTOVF);
856	MKCASE(FLTDIV);
857	MKCASE(FLTOVF);
858	MKCASE(FLTUND);
859	MKCASE(FLTRES);
860	MKCASE(FLTINV);
861	MKCASE(FLTSUB);
862	}
863	fprintf(stderr,"Got SIGFPE (%s) @%p\n", msg, p_info->si_addr);
864	#ifdef __linux__
865	fprintf(stderr,"Resetting FP status 0x%02lx\n", p_ctxt->uc_mcontext.fpregs->sw);
866	p_ctxt->uc_mcontext.fpregs->sw = 0;
867	#ifdef TEST_MISMATCH
868	fp_st1((void*)&p_ctxt->uc_mcontext.fpregs->_st[3],2.345);
869	#endif
870	#endif
871
872	/* Clear FPU; if context is properly saved/restored around exception
873	* then this shouldn't disturb the register contents of the interrupted
874	* task/process.
875	*/
876	asm volatile("finit");
877	sse_clobber(0xdeadbeef);
878	}
879
880	static void
881	test(void)
882	{
883	Context_Control_sse ctxt;
884
885	stor_ctxt(&ctxt);
886	printf("FPCW: 0x%"PRIx16"\nFPSW: 0x%"PRIx16"\n", ctxt.fcw, ctxt.fsw);
887	printf("FTAG: 0x%"PRIx8"\n",ctxt.ftw);
888	}
889
890	int
891	main(int argc, char **argv)
892	{
893	struct sigaction a1, a2;
894	uint32_t mxcsr;
895
896	memset(&a1, 0, sizeof(a1));
897
898	a1.sa_sigaction = fpe_act;
899	a1.sa_flags = SA_SIGINFO;
900
901	if ( sigaction(SIGFPE, &a1, &a2) ) {
902	perror("sigaction");
903	return 1;
904	}
905
906	asm volatile("stmxcsr %0":"=m"(mxcsr));
907	printf("MXCSR: 0x%08"PRIx32"\n", mxcsr);
908
909	test();
910	exc_raise(0);
911	return 0;
912	}
913	#endif
914
915	/* Helpers to access CR4 and MXCSR */
916
917	uint32_t
918	mfcr4()
919	{
920	uint32_t rval;
921	asm volatile("mov %%cr4, %0":"=r"(rval));
922	return rval;
923	}
924
925	void
926	mtcr4(uint32_t rval)
927	{
928	asm volatile("mov %0, %%cr4"::"r"(rval));
929	}
930
931	uint32_t
932	mfmxcsr()
933	{
934	uint32_t rval;
935	asm volatile("stmxcsr %0":"=m"(rval));
936	return rval;
937	}
938
939	void
940	mtmxcsr(uint32_t rval)
941	{
942	asm volatile("ldmxcsr %0"::"m"(rval));
943	}
944
945
946	float
947	sseraise()
948	{
949	__vf f4={-2., -2., -2. -2.};
950	float f;
951	f4 = __builtin_ia32_sqrtps( f4 );
952	memcpy(&f,&f4,sizeof(f));
953	return f;
954	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: