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