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