1/*-
2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
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 * $FreeBSD$
29 */
30
31#include <sys/cdefs.h>
32#include <sys/types.h>
33#include <machine/npx.h>
34
35#define	__fenv_static
36#include "fenv.h"
37
38#ifdef __GNUC_GNU_INLINE__
39#error "This file must be compiled with C99 'inline' semantics"
40#endif
41
42const fenv_t __fe_dfl_env = {
43	__INITIAL_NPXCW__,
44	0x0000,
45	0x0000,
46	0x1f80,
47	0xffffffff,
48	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
49	  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff }
50};
51
52enum __sse_support __has_sse =
53#ifdef __SSE__
54	__SSE_YES;
55#else
56	__SSE_UNK;
57#endif
58
59#define	getfl(x)	__asm __volatile("pushfl\n\tpopl %0" : "=mr" (*(x)))
60#define	setfl(x)	__asm __volatile("pushl %0\n\tpopfl" : : "g" (x))
61#define	cpuid_dx(x)	__asm __volatile("pushl %%ebx\n\tmovl $1, %%eax\n\t"  \
62					 "cpuid\n\tpopl %%ebx"		      \
63					: "=d" (*(x)) : : "eax", "ecx")
64
65/*
66 * Test for SSE support on this processor.  We need to do this because
67 * we need to use ldmxcsr/stmxcsr to get correct results if any part
68 * of the program was compiled to use SSE floating-point, but we can't
69 * use SSE on older processors.
70 */
71int
72__test_sse(void)
73{
74	int flag, nflag;
75	int dx_features;
76
77	/* Am I a 486? */
78	getfl(&flag);
79	nflag = flag ^ 0x200000;
80	setfl(nflag);
81	getfl(&nflag);
82	if (flag != nflag) {
83		/* Not a 486, so CPUID should work. */
84		cpuid_dx(&dx_features);
85		if (dx_features & 0x2000000) {
86			__has_sse = __SSE_YES;
87			return (1);
88		}
89	}
90	__has_sse = __SSE_NO;
91	return (0);
92}
93
94extern inline int feclearexcept(int __excepts);
95extern inline int fegetexceptflag(fexcept_t *__flagp, int __excepts);
96
97int
98fesetexceptflag(const fexcept_t *flagp, int excepts)
99{
100	fenv_t env;
101	__uint32_t mxcsr;
102
103	__fnstenv(&env);
104	env.__status &= ~excepts;
105	env.__status |= *flagp & excepts;
106	__fldenv(&env);
107
108	if (__HAS_SSE()) {
109		__stmxcsr(&mxcsr);
110		mxcsr &= ~excepts;
111		mxcsr |= *flagp & excepts;
112		__ldmxcsr(&mxcsr);
113	}
114
115	return (0);
116}
117
118int
119feraiseexcept(int excepts)
120{
121	fexcept_t ex = excepts;
122
123	fesetexceptflag(&ex, excepts);
124	__fwait();
125	return (0);
126}
127
128extern inline int fetestexcept(int __excepts);
129extern inline int fegetround(void);
130extern inline int fesetround(int __round);
131
132int
133fegetenv(fenv_t *envp)
134{
135	__uint32_t mxcsr;
136
137	__fnstenv(envp);
138	/*
139	 * fnstenv masks all exceptions, so we need to restore
140	 * the old control word to avoid this side effect.
141	 */
142	__fldcw(&envp->__control);
143	if (__HAS_SSE()) {
144		__stmxcsr(&mxcsr);
145		__set_mxcsr(*envp, mxcsr);
146	}
147	return (0);
148}
149
150int
151feholdexcept(fenv_t *envp)
152{
153	__uint32_t mxcsr;
154
155	__fnstenv(envp);
156	__fnclex();
157	if (__HAS_SSE()) {
158		__stmxcsr(&mxcsr);
159		__set_mxcsr(*envp, mxcsr);
160		mxcsr &= ~FE_ALL_EXCEPT;
161		mxcsr |= FE_ALL_EXCEPT << _SSE_EMASK_SHIFT;
162		__ldmxcsr(&mxcsr);
163	}
164	return (0);
165}
166
167extern inline int fesetenv(const fenv_t *__envp);
168
169int
170feupdateenv(const fenv_t *envp)
171{
172	__uint32_t mxcsr;
173	__uint16_t status;
174
175	__fnstsw(&status);
176	if (__HAS_SSE())
177		__stmxcsr(&mxcsr);
178	else
179		mxcsr = 0;
180	fesetenv(envp);
181	feraiseexcept((mxcsr | status) & FE_ALL_EXCEPT);
182	return (0);
183}
184
185int
186__feenableexcept(int mask)
187{
188	__uint32_t mxcsr, omask;
189	__uint16_t control;
190
191	mask &= FE_ALL_EXCEPT;
192	__fnstcw(&control);
193	if (__HAS_SSE())
194		__stmxcsr(&mxcsr);
195	else
196		mxcsr = 0;
197	omask = ~(control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
198	control &= ~mask;
199	__fldcw(&control);
200	if (__HAS_SSE()) {
201		mxcsr &= ~(mask << _SSE_EMASK_SHIFT);
202		__ldmxcsr(&mxcsr);
203	}
204	return (omask);
205}
206
207int
208__fedisableexcept(int mask)
209{
210	__uint32_t mxcsr, omask;
211	__uint16_t control;
212
213	mask &= FE_ALL_EXCEPT;
214	__fnstcw(&control);
215	if (__HAS_SSE())
216		__stmxcsr(&mxcsr);
217	else
218		mxcsr = 0;
219	omask = ~(control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
220	control |= mask;
221	__fldcw(&control);
222	if (__HAS_SSE()) {
223		mxcsr |= mask << _SSE_EMASK_SHIFT;
224		__ldmxcsr(&mxcsr);
225	}
226	return (omask);
227}
228
229__weak_reference(__feenableexcept, feenableexcept);
230__weak_reference(__fedisableexcept, fedisableexcept);
231