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
36 const fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */};
37
38 typedef __uint32_t fpu_control_t; // FPCR, Floating-point Control Register.
39 typedef __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
fegetenv(fenv_t * envp)46 int fegetenv(fenv_t* envp) {
47 __get_fpcr(envp->__control);
48 __get_fpsr(envp->__status);
49 return 0;
50 }
51
fesetenv(const fenv_t * envp)52 int 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
feclearexcept(int excepts)63 int 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
fegetexceptflag(fexcept_t * flagp,int excepts)73 int 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
fesetexceptflag(const fexcept_t * flagp,int excepts)82 int 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
feraiseexcept(int excepts)93 int feraiseexcept(int excepts) {
94 fexcept_t ex = excepts;
95
96 fesetexceptflag(&ex, excepts);
97 return 0;
98 }
99
fetestexcept(int excepts)100 int fetestexcept(int excepts) {
101 fpu_status_t fpsr;
102
103 excepts &= FE_ALL_EXCEPT;
104 __get_fpsr(fpsr);
105 return (fpsr & excepts);
106 }
107
fegetround(void)108 int fegetround(void) {
109 fpu_control_t fpcr;
110
111 __get_fpcr(fpcr);
112 return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO);
113 }
114
fesetround(int round)115 int 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
feholdexcept(fenv_t * envp)128 int 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
feupdateenv(const fenv_t * envp)151 int 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
feenableexcept(int mask)168 int 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
fedisableexcept(int mask)179 int 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
fegetexcept(void)190 int fegetexcept(void) {
191 fpu_control_t fpcr;
192
193 __get_fpcr(fpcr);
194 return ((fpcr & FPCR_EXCEPT_MASK) >> FPCR_EXCEPT_SHIFT);
195 }
196