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
34const fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */};
35
36typedef __uint32_t fpu_control_t;   // FPCR, Floating-point Control Register.
37typedef __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
54int fegetenv(fenv_t* envp) {
55  __get_fpcr(envp->__control);
56  __get_fpsr(envp->__status);
57  return 0;
58}
59
60int fesetenv(const fenv_t* envp) {
61  fpu_control_t fpcr;
62
63  __get_fpcr(fpcr);
64  if (envp->__control != fpcr) {
65    __set_fpcr(envp->__control);
66  }
67  __set_fpsr(envp->__status);
68  return 0;
69}
70
71int feclearexcept(int excepts) {
72  fpu_status_t fpsr;
73
74  excepts &= FE_ALL_EXCEPT;
75  __get_fpsr(fpsr);
76  fpsr &= ~excepts;
77  __set_fpsr(fpsr);
78  return 0;
79}
80
81int fegetexceptflag(fexcept_t* flagp, int excepts) {
82  fpu_status_t fpsr;
83
84  excepts &= FE_ALL_EXCEPT;
85  __get_fpsr(fpsr);
86  *flagp = fpsr & excepts;
87  return 0;
88}
89
90int fesetexceptflag(const fexcept_t* flagp, int excepts) {
91  fpu_status_t fpsr;
92
93  excepts &= FE_ALL_EXCEPT;
94  __get_fpsr(fpsr);
95  fpsr &= ~excepts;
96  fpsr |= *flagp & excepts;
97  __set_fpsr(fpsr);
98  return 0;
99}
100
101int feraiseexcept(int excepts) {
102  fexcept_t ex = excepts;
103
104  fesetexceptflag(&ex, excepts);
105  return 0;
106}
107
108int fetestexcept(int excepts) {
109  fpu_status_t fpsr;
110
111  excepts &= FE_ALL_EXCEPT;
112  __get_fpsr(fpsr);
113  return (fpsr & excepts);
114}
115
116int fegetround(void) {
117  fpu_control_t fpcr;
118
119  __get_fpcr(fpcr);
120  return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO);
121}
122
123int fesetround(int round) {
124  fpu_control_t fpcr, new_fpcr;
125
126  round &= FE_TOWARDZERO;
127  __get_fpcr(fpcr);
128  new_fpcr = fpcr & ~(FE_TOWARDZERO << FPCR_RMODE_SHIFT);
129  new_fpcr |= (round << FPCR_RMODE_SHIFT);
130  if (new_fpcr != fpcr) {
131    __set_fpcr(new_fpcr);
132  }
133  return 0;
134}
135
136int feholdexcept(fenv_t* envp) {
137  fpu_status_t fpsr;
138  __get_fpsr(fpsr);
139  fpu_control_t fpcr;
140  __get_fpcr(fpcr);
141  fenv_t env = { .__status = fpsr, .__control = fpcr };
142  *envp = env;
143
144  // Clear all exceptions.
145  fpsr &= ~FE_ALL_EXCEPT;
146  __set_fpsr(fpsr);
147  return 0;
148}
149
150int feupdateenv(const fenv_t* envp) {
151  fpu_status_t fpsr;
152  fpu_control_t fpcr;
153
154  // Set FPU Control register.
155  __get_fpcr(fpcr);
156  if (envp->__control != fpcr) {
157    __set_fpcr(envp->__control);
158  }
159
160  // Set FPU Status register to status | currently raised exceptions.
161  __get_fpsr(fpsr);
162  fpsr = envp->__status | (fpsr & FE_ALL_EXCEPT);
163  __set_fpsr(fpsr);
164  return 0;
165}
166
167int feenableexcept(int mask __unused) {
168  return -1;
169}
170
171int fedisableexcept(int mask __unused) {
172  return 0;
173}
174
175int fegetexcept(void) {
176  return 0;
177}
178