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/fpu.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	{ 0xffff0000 | __INITIAL_FPUCW__,
41	  0xffff0000,
42	  0xffffffff,
43	  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
44	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff }
45	},
46	__INITIAL_MXCSR__
47};
48
49extern inline int feclearexcept(int __excepts);
50extern inline int fegetexceptflag(fexcept_t *__flagp, int __excepts);
51
52int
53fesetexceptflag(const fexcept_t *flagp, int excepts)
54{
55	fenv_t env;
56
57	__fnstenv(&env.__x87);
58	env.__x87.__status &= ~excepts;
59	env.__x87.__status |= *flagp & excepts;
60	__fldenv(&env.__x87);
61
62	__stmxcsr(&env.__mxcsr);
63	env.__mxcsr &= ~excepts;
64	env.__mxcsr |= *flagp & excepts;
65	__ldmxcsr(&env.__mxcsr);
66
67	return (0);
68}
69
70int
71feraiseexcept(int excepts)
72{
73	fexcept_t ex = excepts;
74
75	fesetexceptflag(&ex, excepts);
76	__fwait();
77	return (0);
78}
79
80extern inline int fetestexcept(int __excepts);
81extern inline int fegetround(void);
82extern inline int fesetround(int __round);
83
84int
85fegetenv(fenv_t *envp)
86{
87
88	__fnstenv(&envp->__x87);
89	__stmxcsr(&envp->__mxcsr);
90	/*
91	 * fnstenv masks all exceptions, so we need to restore the
92	 * control word to avoid this side effect.
93	 */
94	__fldcw(&envp->__x87.__control);
95	return (0);
96}
97
98int
99feholdexcept(fenv_t *envp)
100{
101	__uint32_t mxcsr;
102
103	__stmxcsr(&mxcsr);
104	__fnstenv(&envp->__x87);
105	__fnclex();
106	envp->__mxcsr = mxcsr;
107	mxcsr &= ~FE_ALL_EXCEPT;
108	mxcsr |= FE_ALL_EXCEPT << _SSE_EMASK_SHIFT;
109	__ldmxcsr(&mxcsr);
110	return (0);
111}
112
113extern inline int fesetenv(const fenv_t *__envp);
114
115int
116feupdateenv(const fenv_t *envp)
117{
118	__uint32_t mxcsr;
119	__uint16_t status;
120
121	__fnstsw(&status);
122	__stmxcsr(&mxcsr);
123	fesetenv(envp);
124	feraiseexcept((mxcsr | status) & FE_ALL_EXCEPT);
125	return (0);
126}
127
128int
129__feenableexcept(int mask)
130{
131	__uint32_t mxcsr, omask;
132	__uint16_t control;
133
134	mask &= FE_ALL_EXCEPT;
135	__fnstcw(&control);
136	__stmxcsr(&mxcsr);
137	omask = ~(control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
138	control &= ~mask;
139	__fldcw(&control);
140	mxcsr &= ~(mask << _SSE_EMASK_SHIFT);
141	__ldmxcsr(&mxcsr);
142	return (omask);
143}
144
145int
146__fedisableexcept(int mask)
147{
148	__uint32_t mxcsr, omask;
149	__uint16_t control;
150
151	mask &= FE_ALL_EXCEPT;
152	__fnstcw(&control);
153	__stmxcsr(&mxcsr);
154	omask = ~(control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
155	control |= mask;
156	__fldcw(&control);
157	mxcsr |= mask << _SSE_EMASK_SHIFT;
158	__ldmxcsr(&mxcsr);
159	return (omask);
160}
161
162__weak_reference(__feenableexcept, feenableexcept);
163__weak_reference(__fedisableexcept, fedisableexcept);
164