11c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu/*- 21c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * Copyright (c) 2004 David Schultz <das@FreeBSD.ORG> 31c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * All rights reserved. 41c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * 51c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * Redistribution and use in source and binary forms, with or without 61c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * modification, are permitted provided that the following conditions 71c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * are met: 81c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * 1. Redistributions of source code must retain the above copyright 91c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * notice, this list of conditions and the following disclaimer. 101c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * 2. Redistributions in binary form must reproduce the above copyright 111c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * notice, this list of conditions and the following disclaimer in the 121c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * documentation and/or other materials provided with the distribution. 131c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * 141c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 151c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 161c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 171c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 181c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 191c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 201c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 211c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 221c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 231c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 241c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * SUCH DAMAGE. 251c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * 261c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu * $FreeBSD: libm/aarch64/fenv.c $ 271c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu */ 281c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu 291c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu#include <fenv.h> 301c4f101a9b95d79e0c2307cd1016e8d1870de51aSerban Constantinescu 31a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define FPCR_EXCEPT_SHIFT 8 32a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define FPCR_EXCEPT_MASK (FE_ALL_EXCEPT << FPCR_EXCEPT_SHIFT) 33a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 34a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define FPCR_RMODE_SHIFT 22 35a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 36a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuconst fenv_t __fe_dfl_env = { 0 /* control */, 0 /* status */}; 37a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 38a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescutypedef __uint32_t fpu_control_t; // FPCR, Floating-point Control Register. 39a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescutypedef __uint32_t fpu_status_t; // FPSR, Floating-point Status Register. 40a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 41a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define __get_fpcr(__fpcr) __asm__ __volatile__("mrs %0,fpcr" : "=r" (__fpcr)) 42a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define __get_fpsr(__fpsr) __asm__ __volatile__("mrs %0,fpsr" : "=r" (__fpsr)) 43a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define __set_fpcr(__fpcr) __asm__ __volatile__("msr fpcr,%0" : :"ri" (__fpcr)) 44a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu#define __set_fpsr(__fpsr) __asm__ __volatile__("msr fpsr,%0" : :"ri" (__fpsr)) 452d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 46a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fegetenv(fenv_t* envp) { 47a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(envp->__control); 48a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(envp->__status); 492d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 502d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 512d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 52a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fesetenv(const fenv_t* envp) { 53a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t fpcr; 54a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 55a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(fpcr); 56a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu if (envp->__control != fpcr) { 57a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpcr(envp->__control); 58a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu } 59a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpsr(envp->__status); 602d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 612d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 622d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 63a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint feclearexcept(int excepts) { 64a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_status_t fpsr; 65a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 66a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu excepts &= FE_ALL_EXCEPT; 67a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(fpsr); 68a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpsr &= ~excepts; 69a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpsr(fpsr); 702d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 712d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 722d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 73a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fegetexceptflag(fexcept_t* flagp, int excepts) { 74a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_status_t fpsr; 75a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 76a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu excepts &= FE_ALL_EXCEPT; 77a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(fpsr); 78a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu *flagp = fpsr & excepts; 792d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 802d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 812d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 82a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fesetexceptflag(const fexcept_t* flagp, int excepts) { 83a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_status_t fpsr; 84a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 85a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu excepts &= FE_ALL_EXCEPT; 86a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(fpsr); 87a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpsr &= ~excepts; 88a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpsr |= *flagp & excepts; 89a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpsr(fpsr); 902d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 912d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 922d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 93a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint feraiseexcept(int excepts) { 94a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fexcept_t ex = excepts; 95a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 96a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fesetexceptflag(&ex, excepts); 972d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 982d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 992d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 100a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fetestexcept(int excepts) { 101a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_status_t fpsr; 102a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 103a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu excepts &= FE_ALL_EXCEPT; 104a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(fpsr); 105a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu return (fpsr & excepts); 1062d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1072d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 1082d367905a2e1b950f79b408141eea07c222b590bCalin Juravleint fegetround(void) { 109a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t fpcr; 110a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 111a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(fpcr); 112a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu return ((fpcr >> FPCR_RMODE_SHIFT) & FE_TOWARDZERO); 1132d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1142d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 115a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fesetround(int round) { 116a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t fpcr, new_fpcr; 117a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 118a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu round &= FE_TOWARDZERO; 119a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(fpcr); 120a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu new_fpcr = fpcr & ~(FE_TOWARDZERO << FPCR_RMODE_SHIFT); 121a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu new_fpcr |= (round << FPCR_RMODE_SHIFT); 122a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu if (new_fpcr != fpcr) { 123a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpcr(new_fpcr); 124a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu } 1252d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 1262d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1272d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 128a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint feholdexcept(fenv_t* envp) { 129a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fenv_t env; 130a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_status_t fpsr; 131a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t fpcr, new_fpcr; 132a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 133a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(fpsr); 134a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(fpcr); 135a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu env.__status = fpsr; 136a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu env.__control = fpcr; 137a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu *envp = env; 138a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 139a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu // Set exceptions to untrapped. 140a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu new_fpcr = fpcr & ~(FE_ALL_EXCEPT << FPCR_EXCEPT_SHIFT); 141a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu if (new_fpcr != fpcr) { 142a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpcr(new_fpcr); 143a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu } 144a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 145a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu // Clear all exceptions. 146a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpsr &= ~FE_ALL_EXCEPT; 147a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpsr(fpsr); 1482d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 1492d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1502d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 151a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint feupdateenv(const fenv_t* envp) { 152a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_status_t fpsr; 153a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t fpcr; 154a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 155a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu // Set FPU Control register. 156a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(fpcr); 157a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu if (envp->__control != fpcr) { 158a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpcr(envp->__control); 159a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu } 160a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 161a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu // Set FPU Status register to status | currently raised exceptions. 162a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpsr(fpsr); 163a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpsr = envp->__status | (fpsr & FE_ALL_EXCEPT); 164a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpsr(fpsr); 1652d367905a2e1b950f79b408141eea07c222b590bCalin Juravle return 0; 1662d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1672d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 168a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint feenableexcept(int mask) { 169a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t old_fpcr, new_fpcr; 170a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 171a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(old_fpcr); 172a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu new_fpcr = old_fpcr | ((mask & FE_ALL_EXCEPT) << FPCR_EXCEPT_SHIFT); 173a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu if (new_fpcr != old_fpcr) { 174a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpcr(new_fpcr); 175a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu } 176a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu return ((old_fpcr >> FPCR_EXCEPT_SHIFT) & FE_ALL_EXCEPT); 1772d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1782d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 179a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescuint fedisableexcept(int mask) { 180a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t old_fpcr, new_fpcr; 181a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 182a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(old_fpcr); 183a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu new_fpcr = old_fpcr & ~((mask & FE_ALL_EXCEPT) << FPCR_EXCEPT_SHIFT); 184a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu if (new_fpcr != old_fpcr) { 185a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __set_fpcr(new_fpcr); 186a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu } 187a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu return ((old_fpcr >> FPCR_EXCEPT_SHIFT) & FE_ALL_EXCEPT); 1882d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 1892d367905a2e1b950f79b408141eea07c222b590bCalin Juravle 1902d367905a2e1b950f79b408141eea07c222b590bCalin Juravleint fegetexcept(void) { 191a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu fpu_control_t fpcr; 192a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu 193a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu __get_fpcr(fpcr); 194a147a1da5c268e9d556c207be0d3da0a519b2d54Serban Constantinescu return ((fpcr & FPCR_EXCEPT_MASK) >> FPCR_EXCEPT_SHIFT); 1952d367905a2e1b950f79b408141eea07c222b590bCalin Juravle} 196