1/*-
2 * Copyright (c) 2004-2005 David Schultz <das@FreeBSD.ORG>
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * $FreeBSD$
27 */
28
29#include <sys/cdefs.h>
30#include <sys/types.h>
31#include <machine/npx.h>
32
33#define	__fenv_static
34#include "fenv.h"
35
36#ifdef __GNUC_GNU_INLINE__
37#error "This file must be compiled with C99 'inline' semantics"
38#endif
39
40const fenv_t __fe_dfl_env = {
41	__INITIAL_NPXCW__,
42	0x0000,
43	0x0000,
44	0x1f80,
45	0xffffffff,
46	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
47	  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff }
48};
49
50enum __sse_support __has_sse =
51#ifdef __SSE__
52	__SSE_YES;
53#else
54	__SSE_UNK;
55#endif
56
57#define	getfl(x)	__asm __volatile("pushfl\n\tpopl %0" : "=mr" (*(x)))
58#define	setfl(x)	__asm __volatile("pushl %0\n\tpopfl" : : "g" (x))
59#define	cpuid_dx(x)	__asm __volatile("pushl %%ebx\n\tmovl $1, %%eax\n\t"  \
60					 "cpuid\n\tpopl %%ebx"		      \
61					: "=d" (*(x)) : : "eax", "ecx")
62
63/*
64 * Test for SSE support on this processor.  We need to do this because
65 * we need to use ldmxcsr/stmxcsr to get correct results if any part
66 * of the program was compiled to use SSE floating-point, but we can't
67 * use SSE on older processors.
68 */
69int
70__test_sse(void)
71{
72	int flag, nflag;
73	int dx_features;
74
75	/* Am I a 486? */
76	getfl(&flag);
77	nflag = flag ^ 0x200000;
78	setfl(nflag);
79	getfl(&nflag);
80	if (flag != nflag) {
81		/* Not a 486, so CPUID should work. */
82		cpuid_dx(&dx_features);
83		if (dx_features & 0x2000000) {
84			__has_sse = __SSE_YES;
85			return (1);
86		}
87	}
88	__has_sse = __SSE_NO;
89	return (0);
90}
91
92extern inline int feclearexcept(int __excepts);
93extern inline int fegetexceptflag(fexcept_t *__flagp, int __excepts);
94
95int
96fesetexceptflag(const fexcept_t *flagp, int excepts)
97{
98	fenv_t env;
99	__uint32_t mxcsr;
100
101	__fnstenv(&env);
102	env.__status &= ~excepts;
103	env.__status |= *flagp & excepts;
104	__fldenv(env);
105
106	if (__HAS_SSE()) {
107		__stmxcsr(&mxcsr);
108		mxcsr &= ~excepts;
109		mxcsr |= *flagp & excepts;
110		__ldmxcsr(mxcsr);
111	}
112
113	return (0);
114}
115
116int
117feraiseexcept(int excepts)
118{
119	fexcept_t ex = excepts;
120
121	fesetexceptflag(&ex, excepts);
122	__fwait();
123	return (0);
124}
125
126extern inline int fetestexcept(int __excepts);
127extern inline int fegetround(void);
128extern inline int fesetround(int __round);
129
130int
131fegetenv(fenv_t *envp)
132{
133	__uint32_t mxcsr;
134
135	__fnstenv(envp);
136	/*
137	 * fnstenv masks all exceptions, so we need to restore
138	 * the old control word to avoid this side effect.
139	 */
140	__fldcw(envp->__control);
141	if (__HAS_SSE()) {
142		__stmxcsr(&mxcsr);
143		__set_mxcsr(*envp, mxcsr);
144	}
145	return (0);
146}
147
148int
149feholdexcept(fenv_t *envp)
150{
151	__uint32_t mxcsr;
152
153	__fnstenv(envp);
154	__fnclex();
155	if (__HAS_SSE()) {
156		__stmxcsr(&mxcsr);
157		__set_mxcsr(*envp, mxcsr);
158		mxcsr &= ~FE_ALL_EXCEPT;
159		mxcsr |= FE_ALL_EXCEPT << _SSE_EMASK_SHIFT;
160		__ldmxcsr(mxcsr);
161	}
162	return (0);
163}
164
165extern inline int fesetenv(const fenv_t *__envp);
166
167int
168feupdateenv(const fenv_t *envp)
169{
170	__uint32_t mxcsr;
171	__uint16_t status;
172
173	__fnstsw(&status);
174	if (__HAS_SSE())
175		__stmxcsr(&mxcsr);
176	else
177		mxcsr = 0;
178	fesetenv(envp);
179	feraiseexcept((mxcsr | status) & FE_ALL_EXCEPT);
180	return (0);
181}
182
183int
184__feenableexcept(int mask)
185{
186	__uint32_t mxcsr, omask;
187	__uint16_t control;
188
189	mask &= FE_ALL_EXCEPT;
190	__fnstcw(&control);
191	if (__HAS_SSE())
192		__stmxcsr(&mxcsr);
193	else
194		mxcsr = 0;
195	omask = ~(control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
196	control &= ~mask;
197	__fldcw(control);
198	if (__HAS_SSE()) {
199		mxcsr &= ~(mask << _SSE_EMASK_SHIFT);
200		__ldmxcsr(mxcsr);
201	}
202	return (omask);
203}
204
205int
206__fedisableexcept(int mask)
207{
208	__uint32_t mxcsr, omask;
209	__uint16_t control;
210
211	mask &= FE_ALL_EXCEPT;
212	__fnstcw(&control);
213	if (__HAS_SSE())
214		__stmxcsr(&mxcsr);
215	else
216		mxcsr = 0;
217	omask = ~(control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
218	control |= mask;
219	__fldcw(control);
220	if (__HAS_SSE()) {
221		mxcsr |= mask << _SSE_EMASK_SHIFT;
222		__ldmxcsr(mxcsr);
223	}
224	return (omask);
225}
226
227__weak_reference(__feenableexcept, feenableexcept);
228__weak_reference(__fedisableexcept, fedisableexcept);
229