125c28e8Piotr Jasiukajtis/*
225c28e8Piotr Jasiukajtis * CDDL HEADER START
325c28e8Piotr Jasiukajtis *
425c28e8Piotr Jasiukajtis * The contents of this file are subject to the terms of the
525c28e8Piotr Jasiukajtis * Common Development and Distribution License (the "License").
625c28e8Piotr Jasiukajtis * You may not use this file except in compliance with the License.
725c28e8Piotr Jasiukajtis *
825c28e8Piotr Jasiukajtis * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
925c28e8Piotr Jasiukajtis * or http://www.opensolaris.org/os/licensing.
1025c28e8Piotr Jasiukajtis * See the License for the specific language governing permissions
1125c28e8Piotr Jasiukajtis * and limitations under the License.
1225c28e8Piotr Jasiukajtis *
1325c28e8Piotr Jasiukajtis * When distributing Covered Code, include this CDDL HEADER in each
1425c28e8Piotr Jasiukajtis * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
1525c28e8Piotr Jasiukajtis * If applicable, add the following below this CDDL HEADER, with the
1625c28e8Piotr Jasiukajtis * fields enclosed by brackets "[]" replaced with your own identifying
1725c28e8Piotr Jasiukajtis * information: Portions Copyright [yyyy] [name of copyright owner]
1825c28e8Piotr Jasiukajtis *
1925c28e8Piotr Jasiukajtis * CDDL HEADER END
2025c28e8Piotr Jasiukajtis */
2125c28e8Piotr Jasiukajtis
2225c28e8Piotr Jasiukajtis/*
2325c28e8Piotr Jasiukajtis * Copyright 2011 Nexenta Systems, Inc.  All rights reserved.
2425c28e8Piotr Jasiukajtis */
2525c28e8Piotr Jasiukajtis/*
2625c28e8Piotr Jasiukajtis * Copyright 2006 Sun Microsystems, Inc.  All rights reserved.
2725c28e8Piotr Jasiukajtis * Use is subject to license terms.
2825c28e8Piotr Jasiukajtis */
2925c28e8Piotr Jasiukajtis
3025c28e8Piotr Jasiukajtis#include "libm_inlines.h"
3125c28e8Piotr Jasiukajtis
3225c28e8Piotr Jasiukajtis#ifdef __RESTRICT
33d626ebdToomas Soome#define	restrict _Restrict
3425c28e8Piotr Jasiukajtis#else
35d626ebdToomas Soome#define	restrict
3625c28e8Piotr Jasiukajtis#endif
3725c28e8Piotr Jasiukajtis
3825c28e8Piotr Jasiukajtisextern double sqrt(double);
3925c28e8Piotr Jasiukajtis
40d626ebdToomas Soome/*
41d626ebdToomas Soome * Instead of type punning, use union type.
42d626ebdToomas Soome */
43d626ebdToomas Soometypedef union h32 {
44d626ebdToomas Soome	float f;
45d626ebdToomas Soome	unsigned u;
46d626ebdToomas Soome} h32;
47d626ebdToomas Soome
4825c28e8Piotr Jasiukajtisvoid
49d626ebdToomas Soome__vhypotf(int n, float *restrict x, int stridex, float *restrict y,
50d626ebdToomas Soome    int stridey, float *restrict z, int stridez)
5125c28e8Piotr Jasiukajtis{
5225c28e8Piotr Jasiukajtis	float		x0, x1, x2, y0, y1, y2, z0, z1, z2, *pz0, *pz1, *pz2;
53d626ebdToomas Soome	h32		hx0, hx1, hx2, hy0, hy1, hy2;
54d626ebdToomas Soome	int		i, j0, j1, j2;
5525c28e8Piotr Jasiukajtis
56d626ebdToomas Soome	do {
5725c28e8Piotr JasiukajtisLOOP0:
58d626ebdToomas Soome		hx0.f = *x;
59d626ebdToomas Soome		hy0.f = *y;
60d626ebdToomas Soome		hx0.u &= ~0x80000000;
61d626ebdToomas Soome		hy0.u &= ~0x80000000;
62d626ebdToomas Soome		x0 = hx0.f;
63d626ebdToomas Soome		y0 = hy0.f;
64d626ebdToomas Soome		if (hy0.u > hx0.u) {
65d626ebdToomas Soome			i = hy0.u - hx0.u;
66d626ebdToomas Soome			j0 = hy0.u & 0x7f800000;
67d626ebdToomas Soome			if (hx0.u == 0)
6825c28e8Piotr Jasiukajtis				i = 0x7f800000;
69d626ebdToomas Soome		} else {
70d626ebdToomas Soome			i = hx0.u - hy0.u;
71d626ebdToomas Soome			j0 = hx0.u & 0x7f800000;
72d626ebdToomas Soome			if (hy0.u == 0)
7325c28e8Piotr Jasiukajtis				i = 0x7f800000;
74d626ebdToomas Soome			else if (hx0.u == 0)
7525c28e8Piotr Jasiukajtis				i = 0x7f800000;
7625c28e8Piotr Jasiukajtis		}
77d626ebdToomas Soome		if (i >= 0x0c800000 || j0 >= 0x7f800000) {
7825c28e8Piotr Jasiukajtis			z0 = x0 + y0;
79d626ebdToomas Soome			if (hx0.u == 0x7f800000)
8025c28e8Piotr Jasiukajtis				z0 = x0;
81d626ebdToomas Soome			else if (hy0.u  == 0x7f800000)
8225c28e8Piotr Jasiukajtis				z0 = y0;
83d626ebdToomas Soome			else if (hx0.u > 0x7f800000 || hy0.u > 0x7f800000)
8425c28e8Piotr Jasiukajtis				z0 = *x + *y;
8525c28e8Piotr Jasiukajtis			*z = z0;
8625c28e8Piotr Jasiukajtis			x += stridex;
8725c28e8Piotr Jasiukajtis			y += stridey;
8825c28e8Piotr Jasiukajtis			z += stridez;
8925c28e8Piotr Jasiukajtis			i = 0;
9025c28e8Piotr Jasiukajtis			if (--n <= 0)
9125c28e8Piotr Jasiukajtis				break;
9225c28e8Piotr Jasiukajtis			goto LOOP0;
9325c28e8Piotr Jasiukajtis		}
9425c28e8Piotr Jasiukajtis		pz0 = z;
9525c28e8Piotr Jasiukajtis		x += stridex;
9625c28e8Piotr Jasiukajtis		y += stridey;
9725c28e8Piotr Jasiukajtis		z += stridez;
9825c28e8Piotr Jasiukajtis		i = 1;
9925c28e8Piotr Jasiukajtis		if (--n <= 0)
10025c28e8Piotr Jasiukajtis			break;
10125c28e8Piotr Jasiukajtis
10225c28e8Piotr JasiukajtisLOOP1:
103d626ebdToomas Soome		hx1.f = *x;
104d626ebdToomas Soome		hy1.f = *y;
105d626ebdToomas Soome		hx1.u &= ~0x80000000;
106d626ebdToomas Soome		hy1.u &= ~0x80000000;
107d626ebdToomas Soome		x1 = hx1.f;
108d626ebdToomas Soome		y1 = hy1.f;
109d626ebdToomas Soome		if (hy1.u > hx1.u) {
110d626ebdToomas Soome			i = hy1.u - hx1.u;
111d626ebdToomas Soome			j1 = hy1.u & 0x7f800000;
112d626ebdToomas Soome			if (hx1.u == 0)
11325c28e8Piotr Jasiukajtis				i = 0x7f800000;
114d626ebdToomas Soome		} else {
115d626ebdToomas Soome			i = hx1.u - hy1.u;
116d626ebdToomas Soome			j1 = hx1.u & 0x7f800000;
117d626ebdToomas Soome			if (hy1.u == 0)
11825c28e8Piotr Jasiukajtis				i = 0x7f800000;
119d626ebdToomas Soome			else if (hx1.u == 0)
12025c28e8Piotr Jasiukajtis				i = 0x7f800000;
12125c28e8Piotr Jasiukajtis		}
122d626ebdToomas Soome		if (i >= 0x0c800000 || j1 >= 0x7f800000) {
12325c28e8Piotr Jasiukajtis			z1 = x1 + y1;
124d626ebdToomas Soome			if (hx1.u == 0x7f800000)
12525c28e8Piotr Jasiukajtis				z1 = x1;
126d626ebdToomas Soome			else if (hy1.u == 0x7f800000)
12725c28e8Piotr Jasiukajtis				z1 = y1;
128d626ebdToomas Soome			else if (hx1.u > 0x7f800000 || hy1.u > 0x7f800000)
12925c28e8Piotr Jasiukajtis				z1 = *x + *y;
13025c28e8Piotr Jasiukajtis			*z = z1;
13125c28e8Piotr Jasiukajtis			x += stridex;
13225c28e8Piotr Jasiukajtis			y += stridey;
13325c28e8Piotr Jasiukajtis			z += stridez;
13425c28e8Piotr Jasiukajtis			i = 1;
13525c28e8Piotr Jasiukajtis			if (--n <= 0)
13625c28e8Piotr Jasiukajtis				break;
13725c28e8Piotr Jasiukajtis			goto LOOP1;
13825c28e8Piotr Jasiukajtis		}
13925c28e8Piotr Jasiukajtis		pz1 = z;
14025c28e8Piotr Jasiukajtis		x += stridex;
14125c28e8Piotr Jasiukajtis		y += stridey;
14225c28e8Piotr Jasiukajtis		z += stridez;
14325c28e8Piotr Jasiukajtis		i = 2;
14425c28e8Piotr Jasiukajtis		if (--n <= 0)
14525c28e8Piotr Jasiukajtis			break;
14625c28e8Piotr Jasiukajtis
14725c28e8Piotr JasiukajtisLOOP2:
148d626ebdToomas Soome		hx2.f = *x;
149d626ebdToomas Soome		hy2.f = *y;
150d626ebdToomas Soome		hx2.u &= ~0x80000000;
151d626ebdToomas Soome		hy2.u &= ~0x80000000;
152d626ebdToomas Soome		x2 = hx2.f;
153d626ebdToomas Soome		y2 = hy2.f;
154d626ebdToomas Soome		if (hy2.u > hx2.u) {
155d626ebdToomas Soome			i = hy2.u - hx2.u;
156d626ebdToomas Soome			j2 = hy2.u & 0x7f800000;
157d626ebdToomas Soome			if (hx2.u == 0)
15825c28e8Piotr Jasiukajtis				i = 0x7f800000;
159d626ebdToomas Soome		} else {
160d626ebdToomas Soome			i = hx2.u - hy2.u;
161d626ebdToomas Soome			j2 = hx2.u & 0x7f800000;
162d626ebdToomas Soome			if (hy2.u == 0)
16325c28e8Piotr Jasiukajtis				i = 0x7f800000;
164d626ebdToomas Soome			else if (hx2.u == 0)
16525c28e8Piotr Jasiukajtis				i = 0x7f800000;
16625c28e8Piotr Jasiukajtis		}
167d626ebdToomas Soome		if (i >= 0x0c800000 || j2 >= 0x7f800000) {
16825c28e8Piotr Jasiukajtis			z2 = x2 + y2;
169d626ebdToomas Soome			if (hx2.u == 0x7f800000)
17025c28e8Piotr Jasiukajtis				z2 = x2;
171d626ebdToomas Soome			else if (hy2.u == 0x7f800000)
17225c28e8Piotr Jasiukajtis				z2 = y2;
173d626ebdToomas Soome			else if (hx2.u > 0x7f800000 || hy2.u > 0x7f800000)
17425c28e8Piotr Jasiukajtis				z2 = *x + *y;
17525c28e8Piotr Jasiukajtis			*z = z2;
17625c28e8Piotr Jasiukajtis			x += stridex;
17725c28e8Piotr Jasiukajtis			y += stridey;
17825c28e8Piotr Jasiukajtis			z += stridez;
17925c28e8Piotr Jasiukajtis			i = 2;
18025c28e8Piotr Jasiukajtis			if (--n <= 0)
18125c28e8Piotr Jasiukajtis				break;
18225c28e8Piotr Jasiukajtis			goto LOOP2;
18325c28e8Piotr Jasiukajtis		}
18425c28e8Piotr Jasiukajtis		pz2 = z;
18525c28e8Piotr Jasiukajtis
18625c28e8Piotr Jasiukajtis		z0 = sqrt(x0 * (double)x0 + y0 * (double)y0);
18725c28e8Piotr Jasiukajtis		z1 = sqrt(x1 * (double)x1 + y1 * (double)y1);
18825c28e8Piotr Jasiukajtis		z2 = sqrt(x2 * (double)x2 + y2 * (double)y2);
18925c28e8Piotr Jasiukajtis		*pz0 = z0;
19025c28e8Piotr Jasiukajtis		*pz1 = z1;
19125c28e8Piotr Jasiukajtis		*pz2 = z2;
19225c28e8Piotr Jasiukajtis
19325c28e8Piotr Jasiukajtis		x += stridex;
19425c28e8Piotr Jasiukajtis		y += stridey;
19525c28e8Piotr Jasiukajtis		z += stridez;
19625c28e8Piotr Jasiukajtis		i = 0;
19725c28e8Piotr Jasiukajtis	} while (--n > 0);
19825c28e8Piotr Jasiukajtis
199d626ebdToomas Soome	if (i > 0) {
200d626ebdToomas Soome		if (i > 1) {
20125c28e8Piotr Jasiukajtis			z1 = sqrt(x1 * (double)x1 + y1 * (double)y1);
20225c28e8Piotr Jasiukajtis			*pz1 = z1;
20325c28e8Piotr Jasiukajtis		}
20425c28e8Piotr Jasiukajtis		z0 = sqrt(x0 * (double)x0 + y0 * (double)y0);
20525c28e8Piotr Jasiukajtis		*pz0 = z0;
20625c28e8Piotr Jasiukajtis	}
20725c28e8Piotr Jasiukajtis}
208