math16f.h

FreeRTOS 是一个源码公开的免费的嵌入式实时操作系统

// *************************************************
// 16 bit basic floating point math operations
// Copyright (c) B Knudsen Data, Norway, 2000 - 2005
// *************************************************

#pragma library 1
/* PROTOTYPES for page definition in application header file:
float16 operator* _fmul16( float16 arg1f16, float16 arg2f16);
float16 operator/ _fdiv16( float16 arg1f16, float16 arg2f16);
float16 operator+ _fadd16( float16 arg1f16, float16 arg2f16);
float16 operator- _fsub16( float16 arg1f16, float16 arg2f16);
float16 operator= _int32ToFloat16( int32 arg32);
int16 operator= _float16ToInt16( float16 arg1f16);
bit operator< _f16_LT_f16( float16 arg1f16, float16 arg2f16);
bit operator>= _f16_GE_f16( float16 arg1f16, float16 arg2f16);
bit operator> _f16_GT_f16( float16 arg1f16, float16 arg2f16);
bit operator<= _f16_LE_f16( float16 arg1f16, float16 arg2f16);
*/

// DEFINABLE SYMBOLS (in the application code):
//#define FP_OPTIM_SPEED  // optimize for SPEED: default
//#define FP_OPTIM_SIZE   // optimize for SIZE
//#define DISABLE_ROUNDING   // disable rounding and save code space

/*  16 bit floating point format:

  address  ID
    X      a.low8  : LSB, bit 0-6 of mantissa, bit 7 is the sign bit
    X+1    a.high8 : MSB, bit 0-7 of exponent, with bias 0x7F

    bit 7 of mantissa is a hidden bit, always equal to 1
    zero (0.0) :  a.high8 = 0 (mantissa & sign ignored)

   MSB LSB
    7F 00 : 1.0   =  1.0  * 2**(0x7F-0x7F) = 1.0 * 1
    7F 80 : -1.0  = -1.0  * 2**(0x7F-0x7F) = -1.0 * 1
    80 00 : 2.0   =  1.0  * 2**(0x80-0x7F) = 1.0 * 2
    80 40 : 3.0   =  1.5  * 2**(0x80-0x7F) = 1.5 * 2
    7E 60 : 0.875 =  1.75 * 2**(0x7E-0x7F) = 1.75 * 0.5
    7F 60 : 1.75  =  1.75 * 2**(0x7E-0x7F) = 1.75 * 1
    7F 7F : 1.9921875
    00 7C : 0.0 (mantissa & sign ignored)
    01 00 : 1.175494-38 =  1.0 * 2**(0x01-0x7F)
    FE 7F : 3.389531+38 =  1.9921875 * 2**(0xFE-0x7F)
    FF 00 : +INF : positive infinity
    FF 80 : -INF : negative infinity
*/                 

#define  FpBIAS  0x7F

#ifndef FpFlags_defined
 #define FpFlags_defined

 char FpFlags;
 //bit IOV         @ FpFlags.0; // integer overflow flag: NOT USED
 bit FpOverflow    @ FpFlags.1; // floating point overflow flag
 bit FpUnderFlow   @ FpFlags.2; // floating point underflow flag
 bit FpDiv0        @ FpFlags.3; // floating point divide by zero flag
 //bit FpNAN       @ FpFlags.4; // not-a-number exception flag: NOT USED
 bit FpDomainError @ FpFlags.5; // domain error exception flag
 bit FpRounding    @ FpFlags.6; // floating point rounding flag, 0=truncation
                                // 1 = unbiased rounding to nearest LSB
 //bit FpSaturate  @ FpFlags.7; // floating point saturate flag: NOT USED

 #pragma floatOverflow FpOverflow
 #pragma floatUnderflow FpUnderFlow

 #define InitFpFlags()  FpFlags = 0x40 /* enable rounding as default */
#endif

#ifdef DISABLE_ROUNDING
 #pragma floatRounding 0
#endif


