floatlib.c

来自「深圳市微逻辑电子有限公司 巨果&#8226 Kingmos&reg 系统核心」· C语言 代码 · 共 969 行 · 第 1/2 页

  fl1.f = a1;
  if (!fl1.l)
    return (0);

  fl1.l ^= SIGNBIT;
  return (fl1.f);
}

/* negate a double */
double __negdf2 (double a1)
{
  register union double_long dl1;

  dl1.d = a1;

  if (!dl1.l.upper && !dl1.l.lower)
      return (dl1.d);

  dl1.l.upper ^= SIGNBIT;
  return (dl1.d);
}

/* convert float to double */
double __extendsfdf2 (float a1)
{
  register union float_long fl1;
  register union double_long dl;
  register int exp;

  fl1.f = a1;

  if (!fl1.l)
    {
      dl.l.upper = dl.l.lower = 0;
      return (dl.d);
    }

  dl.l.upper = SIGN (fl1.l);
  exp = EXP (fl1.l) - EXCESS + EXCESSD;
  dl.l.upper |= exp << 20;
  dl.l.upper |= (MANT (fl1.l) & ~HIDDEN) >> 3;
  dl.l.lower = MANT (fl1.l) << 29;

  return (dl.d);
}

/* convert double to float */
float __truncdfsf2 (double a1)
{
  register int exp;
  register long mant;
  register union float_long fl;
  register union double_long dl1;

  dl1.d = a1;

  if (!dl1.l.upper && !dl1.l.lower)
    return (float)(0);

  exp = EXPD (dl1) - EXCESSD + EXCESS;

  /* shift double mantissa 6 bits so we can round */
  mant = MANTD (dl1) >> 6;

  /* now round and shift down */
  mant += 1;
  mant >>= 1;

  /* did the round overflow? */
  if (mant & 0xFE000000)
    {
      mant >>= 1;
      exp++;
    }

  mant &= ~HIDDEN;

  /* pack up and go home */
  fl.l = PACK (SIGND (dl1), exp, mant);
  return (fl.f);
}

/* compare two doubles */
long __cmpdf2 (double a1, double a2)
{
  register union double_long dl1, dl2;

  dl1.d = a1;
  dl2.d = a2;

  if (SIGND (dl1) && SIGND (dl2))
    {
      dl1.l.upper ^= SIGNBIT;
      dl2.l.upper ^= SIGNBIT;
    }
  if (dl1.l.upper < dl2.l.upper)
    return (-1);
  if (dl1.l.upper > dl2.l.upper)
    return (1);
  if (dl1.l.lower < dl2.l.lower)
    return (-1);
  if (dl1.l.lower > dl2.l.lower)
    return (1);
  return (0);
}

/* convert double to int */
long __fixdfsi (double a1)
{
  register union double_long dl1;
  register int exp;
  register long l;

  dl1.d = a1;

  if (!dl1.l.upper && !dl1.l.lower)
    return (0);

  exp = EXPD (dl1) - EXCESSD - 31;
  l = MANTD (dl1);

  if (exp > 0)
      return SIGND(dl1) ? (1<<31) : ((1ul<<31)-1);

  /* shift down until exp = 0 or l = 0 */
  if (exp < 0 && exp > -32 && l)
    l >>= -exp;
  else
    return (0);

  return (SIGND (dl1) ? -l : l);
}

/* convert double to int */
INT64 __fixdfdi (double a1)
{
    register union double_long dl1;
    register int exp;
    register INT64 l;

    dl1.d = a1;

    if (!dl1.l.upper && !dl1.l.lower)
	return (0);

    exp = EXPD (dl1) - EXCESSD - 64;
    l = MANTD_LL(dl1);

    if (exp > 0) {
	l = (INT64)1<<63;
	if (!SIGND(dl1))
	    l--;
	return l;
    }

    /* shift down until exp = 0 or l = 0 */
    if (exp < 0 && exp > -64 && l)
	{
	    l >>= -exp;  //orgin code
	}
    else
	return (0);

    return (SIGND (dl1) ? -l : l);
}

