1/*	$NetBSD: fpu_trig.c,v 1.5 2011/07/18 07:44:30 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.5 2011/07/18 07:44:30 isaki Exp $");
61
62#include "fpu_emulate.h"
63
64static inline struct fpn *fpu_cos_halfpi(struct fpemu *);
65static inline struct fpn *fpu_sin_halfpi(struct fpemu *);
66
67struct fpn *
68fpu_acos(struct fpemu *fe)
69{
70	/* stub */
71	return &fe->fe_f2;
72}
73
74struct fpn *
75fpu_asin(struct fpemu *fe)
76{
77	/* stub */
78	return &fe->fe_f2;
79}
80
81struct fpn *
82fpu_atan(struct fpemu *fe)
83{
84	/* stub */
85	return &fe->fe_f2;
86}
87
88/*
89 * taylor expansion used by sin(), cos(), sinh(), cosh().
90 * hyperb is for sinh(), cosh().
91 */
92struct fpn *
93fpu_sincos_taylor(struct fpemu *fe, struct fpn *s0, u_int f, int hyperb)
94{
95	struct fpn res;
96	struct fpn x2;
97	struct fpn *s1;
98	struct fpn *r;
99	int sign;
100	u_int k;
101
102	/* x2 := x * x */
103	CPYFPN(&fe->fe_f1, &fe->fe_f2);
104	r = fpu_mul(fe);
105	CPYFPN(&x2, r);
106
107	/* res := s0 */
108	CPYFPN(&res, s0);
109
110	sign = 1;	/* sign := (-1)^n */
111
112	for (;;) {
113		/* (f1 :=) s0 * x^2 */
114		CPYFPN(&fe->fe_f1, s0);
115		CPYFPN(&fe->fe_f2, &x2);
116		r = fpu_mul(fe);
117		CPYFPN(&fe->fe_f1, r);
118
119		/*
120		 * for sin(),  s1 := s0 * x^2 / (2n+1)2n
121		 * for cos(),  s1 := s0 * x^2 / 2n(2n-1)
122		 */
123		k = f * (f + 1);
124		fpu_explode(fe, &fe->fe_f2, FTYPE_LNG, &k);
125		s1 = fpu_div(fe);
126
127		/* break if s1 is enough small */
128		if (ISZERO(s1))
129			break;
130		if (res.fp_exp - s1->fp_exp >= FP_NMANT)
131			break;
132
133		/* s0 := s1 for next loop */
134		CPYFPN(s0, s1);
135
136		CPYFPN(&fe->fe_f2, s1);
137		if (!hyperb) {
138			/* (-1)^n */
139			fe->fe_f2.fp_sign = sign;
140		}
141
142		/* res += s1 */
143		CPYFPN(&fe->fe_f1, &res);
144		r = fpu_add(fe);
145		CPYFPN(&res, r);
146
147		f += 2;
148		sign ^= 1;
149	}
150
151	CPYFPN(&fe->fe_f2, &res);
152	return &fe->fe_f2;
153}
154
155/*
156 *           inf   (-1)^n    2n
157 * cos(x) = \sum { ------ * x   }
158 *           n=0     2n!
159 *
160 *              x^2   x^4   x^6
161 *        = 1 - --- + --- - --- + ...
162 *               2!    4!    6!
163 *
164 *        = s0 - s1 + s2  - s3  + ...
165 *
166 *               x*x           x*x           x*x
167 * s0 = 1,  s1 = ---*s0,  s2 = ---*s1,  s3 = ---*s2, ...
168 *               1*2           3*4           5*6
169 *
170 * here 0 <= x < pi/2
171 */
172static inline struct fpn *
173fpu_cos_halfpi(struct fpemu *fe)
174{
175	struct fpn s0;
176
177	/* s0 := 1 */
178	fpu_const(&s0, 0x32);
179
180	return fpu_sincos_taylor(fe, &s0, 1, 0);
181}
182
183/*
184 *          inf    (-1)^n     2n+1
185 * sin(x) = \sum { ------- * x     }
186 *          n=0    (2n+1)!
187 *
188 *              x^3   x^5   x^7
189 *        = x - --- + --- - --- + ...
190 *               3!    5!    7!
191 *
192 *        = s0 - s1 + s2  - s3  + ...
193 *
194 *               x*x           x*x           x*x
195 * s0 = x,  s1 = ---*s0,  s2 = ---*s1,  s3 = ---*s2, ...
196 *               2*3           4*5           6*7
197 *
198 * here 0 <= x < pi/2
199 */
200static inline struct fpn *
201fpu_sin_halfpi(struct fpemu *fe)
202{
203	struct fpn s0;
204
205	/* s0 := x */
206	CPYFPN(&s0, &fe->fe_f2);
207
208	return fpu_sincos_taylor(fe, &s0, 2, 0);
209}
210
211/*
212 * cos(x):
213 *
214 *	if (x < 0) {
215 *		x = abs(x);
216 *	}
217 *	if (x > 2*pi) {
218 *		x %= 2*pi;
219 *	}
220 *	if (x > pi) {
221 *		x -= pi;
222 *		sign inverse;
223 *	}
224 *	if (x > pi/2) {
225 *		y = sin(x - pi/2);
226 *		sign inverse;
227 *	} else {
228 *		y = cos(x);
229 *	}
230 *	if (sign) {
231 *		y = -y;
232 *	}
233 */
234struct fpn *
235fpu_cos(struct fpemu *fe)
236{
237	struct fpn x;
238	struct fpn p;
239	struct fpn *r;
240	int sign;
241
242	fe->fe_fpsr &= ~FPSR_EXCP; /* clear all exceptions */
243
244	if (ISNAN(&fe->fe_f2))
245		return &fe->fe_f2;
246	if (ISINF(&fe->fe_f2))
247		return fpu_newnan(fe);
248
249	CPYFPN(&x, &fe->fe_f2);
250
251	/* x = abs(input) */
252	x.fp_sign = 0;
253	sign = 0;
254
255	/* p <- 2*pi */
256	fpu_const(&p, 0);
257	p.fp_exp++;
258
259	/*
260	 * if (x > 2*pi*N)
261	 *  cos(x) is cos(x - 2*pi*N)
262	 */
263	CPYFPN(&fe->fe_f1, &x);
264	CPYFPN(&fe->fe_f2, &p);
265	r = fpu_cmp(fe);
266	if (r->fp_sign == 0) {
267		CPYFPN(&fe->fe_f1, &x);
268		CPYFPN(&fe->fe_f2, &p);
269		r = fpu_mod(fe);
270		CPYFPN(&x, r);
271	}
272
273	/* p <- pi */
274	p.fp_exp--;
275
276	/*
277	 * if (x > pi)
278	 *  cos(x) is -cos(x - pi)
279	 */
280	CPYFPN(&fe->fe_f1, &x);
281	CPYFPN(&fe->fe_f2, &p);
282	r = fpu_cmp(fe);
283	if (r->fp_sign == 0) {
284		CPYFPN(&fe->fe_f1, &x);
285		CPYFPN(&fe->fe_f2, &p);
286		fe->fe_f2.fp_sign = 1;
287		r = fpu_add(fe);
288		CPYFPN(&x, r);
289
290		sign ^= 1;
291	}
292
293	/* p <- pi/2 */
294	p.fp_exp--;
295
296	/*
297	 * if (x > pi/2)
298	 *  cos(x) is -sin(x - pi/2)
299	 * else
300	 *  cos(x)
301	 */
302	CPYFPN(&fe->fe_f1, &x);
303	CPYFPN(&fe->fe_f2, &p);
304	r = fpu_cmp(fe);
305	if (r->fp_sign == 0) {
306		CPYFPN(&fe->fe_f1, &x);
307		CPYFPN(&fe->fe_f2, &p);
308		fe->fe_f2.fp_sign = 1;
309		r = fpu_add(fe);
310
311		CPYFPN(&fe->fe_f2, r);
312		r = fpu_sin_halfpi(fe);
313		sign ^= 1;
314	} else {
315		CPYFPN(&fe->fe_f2, &x);
316		r = fpu_cos_halfpi(fe);
317	}
318
319	CPYFPN(&fe->fe_f2, r);
320	fe->fe_f2.fp_sign = sign;
321
322	fpu_upd_fpsr(fe, &fe->fe_f2);
323	return &fe->fe_f2;
324}
325
326/*
327 * sin(x):
328 *
329 *	if (x < 0) {
330 *		x = abs(x);
331 *		sign = 1;
332 *	}
333 *	if (x > 2*pi) {
334 *		x %= 2*pi;
335 *	}
336 *	if (x > pi) {
337 *		x -= pi;
338 *		sign inverse;
339 *	}
340 *	if (x > pi/2) {
341 *		y = cos(x - pi/2);
342 *	} else {
343 *		y = sin(x);
344 *	}
345 *	if (sign) {
346 *		y = -y;
347 *	}
348 */
349struct fpn *
350fpu_sin(struct fpemu *fe)
351{
352	struct fpn x;
353	struct fpn p;
354	struct fpn *r;
355	int sign;
356
357	fe->fe_fpsr &= ~FPSR_EXCP; /* clear all exceptions */
358
359	if (ISNAN(&fe->fe_f2))
360		return &fe->fe_f2;
361	if (ISINF(&fe->fe_f2))
362		return fpu_newnan(fe);
363
364	CPYFPN(&x, &fe->fe_f2);
365
366	/* x = abs(input) */
367	sign = x.fp_sign;
368	x.fp_sign = 0;
369
370	/* p <- 2*pi */
371	fpu_const(&p, 0);
372	p.fp_exp++;
373
374	/*
375	 * if (x > 2*pi*N)
376	 *  sin(x) is sin(x - 2*pi*N)
377	 */
378	CPYFPN(&fe->fe_f1, &x);
379	CPYFPN(&fe->fe_f2, &p);
380	r = fpu_cmp(fe);
381	if (r->fp_sign == 0) {
382		CPYFPN(&fe->fe_f1, &x);
383		CPYFPN(&fe->fe_f2, &p);
384		r = fpu_mod(fe);
385		CPYFPN(&x, r);
386	}
387
388	/* p <- pi */
389	p.fp_exp--;
390
391	/*
392	 * if (x > pi)
393	 *  sin(x) is -sin(x - pi)
394	 */
395	CPYFPN(&fe->fe_f1, &x);
396	CPYFPN(&fe->fe_f2, &p);
397	r = fpu_cmp(fe);
398	if (r->fp_sign == 0) {
399		CPYFPN(&fe->fe_f1, &x);
400		CPYFPN(&fe->fe_f2, &p);
401		fe->fe_f2.fp_sign = 1;
402		r = fpu_add(fe);
403		CPYFPN(&x, r);
404
405		sign ^= 1;
406	}
407
408	/* p <- pi/2 */
409	p.fp_exp--;
410
411	/*
412	 * if (x > pi/2)
413	 *  sin(x) is cos(x - pi/2)
414	 * else
415	 *  sin(x)
416	 */
417	CPYFPN(&fe->fe_f1, &x);
418	CPYFPN(&fe->fe_f2, &p);
419	r = fpu_cmp(fe);
420	if (r->fp_sign == 0) {
421		CPYFPN(&fe->fe_f1, &x);
422		CPYFPN(&fe->fe_f2, &p);
423		fe->fe_f2.fp_sign = 1;
424		r = fpu_add(fe);
425
426		CPYFPN(&fe->fe_f2, r);
427		r = fpu_cos_halfpi(fe);
428	} else {
429		CPYFPN(&fe->fe_f2, &x);
430		r = fpu_sin_halfpi(fe);
431	}
432
433	CPYFPN(&fe->fe_f2, r);
434	fe->fe_f2.fp_sign = sign;
435
436	fpu_upd_fpsr(fe, &fe->fe_f2);
437	return &fe->fe_f2;
438}
439
440/*
441 * tan(x) = sin(x) / cos(x)
442 */
443struct fpn *
444fpu_tan(struct fpemu *fe)
445{
446	struct fpn x;
447	struct fpn s;
448	struct fpn *r;
449
450	fe->fe_fpsr &= ~FPSR_EXCP; /* clear all exceptions */
451
452	if (ISNAN(&fe->fe_f2))
453		return &fe->fe_f2;
454	if (ISINF(&fe->fe_f2))
455		return fpu_newnan(fe);
456
457	CPYFPN(&x, &fe->fe_f2);
458
459	/* sin(x) */
460	CPYFPN(&fe->fe_f2, &x);
461	r = fpu_sin(fe);
462	CPYFPN(&s, r);
463
464	/* cos(x) */
465	CPYFPN(&fe->fe_f2, &x);
466	r = fpu_cos(fe);
467	CPYFPN(&fe->fe_f2, r);
468
469	CPYFPN(&fe->fe_f1, &s);
470	r = fpu_div(fe);
471
472	CPYFPN(&fe->fe_f2, r);
473
474	fpu_upd_fpsr(fe, &fe->fe_f2);
475	return &fe->fe_f2;
476}
477
478struct fpn *
479fpu_sincos(struct fpemu *fe, int regc)
480{
481	struct fpn x;
482	struct fpn *r;
483
484	CPYFPN(&x, &fe->fe_f2);
485
486	/* cos(x) */
487	r = fpu_cos(fe);
488	fpu_implode(fe, r, FTYPE_EXT, &fe->fe_fpframe->fpf_regs[regc]);
489
490	/* sin(x) */
491	CPYFPN(&fe->fe_f2, &x);
492	r = fpu_sin(fe);
493	fpu_upd_fpsr(fe, r);
494	return r;
495}
496