1 /* $NetBSD: fpu_trig.c,v 1.6 2011/10/15 15:14:30 tsutsui 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.6 2011/10/15 15:14:30 tsutsui Exp $"); 61 62 #include "fpu_emulate.h" 63 64 static inline struct fpn *fpu_cos_halfpi(struct fpemu *); 65 static inline struct fpn *fpu_sin_halfpi(struct fpemu *); 66 67 struct fpn * 68 fpu_acos(struct fpemu *fe) 69 { 70 /* stub */ 71 return &fe->fe_f2; 72 } 73 74 struct fpn * 75 fpu_asin(struct fpemu *fe) 76 { 77 /* stub */ 78 return &fe->fe_f2; 79 } 80 81 struct fpn * 82 fpu_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 */ 92 struct fpn * 93 fpu_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 */ 172 static inline struct fpn * 173 fpu_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 */ 200 static inline struct fpn * 201 fpu_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 */ 234 struct fpn * 235 fpu_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 */ 349 struct fpn * 350 fpu_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 */ 443 struct fpn * 444 fpu_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 478 struct fpn * 479 fpu_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