1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
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
inc(long double x)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
dec(long double x)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
sqrtl(long double x)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