1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD 3 * 4 * Copyright (c) 2007 Steven G. Kargl 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice unmodified, this list of conditions, and the following 12 * disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 */ 28 29 #include <sys/cdefs.h> 30 __FBSDID("$FreeBSD$"); 31 32 #include <fenv.h> 33 #include <float.h> 34 35 #include "fpmath.h" 36 #include "math.h" 37 38 /* Return (x + ulp) for normal positive x. Assumes no overflow. */ 39 static inline long double 40 inc(long double x) 41 { 42 union IEEEl2bits u; 43 44 u.e = x; 45 if (++u.bits.manl == 0) { 46 if (++u.bits.manh == 0) { 47 u.bits.exp++; 48 u.bits.manh |= LDBL_NBIT; 49 } 50 } 51 return (u.e); 52 } 53 54 /* Return (x - ulp) for normal positive x. Assumes no underflow. */ 55 static inline long double 56 dec(long double x) 57 { 58 union IEEEl2bits u; 59 60 u.e = x; 61 if (u.bits.manl-- == 0) { 62 if (u.bits.manh-- == LDBL_NBIT) { 63 u.bits.exp--; 64 u.bits.manh |= LDBL_NBIT; 65 } 66 } 67 return (u.e); 68 } 69 70 #pragma STDC FENV_ACCESS ON 71 72 /* 73 * This is slow, but simple and portable. You should use hardware sqrt 74 * if possible. 75 */ 76 77 long double 78 sqrtl(long double x) 79 { 80 union IEEEl2bits u; 81 int k, r; 82 long double lo, xn; 83 fenv_t env; 84 85 u.e = x; 86 87 /* If x = NaN, then sqrt(x) = NaN. */ 88 /* If x = Inf, then sqrt(x) = Inf. */ 89 /* If x = -Inf, then sqrt(x) = NaN. */ 90 if (u.bits.exp == LDBL_MAX_EXP * 2 - 1) 91 return (x * x + x); 92 93 /* If x = +-0, then sqrt(x) = +-0. */ 94 if ((u.bits.manh | u.bits.manl | u.bits.exp) == 0) 95 return (x); 96 97 /* If x < 0, then raise invalid and return NaN */ 98 if (u.bits.sign) 99 return ((x - x) / (x - x)); 100 101 feholdexcept(&env); 102 103 if (u.bits.exp == 0) { 104 /* Adjust subnormal numbers. */ 105 u.e *= 0x1.0p514; 106 k = -514; 107 } else { 108 k = 0; 109 } 110 /* 111 * u.e is a normal number, so break it into u.e = e*2^n where 112 * u.e = (2*e)*2^2k for odd n and u.e = (4*e)*2^2k for even n. 113 */ 114 if ((u.bits.exp - 0x3ffe) & 1) { /* n is odd. */ 115 k += u.bits.exp - 0x3fff; /* 2k = n - 1. */ 116 u.bits.exp = 0x3fff; /* u.e in [1,2). */ 117 } else { 118 k += u.bits.exp - 0x4000; /* 2k = n - 2. */ 119 u.bits.exp = 0x4000; /* u.e in [2,4). */ 120 } 121 122 /* 123 * Newton's iteration. 124 * Split u.e into a high and low part to achieve additional precision. 125 */ 126 xn = sqrt(u.e); /* 53-bit estimate of sqrtl(x). */ 127 #if LDBL_MANT_DIG > 100 128 xn = (xn + (u.e / xn)) * 0.5; /* 106-bit estimate. */ 129 #endif 130 lo = u.e; 131 u.bits.manl = 0; /* Zero out lower bits. */ 132 lo = (lo - u.e) / xn; /* Low bits divided by xn. */ 133 xn = xn + (u.e / xn); /* High portion of estimate. */ 134 u.e = xn + lo; /* Combine everything. */ 135 u.bits.exp += (k >> 1) - 1; 136 137 feclearexcept(FE_INEXACT); 138 r = fegetround(); 139 fesetround(FE_TOWARDZERO); /* Set to round-toward-zero. */ 140 xn = x / u.e; /* Chopped quotient (inexact?). */ 141 142 if (!fetestexcept(FE_INEXACT)) { /* Quotient is exact. */ 143 if (xn == u.e) { 144 fesetenv(&env); 145 return (u.e); 146 } 147 /* Round correctly for inputs like x = y**2 - ulp. */ 148 xn = dec(xn); /* xn = xn - ulp. */ 149 } 150 151 if (r == FE_TONEAREST) { 152 xn = inc(xn); /* xn = xn + ulp. */ 153 } else if (r == FE_UPWARD) { 154 u.e = inc(u.e); /* u.e = u.e + ulp. */ 155 xn = inc(xn); /* xn = xn + ulp. */ 156 } 157 u.e = u.e + xn; /* Chopped sum. */ 158 feupdateenv(&env); /* Restore env and raise inexact */ 159 u.bits.exp--; 160 return (u.e); 161 } 162