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