singrdk/base/Kernel/Native/ix86/Math.cpp

851 lines
22 KiB
C++

////////////////////////////////////////////////////////////////////////////////
//
// Microsoft Research Singularity
//
// Copyright (c) Microsoft Corporation. All rights reserved.
//
// File: Math.cpp
//
// Note:
//
#define LITTL_ENDIAN
#include "hal.h"
#pragma warning(disable: 4725)
//////////////////////////////////////////////////////////////////////////////
//
#define IMCW_EM 0x003f // interrupt Exception Masks
#define IEM_INVALID 0x0001 // invalid
#define IEM_DENORMAL 0x0002 // denormal
#define IEM_ZERODIVIDE 0x0004 // zero divide
#define IEM_OVERFLOW 0x0008 // overflow
#define IEM_UNDERFLOW 0x0010 // underflow
#define IEM_PRECISION 0x0020 // precision
#define IMCW_RC 0x0c00 // Rounding Control
#define IRC_CHOP 0x0c00 // chop
#define IRC_UP 0x0800 // up
#define IRC_DOWN 0x0400 // down
#define IRC_NEAR 0x0000 // near
#define ISW_INVALID 0x0001 // invalid
#define ISW_DENORMAL 0x0002 // denormal
#define ISW_ZERODIVIDE 0x0004 // zero divide
#define ISW_OVERFLOW 0x0008 // overflow
#define ISW_UNDERFLOW 0x0010 // underflow
#define ISW_PRECISION 0x0020 // inexact
#define IMCW_PC 0x0300 // Precision Control
#define IPC_24 0x0000 // 24 bits
#define IPC_53 0x0200 // 53 bits
#define IPC_64 0x0300 // 64 bits
//////////////////////////////////////////////////////////////////////////////
//
#ifdef BIG_ENDIAN
// big endian
#define D_EXP(x) ((unsigned short *)&(x))
#define D_HI(x) ((unsigned long *)&(x))
#define D_LO(x) ((unsigned long *)&(x)+1)
#else
#define D_EXP(x) ((unsigned short *)&(x)+3)
#define D_HI(x) ((unsigned long *)&(x)+1)
#define D_LO(x) ((unsigned long *)&(x))
#endif
#define D_BIASM1 0x3fe // off by one to compensate for the implied bit
#define MAXEXP 1024
#define MINEXP -1021
// return the int representation of the exponent
// if x = .f * 2^n, 0.5<=f<1, return n (unbiased)
// e.g. INTEXP(3.0) == 2
#define INTEXP(x) ((signed short)((*D_EXP(x) & 0x7ff0) >> 4) - D_BIASM1)
// check for infinity, NAN
#define D_ISINF(x) ((*D_HI(x) & 0x7fffffff) == 0x7ff00000 && *D_LO(x) == 0)
#define IS_D_SPECIAL(x) ((*D_EXP(x) & 0x7ff0) == 0x7ff0)
#define IS_D_NAN(x) (IS_D_SPECIAL(x) && !D_ISINF(x))
#define IS_D_QNAN(x) ((*D_EXP(x) & 0x7ff8) == 0x7ff8)
#define IS_D_SNAN(x) ((*D_EXP(x) & 0x7ff8) == 0x7ff0 && \
(*D_HI(x) << 13 || *D_LO(x)))
#define IS_D_DENORM(x) ((*D_EXP(x) & 0x7ff0) == 0 && \
(*D_HI(x) << 12 || *D_LO(x)))
#define IS_D_INF(x) (*D_HI(x) == 0x7ff00000 && *D_LO(x) == 0)
#define IS_D_MINF(x) (*D_HI(x) == 0xfff00000 && *D_LO(x) == 0)
/////////////////////////////////////////////////////// Define special values.
//
typedef union {
uint64 lng;
float64 dbl;
} _dbl;
static _dbl _d_pos_inf = { 0x7ff0000000000000 }; // positive infinity
static _dbl _d_neg_inf = { 0xfff0000000000000 }; // negative infinity
static _dbl _d_ind = { 0xfff8000000000000 }; // real indefinite
static _dbl _d_neg_zer = { 0x8000000000000000 }; // negative zero
//////////////////////////////////////////////////////////////////////////////
//
__declspec(naked) uint16 __fastcall g_ClearFp()
{
__asm {
push ebp
mov ebp, esp
// Save the old SW
fnstsw [esp-4];
fnclex
mov eax, [esp-4];
and eax, 0xffff;
pop ebp;
ret; // Returns result in EAX
}
}
// Doesn't alter any additional registers
__declspec(naked) uint16 __fastcall g_ControlFp(uint16 newctrl, uint16 mask)
{
(void)newctrl; (void)mask; // accessed directly via ecx, edx respectively.
__asm {
push ebp
mov ebp, esp
// Save the old CW
fstcw [esp-4];
mov eax, [esp-4];
and eax, 0xffff;
// Load the new CW
and ecx,edx;
not edx;
and edx,eax;
or edx,ecx;
mov [esp-4], edx;
fldcw [esp-4];
pop ebp
ret;
}
}
// Doesn't alter any additional registers
__declspec(naked) void __fastcall g_RestoreFp(uint16 newctrl)
{
(void)newctrl; // accessed directly via ecx.
__asm {
push ebp
mov ebp, esp
// Load the new CW
and ecx, 0xffff;
mov [esp-4], ecx;
fldcw [esp-4];
pop ebp
ret;
}
}
//////////////////////////////////////////////////////////////////////////////
//
static float64 __fastcall _frnd(float64 v)
{
_asm {
fld v;
frndint;
}
}
static int32 __fastcall _ftoi(float64 v)
{
int32 intval;
int32 oldcw;
int32 newcw;
_asm {
fstcw [oldcw]; // get control word
mov eax, [oldcw]; // round mode saved
or eax, IRC_CHOP; // set chop rounding mode
mov [newcw], eax; // back to memory
fldcw [newcw]; // reset rounding
fld v;
fistp [intval]; // store chopped integer
fwait;
fldcw [oldcw]; // restore rounding
}
return intval;
}
static float64 _abs(float64 x)
{
(*(uint64 *)&x) &= 0x7fffffffffffffff;
return x;
}
static float64 _set_exp(float64 x, int exp) // does not check validity of exp
{
float64 retval = x;
int biased_exp = exp + D_BIASM1;
*D_EXP(retval) = (unsigned short) (*D_EXP(x) & 0x800f | (biased_exp << 4));
return retval;
}
int _get_exp(float64 x)
{
signed short exp;
exp = (signed short)((*D_EXP(x) & 0x7ff0) >> 4);
exp -= D_BIASM1; //unbias
return (int) exp;
}
// Provide the mantissa and the exponent of e^x
//
// Entry:
// x : a (non special) float64 precision number
//
// Exit:
// *newexp: the exponent of e^x
// return value: the mantissa m of e^x scaled by a factor
// (the value of this factor has no significance.
// The mantissa can be obtained with _set_exp(m, 0).
//
// _set_exp(m, *pnewexp) may be used for constructing the final
// result, if it is within the representable range.
//
static inline float64 r_exp_p(float64 z)
{
static float64 const p0 = 0.249999999999999993e+0;
static float64 const p1 = 0.694360001511792852e-2;
static float64 const p2 = 0.165203300268279130e-4;
return ( (p2 * (z) + p1) * (z) + p0 );
}
static inline float64 r_exp_q(float64 z)
{
static float64 const q0 = 0.500000000000000000e+0;
static float64 const q1 = 0.555538666969001188e-1;
static float64 const q2 = 0.495862884905441294e-3;
return ( (q2 * (z) + q1) * (z) + q0 );
}
float64 _exphlp(float64 x, int * pnewexp)
{
static float64 const LN2INV = 1.442695040889634074; // 1/ln(2)
static float64 const C1 = 0.693359375000000000;
static float64 const C2 = -2.1219444005469058277e-4;
float64 xn;
float64 g,z,gpz,qz,rg;
int n;
xn = _frnd(x * LN2INV);
n = _ftoi(xn);
// assume guard digit is present
g = (x - xn * C1) - xn * C2;
z = g*g;
gpz = g * r_exp_p(z);
qz = r_exp_q(z);
rg = 0.5 + gpz/(qz-gpz);
n++;
*pnewexp = _get_exp(rg) + n;
return rg;
}
//
// Decompose a number to a normalized mantissa and exponent.
//
static float64 _decomp(float64 x, int *pexp)
{
int exp;
float64 man;
if (x == 0.0) {
man = 0.0;
exp = 0;
}
else if (IS_D_DENORM(x)) {
int neg;
exp = 1 - D_BIASM1;
neg = x < 0.0;
while((*D_EXP(x) & 0x0010) == 0) {
// shift mantissa to the left until bit 52 is 1
(*D_HI(x)) <<= 1;
if (*D_LO(x) & 0x80000000)
(*D_HI(x)) |= 0x1;
(*D_LO(x)) <<= 1;
exp--;
}
(*D_EXP(x)) &= 0xffef; // clear bit 52
if (neg) {
(*D_EXP(x)) |= 0x8000; // set sign bit
}
man = _set_exp(x,0);
}
else {
man = _set_exp(x,0);
exp = INTEXP(x);
}
*pexp = exp;
return man;
}
static bool is_odd_integer(float64 y)
{
int exp = INTEXP(y);
if (exp < 1 || exp > 63) {
return false;
}
return(((*(uint64*)&y) | 0x10000000000000u) << (10 + exp) == 0x8000000000000000);
}
static bool is_even_integer(float64 y)
{
int exp = INTEXP(y);
if (exp < 1 || exp > 63) {
return false;
}
return (((*(uint64*)&y) | 0x10000000000000u) << (10 + exp) == 0);
}
//////////////////////////////////////////////////////////////////////////////
float64 Class_System_Math::g_Atan2(float64 v, float64 w)
{
__asm {
fld v;
fld w;
fpatan;
}
}
float64 Class_System_Math::g_Abs(float64 v)
{
__asm {
fld v;
fabs;
}
}
float64 Class_System_Math::g_Sqrt(float64 v)
{
__asm {
fld v;
fsqrt;
}
}
float64 Class_System_Math::g_Log10(float64 v)
{
__asm {
fld v;
fldlg2;
fxch ST(1);
fyl2x; // Returns result in ST(0)
}
}
float64 Class_System_Math::g_Exp(float64 v)
{
__asm {
fldl2e;
fmul v;
fld ST(0);
frndint;
fxch ST(1);
fsub ST(0), ST(1);
f2xm1;
fld1;
faddp ST(1), ST(0);
fscale;
fstp ST(1); // Returns result in ST(0)
}
}
// constants for the rational approximation
static inline float64 r_sinh_p(float64 f)
{
static float64 const p0 = -0.35181283430177117881e+6;
static float64 const p1 = -0.11563521196851768270e+5;
static float64 const p2 = -0.16375798202630751372e+3;
static float64 const p3 = -0.78966127417357099479e+0;
return (((p3 * (f) + p2) * (f) + p1) * (f) + p0);
}
static inline float64 r_sinh_q(float64 f)
{
static float64 const q0 = -0.21108770058106271242e+7;
static float64 const q1 = 0.36162723109421836460e+5;
static float64 const q2 = -0.27773523119650701667e+3;
// q3 = 1 is not used (avoid multiplication by 1)
return ((((f) + q2) * (f) + q1) * (f) + q0);
}
// Compute the hyperbolic sine of a number.
// The algorithm (reduction / rational approximation) is
// taken from Cody & Waite.
float64 Class_System_Math::g_Sinh(float64 v)
{
static float64 const EPS = 5.16987882845642297e-26; // 2^(-53) / 2
// exp(YBAR) should be close to but less than XMAX
// and 1/exp(YBAR) should not underflow
static float64 const YBAR = 7.00e2;
// WMAX=ln(OVFX)+0.69 (Cody & Waite),omitted LNV, used OVFX instead of BIGX
static float64 const WMAX = 1.77514678223345998953e+003;
float64 result;
if (IS_D_SPECIAL(v)) {
if (IS_D_INF(v) || IS_D_MINF(v)) {
}
else if (IS_D_QNAN(v)) {
// TODO: should throw a soft exception.
}
else if (IS_D_SNAN(v)) {
// TODO: should throw a hard exception.
}
result = v;
}
else if (v == 0.0) {
// no precision exception
result = v;
}
else {
bool neg = (v < 0.0);
float64 y = _abs(v);
if (y > 1.0) {
int newexp;
if (y > YBAR) {
if (y > WMAX) {
// result too large, even after scaling
result = v * _d_pos_inf.dbl;
// TODO: should through hard exception.
goto exit;
}
//
// result = exp(y)/2
//
result = _exphlp(y, &newexp);
newexp --; //divide by 2
if (newexp > MAXEXP) {
result = 0.0; //result = _set_exp(result, newexp-IEEE_ADJUST);
// TODO: should through hard exception.
goto exit;
}
else {
result = _set_exp(result, newexp);
}
}
else {
float64 z = _exphlp(y, &newexp);
z = _set_exp(z, newexp);
result = (z - 1.0/z) / 2.0;
}
if (neg) {
result = -result;
}
}
else {
if (y < EPS) {
result = v;
if (IS_D_DENORM(result)) {
result = 0.0; // result = _add_exp(result, IEEE_ADJUST);
// TODO: should through hard exception.
goto exit;
}
}
else {
float64 f = v * v;
float64 r = f * (r_sinh_p(f) / r_sinh_q(f));
result = v + v * r;
}
}
}
exit:
return result;
}
// Compute the hyperbolic cosine of a number.
// The algorithm (reduction / rational approximation) is
// taken from Cody & Waite.
//
float64 Class_System_Math::g_Cosh(float64 v)
{
// exp(YBAR) should be close to but less than XMAX
// and 1/exp(YBAR) should not underflow
static float64 const YBAR = 7.00e2;
// WMAX=ln(OVFX)+0.69 (Cody & Waite),omitted LNV, used OVFX instead of BIGX
static float64 const WMAX = 1.77514678223345998953e+003;
float64 result;
if (IS_D_SPECIAL(v)) {
if (IS_D_INF(v) || IS_D_MINF(v)) {
result = _d_pos_inf.dbl;
}
else if (IS_D_QNAN(v)) {
// TODO: should throw a soft exception.
result = v;
}
else {
// TODO: should throw a hard exception.
result = v;
}
}
else if (v == 0.0) {
result = 1.0;
}
else {
float64 y = _abs(v);
if (y > YBAR) {
if (y > WMAX) {
// TODO: should throw a hard exception.
result = v;
goto exit;
}
//
// result = exp(y)/2
//
int newexp;
result = _exphlp(y, &newexp);
newexp --; //divide by 2
if (newexp > MAXEXP) {
// TODO: should throw a hard exception.
result = 0.0; //result = _set_exp(result, newexp-IEEE_ADJUST);
goto exit;
}
else {
result = _set_exp(result, newexp);
}
}
else {
int newexp;
float64 z = _exphlp(y, &newexp);
z = _set_exp(z, newexp);
result = (z + 1.0/z) / 2.0;
}
// TODO: should throw a hard exception if exactness is required.
}
exit:
return result;
}
// Compute the hyperbolic tangent of a number.
// The algorithm (reduction / rational approximation) is
// taken from Cody & Waite.
static inline float64 r_tanh(float64 g)
{
// constants for rational approximation
static float64 const p0 = -0.16134119023996228053e+4;
static float64 const p1 = -0.99225929672236083313e+2;
static float64 const p2 = -0.96437492777225469787e+0;
static float64 const q0 = 0.48402357071988688686e+4;
static float64 const q1 = 0.22337720718962312926e+4;
static float64 const q2 = 0.11274474380534949335e+3;
static float64 const q3 = 0.10000000000000000000e+1;
return ((((p2 * (g) + p1) * (g) + p0) * (g)) / ((((g) + q2) * (g) + q1) * (g) + q0));
}
float64 Class_System_Math::g_Tanh(float64 v)
{
// constants
static float64 const EPS = 5.16987882845642297e-26; // 2^(-53) / 2
static float64 const XBIG = 1.90615474653984960096e+001; // ln(2)(53+2)/2
static float64 const C0 = 0.54930614433405484570; // ln(3)/2
if (IS_D_SPECIAL(v)) {
if (IS_D_INF(v)) {
v = 1.0;
}
else if (IS_D_MINF(v)) {
v = -1.0;
}
else if (IS_D_QNAN(v)) {
// TODO: should throw a soft exception.
}
else if (IS_D_SNAN(v)) {
// TODO: should throw a hard exception.
}
}
else if (v == 0.0) {
// no precision exception
}
else {
bool neg = false;
if (v < 0.0) {
neg = true;
v = -v;
}
if (v > XBIG) {
v = 1;
}
else if (v > C0) {
v = 0.5 - 1.0 / (g_Exp(v+v) + 1.0);
v = v + v;
}
else if (v < EPS) {
if (IS_D_DENORM(v)) {
// TODO: should throw a heard exception.
}
}
else {
v = v + v * r_tanh(v * v);
}
if (neg) {
v = -v;
}
}
return v;
}
float64 Class_System_Math::g_Acos(float64 v)
{
__asm {
fld v;
fld1; // load 1.0
fadd st, st(1); // 1+x
fld1; // load 1.0
fsub st, st(2); // 1-x
fmul; // (1+x)(1-x)
fsqrt; // sqrt((1+x)(1-x))
fxch;
fpatan; // fpatan(x,sqrt((1+x)(1-x)))
}
}
float64 Class_System_Math::g_Asin(float64 v)
{
__asm {
fld v;
fld1; // load 1.0
fadd st, st(1); // 1+x
fld1; // load 1.0
fsub st, st(2); // 1-x
fmul; // (1+x)(1-x)
fsqrt; // sqrt((1+x)(1-x))
fpatan; // fpatan(x,sqrt((1+x)(1-x)))
}
}
static float64 _fastpow(float64 v, float64 w)
{
__asm {
fld w; // neither v or w can be a boundary cases.
fld v;
fyl2x; // compute y*log2(x)
fld st(0); // duplicate stack top
frndint; // N = round(y)
fsubr st(1), st;
fxch;
fchs; // g = y - N where abs(g) < 1
f2xm1; // 2**g - 1
fld1;
fadd; // 2**g
fscale; // (2**g) * (2**N) - gives 2**y
fstp st(1); // pop extra stuff from fp stack
}
}
float64 Class_System_Math::g_Pow(float64 v, float64 w)
{
float64 result = 0.0;
// check for infinity or NAN
if (IS_D_SPECIAL(w) || IS_D_SPECIAL(v)) {
if (IS_D_INF(w)) {
float64 absv = _abs(v);
if (absv > 1.0) {
result = _d_pos_inf.dbl;
}
else if (absv < 1.0) {
result = 0.0;
}
else {
result = _d_ind.dbl;
// TODO: Should throw a hard exception.
goto exit;
}
}
else if (IS_D_MINF(w)) {
float64 absv = _abs(v);
if (absv > 1.0) {
result = 0.0;
}
else if (absv < 1.0) {
result = _d_pos_inf.dbl;
}
else {
result = _d_ind.dbl;
// TODO: Should throw a hard exception.
goto exit;
}
}
else if (IS_D_INF(v)) {
if (w > 0.0) {
result = _d_pos_inf.dbl;
}
else if (w < 0.0) {
result = 0.0;
}
else {
result = 1.0;
}
}
else if (IS_D_MINF(v)) {
if (w > 0.0) {
result = is_odd_integer(w) ? _d_neg_inf.dbl : _d_pos_inf.dbl;
}
else if (w < 0.0) {
result = is_odd_integer(w) ? _d_neg_zer.dbl : 0.0;
}
else {
result = 1.0;
}
}
}
else if (w == 0.0) {
result = 1.0;
}
else if (v == 0.0) {
if (w < 0.0) {
// TODO: Should throw a hard exception.
result = is_odd_integer(v) ? _d_neg_inf.dbl : _d_pos_inf.dbl;
}
else {
result = is_odd_integer(v) ? w : 0.0;
}
}
else if (v < 0.0) {
if (is_odd_integer(w)) {
result = - _fastpow(-v, w);
}
else if (is_even_integer(w)) {
result = _fastpow(-v, w);
}
else {
// TODO: Should throw a hard exception.
result = v;
}
}
else {
result = _fastpow(v, w);
}
exit:
return result;
}
float64 Class_System_Math::g_Mod(float64 x, float64 y)
{
float64 result = 0.0;
// check for infinity or NAN
if (IS_D_SPECIAL(y) || IS_D_SPECIAL(x)) {
if (IS_D_SNAN(y) || IS_D_SNAN(x)) {
// TODO: Should throw a hard exception.
}
else if (IS_D_QNAN(y) || IS_D_QNAN(x)) {
// TODO: Should throw a soft exception.
result = x;
}
else if (IS_D_INF(x) || IS_D_MINF(x)) {
// TODO: Should throw a hard exception.
}
}
else if (y == 0.0) {
// TODO: Should throw a hard exception.
}
else if (x == 0.0) {
result = x;
}
else {
const int SCALE = 53;
bool neg = false;
bool denorm = false;
float64 d,ty,fx,fy;
int nx, ny, nexp;
if (x < 0.0) {
result = -x;
neg = 1;
}
else {
result = x;
}
ty = _abs(y);
while (result >= ty) {
fx = _decomp(result, &nx);
fy = _decomp(ty, &ny);
if (nx < MINEXP) {
// result is a denormalized number
denorm = true;
nx += SCALE;
ny += SCALE;
result = _set_exp(fx, nx);
ty = _set_exp(fy, ny);
}
if (fx >= fy) {
nexp = nx ;
}
else {
nexp = nx - 1;
}
d = _set_exp(fy, nexp);
result -= d;
}
if (denorm) {
// TODO: should raise only FP_U exception.
}
if (neg) {
result = -result;
}
}
return result;
}
//
///////////////////////////////////////////////////////////////// End of File.