1/*	$NetBSD: fpu_trig.c,v 1.18 2017/01/16 12:05:40 isaki Exp $	*/
2
3/*
4 * Copyright (c) 1995  Ken Nakata
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, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 *    notice, this list of conditions and the following disclaimer in the
14 *    documentation and/or other materials provided with the distribution.
15 * 3. Neither the name of the author nor the names of its contributors
16 *    may be used to endorse or promote products derived from this software
17 *    without specific prior written permission.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
20 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
23 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
25 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
26 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
28 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
29 * SUCH DAMAGE.
30 *
31 *	@(#)fpu_trig.c	10/24/95
32 */
33
34/*
35 * Copyright (c) 2011 Tetsuya Isaki. All rights reserved.
36 *
37 * Redistribution and use in source and binary forms, with or without
38 * modification, are permitted provided that the following conditions
39 * are met:
40 * 1. Redistributions of source code must retain the above copyright
41 *    notice, this list of conditions and the following disclaimer.
42 * 2. Redistributions in binary form must reproduce the above copyright
43 *    notice, this list of conditions and the following disclaimer in the
44 *    documentation and/or other materials provided with the distribution.
45 *
46 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
47 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
48 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
49 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
50 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
51 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
52 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
53 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
54 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
55 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
56 * SUCH DAMAGE.
57 */
58
59#include <sys/cdefs.h>
60__KERNEL_RCSID(0, "$NetBSD: fpu_trig.c,v 1.18 2017/01/16 12:05:40 isaki Exp $");
61
62#include "fpu_emulate.h"
63
64/*
65 * arccos(x) = pi/2 - arcsin(x)
66 */
67struct fpn *
68fpu_acos(struct fpemu *fe)
69{
70	struct fpn *r;
71
72	if (ISNAN(&fe->fe_f2))
73		return &fe->fe_f2;
74	if (ISINF(&fe->fe_f2))
75		return fpu_newnan(fe);
76
77	r = fpu_asin(fe);
78	CPYFPN(&fe->fe_f2, r);
79
80	/* pi/2 - asin(x) */
81	fpu_const(&fe->fe_f1, FPU_CONST_PI);
82	fe->fe_f1.fp_exp--;
83	fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign;
84	r = fpu_add(fe);
85
86	return r;
87}
88
89/*
90 *                          x
91 * arcsin(x) = arctan(---------------)
92 *                     sqrt(1 - x^2)
93 */
94struct fpn *
95fpu_asin(struct fpemu *fe)
96{
97	struct fpn x;
98	struct fpn *r;
99
100	if (ISNAN(&fe->fe_f2))
101		return &fe->fe_f2;
102	if (ISZERO(&fe->fe_f2))
103		return &fe->fe_f2;
104
105	if (ISINF(&fe->fe_f2))
106		return fpu_newnan(fe);
107
108	CPYFPN(&x, &fe->fe_f2);
109
110	/* x^2 */
111	CPYFPN(&fe->fe_f1, &fe->fe_f2);
112	r = fpu_mul(fe);
113
114	/* 1 - x^2 */
115	CPYFPN(&fe->fe_f2, r);
116	fe->fe_f2.fp_sign = 1;
117	fpu_const(&fe->fe_f1, FPU_CONST_1);
118	r = fpu_add(fe);
119
120	/* sqrt(1-x^2) */
121	CPYFPN(&fe->fe_f2, r);
122	r = fpu_sqrt(fe);
123
124	/* x/sqrt */
125	CPYFPN(&fe->fe_f2, r);
126	CPYFPN(&fe->fe_f1, &x);
127	r = fpu_div(fe);
128
129	/* arctan */
130	CPYFPN(&fe->fe_f2, r);
131	return fpu_atan(fe);
132}
133
134/*
135 * arctan(x):
136 *
137 *	if (x < 0) {
138 *		x = abs(x);
139 *		sign = 1;
140 *	}
141 *	y = arctan(x);
142 *	if (sign) {
143 *		y = -y;
144 *	}
145 */
146struct fpn *
147fpu_atan(struct fpemu *fe)
148{
149	struct fpn a;
150	struct fpn x;
151	struct fpn v;
152
153	if (ISNAN(&fe->fe_f2))
154		return &fe->fe_f2;
155	if (ISZERO(&fe->fe_f2))
156		return &fe->fe_f2;
157
158	CPYFPN(&a, &fe->fe_f2);
159
160	if (ISINF(&fe->fe_f2)) {
161		/* f2 <- pi/2 */
162		fpu_const(&fe->fe_f2, FPU_CONST_PI);
163		fe->fe_f2.fp_exp--;
164
165		fe->fe_f2.fp_sign = a.fp_sign;
166		return &fe->fe_f2;
167	}
168
169	fpu_const(&x, FPU_CONST_1);
170	fpu_const(&fe->fe_f2, FPU_CONST_0);
171	CPYFPN(&v, &fe->fe_f2);
172	fpu_cordit1(fe, &x, &a, &fe->fe_f2, &v);
173
174	return &fe->fe_f2;
175}
176
177
178/*
179 * fe_f1 := sin(in)
180 * fe_f2 := cos(in)
181 */
182static void
183__fpu_sincos_cordic(struct fpemu *fe, const struct fpn *in)
184{
185	struct fpn a;
186	struct fpn v;
187
188	CPYFPN(&a, in);
189	fpu_const(&fe->fe_f1, FPU_CONST_0);
190	CPYFPN(&fe->fe_f2, &fpu_cordic_inv_gain1);
191	fpu_const(&v, FPU_CONST_1);
192	v.fp_sign = 1;
193	fpu_cordit1(fe, &fe->fe_f2, &fe->fe_f1, &a, &v);
194}
195
196/*
197 * cos(x):
198 *
199 *	if (x < 0) {
200 *		x = abs(x);
201 *	}
202 *	if (x > 2*pi) {
203 *		x %= 2*pi;
204 *	}
205 *	if (x > pi) {
206 *		x -= pi;
207 *		sign inverse;
208 *	}
209 *	if (x > pi/2) {
210 *		y = sin(x - pi/2);
211 *		sign inverse;
212 *	} else {
213 *		y = cos(x);
214 *	}
215 *	if (sign) {
216 *		y = -y;
217 *	}
218 */
219struct fpn *
220fpu_cos(struct fpemu *fe)
221{
222	struct fpn x;
223	struct fpn p;
224	struct fpn *r;
225	int sign;
226
227	if (ISNAN(&fe->fe_f2))
228		return &fe->fe_f2;
229	if (ISINF(&fe->fe_f2))
230		return fpu_newnan(fe);
231
232	/* x = abs(input) */
233	sign = 0;
234	CPYFPN(&x, &fe->fe_f2);
235	x.fp_sign = 0;
236
237	/* p <- 2*pi */
238	fpu_const(&p, FPU_CONST_PI);
239	p.fp_exp++;
240
241	/*
242	 * if (x > 2*pi*N)
243	 *  cos(x) is cos(x - 2*pi*N)
244	 */
245	CPYFPN(&fe->fe_f1, &x);
246	CPYFPN(&fe->fe_f2, &p);
247	r = fpu_cmp(fe);
248	if (r->fp_sign == 0) {
249		CPYFPN(&fe->fe_f1, &x);
250		CPYFPN(&fe->fe_f2, &p);
251		r = fpu_mod(fe);
252		CPYFPN(&x, r);
253	}
254
255	/* p <- pi */
256	p.fp_exp--;
257
258	/*
259	 * if (x > pi)
260	 *  cos(x) is -cos(x - pi)
261	 */
262	CPYFPN(&fe->fe_f1, &x);
263	CPYFPN(&fe->fe_f2, &p);
264	fe->fe_f2.fp_sign = 1;
265	r = fpu_add(fe);
266	if (r->fp_sign == 0) {
267		CPYFPN(&x, r);
268		sign ^= 1;
269	}
270
271	/* p <- pi/2 */
272	p.fp_exp--;
273
274	/*
275	 * if (x > pi/2)
276	 *  cos(x) is -sin(x - pi/2)
277	 * else
278	 *  cos(x)
279	 */
280	CPYFPN(&fe->fe_f1, &x);
281	CPYFPN(&fe->fe_f2, &p);
282	fe->fe_f2.fp_sign = 1;
283	r = fpu_add(fe);
284	if (r->fp_sign == 0) {
285		__fpu_sincos_cordic(fe, r);
286		r = &fe->fe_f1;
287		sign ^= 1;
288	} else {
289		__fpu_sincos_cordic(fe, &x);
290		r = &fe->fe_f2;
291	}
292	r->fp_sign = sign;
293	return r;
294}
295
296/*
297 * sin(x):
298 *
299 *	if (x < 0) {
300 *		x = abs(x);
301 *		sign = 1;
302 *	}
303 *	if (x > 2*pi) {
304 *		x %= 2*pi;
305 *	}
306 *	if (x > pi) {
307 *		x -= pi;
308 *		sign inverse;
309 *	}
310 *	if (x > pi/2) {
311 *		y = cos(x - pi/2);
312 *	} else {
313 *		y = sin(x);
314 *	}
315 *	if (sign) {
316 *		y = -y;
317 *	}
318 */
319struct fpn *
320fpu_sin(struct fpemu *fe)
321{
322	struct fpn x;
323	struct fpn p;
324	struct fpn *r;
325	int sign;
326
327	if (ISNAN(&fe->fe_f2))
328		return &fe->fe_f2;
329	if (ISINF(&fe->fe_f2))
330		return fpu_newnan(fe);
331
332	/* if x is +0/-0, return +0/-0 */
333	if (ISZERO(&fe->fe_f2))
334		return &fe->fe_f2;
335
336	/* x = abs(input) */
337	sign = fe->fe_f2.fp_sign;
338	CPYFPN(&x, &fe->fe_f2);
339	x.fp_sign = 0;
340
341	/* p <- 2*pi */
342	fpu_const(&p, FPU_CONST_PI);
343	p.fp_exp++;
344
345	/*
346	 * if (x > 2*pi*N)
347	 *  sin(x) is sin(x - 2*pi*N)
348	 */
349	CPYFPN(&fe->fe_f1, &x);
350	CPYFPN(&fe->fe_f2, &p);
351	r = fpu_cmp(fe);
352	if (r->fp_sign == 0) {
353		CPYFPN(&fe->fe_f1, &x);
354		CPYFPN(&fe->fe_f2, &p);
355		r = fpu_mod(fe);
356		CPYFPN(&x, r);
357	}
358
359	/* p <- pi */
360	p.fp_exp--;
361
362	/*
363	 * if (x > pi)
364	 *  sin(x) is -sin(x - pi)
365	 */
366	CPYFPN(&fe->fe_f1, &x);
367	CPYFPN(&fe->fe_f2, &p);
368	fe->fe_f2.fp_sign = 1;
369	r = fpu_add(fe);
370	if (r->fp_sign == 0) {
371		CPYFPN(&x, r);
372		sign ^= 1;
373	}
374
375	/* p <- pi/2 */
376	p.fp_exp--;
377
378	/*
379	 * if (x > pi/2)
380	 *  sin(x) is cos(x - pi/2)
381	 * else
382	 *  sin(x)
383	 */
384	CPYFPN(&fe->fe_f1, &x);
385	CPYFPN(&fe->fe_f2, &p);
386	fe->fe_f2.fp_sign = 1;
387	r = fpu_add(fe);
388	if (r->fp_sign == 0) {
389		__fpu_sincos_cordic(fe, r);
390		r = &fe->fe_f2;
391	} else {
392		__fpu_sincos_cordic(fe, &x);
393		r = &fe->fe_f1;
394	}
395	r->fp_sign = sign;
396	return r;
397}
398
399/*
400 * tan(x) = sin(x) / cos(x)
401 */
402struct fpn *
403fpu_tan(struct fpemu *fe)
404{
405	struct fpn x;
406	struct fpn s;
407	struct fpn *r;
408
409	if (ISNAN(&fe->fe_f2))
410		return &fe->fe_f2;
411	if (ISINF(&fe->fe_f2))
412		return fpu_newnan(fe);
413
414	/* if x is +0/-0, return +0/-0 */
415	if (ISZERO(&fe->fe_f2))
416		return &fe->fe_f2;
417
418	CPYFPN(&x, &fe->fe_f2);
419
420	/* sin(x) */
421	CPYFPN(&fe->fe_f2, &x);
422	r = fpu_sin(fe);
423	CPYFPN(&s, r);
424
425	/* cos(x) */
426	CPYFPN(&fe->fe_f2, &x);
427	r = fpu_cos(fe);
428	CPYFPN(&fe->fe_f2, r);
429
430	CPYFPN(&fe->fe_f1, &s);
431	r = fpu_div(fe);
432	return r;
433}
434
435struct fpn *
436fpu_sincos(struct fpemu *fe, int regc)
437{
438	__fpu_sincos_cordic(fe, &fe->fe_f2);
439
440	/* cos(x) */
441	fpu_implode(fe, &fe->fe_f2, FTYPE_EXT, &fe->fe_fpframe->fpf_regs[regc * 3]);
442
443	/* sin(x) */
444	return &fe->fe_f1;
445}
446