fenv.c revision a147a1da5c268e9d556c207be0d3da0a519b2d54 
1/*-
2 * Copyright (c) 2004 David Schultz <das@FreeBSD.ORG>
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * $FreeBSD: libm/aarch64/fenv.c $
27 */
28
29#include <fenv.h>
30
31#define FPCR_EXCEPT_SHIFT 8
32#define FPCR_EXCEPT_MASK  (FE_ALL_EXCEPT << FPCR_EXCEPT_SHIFT)
33
34#define FPCR_RMODE_SHIFT 22
35
36const fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */};
37
38typedef __uint32_t fpu_control_t;   // FPCR, Floating-point Control Register.
39typedef __uint32_t fpu_status_t;    // FPSR, Floating-point Status Register.
40
41#define __get_fpcr(__fpcr) __asm__ __volatile__("mrs %0,fpcr" : "=r" (__fpcr))
42#define __get_fpsr(__fpsr) __asm__ __volatile__("mrs %0,fpsr" : "=r" (__fpsr))
43#define __set_fpcr(__fpcr) __asm__ __volatile__("msr fpcr,%0" : :"ri" (__fpcr))
44#define __set_fpsr(__fpsr) __asm__ __volatile__("msr fpsr,%0" : :"ri" (__fpsr))
45
46int fegetenv(fenv_t* envp) {
47  __get_fpcr(envp->__control);
48  __get_fpsr(envp->__status);
49  return 0;
50}
51
52int fesetenv(const fenv_t* envp) {
53  fpu_control_t fpcr;
54
55  __get_fpcr(fpcr);
56  if (envp->__control != fpcr) {
57    __set_fpcr(envp->__control);
58  }
59  __set_fpsr(envp->__status);
60  return 0;
61}
62
63int feclearexcept(int excepts) {
64  fpu_status_t fpsr;
65
66  excepts &= FE_ALL_EXCEPT;
67  __get_fpsr(fpsr);
68  fpsr &= ~excepts;
69  __set_fpsr(fpsr);
70  return 0;
71}
72
73int fegetexceptflag(fexcept_t* flagp, int excepts) {
74  fpu_status_t fpsr;
75
76  excepts &= FE_ALL_EXCEPT;
77  __get_fpsr(fpsr);
78  *flagp = fpsr & excepts;
79  return 0;
80}
81
82int fesetexceptflag(const fexcept_t* flagp, int excepts) {
83  fpu_status_t fpsr;
84
85  excepts &= FE_ALL_EXCEPT;
86  __get_fpsr(fpsr);
87  fpsr &= ~excepts;
88  fpsr |= *flagp & excepts;
89  __set_fpsr(fpsr);
90  return 0;
91}
92
93int feraiseexcept(int excepts) {
94  fexcept_t ex = excepts;
95
96  fesetexceptflag(&ex, excepts);
97  return 0;
98}
99
100int fetestexcept(int excepts) {
101  fpu_status_t fpsr;
102
103  excepts &= FE_ALL_EXCEPT;
104  __get_fpsr(fpsr);
105  return (fpsr & excepts);
106}
107
108int fegetround(void) {
109  fpu_control_t fpcr;
110
111  __get_fpcr(fpcr);
112  return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO);
113}
114
115int fesetround(int round) {
116  fpu_control_t fpcr, new_fpcr;
117
118  round &= FE_TOWARDZERO;
119  __get_fpcr(fpcr);
120  new_fpcr = fpcr & ~(FE_TOWARDZERO << FPCR_RMODE_SHIFT);
121  new_fpcr |= (round << FPCR_RMODE_SHIFT);
122  if (new_fpcr != fpcr) {
123    __set_fpcr(new_fpcr);
124  }
125  return 0;
126}
127
128int feholdexcept(fenv_t* envp) {
129  fenv_t env;
130  fpu_status_t fpsr;
131  fpu_control_t fpcr, new_fpcr;
132
133  __get_fpsr(fpsr);
134  __get_fpcr(fpcr);
135  env.__status = fpsr;
136  env.__control = fpcr;
137  *envp = env;
138
139  // Set exceptions to untrapped.
140  new_fpcr = fpcr & ~(FE_ALL_EXCEPT << FPCR_EXCEPT_SHIFT);
141  if (new_fpcr != fpcr) {
142    __set_fpcr(new_fpcr);
143  }
144
145  // Clear all exceptions.
146  fpsr &= ~FE_ALL_EXCEPT;
147  __set_fpsr(fpsr);
148  return 0;
149}
150
151int feupdateenv(const fenv_t* envp) {
152  fpu_status_t fpsr;
153  fpu_control_t fpcr;
154
155  // Set FPU Control register.
156  __get_fpcr(fpcr);
157  if (envp->__control != fpcr) {
158    __set_fpcr(envp->__control);
159  }
160
161  // Set FPU Status register to status | currently raised exceptions.
162  __get_fpsr(fpsr);
163  fpsr = envp->__status | (fpsr & FE_ALL_EXCEPT);
164  __set_fpsr(fpsr);
165  return 0;
166}
167
168int feenableexcept(int mask) {
169  fpu_control_t old_fpcr, new_fpcr;
170
171  __get_fpcr(old_fpcr);
172  new_fpcr = old_fpcr | ((mask & FE_ALL_EXCEPT) << FPCR_EXCEPT_SHIFT);
173  if (new_fpcr != old_fpcr) {
174    __set_fpcr(new_fpcr);
175  }
176  return ((old_fpcr >> FPCR_EXCEPT_SHIFT) & FE_ALL_EXCEPT);
177}
178
179int fedisableexcept(int mask) {
180  fpu_control_t old_fpcr, new_fpcr;
181
182  __get_fpcr(old_fpcr);
183  new_fpcr = old_fpcr & ~((mask & FE_ALL_EXCEPT) << FPCR_EXCEPT_SHIFT);
184  if (new_fpcr != old_fpcr) {
185    __set_fpcr(new_fpcr);
186  }
187  return ((old_fpcr >> FPCR_EXCEPT_SHIFT) & FE_ALL_EXCEPT);
188}
189
190int fegetexcept(void) {
191  fpu_control_t fpcr;
192
193  __get_fpcr(fpcr);
194  return ((fpcr & FPCR_EXCEPT_MASK) >> FPCR_EXCEPT_SHIFT);
195}
196