1 /*
2 * SPDX-License-Identifier: BSD-3-Clause
3 *
4 * Copyright © 2023 Keith Packard
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 *
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 *
13 * 2. Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 *
18 * 3. Neither the name of the copyright holder nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
27 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
30 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
31 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
32 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
33 * OF THE POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 #define _vmrs_fpscr(__r) __asm __volatile("vmrs %0, fpscr" : "=&r"(__r))
37 #define _vmsr_fpscr(__r) __asm __volatile("vmsr fpscr, %0" : : "r"(__r))
38 #define _FPU_MASK_SHIFT 8
39 #define _ROUND_MASK (FE_TONEAREST | FE_DOWNWARD | \
40 FE_UPWARD | FE_TOWARDZERO)
41
feclearexcept(int excepts)42 __declare_fenv_inline(int) feclearexcept(int excepts)
43 {
44 fexcept_t __fpsr;
45
46 _vmrs_fpscr(__fpsr);
47 __fpsr &= ~excepts;
48 _vmsr_fpscr(__fpsr);
49 return (0);
50 }
51
fedisableexcept(int __mask)52 __declare_fenv_inline(int) fedisableexcept(int __mask)
53 {
54 fenv_t __old_fpsr, __new_fpsr;
55
56 _vmrs_fpscr(__old_fpsr);
57 __new_fpsr = __old_fpsr &
58 ~((__mask & FE_ALL_EXCEPT) << _FPU_MASK_SHIFT);
59 _vmsr_fpscr(__new_fpsr);
60 return ((__old_fpsr >> _FPU_MASK_SHIFT) & FE_ALL_EXCEPT);
61 }
feenableexcept(int __mask)62 __declare_fenv_inline(int) feenableexcept(int __mask)
63 {
64 fenv_t __old_fpsr, __new_fpsr, __check_fpsr;
65
66 _vmrs_fpscr(__old_fpsr);
67 __new_fpsr = __old_fpsr |
68 ((__mask & FE_ALL_EXCEPT) << _FPU_MASK_SHIFT);
69 _vmsr_fpscr(__new_fpsr);
70 _vmrs_fpscr(__check_fpsr);
71 if (__check_fpsr != __new_fpsr)
72 return -1;
73 return ((__old_fpsr >> _FPU_MASK_SHIFT) & FE_ALL_EXCEPT);
74 }
75
fegetenv(fenv_t * envp)76 __declare_fenv_inline(int) fegetenv(fenv_t *envp)
77 {
78 _vmrs_fpscr(*envp);
79 return (0);
80 }
81
fegetexcept(void)82 __declare_fenv_inline(int) fegetexcept(void)
83 {
84 fenv_t __fpsr;
85
86 _vmrs_fpscr(__fpsr);
87 return (__fpsr & FE_ALL_EXCEPT);
88 }
89
fegetexceptflag(fexcept_t * flagp,int excepts)90 __declare_fenv_inline(int) fegetexceptflag(fexcept_t *flagp, int excepts)
91 {
92 fexcept_t __fpsr;
93
94 _vmrs_fpscr(__fpsr);
95 *flagp = __fpsr & excepts;
96 return (0);
97 }
98
fegetround(void)99 __declare_fenv_inline(int) fegetround(void)
100 {
101 fenv_t __fpsr;
102
103 _vmrs_fpscr(__fpsr);
104 return (__fpsr & _ROUND_MASK);
105 }
feholdexcept(fenv_t * envp)106 __declare_fenv_inline(int) feholdexcept(fenv_t *envp)
107 {
108 fenv_t __env;
109
110 _vmrs_fpscr(__env);
111 *envp = __env;
112 __env &= ~(FE_ALL_EXCEPT);
113 _vmsr_fpscr(__env);
114 return (0);
115 }
116
fesetenv(const fenv_t * envp)117 __declare_fenv_inline(int) fesetenv(const fenv_t *envp)
118 {
119 _vmsr_fpscr(*envp);
120 return (0);
121 }
122
fesetexceptflag(const fexcept_t * flagp,int excepts)123 __declare_fenv_inline(int) fesetexceptflag(const fexcept_t *flagp, int excepts)
124 {
125 fexcept_t __fpsr;
126
127 _vmrs_fpscr(__fpsr);
128 __fpsr &= ~excepts;
129 __fpsr |= *flagp & excepts;
130 _vmsr_fpscr(__fpsr);
131 return (0);
132 }
133
feraiseexcept(int excepts)134 __declare_fenv_inline(int) feraiseexcept(int excepts)
135 {
136 fexcept_t __ex = excepts;
137
138 return fesetexceptflag(&__ex, excepts);
139 }
140
fesetexcept(int excepts)141 __declare_fenv_inline(int) fesetexcept(int excepts)
142 {
143 return feraiseexcept(excepts);
144 }
145
fesetround(int round)146 __declare_fenv_inline(int) fesetround(int round)
147 {
148 fenv_t __fpsr;
149
150 _vmrs_fpscr(__fpsr);
151 __fpsr &= ~(_ROUND_MASK);
152 __fpsr |= round;
153 _vmsr_fpscr(__fpsr);
154 return (0);
155 }
156
fetestexcept(int excepts)157 __declare_fenv_inline(int) fetestexcept(int excepts)
158 {
159 fexcept_t __fpsr;
160
161 _vmrs_fpscr(__fpsr);
162 return (__fpsr & excepts);
163 }
164
feupdateenv(const fenv_t * envp)165 __declare_fenv_inline(int) feupdateenv(const fenv_t *envp)
166 {
167 fexcept_t __fpsr;
168
169 _vmrs_fpscr(__fpsr);
170 _vmsr_fpscr(*envp);
171 feraiseexcept(__fpsr & FE_ALL_EXCEPT);
172 return (0);
173 }
174
