1 /*- 2 * Copyright (c) 2004 David Schultz <das (at) 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 57 int fegetenv(fenv_t* envp) { 58 __get_fpcr(envp->__control); 59 __get_fpsr(envp->__status); 60 return 0; 61 } 62 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 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 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 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 104 int feraiseexcept(int excepts) { 105 fexcept_t ex = excepts; 106 107 fesetexceptflag(&ex, excepts); 108 return 0; 109 } 110 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 119 int fegetround(void) { 120 fpu_control_t fpcr; 121 122 __get_fpcr(fpcr); 123 return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO); 124 } 125 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 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 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 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 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 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
