1 /*-
2 * Copyright (c) 2013 The NetBSD Foundation, Inc.
3 * All rights reserved.
4 *
5 * This code is derived from software contributed to The NetBSD Foundation
6 * by Matt Thomas of 3am Software Foundry.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * 2. Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
18 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
19 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
20 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
21 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 * POSSIBILITY OF SUCH DAMAGE.
28 */
29
30 #include <sys/cdefs.h>
31 __RCSID("$NetBSD: fenv.c,v 1.6 2014/12/29 19:11:13 martin Exp $");
32
33 #include <sys/types.h>
34 #include <assert.h>
35 #include <fenv.h>
36 #include <string.h>
37 #include <unistd.h>
38 #include <inttypes.h>
39
40 #ifdef __SOFTFP__
41 #include <ieeefp.h>
42 #include <sys/signal.h>
43 #include <sys/siginfo.h>
44 #else
45 #include <arm/armreg.h>
46 #endif
47
48 #include <arm/vfpreg.h>
49
50 const fenv_t __fe_dfl_env = VFP_FPSCR_FZ|VFP_FPSCR_DN|VFP_FPSCR_RN;
51
52 /*
53 * The feclearexcept() function shall attempt to clear the supported
54 * floating-point exceptions represented by excepts.
55 */
56 int
feclearexcept(int excepts)57 feclearexcept(int excepts)
58 {
59 #ifndef lint
60 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
61 #endif
62 #ifdef __SOFTFP__
63 fpsetsticky(fpgetsticky() & ~excepts);
64 return 0;
65 #else
66 int tmp = armreg_fpscr_read() & ~__SHIFTIN(excepts, VFP_FPSCR_CSUM);
67 armreg_fpscr_write(tmp);
68 return 0;
69 #endif
70 }
71
72 /*
73 * The fegetexceptflag() function shall attempt to store an
74 * implementation-defined representation of the states of the floating-point
75 * status flags indicated by the argument excepts in the object pointed to by
76 * the argument flagp.
77 */
78 int
fegetexceptflag(fexcept_t * flagp,int excepts)79 fegetexceptflag(fexcept_t *flagp, int excepts)
80 {
81 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
82 #ifdef __SOFTFP__
83 *flagp = fpgetsticky() & excepts;
84 #else
85 *flagp = __SHIFTOUT(armreg_fpscr_read(), VFP_FPSCR_CSUM) & excepts;
86 #endif
87 return 0;
88 }
89
90 /*
91 * The feraiseexcept() function shall attempt to raise the supported
92 * floating-point exceptions represented by the argument excepts. The order
93 * in which these floating-point exceptions are raised is unspecified.
94 */
95 int
feraiseexcept(int excepts)96 feraiseexcept(int excepts)
97 {
98 #ifndef lint
99 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
100 #endif
101 #if !defined(__minix) /* LSC: No sigqueueinfo on Minix. */
102 #ifdef __SOFTFP__
103 excepts &= fpgetsticky();
104
105 if (excepts) {
106 siginfo_t info;
107 memset(&info, 0, sizeof info);
108 info.si_signo = SIGFPE;
109 info.si_pid = getpid();
110 info.si_uid = geteuid();
111 if (excepts & FE_UNDERFLOW)
112 info.si_code = FPE_FLTUND;
113 else if (excepts & FE_OVERFLOW)
114 info.si_code = FPE_FLTOVF;
115 else if (excepts & FE_DIVBYZERO)
116 info.si_code = FPE_FLTDIV;
117 else if (excepts & FE_INVALID)
118 info.si_code = FPE_FLTINV;
119 else if (excepts & FE_INEXACT)
120 info.si_code = FPE_FLTRES;
121 sigqueueinfo(getpid(), &info);
122 }
123 #else
124 int fpscr = armreg_fpscr_read();
125 fpscr = (fpscr & ~VFP_FPSCR_ESUM) | __SHIFTIN(excepts, VFP_FPSCR_ESUM);
126 armreg_fpscr_write(fpscr);
127 #endif
128 #endif /* !defined(__minix) */
129 return 0;
130 }
131
132 /*
133 * The fesetexceptflag() function shall attempt to set the floating-point
134 * status flags indicated by the argument excepts to the states stored in the
135 * object pointed to by flagp. The value pointed to by flagp shall have been
136 * set by a previous call to fegetexceptflag() whose second argument
137 * represented at least those floating-point exceptions represented by the
138 * argument excepts. This function does not raise floating-point exceptions,
139 * but only sets the state of the flags.
140 */
141 int
fesetexceptflag(const fexcept_t * flagp,int excepts)142 fesetexceptflag(const fexcept_t *flagp, int excepts)
143 {
144 #ifndef lint
145 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
146 #endif
147 #ifdef __SOFTFP__
148 fpsetsticky((fpgetsticky() & ~excepts) | (excepts & *flagp));
149 #else
150 int fpscr = armreg_fpscr_read();
151 fpscr &= ~__SHIFTIN(excepts, VFP_FPSCR_CSUM);
152 fpscr |= __SHIFTIN((*flagp & excepts), VFP_FPSCR_CSUM);
153 armreg_fpscr_write(fpscr);
154 #endif
155 return 0;
156 }
157
158 int
feenableexcept(int excepts)159 feenableexcept(int excepts)
160 {
161 #ifndef lint
162 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
163 #endif
164 #ifdef __SOFTFP__
165 int old = fpgetmask();
166 fpsetmask(old | excepts);
167 return old;
168 #else
169 int fpscr = armreg_fpscr_read();
170 armreg_fpscr_write(fpscr | __SHIFTIN((excepts), VFP_FPSCR_ESUM));
171 return __SHIFTOUT(fpscr, VFP_FPSCR_ESUM) & FE_ALL_EXCEPT;
172 #endif
173 }
174
175 int
fedisableexcept(int excepts)176 fedisableexcept(int excepts)
177 {
178 #ifndef lint
179 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
180 #endif
181 #ifdef __SOFTFP__
182 int old = fpgetmask();
183 fpsetmask(old & ~excepts);
184 return old;
185 #else
186 int fpscr = armreg_fpscr_read();
187 armreg_fpscr_write(fpscr & ~__SHIFTIN((excepts), VFP_FPSCR_ESUM));
188 return __SHIFTOUT(fpscr, VFP_FPSCR_ESUM) & FE_ALL_EXCEPT;
189 #endif
190 }
191
192 /*
193 * The fetestexcept() function shall determine which of a specified subset of
194 * the floating-point exception flags are currently set. The excepts argument
195 * specifies the floating-point status flags to be queried.
196 */
197 int
fetestexcept(int excepts)198 fetestexcept(int excepts)
199 {
200 _DIAGASSERT((except & ~FE_ALL_EXCEPT) == 0);
201 #ifdef __SOFTFP__
202 return fpgetsticky() & excepts;
203 #else
204 return __SHIFTOUT(armreg_fpscr_read(), VFP_FPSCR_CSUM) & excepts;
205 #endif
206 }
207
208 int
fegetexcept(void)209 fegetexcept(void)
210 {
211 #ifdef __SOFTFP__
212 return fpgetmask();
213 #else
214 return __SHIFTOUT(armreg_fpscr_read(), VFP_FPSCR_ESUM);
215 #endif
216 }
217
218 /*
219 * The fegetround() function shall get the current rounding direction.
220 */
221 int
fegetround(void)222 fegetround(void)
223 {
224 #ifdef __SOFTFP__
225 return fpgetround();
226 #else
227 return __SHIFTOUT(armreg_fpscr_read(), VFP_FPSCR_RMODE);
228 #endif
229 }
230
231 /*
232 * The fesetround() function shall establish the rounding direction represented
233 * by its argument round. If the argument is not equal to the value of a
234 * rounding direction macro, the rounding direction is not changed.
235 */
236 int
fesetround(int round)237 fesetround(int round)
238 {
239 #ifndef lint
240 _DIAGASSERT(!(round & ~__SHIFTOUT(VFP_FPSCR_RMODE, VFP_FPSCR_RMODE)));
241 #endif
242 #ifdef __SOFTFP__
243 (void)fpsetround(round);
244 #else
245 int fpscr = armreg_fpscr_read() & ~VFP_FPSCR_RMODE;
246 fpscr |= __SHIFTIN(round, VFP_FPSCR_RMODE);
247 armreg_fpscr_write(fpscr);
248 #endif
249 return 0;
250 }
251
252 /*
253 * The fegetenv() function shall attempt to store the current floating-point
254 * environment in the object pointed to by envp.
255 */
256 int
fegetenv(fenv_t * envp)257 fegetenv(fenv_t *envp)
258 {
259 #ifdef __SOFTFP__
260 *envp = __SHIFTIN(fpgetround(), VFP_FPSCR_RMODE)
261 | __SHIFTIN(fpgetmask(), VFP_FPSCR_ESUM)
262 | __SHIFTIN(fpgetsticky(), VFP_FPSCR_CSUM);
263 #else
264 *envp = armreg_fpscr_read();
265 #endif
266 return 0;
267 }
268
269 /*
270 * The feholdexcept() function shall save the current floating-point
271 * environment in the object pointed to by envp, clear the floating-point
272 * status flags, and then install a non-stop (continue on floating-point
273 * exceptions) mode, if available, for all floating-point exceptions.
274 */
275 int
feholdexcept(fenv_t * envp)276 feholdexcept(fenv_t *envp)
277 {
278 #ifdef __SOFTFP__
279 *envp = __SHIFTIN(fpgetround(), VFP_FPSCR_RMODE)
280 | __SHIFTIN(fpgetmask(), VFP_FPSCR_ESUM)
281 | __SHIFTIN(fpgetsticky(), VFP_FPSCR_CSUM);
282 fpsetmask(0);
283 fpsetsticky(0);
284 #else
285 *envp = armreg_fpscr_read();
286 armreg_fpscr_write((*envp) & ~(VFP_FPSCR_ESUM|VFP_FPSCR_CSUM));
287 #endif
288 return 0;
289 }
290
291 /*
292 * The fesetenv() function shall attempt to establish the floating-point
293 * environment represented by the object pointed to by envp. The fesetenv()
294 * function does not raise floating-point exceptions, but only installs the
295 * state of the floating-point status flags represented through its argument.
296 */
297 int
fesetenv(const fenv_t * envp)298 fesetenv(const fenv_t *envp)
299 {
300 #ifdef __SOFTFP__
301 (void)fpsetround(__SHIFTIN(*envp, VFP_FPSCR_RMODE));
302 (void)fpsetmask(__SHIFTOUT(*envp, VFP_FPSCR_ESUM));
303 (void)fpsetsticky(__SHIFTOUT(*envp, VFP_FPSCR_CSUM));
304 #else
305 armreg_fpscr_write(*envp);
306 #endif
307 return 0;
308 }
309
310 /*
311 * The feupdateenv() function shall attempt to save the currently raised
312 * floating-point exceptions in its automatic storage, attempt to install the
313 * floating-point environment represented by the object pointed to by envp,
314 * and then attempt to raise the saved floating-point exceptions.
315 */
316 int
feupdateenv(const fenv_t * envp)317 feupdateenv(const fenv_t *envp)
318 {
319 #ifndef lint
320 _DIAGASSERT(envp != NULL);
321 #endif
322 #ifdef __SOFTFP__
323 (void)fpsetround(__SHIFTIN(*envp, VFP_FPSCR_RMODE));
324 (void)fpsetmask(fpgetmask() | __SHIFTOUT(*envp, VFP_FPSCR_ESUM));
325 (void)fpsetsticky(fpgetsticky() | __SHIFTOUT(*envp, VFP_FPSCR_CSUM));
326 #else
327 int fpscr = armreg_fpscr_read() & ~(VFP_FPSCR_ESUM|VFP_FPSCR_CSUM);
328 fpscr |= *envp;
329 armreg_fpscr_write(fpscr);
330 #endif
331
332 /* Success */
333 return 0;
334 }
335