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_RMODE_SHIFT 22
33
34 const fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */};
35
36 typedef __uint32_t fpu_control_t; // FPCR, Floating-point Control Register.
37 typedef __uint32_t fpu_status_t; // FPSR, Floating-point Status Register.
38
39 #define __get(REGISTER, __value) { \
40 uint64_t __value64; \
41 __asm__ __volatile__("mrs %0," REGISTER : "=r" (__value64)); \
42 __value = (__uint32_t) __value64; \
43 }
44 #define __get_fpcr(__fpcr) __get("fpcr", __fpcr)
45 #define __get_fpsr(__fpsr) __get("fpsr", __fpsr)
46
47 #define __set(REGISTER, __value) { \
48 uint64_t __value64 = __value; \
49 __asm__ __volatile__("msr " REGISTER ",%0" : : "ri" (__value64)); \
50 }
51 #define __set_fpcr(__fpcr) __set("fpcr", __fpcr)
52 #define __set_fpsr(__fpsr) __set("fpsr", __fpsr)
53
fegetenv(fenv_t * envp)54 int fegetenv(fenv_t* envp) {
55 __get_fpcr(envp->__control);
56 __get_fpsr(envp->__status);
57 return 0;
58 }
59
fesetenv(const fenv_t * envp)60 int fesetenv(const fenv_t* envp) {
61 fpu_control_t fpcr;
62 __get_fpcr(fpcr);
63 if (envp->__control != fpcr) {
64 __set_fpcr(envp->__control);
65 }
66 __set_fpsr(envp->__status);
67 return 0;
68 }
69
feclearexcept(int excepts)70 int feclearexcept(int excepts) {
71 fpu_status_t fpsr;
72 __get_fpsr(fpsr);
73 fpsr &= ~(excepts & FE_ALL_EXCEPT);
74 __set_fpsr(fpsr);
75 return 0;
76 }
77
fegetexceptflag(fexcept_t * flagp,int excepts)78 int fegetexceptflag(fexcept_t* flagp, int excepts) {
79 fpu_status_t fpsr;
80 __get_fpsr(fpsr);
81 *flagp = fpsr & (excepts & FE_ALL_EXCEPT);
82 return 0;
83 }
84
fesetexceptflag(const fexcept_t * flagp,int excepts)85 int fesetexceptflag(const fexcept_t* flagp, int excepts) {
86 excepts &= FE_ALL_EXCEPT;
87 fpu_status_t fpsr;
88 __get_fpsr(fpsr);
89 fpsr &= ~excepts;
90 fpsr |= *flagp & excepts;
91 __set_fpsr(fpsr);
92 return 0;
93 }
94
feraiseexcept(int excepts)95 int feraiseexcept(int excepts) {
96 fexcept_t ex = excepts;
97 fesetexceptflag(&ex, excepts);
98 return 0;
99 }
100
fetestexcept(int excepts)101 int fetestexcept(int excepts) {
102 fpu_status_t fpsr;
103 __get_fpsr(fpsr);
104 return (fpsr & (excepts & FE_ALL_EXCEPT));
105 }
106
fegetround(void)107 int fegetround(void) {
108 fpu_control_t fpcr;
109 __get_fpcr(fpcr);
110 return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO);
111 }
112
fesetround(int round)113 int fesetround(int round) {
114 if (round < FE_TONEAREST || round > FE_TOWARDZERO) return -1;
115 fpu_control_t fpcr;
116 __get_fpcr(fpcr);
117 fpu_control_t new_fpcr = fpcr & ~(FE_TOWARDZERO << FPCR_RMODE_SHIFT);
118 new_fpcr |= (round << FPCR_RMODE_SHIFT);
119 if (new_fpcr != fpcr) {
120 __set_fpcr(new_fpcr);
121 }
122 return 0;
123 }
124
feholdexcept(fenv_t * envp)125 int feholdexcept(fenv_t* envp) {
126 fegetenv(envp);
127 feclearexcept(FE_ALL_EXCEPT);
128 return 0;
129 }
130
feupdateenv(const fenv_t * envp)131 int feupdateenv(const fenv_t* envp) {
132 int excepts = fetestexcept(FE_ALL_EXCEPT);
133 fesetenv(envp);
134 feraiseexcept(excepts);
135 return 0;
136 }
137
feenableexcept(int mask __unused)138 int feenableexcept(int mask __unused) {
139 return -1;
140 }
141
fedisableexcept(int mask __unused)142 int fedisableexcept(int mask __unused) {
143 return 0;
144 }
145
fegetexcept(void)146 int fegetexcept(void) {
147 return 0;
148 }
149