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 */
67 struct fpn *
fpu_acos(struct fpemu * fe)68 fpu_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 */
94 struct fpn *
fpu_asin(struct fpemu * fe)95 fpu_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 */
146 struct fpn *
fpu_atan(struct fpemu * fe)147 fpu_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 */
182 static void
__fpu_sincos_cordic(struct fpemu * fe,const struct fpn * in)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 */
219 struct fpn *
fpu_cos(struct fpemu * fe)220 fpu_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 */
319 struct fpn *
fpu_sin(struct fpemu * fe)320 fpu_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 */
402 struct fpn *
fpu_tan(struct fpemu * fe)403 fpu_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
435 struct fpn *
fpu_sincos(struct fpemu * fe,int regc)436 fpu_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