1/* $NetBSD: fenv.c,v 1.1.8.1 2012/08/12 18:53:11 martin Exp $ */
2
3/*-
4 * Copyright (c) 2004-2005 David Schultz <das (at) 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/cdefs.h>
30__RCSID("$NetBSD: fenv.c,v 1.1.8.1 2012/08/12 18:53:11 martin Exp $");
31
32#include <assert.h>
33#include <fenv.h>
34#include <stddef.h>
35#include <string.h>
36
37/* Load x87 Control Word */
38#define	__fldcw(__cw)		__asm__ __volatile__ \
39	("fldcw %0" : : "m" (__cw))
40
41/* No-Wait Store Control Word */
42#define	__fnstcw(__cw)		__asm__ __volatile__ \
43	("fnstcw %0" : "=m" (*(__cw)))
44
45/* No-Wait Store Status Word */
46#define	__fnstsw(__sw)		__asm__ __volatile__ \
47	("fnstsw %0" : "=am" (*(__sw)))
48
49/* No-Wait Clear Exception Flags */
50#define	__fnclex()		__asm__ __volatile__ \
51	("fnclex")
52
53/* Load x87 Environment */
54#define	__fldenv(__env)		__asm__ __volatile__ \
55	("fldenv %0" : : "m" (__env))
56
57/* No-Wait Store x87 environment */
58#define	__fnstenv(__env)	__asm__ __volatile__ \
59	("fnstenv %0" : "=m" (*(__env)))
60
61/* Check for and handle pending unmasked x87 pending FPU exceptions */
62#define	__fwait(__env)		__asm__	__volatile__	\
63	("fwait")
64
65/* Load the MXCSR register */
66#define	__ldmxcsr(__mxcsr)	__asm__ __volatile__ \
67	("ldmxcsr %0" : : "m" (__mxcsr))
68
69/* Store the MXCSR register state */
70#define	__stmxcsr(__mxcsr)	__asm__ __volatile__ \
71	("stmxcsr %0" : "=m" (*(__mxcsr)))
72
73/*
74 * The following constant represents the default floating-point environment
75 * (that is, the one installed at program startup) and has type pointer to
76 * const-qualified fenv_t.
77 *
78 * It can be used as an argument to the functions within the <fenv.h> header
79 * that manage the floating-point environment, namely fesetenv() and
80 * feupdateenv().
81 *
82 * x87 fpu registers are 16bit wide. The upper bits, 31-16, are marked as
83 * RESERVED. We provide a partial floating-point environment, where we
84 * define only the lower bits. The reserved bits are extracted and set by
85 * the consumers of FE_DFL_ENV, during runtime.
86 */
87fenv_t __fe_dfl_env = {
88	{
89		__NetBSD_NPXCW__,	/* Control word register */
90		0x00000000,		/* Status word register */
91		0x0000ffff,		/* Tag word register */
92		{
93			0x00000000,
94			0x00000000,
95			0x00000000,
96			0x00000000,
97		},
98	},
99	__INITIAL_MXCSR__       /* MXCSR register */
100};
101#define FE_DFL_ENV      ((const fenv_t *) &__fe_dfl_env)
102
103
104/*
105 * The feclearexcept() function clears the supported floating-point exceptions
106 * represented by `excepts'.
107 */
108int
109feclearexcept(int excepts)
110{
111	fenv_t fenv;
112	int ex;
113
114	_DIAGASSERT((excepts & ~FE_ALL_EXCEPT) == 0);
115
116	ex = excepts & FE_ALL_EXCEPT;
117
118	/* Store the current x87 floating-point environment */
119	__fnstenv(&fenv);
120
121	/* Clear the requested floating-point exceptions */
122	fenv.x87.status &= ~ex;
123
124	/* Load the x87 floating-point environent */
125	__fldenv(fenv);
126
127	/* Same for SSE environment */
128	__stmxcsr(&fenv.mxcsr);
129	fenv.mxcsr &= ~ex;
130	__ldmxcsr(fenv.mxcsr);
131
132	/* Success */
133	return (0);
134}
135
136/*
137 * The fegetexceptflag() function stores an implementation-defined
138 * representation of the states of the floating-point status flags indicated by
139 * the argument excepts in the object pointed to by the argument flagp.
140 */
141int
142fegetexceptflag(fexcept_t *flagp, int excepts)
143{
144	uint32_t mxcsr;
145	uint16_t x87_status;
146	int ex;
147
148	_DIAGASSERT(flagp != NULL);
149	_DIAGASSERT((excepts & ~FE_ALL_EXCEPT) == 0);
150
151	ex = excepts & FE_ALL_EXCEPT;
152
153	/* Store the current x87 status register */
154	__fnstsw(&x87_status);
155
156	/* Store the MXCSR register */
157	__stmxcsr(&mxcsr);
158
159	/* Store the results in flagp */
160	*flagp = (x87_status | mxcsr) & ex;
161
162	/* Success */
163	return (0);
164}
165
166/*
167 * The feraiseexcept() function raises the supported floating-point exceptions
168 * represented by the argument `excepts'.
169 *
170 * The standard explicitly allows us to execute an instruction that has the
171 * exception as a side effect, but we choose to manipulate the status register
172 * directly.
173 *
174 * The validation of input is being deferred to fesetexceptflag().
175 */
176int
177feraiseexcept(int excepts)
178{
179	int ex;
180
181	_DIAGASSERT((excepts & ~FE_ALL_EXCEPT) == 0);
182
183	ex = excepts & FE_ALL_EXCEPT;
184	fesetexceptflag((unsigned int *)&excepts, excepts);
185	__fwait();
186
187	/* Success */
188	return (0);
189}
190
191/*
192 * This function sets the floating-point status flags indicated by the argument
193 * `excepts' to the states stored in the object pointed to by `flagp'. It does
194 * NOT raise any floating-point exceptions, but only sets the state of the flags.
195 */
196int
197fesetexceptflag(const fexcept_t *flagp, int excepts)
198{
199	fenv_t fenv;
200	int ex;
201
202	_DIAGASSERT(flagp != NULL);
203	_DIAGASSERT((excepts & ~FE_ALL_EXCEPT) == 0);
204
205	ex = excepts & FE_ALL_EXCEPT;
206
207	/* Store the current x87 floating-point environment */
208	__fnstenv(&fenv);
209
210	/* Set the requested status flags */
211	fenv.x87.status |= *flagp & ex;
212
213	/* Load the x87 floating-point environent */
214	__fldenv(fenv);
215
216	/* Same for SSE environment */
217	__stmxcsr(&fenv.mxcsr);
218	fenv.mxcsr |= *flagp & ex;
219	__ldmxcsr(fenv.mxcsr);
220
221	/* Success */
222	return (0);
223}
224
225/*
226 * The fetestexcept() function determines which of a specified subset of the
227 * floating-point exception flags are currently set. The `excepts' argument
228 * specifies the floating-point status flags to be queried.
229 */
230int
231fetestexcept(int excepts)
232{
233	fenv_t fenv;
234	uint32_t mxcsr;
235	uint16_t status;
236	int ex;
237
238	_DIAGASSERT((excepts & ~FE_ALL_EXCEPT) == 0);
239
240	ex = excepts & FE_ALL_EXCEPT;
241
242	/* Store the current x87 floating-point environment */
243	memset(&fenv, 0, sizeof(fenv));
244
245	__fnstenv(&fenv);
246	__fnstsw(&status);
247
248	/* Store the MXCSR register state */
249	__stmxcsr(&fenv.mxcsr);
250	__stmxcsr(&mxcsr);
251
252	return ((fenv.x87.status | fenv.mxcsr) & ex);
253}
254
255/*
256 * The fegetround() function gets the current rounding direction.
257 */
258int
259fegetround(void)
260{
261	uint32_t mxcsr;
262	uint16_t control;
263
264	/*
265	 * We check both the x87 floating-point unit _and_ the SSE unit.
266	 * Normally, those two must agree with respect to each other. If they
267	 * don't, it's not our fault and the result is non-determinable, in
268	 * which case POSIX says that a negative value should be returned.
269	 */
270	__fnstcw(&control);
271	__stmxcsr(&mxcsr);
272
273	if ((control & _X87_ROUNDING_MASK)
274	    != ((mxcsr & _SSE_ROUNDING_MASK) >> 3)) {
275		return (-1);
276	}
277
278	return (control & _X87_ROUNDING_MASK);
279}
280
281/*
282 * The fesetround() function establishes the rounding direction represented by
283 * its argument `round'. If the argument is not equal to the value of a rounding
284 * direction macro, the rounding direction is not changed.
285 */
286int
287fesetround(int round)
288{
289	uint32_t  mxcsr;
290	uint16_t control;
291
292	/* Check whether requested rounding direction is supported */
293	if (round & (~_X87_ROUNDING_MASK))
294		return (-1);
295
296	/* Store the current x87 control word register  */
297	__fnstcw(&control);
298
299	/*
300	 * Set the rounding direction
301	 * Rounding Control is bits 10-11, so shift appropriately
302	 */
303	control &= ~_X87_ROUNDING_MASK;
304	control |= round;
305
306	/* Load the x87 control word register */
307	__fldcw(control);
308
309	/*
310	 * Same for the SSE environment
311	 * Rounding Control is bits 13-14, so shift appropriately
312	 */
313	__stmxcsr(&mxcsr);
314	mxcsr &= ~_SSE_ROUNDING_MASK;
315	mxcsr |= (round << _SSE_ROUND_SHIFT);
316	__ldmxcsr(mxcsr);
317
318	/* Success */
319	return (0);
320}
321
322/*
323 * The fegetenv() function attempts to store the current floating-point
324 * environment in the object pointed to by envp.
325 */
326int
327fegetenv(fenv_t *envp)
328{
329	_DIAGASSERT(envp != NULL);
330
331	/* Store the current x87 floating-point environment */
332	__fnstenv(envp);
333
334	/* Store the MXCSR register state */
335	__stmxcsr(&envp->mxcsr);
336
337     /*
338      * When an FNSTENV instruction is executed, all pending exceptions are
339      * essentially lost (either the x87 FPU status register is cleared or all
340      * exceptions are masked).
341      *
342      * 8.6 X87 FPU EXCEPTION SYNCHRONIZATION -
343      * Intel(R) 64 and IA-32 Architectures Softare Developer's Manual - Vol 1
344      *
345      */
346	__fldcw(envp->x87.control);
347
348	/* Success */
349	return (0);
350}
351
352/*
353 * The feholdexcept() function saves the current floating-point environment
354 * in the object pointed to by envp, clears the floating-point status flags, and
355 * then installs a non-stop (continue on floating-point exceptions) mode, if
356 * available, for all floating-point exceptions.
357 */
358int
359feholdexcept(fenv_t *envp)
360{
361	uint32_t mxcsr;
362
363	_DIAGASSERT(envp != NULL);
364
365	/* Store the current x87 floating-point environment */
366	__fnstenv(envp);
367
368	/* Clear all exception flags in FPU */
369	__fnclex();
370
371	/* Store the MXCSR register state */
372	__stmxcsr(&envp->mxcsr);
373
374	/* Clear exception flags in MXCSR XXX */
375	mxcsr = envp->mxcsr;
376	mxcsr &= ~FE_ALL_EXCEPT;
377
378	/* Mask all exceptions */
379	mxcsr |= FE_ALL_EXCEPT << _SSE_EMASK_SHIFT;
380
381	__ldmxcsr(mxcsr);
382
383	/* Success */
384	return (0);
385}
386
387/*
388 * The fesetenv() function attempts to establish the floating-point environment
389 * represented by the object pointed to by envp. The argument `envp' points
390 * to an object set by a call to fegetenv() or feholdexcept(), or equal a
391 * floating-point environment macro. The fesetenv() function does not raise
392 * floating-point exceptions, but only installs the state of the floating-point
393 * status flags represented through its argument.
394 */
395int
396fesetenv(const fenv_t *envp)
397{
398	fenv_t fenv;
399
400	_DIAGASSERT(envp != NULL);
401
402	/* Store the x87 floating-point environment */
403	memset(&fenv, 0, sizeof fenv);
404	__fnstenv(&fenv);
405
406	__fe_dfl_env.x87.control = (fenv.x87.control & 0xffff0000)
407	    | (__fe_dfl_env.x87.control & 0x0000ffff);
408	__fe_dfl_env.x87.status = (fenv.x87.status & 0xffff0000)
409	    | (__fe_dfl_env.x87.status & 0x0000ffff);
410	__fe_dfl_env.x87.tag = (fenv.x87.tag & 0xffff0000)
411	    | (__fe_dfl_env.x87.tag & 0x0000ffff);
412	__fe_dfl_env.x87.others[3] = (fenv.x87.others[3] & 0xffff0000)
413	    | (__fe_dfl_env.x87.others[3] & 0x0000ffff);
414	__fldenv(*envp);
415
416	/* Store the MXCSR register */
417	__ldmxcsr(envp->mxcsr);
418
419	/* Success */
420	return (0);
421}
422
423/*
424 * The feupdateenv() function saves the currently raised floating-point
425 * exceptions in its automatic storage, installs the floating-point environment
426 * represented by the object pointed to by `envp', and then raises the saved
427 * floating-point exceptions. The argument `envp' shall point to an object set
428 * by a call to feholdexcept() or fegetenv(), or equal a floating-point
429 * environment macro.
430 */
431int
432feupdateenv(const fenv_t *envp)
433{
434	fenv_t fenv;
435	uint32_t mxcsr;
436	uint16_t sw;
437
438	_DIAGASSERT(envp != NULL);
439
440	/* Store the x87 floating-point environment */
441	memset(&fenv, 0, sizeof(fenv));
442	__fnstenv(&fenv);
443
444	__fe_dfl_env.x87.control = (fenv.x87.control & 0xffff0000)
445	    | (__fe_dfl_env.x87.control & 0x0000ffff);
446	__fe_dfl_env.x87.status = (fenv.x87.status & 0xffff0000)
447	    | (__fe_dfl_env.x87.status & 0x0000ffff);
448	__fe_dfl_env.x87.tag = (fenv.x87.tag & 0xffff0000)
449	    | (__fe_dfl_env.x87.tag & 0x0000ffff);
450	__fe_dfl_env.x87.others[3] = (fenv.x87.others[3] & 0xffff0000)
451	    | (__fe_dfl_env.x87.others[3] & 0x0000ffff);
452
453	/* Store the x87 status register */
454	__fnstsw(&sw);
455
456	/* Store the MXCSR register */
457	__stmxcsr(&mxcsr);
458
459	/* Install new floating-point environment */
460	fesetenv(envp);
461
462	/* Raise any previously accumulated exceptions */
463	feraiseexcept((sw | mxcsr) & FE_ALL_EXCEPT);
464
465	/* Success */
466	return (0);
467}
468
469/*
470 * The following functions are extentions to the standard
471 */
472int
473feenableexcept(int mask)
474{
475	uint32_t mxcsr, omask;
476	uint16_t control;
477
478	_DIAGASSERT((mask & ~FE_ALL_EXCEPT) == 0);
479	mask &= FE_ALL_EXCEPT;
480
481	__fnstcw(&control);
482	__stmxcsr(&mxcsr);
483
484	omask = (control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
485	control &= ~mask;
486	__fldcw(control);
487
488	mxcsr &= ~(mask << _SSE_EMASK_SHIFT);
489	__ldmxcsr(mxcsr);
490
491	return (FE_ALL_EXCEPT & ~omask);
492
493}
494
495int
496fedisableexcept(int mask)
497{
498	uint32_t mxcsr, omask;
499	uint16_t control;
500
501	_DIAGASSERT((mask & ~FE_ALL_EXCEPT) == 0);
502
503	__fnstcw(&control);
504	__stmxcsr(&mxcsr);
505
506	omask = (control | mxcsr >> _SSE_EMASK_SHIFT) & FE_ALL_EXCEPT;
507	control |= mask;
508	__fldcw(control);
509
510	mxcsr |= mask << _SSE_EMASK_SHIFT;
511	__ldmxcsr(mxcsr);
512
513	return (FE_ALL_EXCEPT & ~omask);
514}
515
516int
517fegetexcept(void)
518{
519	uint16_t control;
520
521	/*
522	 * We assume that the masks for the x87 and the SSE unit are
523	 * the same.
524	 */
525	__fnstcw(&control);
526
527	return (~control & FE_ALL_EXCEPT);
528}
529
530