/*
* arctan(x):
*
* if (x < 0) {
* x = abs(x);
* sign = 1;
* }
* y = arctan(x);
* if (sign) {
* y = -y;
* }
*/
struct fpn *
fpu_atan(struct fpemu *fe)
{
struct fpn a;
struct fpn x;
struct fpn v;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
CPYFPN(&a, &fe->fe_f2);
if (ISINF(&fe->fe_f2)) {
/* f2 <- pi/2 */
fpu_const(&fe->fe_f2, FPU_CONST_PI);
fe->fe_f2.fp_exp--;
fe->fe_f2.fp_sign = a.fp_sign;
return &fe->fe_f2;
}
fpu_const(&x, FPU_CONST_1);
fpu_const(&fe->fe_f2, FPU_CONST_0);
CPYFPN(&v, &fe->fe_f2);
fpu_cordit1(fe, &x, &a, &fe->fe_f2, &v);
return &fe->fe_f2;
}
/*
* fe_f1 := sin(in)
* fe_f2 := cos(in)
*/
static void
__fpu_sincos_cordic(struct fpemu *fe, const struct fpn *in)
{
struct fpn a;
struct fpn v;
CPYFPN(&a, in);
fpu_const(&fe->fe_f1, FPU_CONST_0);
CPYFPN(&fe->fe_f2, &fpu_cordic_inv_gain1);
fpu_const(&v, FPU_CONST_1);
v.fp_sign = 1;
fpu_cordit1(fe, &fe->fe_f2, &fe->fe_f1, &a, &v);
}
/*
* tan(x) = sin(x) / cos(x)
*/
struct fpn *
fpu_tan(struct fpemu *fe)
{
struct fpn x;
struct fpn s;
struct fpn *r;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
/* if x is +0/-0, return +0/-0 */
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
CPYFPN(&x, &fe->fe_f2);
/* sin(x) */
CPYFPN(&fe->fe_f2, &x);
r = fpu_sin(fe);
CPYFPN(&s, r);
/* cos(x) */
CPYFPN(&fe->fe_f2, &x);
r = fpu_cos(fe);
CPYFPN(&fe->fe_f2, r);
CPYFPN(&fe->fe_f1, &s);
r = fpu_div(fe);
return r;
}
/*
* arccos(x) = pi/2 - arcsin(x)
*/
struct fpn *
fpu_acos(struct fpemu *fe)
{
struct fpn *r;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
r = fpu_asin(fe);
CPYFPN(&fe->fe_f2, r);
/* pi/2 - asin(x) */
fpu_const(&fe->fe_f1, FPU_CONST_PI);
fe->fe_f1.fp_exp--;
fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign;
r = fpu_add(fe);
return r;
}
/*
* x
* arcsin(x) = arctan(---------------)
* sqrt(1 - x^2)
*/
struct fpn *
fpu_asin(struct fpemu *fe)
{
struct fpn x;
struct fpn *r;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
CPYFPN(&x, &fe->fe_f2);
/* x^2 */
CPYFPN(&fe->fe_f1, &fe->fe_f2);
r = fpu_mul(fe);
/* 1 - x^2 */
CPYFPN(&fe->fe_f2, r);
fe->fe_f2.fp_sign = 1;
fpu_const(&fe->fe_f1, FPU_CONST_1);
r = fpu_add(fe);
/* sqrt(1-x^2) */
CPYFPN(&fe->fe_f2, r);
r = fpu_sqrt(fe);
/* x/sqrt */
CPYFPN(&fe->fe_f2, r);
CPYFPN(&fe->fe_f1, &x);
r = fpu_div(fe);
/* arctan */
CPYFPN(&fe->fe_f2, r);
return fpu_atan(fe);
}
static struct fpn *
__fpu_modrem(struct fpemu *fe, int is_mod)
{
static struct fpn X, Y;
struct fpn *x, *y, *r;
uint32_t signX, signY, signQ;
int j, k, l, q;
int cmp;
if (ISNAN(&fe->fe_f1) || ISNAN(&fe->fe_f2))
return fpu_newnan(fe);
if (ISINF(&fe->fe_f1) || ISZERO(&fe->fe_f2))
return fpu_newnan(fe);
CPYFPN(&X, &fe->fe_f1);
CPYFPN(&Y, &fe->fe_f2);
x = &X;
y = &Y;
q = 0;
r = &fe->fe_f2;
/*
* Step 1
*/
signX = x->fp_sign;
signY = y->fp_sign;
signQ = (signX ^ signY);
x->fp_sign = y->fp_sign = 0;
/* Special treatment that just return input value but Q is necessary */
if (ISZERO(x) || ISINF(y)) {
r = &fe->fe_f1;
goto Step7;
}
/*
* Step 2
*/
l = x->fp_exp - y->fp_exp;
k = 0;
CPYFPN(r, x);
if (l >= 0) {
r->fp_exp -= l;
j = l;
/*
* Step 3
*/
for (;;) {
cmp = abscmp3(r, y);
/* Step 3.1 */
if (cmp == 0)
break;
/* Step 3.2 */
if (cmp > 0) {
CPYFPN(&fe->fe_f1, r);
CPYFPN(&fe->fe_f2, y);
fe->fe_f2.fp_sign = 1;
r = fpu_add(fe);
q++;
}
/* Step 3.3 */
if (j == 0)
goto Step4;
/* Step 3.4 */
k++;
j--;
q += q;
r->fp_exp++;
}
/* R == Y */
q++;
r->fp_class = FPC_ZERO;
goto Step7;
}
Step4:
r->fp_sign = signX;
/*
* Step 5
*/
if (is_mod)
goto Step7;
/*
* Step 6
*/
/* y = y / 2 */
y->fp_exp--;
/* abscmp3 ignore sign */
cmp = abscmp3(r, y);
/* revert y */
y->fp_exp++;
if (cmp > 0 || (cmp == 0 && q % 2)) {
q++;
CPYFPN(&fe->fe_f1, r);
CPYFPN(&fe->fe_f2, y);
fe->fe_f2.fp_sign = !signX;
r = fpu_add(fe);
}
/*
* Step 7
*/
Step7:
q &= 0x7f;
q |= (signQ << 7);
fe->fe_fpframe->fpf_fpsr =
fe->fe_fpsr =
(fe->fe_fpsr & ~FPSR_QTT) | (q << 16);
return r;
}
/*
* sin(x):
*
* if (x < 0) {
* x = abs(x);
* sign = 1;
* }
* if (x > 2*pi) {
* x %= 2*pi;
* }
* if (x > pi) {
* x -= pi;
* sign inverse;
* }
* if (x > pi/2) {
* y = cos(x - pi/2);
* } else {
* y = sin(x);
* }
* if (sign) {
* y = -y;
* }
*/
struct fpn *
fpu_sin(struct fpemu *fe)
{
struct fpn x;
struct fpn p;
struct fpn *r;
int sign;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
/* if x is +0/-0, return +0/-0 */
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
CPYFPN(&x, &fe->fe_f2);
/* x = abs(input) */
sign = x.fp_sign;
x.fp_sign = 0;
/* p <- 2*pi */
fpu_const(&p, FPU_CONST_PI);
p.fp_exp++;
/*
* if (x > 2*pi*N)
* sin(x) is sin(x - 2*pi*N)
*/
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
r = fpu_cmp(fe);
if (r->fp_sign == 0) {
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
r = fpu_mod(fe);
CPYFPN(&x, r);
}
/* p <- pi */
p.fp_exp--;
/*
* if (x > pi)
* sin(x) is -sin(x - pi)
*/
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
fe->fe_f2.fp_sign = 1;
r = fpu_add(fe);
if (r->fp_sign == 0) {
CPYFPN(&x, r);
sign ^= 1;
}
/* p <- pi/2 */
p.fp_exp--;
/*
* if (x > pi/2)
* sin(x) is cos(x - pi/2)
* else
* sin(x)
*/
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
fe->fe_f2.fp_sign = 1;
r = fpu_add(fe);
if (r->fp_sign == 0) {
__fpu_sincos_cordic(fe, r);
r = &fe->fe_f2;
} else {
__fpu_sincos_cordic(fe, &x);
r = &fe->fe_f1;
}
r->fp_sign = sign;
return r;
}