#if __CoreSet__ < 1600
 #define genAdd(r,a) W=a; btsc(Carry); W=incsz(a); r+=W;
 #define genSub(r,a) W=a; btss(Carry); W=incsz(a); r-=W;
 #define genAddW(r,a) W=a; btsc(Carry); W=incsz(a); W=r+W;
 #define genSubW(r,a) W=a; btss(Carry); W=incsz(a); W=r-W;
#else
 #define genAdd(r,a) W=a; r=addWFC(r);
 #define genSub(r,a) W=a; r=subWFB(r);
 #define genAddW(r,a) W=a; W=addWFC(r);
 #define genSubW(r,a) W=a; W=subWFB(r);
#endif


float16 operator* _fmul16( sharedM float16 arg1f16, sharedM float16 arg2f16)
{
    uns8 aarg;
    W = arg1f16.low8;
    aarg = W;

    // save sign
    char sign = arg2f16.low8 ^ W;  // before first overflow test

    W = arg1f16.high8;
   #if __CoreSet__ / 100 == 17
    if (W != 0)
        W = arg2f16.high8;
    if (W == 0)
        goto RES0;
   #else
    if (!Zero_)
        W = arg2f16.high8;
    if (Zero_)
        goto RES0;
   #endif

    arg1f16.high8 += W /* arg2f16.high8 */;
    W = FpBIAS-1;
    if (Carry)  {
        arg1f16.high8 -= W;
        if (Carry)
            goto OVERFLOW;
    }
    else  {
        arg1f16.high8 -= W;
        if (!Carry)
            goto UNDERFLOW;
    }


    aarg.7 = 1;
    arg2f16.7 = 1;

   #if __CoreSet__ == 1700 || __CoreSet__ == 1800

    W = arg2f16.low8;
    multiply( aarg);
    arg1f16.low8 = PRODH;

    if (!arg1f16.7)  {
        PRODL = rl(PRODL);
        arg1f16.low8 = rl( arg1f16.low8);
        if (arg1f16.high8 == 0)
            goto UNDERFLOW;
        arg1f16.high8 -= 1;
        W = rl( PRODL);  // restore bit behind LSB in Carry
    }

   #elif __CoreSet__ == 2000

    char tmpL = arg2f16.low8 * aarg;
    arg1f15.low8 = MULH;

    if (!arg1f16.7)  {
        tmpL = rl(tmpL);
        arg1f16.low8 = rl( arg1f16.low8);
        if (arg1f16.high8 == 0)
            goto UNDERFLOW;
        arg1f16.high8 -= 1;
        W = rl( tmpL);  // restore bit behind LSB in Carry
    }

   #else

    arg1f16.low8 = 0;

    char counter = sizeof(aarg)*8;

    W = arg2f16.low8;
    do  {
        aarg = rr( aarg);
        if (Carry)
            arg1f16.low8 += W;
        arg1f16.low8 = rr( arg1f16.low8);
        counter = decsz(counter);
    } while (1);

    if (!arg1f16.7)  {
        // catch Carry bit that was shifted out previously
        arg1f16.low8 = rl( arg1f16.low8);
        if (arg1f16.high8 == 0)
            goto UNDERFLOW;
        arg1f16.high8 -= 1;
        W = rl( aarg);  // restore bit behind LSB in Carry
    }

   #endif

   #ifndef DISABLE_ROUNDING
    if (FpRounding  &&  Carry)  {
        arg1f16.low8 += 1;
        if (!arg1f16.low8)  {
           #if __CoreSet__ >= 1700 && __CoreSet__ <= 1800
            Carry = 1; // previous INCF changes Carry
           #else
            // Carry = 1; //OK
           #endif
            arg1f16.low8 = rr( arg1f16.low8);
            arg1f16.high8 += 1;
            if (Zero_)
                goto OVERFLOW;
        }
    }
   #endif
    goto SET_SIGN;

  UNDERFLOW:
    FpUnderFlow = 1;
  RES0:
    arg1f16.high8 = 0;
    goto MANTISSA;

  OVERFLOW:
    FpOverflow = 1;
    arg1f16.high8 = 0xFF;
  MANTISSA:
    arg1f16.low8 = 0x80;

  SET_SIGN:
    if (!(sign & 0x80))
        arg1f16.7 = 0;
    return arg1f16;
}