/* convert double to unsigned int */
unsigned long __fixunsdfsi (double a1)
{
  register union double_long dl1;
  register int exp;
  register unsigned long l;

  dl1.d = a1;

  if (!dl1.l.upper && !dl1.l.lower)
    return (0);

  exp = EXPD (dl1) - EXCESSD - 32;
  l = (((((dl1.l.upper) & 0xFFFFF) | HIDDEND) << 11) | (dl1.l.lower >> 21));

  if (exp > 0)
    return (0xFFFFFFFFul);	/* largest integer */

  /* shift down until exp = 0 or l = 0 */
  if (exp < 0 && exp > -32 && l)
    l >>= -exp;
  else
    return (0);

  return (l);
}

/* convert double to unsigned int */
UINT64 __fixunsdfdi (double a1)
{
    register union double_long dl1;
    register int exp;
    register UINT64 l;

    dl1.d = a1;

    if (dl1.ll == 0)
	return (0);

    exp = EXPD (dl1) - EXCESSD - 64;

    l = dl1.ll;

    if (exp > 0)
	return (UINT64)-1;

    /* shift down until exp = 0 or l = 0 */
    if (exp < 0 && exp > -64 && l)
	l >>= -exp;
    else
	return (0);

    return (l);
}

/* add two doubles */
double __adddf3 (double a1, double a2)
{
    register INT64 mant1, mant2;
    register union double_long fl1, fl2;
    register int exp1, exp2;
    int sign = 0;

    fl1.d = a1;
    fl2.d = a2;

    /* check for zero args */
    if (!fl2.ll)
	goto test_done;
    if (!fl1.ll) {
	fl1.d = fl2.d;
	goto test_done;
    }

    exp1 = EXPD(fl1);
    exp2 = EXPD(fl2);

    if (exp1 > exp2 + 54)
	goto test_done;
    if (exp2 > exp1 + 54) {
	fl1.d = fl2.d;
	goto test_done;
    }

    /* do everything in excess precision so's we can round later */
    mant1 = MANTD_LL(fl1) << 9;
    mant2 = MANTD_LL(fl2) << 9;

    if (SIGND(fl1))
	mant1 = -mant1;
    if (SIGND(fl2))
	mant2 = -mant2;

    if (exp1 > exp2)
	mant2 >>= exp1 - exp2;
    else {
	mant1 >>= exp2 - exp1;
	exp1 = exp2;
    }
    mant1 += mant2;

    if (mant1 < 0) {
	mant1 = -mant1;
	sign = SIGNBIT;
    } else if (!mant1) {
	fl1.d = 0;
	goto test_done;
    }

    /* normalize up */
    while (!(mant1 & ((INT64)7<<61))) {
	mant1 <<= 1;
	exp1--;
    }

    /* normalize down? */
    if (mant1 & ((INT64)3<<62)) {
	mant1 >>= 1;
	exp1++;
    }

    /* round to even */
    mant1 += (mant1 & (1<<9)) ? (1<<8) : ((1<<8)-1);

    /* normalize down? */
    if (mant1 & ((INT64)3<<62)) {
	mant1 >>= 1;
	exp1++;
    }

    /* lose extra precision */
    mant1 >>= 9;

    /* turn off hidden bit */
    mant1 &= ~HIDDEND_LL;

    /* pack up and go home */
    fl1.ll = PACKD_LL(sign,exp1,mant1);

test_done:
    return (fl1.d);
}

/* subtract two doubles */
double __subdf3 (double a1, double a2)
{
    register union double_long fl1, fl2;

    fl1.d = a1;
    fl2.d = a2;

    /* check for zero args */
    if (!fl2.ll)
	return (fl1.d);
    /* twiddle sign bit and add */
    fl2.l.upper ^= SIGNBIT;
    if (!fl1.ll)
	return (fl2.d);
    return __adddf3 (a1, fl2.d);
}

