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 <stdint.h>
30 #include <fenv.h>
31
32 #define FPCR_EXCEPT_SHIFT 8
33 #define FPCR_EXCEPT_MASK (FE_ALL_EXCEPT << FPCR_EXCEPT_SHIFT)
34
35 #define FPCR_RMODE_SHIFT 22
36
37 const fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */};
38
39 typedef __uint32_t fpu_control_t; // FPCR, Floating-point Control Register.
40 typedef __uint32_t fpu_status_t; // FPSR, Floating-point Status Register.
41
42 #define __get(REGISTER, __value) { \
43 uint64_t __value64; \
44 __asm__ __volatile__("mrs %0," REGISTER : "=r" (__value64)); \
45 __value = (__uint32_t) __value64; \
46 }
47 #define __get_fpcr(__fpcr) __get("fpcr", __fpcr)
48 #define __get_fpsr(__fpsr) __get("fpsr", __fpsr)
49
50 #define __set(REGISTER, __value) { \
51 uint64_t __value64 = __value; \
52 __asm__ __volatile__("msr " REGISTER ",%0" : : "ri" (__value64)); \
53 }
54 #define __set_fpcr(__fpcr) __set("fpcr", __fpcr)
55 #define __set_fpsr(__fpsr) __set("fpsr", __fpsr)
56
fegetenv(fenv_t * envp)57 int fegetenv(fenv_t* envp) {
58 __get_fpcr(envp->__control);
59 __get_fpsr(envp->__status);
60 return 0;
61 }
62
fesetenv(const fenv_t * envp)63 int fesetenv(const fenv_t* envp) {
64 fpu_control_t fpcr;
65
66 __get_fpcr(fpcr);
67 if (envp->__control != fpcr) {
68 __set_fpcr(envp->__control);
69 }
70 __set_fpsr(envp->__status);
71 return 0;
72 }
73
feclearexcept(int excepts)74 int feclearexcept(int excepts) {
75 fpu_status_t fpsr;
76
77 excepts &= FE_ALL_EXCEPT;
78 __get_fpsr(fpsr);
79 fpsr &= ~excepts;
80 __set_fpsr(fpsr);
81 return 0;
82 }
83
fegetexceptflag(fexcept_t * flagp,int excepts)84 int fegetexceptflag(fexcept_t* flagp, int excepts) {
85 fpu_status_t fpsr;
86
87 excepts &= FE_ALL_EXCEPT;
88 __get_fpsr(fpsr);
89 *flagp = fpsr & excepts;
90 return 0;
91 }
92
fesetexceptflag(const fexcept_t * flagp,int excepts)93 int fesetexceptflag(const fexcept_t* flagp, int excepts) {
94 fpu_status_t fpsr;
95
96 excepts &= FE_ALL_EXCEPT;
97 __get_fpsr(fpsr);
98 fpsr &= ~excepts;
99 fpsr |= *flagp & excepts;
100 __set_fpsr(fpsr);
101 return 0;
102 }
103
feraiseexcept(int excepts)104 int feraiseexcept(int excepts) {
105 fexcept_t ex = excepts;
106
107 fesetexceptflag(&ex, excepts);
108 return 0;
109 }
110
fetestexcept(int excepts)111 int fetestexcept(int excepts) {
112 fpu_status_t fpsr;
113
114 excepts &= FE_ALL_EXCEPT;
115 __get_fpsr(fpsr);
116 return (fpsr & excepts);
117 }
118
fegetround(void)119 int fegetround(void) {
120 fpu_control_t fpcr;
121
122 __get_fpcr(fpcr);
123 return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO);
124 }
125
fesetround(int round)126 int fesetround(int round) {
127 fpu_control_t fpcr, new_fpcr;
128
129 round &= FE_TOWARDZERO;
130 __get_fpcr(fpcr);
131 new_fpcr = fpcr & ~(FE_TOWARDZERO << FPCR_RMODE_SHIFT);
132 new_fpcr |= (round << FPCR_RMODE_SHIFT);
133 if (new_fpcr != fpcr) {
134 __set_fpcr(new_fpcr);
135 }
136 return 0;
137 }
138
feholdexcept(fenv_t * envp)139 int feholdexcept(fenv_t* envp) {
140 fenv_t env;
141 fpu_status_t fpsr;
142 fpu_control_t fpcr, new_fpcr;
143
144 __get_fpsr(fpsr);
145 __get_fpcr(fpcr);
146 env.__status = fpsr;
147 env.__control = fpcr;
148 *envp = env;
149
150 // Set exceptions to untrapped.
151 new_fpcr = fpcr & ~(FE_ALL_EXCEPT << FPCR_EXCEPT_SHIFT);
152 if (new_fpcr != fpcr) {
153 __set_fpcr(new_fpcr);
154 }
155
156 // Clear all exceptions.
157 fpsr &= ~FE_ALL_EXCEPT;
158 __set_fpsr(fpsr);
159 return 0;
160 }
161
feupdateenv(const fenv_t * envp)162 int feupdateenv(const fenv_t* envp) {
163 fpu_status_t fpsr;
164 fpu_control_t fpcr;
165
166 // Set FPU Control register.
167 __get_fpcr(fpcr);
168 if (envp->__control != fpcr) {
169 __set_fpcr(envp->__control);
170 }
171
172 // Set FPU Status register to status | currently raised exceptions.
173 __get_fpsr(fpsr);
174 fpsr = envp->__status | (fpsr & FE_ALL_EXCEPT);
175 __set_fpsr(fpsr);
176 return 0;
177 }
178
feenableexcept(int mask)179 int feenableexcept(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
fedisableexcept(int mask)190 int fedisableexcept(int mask) {
191 fpu_control_t old_fpcr, new_fpcr;
192
193 __get_fpcr(old_fpcr);
194 new_fpcr = old_fpcr & ~((mask & FE_ALL_EXCEPT) << FPCR_EXCEPT_SHIFT);
195 if (new_fpcr != old_fpcr) {
196 __set_fpcr(new_fpcr);
197 }
198 return ((old_fpcr >> FPCR_EXCEPT_SHIFT) & FE_ALL_EXCEPT);
199 }
200
fegetexcept(void)201 int fegetexcept(void) {
202 fpu_control_t fpcr;
203
204 __get_fpcr(fpcr);
205 return ((fpcr & FPCR_EXCEPT_MASK) >> FPCR_EXCEPT_SHIFT);
206 }
207