float16 operator/ _fdiv16( sharedM float16 arg1f16, sharedM float16 arg2f16)
{
    uns8 aarg;
    W = arg1f16.low8;
    aarg = W;

    // save sign
    char sign = arg2f16.low8 ^ W;  // before first overflow test

   #if __CoreSet__ / 100 == 17
    if (!arg2f16.high8)
        goto Div0;
   #else
    W = arg2f16.high8;
    if (Zero_)
        goto Div0;
   #endif
    if (!arg1f16.high8)
        goto RES0;

    arg1f16.high8 -= arg2f16.high8;
    W = FpBIAS;
    if (!Carry)  {
        arg1f16.high8 += W;
        if (!Carry)
            goto UNDERFLOW;
    }
    else  {
        arg1f16.high8 += W;
        if (Carry)
            goto OVERFLOW;
    }


    aarg.7 = 1;
    arg2f16.7 = 1;

    // division: shift & add
    char counter = 8;
    arg1f16.low8 = 0;  // speedup


#if defined FP_OPTIM_SPEED || !defined FP_OPTIM_SIZE  // SPEED

    W = arg2f16.low8;
    goto START;

// MAIN LOOP
    do  {
       LOOP:
        if (!Carry)  {
           START:
            aarg -= W;
            if (Carry)
                goto SHIFT_IN_CARRY;
            aarg += W;
            Carry = 0;
            arg1f16.low8 = rl( arg1f16.low8);
            // Carry = 0;  // ok, speedup
            aarg = rl( aarg);
            counter = decsz(counter);
            goto LOOP;
            goto EXIT_LOOP;
        }
        aarg -= W;
        Carry = 1;
      SHIFT_IN_CARRY:
        arg1f16.low8 = rl( arg1f16.low8);
        // Carry = 0;  // ok, speedup
        aarg = rl( aarg);
        counter = decsz(counter);
    } while (1);

   EXIT_LOOP:



#else

    goto START;

// MAIN LOOP
    do  {
      LOOP:
        if (Carry)
            goto SUBTRACT;
      START:
        W = aarg - arg2f16.low8;
        if (!Carry)
            goto SKIP_SUB;
       SUBTRACT:
        aarg -= arg2f16.low8;
        Carry = 1;
       SKIP_SUB:
        arg1f16.low8 = rl( arg1f16.low8);
        // Carry = 0;  // ok
        aarg = rl( aarg);
        counter = decsz(counter);
    } while (1);

#endif

    if (!arg1f16.7)  {
       #if __CoreSet__ >= 1700 && __CoreSet__ <= 1800
        aarg = rr( aarg);   // Save Carry
       #endif
        if (!arg1f16.high8)
            goto UNDERFLOW;
        arg1f16.high8 --;
        counter ++;
       #if __CoreSet__ >= 1700 && __CoreSet__ <= 1800
        aarg = rl( aarg);   // Restore Carry, changed by INCF/DECF
       #endif
        goto LOOP;
    }

   #ifndef DISABLE_ROUNDING
    if (FpRounding)  {
        if (Carry)
            goto ADD_1;
        aarg.low8 -= arg2f16.low8;
        if (Carry)  {
          ADD_1:
            arg1f16.low8 += 1;
            if (!arg1f16.low8)  {
                arg1f16.low8 = rr( arg1f16.low8);
                arg1f16.high8 ++;
                if (!arg1f16.high8)
                    goto OVERFLOW;
            }
        }
    }
   #endif
    goto SET_SIGN;

  Div0:
    FpDiv0 = 1;
    goto SATURATE;

  UNDERFLOW:
    FpUnderFlow = 1;
  RES0:
    arg1f16.high8 = 0;
    goto MANTISSA;

  OVERFLOW:
    FpOverflow = 1;
  SATURATE:
    arg1f16.high8 = 0xFF;
  MANTISSA:
    arg1f16.low8 = 0x80;

  SET_SIGN:
    if (!(sign & 0x80))