/* multiply two doubles */
double __muldf3 (double a1, double a2)
{
    register union double_long fl1, fl2;
    register UINT64 result;
    register int exp;
    int sign;

    fl1.d = a1;
    fl2.d = a2;

    if (!fl1.ll || !fl2.ll) {
	fl1.d = 0;
	goto test_done;
    }

    /* compute sign and exponent */
    sign = SIGND(fl1) ^ SIGND(fl2);
    exp = EXPD(fl1) - EXCESSD;
    exp += EXPD(fl2);

    fl1.ll = MANTD_LL(fl1);
    fl2.ll = MANTD_LL(fl2);

  /* the multiply is done as one 31x31 multiply and two 31x21 multiples */
    result = (fl1.ll >> 21) * (fl2.ll >> 21);
    result += ((fl1.ll & 0x1FFFFF) * (fl2.ll >> 21)) >> 21;
    result += ((fl2.ll & 0x1FFFFF) * (fl1.ll >> 21)) >> 21;

    result >>= 2;
    if (result & ((INT64)1<<61)) {
	/* round */
	result += 1<<8;
	result >>= 9;
    } else {
	/* round */
	result += 1<<7;
	result >>= 8;
	exp--;
    }
    if (result & (HIDDEND_LL<<1)) {
	result >>= 1;
	exp++;
    }

    result &= ~HIDDEND_LL;

    /* pack up and go home */
    fl1.ll = PACKD_LL(sign,exp,result);
test_done:
    return (fl1.d);
}

/* divide two doubles */
double __divdf3 (double a1, double a2)
{
    register union double_long fl1, fl2;
    register INT64 mask,result;
    register int exp, sign;

    fl1.d = a1;
    fl2.d = a2;

    /* subtract exponents */
    exp = EXPD(fl1) - EXPD(fl2) + EXCESSD;

    /* compute sign */
    sign = SIGND(fl1) ^ SIGND(fl2);

    /* numerator zero??? */
    if (fl1.ll == 0) {
	/* divide by zero??? */
	if (fl2.ll == 0)
	    fl1.ll = ((UINT64)1<<63)-1;	/* NaN */
	else
	    fl1.ll = 0;
	goto test_done;
    }

    /* return +Inf or -Inf */
    if (fl2.ll == 0) {
	fl1.ll = PACKD_LL(SIGND(fl1),2047,0);
	goto test_done;
    }


    /* now get mantissas */
    fl1.ll = MANTD_LL(fl1);
    fl2.ll = MANTD_LL(fl2);

    /* this assures we have 54 bits of precision in the end */
    if (fl1.ll < fl2.ll) {
	fl1.ll <<= 1;
	exp--;
    }

    /* now we perform repeated subtraction of fl2.ll from fl1.ll */
    mask = (INT64)1<<53;
    result = 0;
    while (mask) {
	if (fl1.ll >= fl2.ll)
	{
	    result |= mask;
	    fl1.ll -= fl2.ll;
	}
	fl1.ll <<= 1;
	mask >>= 1;
    }

    /* round */
    result += 1;

    /* normalize down */
    exp++;
    result >>= 1;

    result &= ~HIDDEND_LL;

    /* pack up and go home */
    fl1.ll = PACKD_LL(sign, exp, result);

test_done:
    return (fl1.d);
}

int __gtdf2 (double a1, double a2)
{
    return __cmpdf2 ((float) a1, (float) a2) > 0;
}

int __gedf2 (double a1, double a2)
{
    return (__cmpdf2 ((float) a1, (float) a2) >= 0) - 1;
}

int __ltdf2 (double a1, double a2)
{
    return - (__cmpdf2 ((float) a1, (float) a2) < 0);
}

int __ledf2 (double a1, double a2)
{
    return __cmpdf2 ((float) a1, (float) a2) > 0;
}

int __eqdf2 (double a1, double a2)
{
    return *(INT64 *) &a1 == *(INT64 *) &a2;
}

int __nedf2 (double a1, double a2)
{
    return *(INT64 *) &a1 != *(INT64 *) &a2;
}