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
37const fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */};
38
39typedef __uint32_t fpu_control_t;   // FPCR, Floating-point Control Register.
40typedef __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
57int fegetenv(fenv_t* envp) {
58  __get_fpcr(envp->__control);
59  __get_fpsr(envp->__status);
60  return 0;
61}
62
63int 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
74int 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
84int 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
93int 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
104int feraiseexcept(int excepts) {
105  fexcept_t ex = excepts;
106
107  fesetexceptflag(&ex, excepts);
108  return 0;
109}
110
111int fetestexcept(int excepts) {
112  fpu_status_t fpsr;
113
114  excepts &= FE_ALL_EXCEPT;
115  __get_fpsr(fpsr);
116  return (fpsr & excepts);
117}
118
119int fegetround(void) {
120  fpu_control_t fpcr;
121
122  __get_fpcr(fpcr);
123  return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO);
124}
125
126int 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
139int 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
162int 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
179int 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
190int 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
201int fegetexcept(void) {
202  fpu_control_t fpcr;
203
204  __get_fpcr(fpcr);
205  return ((fpcr & FPCR_EXCEPT_MASK) >> FPCR_EXCEPT_SHIFT);
206}
207