math24x.h

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

#pragma library 1
/*
fixed16_8 operator* _xmul16_8( fixed16_8 arg1, fixed16_8 arg2);
fixedU16_8 operator* _xmulU16_8( fixedU16_8 arg1, fixedU16_8 arg2);
fixed16_8 operator/ _xdiv16_8( fixed16_8 arg1, fixed16_8 arg2);
fixedU16_8 operator/ _xdivU16_8( fixedU16_8 arg1, fixedU16_8 arg2);
fixed8_16 operator* _xmul8_16( fixed8_16 arg1, fixed8_16 arg2);
fixedU8_16 operator* _xmulU8_16( fixedU8_16 arg1, fixedU8_16 arg2);
fixed8_16 operator/ _xdiv8_16( fixed8_16 arg1, fixed8_16 arg2);
fixedU8_16 operator/ _xdivU8_16( fixedU8_16 arg1, fixedU8_16 arg2);
*/

// DEFINABLE SYMBOLS (in the application code):
//#define FX_OPTIM_SPEED  // optimize for SPEED: default
//#define FX_OPTIM_SIZE   // optimize for SIZE

#if __CoreSet__ < 1200  ||  __CoreSet__ >= 2100
 #error math24x.h does not support the selected device
#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

#if defined FX_OPTIM_SIZE
 // do not use 8*8 bit HW multiplier
#elif __CoreSet__ == 1700 || __CoreSet__ == 1800
 #define hw_mult8x8(a,b)  { W = a; multiply(b); }
 #define loRES PRODL
 #define hiRES PRODH
#elif __CoreSet__ == 2000
 #define hw_mult8x8(a,b)  { W = a; multiply(b); }
 #define loRES W
 #define hiRES MULH
#endif



fixed16_8 operator* _xmul16_8( fixed16_8 arg1, fixed16_8 arg2)
{
    fixed16_8 rval;
    char sign = arg1.high8 ^ arg2.high8;
    if (arg1 < 0)
        arg1 = -arg1;
    if (arg2 < 0)
        arg2 = -arg2;

   #ifdef hw_mult8x8

    hw_mult8x8( arg1.low8, arg2.low8);   // p1
    rval.low8 = hiRES;
    rval.high16 = 0;

    hw_mult8x8( arg1.mid8, arg2.low8);  // p2
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.low8, arg2.mid8);  // p2
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.high8, arg2.low8);  // p3
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.mid8, arg2.mid8);  // p3
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.low8, arg2.high8);  // p3
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.high8, arg2.mid8);  // p4
    rval.high8 += hiRES;

    hw_mult8x8( arg1.mid8, arg2.high8);  // p4
    rval.high8 += hiRES;

   #else

    char counter = sizeof(arg1)*8;
    char rvalL = 0;
    do  {
        Carry = 0;
        rvalL = rl( rvalL);
        rval = rl( rval);
        arg1 = rl( arg1);
        if (Carry)  {
            rvalL += arg2.low8;
            genAdd( rval.low8, arg2.mid8);
            genAdd( rval.mid8, arg2.high8);
            if (Carry)
                rval.high8++;
        }
        counter = decsz(counter);
    } while (1);

   #endif

    if (sign & 0x80)
        rval = -rval;
    return rval;
}


fixedU16_8 operator* _xmulU16_8( fixedU16_8 arg1, fixedU16_8 arg2)
{
    fixedU16_8 rval;

   #ifdef hw_mult8x8

    hw_mult8x8( arg1.low8, arg2.low8);   // p1
    rval.low8 = hiRES;
    rval.high16 = 0;

    hw_mult8x8( arg1.mid8, arg2.low8);  // p2
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.low8, arg2.mid8);  // p2
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.high8, arg2.low8);  // p3
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.mid8, arg2.mid8);  // p3
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.low8, arg2.high8);  // p3
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.high8, arg2.mid8);  // p4
    rval.high8 += hiRES;

    hw_mult8x8( arg1.mid8, arg2.high8);  // p4
    rval.high8 += hiRES;

   #else

    char counter = sizeof(arg1)*8;
    char rvalL = 0;
    do  {
        Carry = 0;
        rvalL = rl( rvalL);
        rval = rl( rval);
        arg1 = rl( arg1);
        if (Carry)  {
            rvalL += arg2.low8;
            genAdd( rval.low8, arg2.mid8);
            genAdd( rval.mid8, arg2.high8);
            if (Carry)
                rval.high8++;
        }
        counter = decsz(counter);
    } while (1);

   #endif

    return rval;
}


fixed16_8 operator/ _xdiv16_8( fixed16_8 arg1, fixed16_8 arg2)
{
    fixedU16_8 rm = 0;
    char counter = sizeof(arg1)*8+8+1;
    char c2 = sizeof(arg1)*8 + 1;
    char sign = arg1.high8 ^ arg2.high8;
    if (arg1 < 0)  {
       INVERT:
        arg1 = -arg1;
        if (c2 & 0x80)
            goto EXIT;
    }
    if (arg2 < 0)
        arg2 = -arg2;
    goto ENTRY;
    do  {
        c2 = decsz(c2);
        btsc(c2.7);
        Carry = 0;
        rm = rl( rm);
        W = rm.low8 - arg2.low8;
        genSubW( rm.mid8, arg2.mid8);
        genSubW( rm.high8, arg2.high8);
        if (!Carry)
            goto ENTRY;
        rm.high8 = W;
        rm.low8 -= arg2.low8;
        genSub( rm.mid8, arg2.mid8);
        Carry = 1;
       ENTRY:
        arg1 = rl( arg1);
        counter = decsz(counter);
    } while (1);
    if (sign & 0x80)
        goto INVERT;
   EXIT:
    return arg1;
}



fixedU16_8 operator/ _xdivU16_8( fixedU16_8 arg1, fixedU16_8 arg2)
{
    fixedU16_8 rm = 0;
    char counter = sizeof(arg1)*8+8+1;
    char c2 = sizeof(arg1)*8 + 1;
    goto ENTRY;
    do  {
        c2 = decsz(c2);
        btsc(c2.7);
        Carry = 0;
        rm = rl( rm);
        if (Carry)
            goto SUBTRACT;
        W = rm.low8 - arg2.low8;
        genSubW( rm.mid8, arg2.mid8);
        genSubW( rm.high8, arg2.high8);
        if (!Carry)
            goto ENTRY;
       SUBTRACT:
        rm.low8 -= arg2.low8;
        genSub( rm.mid8, arg2.mid8);
        genSub( rm.high8, arg2.high8);
        Carry = 1;
       ENTRY:
        arg1 = rl( arg1);
        counter = decsz(counter);
    } while (1);
    return arg1;
}




fixed8_16 operator* _xmul8_16( fixed8_16 arg1, fixed8_16 arg2)
{
    fixed8_16 rval;
    char sign = arg1.high8 ^ arg2.high8;
    if (arg1 < 0)
        arg1 = -arg1;
    if (arg2 < 0)
        arg2 = -arg2;

   #ifdef hw_mult8x8

    uns8 tmpL;

    hw_mult8x8( arg1.low8, arg2.low8);   // p1
    tmpL = hiRES;
    rval = 0;

    hw_mult8x8( arg1.mid8, arg2.low8);  // p2
    tmpL += loRES;
    genAdd( rval.low8, hiRES);
    genAdd( rval.mid8, 0);

    hw_mult8x8( arg1.low8, arg2.mid8);  // p2
    tmpL += loRES;
    genAdd( rval.low8, hiRES);
    genAdd( rval.mid8, 0);

    hw_mult8x8( arg1.high8, arg2.low8);  // p3
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.mid8, arg2.mid8);  // p3
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.low8, arg2.high8);  // p3
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.high8, arg2.mid8);  // p4
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.mid8, arg2.high8);  // p4
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.high8, arg2.high8);  // p5
    rval.high8 += loRES;

   #else

    char counter = sizeof(arg2)*8;
    uns8 rvalH = 0;
    rval.high16 = 0;

    do  {
        arg2 = rr( arg2);
        if (Carry)  {
            rval.mid8 += arg1.low8;
            genAdd( rval.high8, arg1.mid8);
            genAdd( rvalH, arg1.high8);
        }
        rvalH = rr( rvalH);
        rval = rr( rval);
        counter = decsz(counter);
    } while (1);

   #endif

    if (sign & 0x80)
        rval = -rval;
    return rval;
}


fixedU8_16 operator* _xmulU8_16( fixedU8_16 arg1, fixedU8_16 arg2)
{
    fixedU8_16 rval;

   #ifdef hw_mult8x8

    uns8 tmpL;

    hw_mult8x8( arg1.low8, arg2.low8);   // p1
    tmpL = hiRES;
    rval = 0;

    hw_mult8x8( arg1.mid8, arg2.low8);  // p2
    tmpL += loRES;
    genAdd( rval.low8, hiRES);
    genAdd( rval.mid8, 0);

    hw_mult8x8( arg1.low8, arg2.mid8);  // p2
    tmpL += loRES;
    genAdd( rval.low8, hiRES);
    genAdd( rval.mid8, 0);

    hw_mult8x8( arg1.high8, arg2.low8);  // p3
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.mid8, arg2.mid8);  // p3
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.low8, arg2.high8);  // p3
    rval.low8 += loRES;
    genAdd( rval.mid8, hiRES);
    genAdd( rval.high8, 0);

    hw_mult8x8( arg1.high8, arg2.mid8);  // p4
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.mid8, arg2.high8);  // p4
    rval.mid8 += loRES;
    genAdd( rval.high8, hiRES);

    hw_mult8x8( arg1.high8, arg2.high8);  // p5
    rval.high8 += loRES;

   #else

    char counter = sizeof(arg2)*8;
    uns8 rvalH = 0;
    rval.high16 = 0;
    do  {
        arg2 = rr( arg2);
        if (Carry)  {
            rval.mid8 += arg1.low8;
            genAdd( rval.high8, arg1.mid8);
            genAdd( rvalH, arg1.high8);
        }
        rvalH = rr( rvalH);
        rval = rr( rval);
        counter = decsz(counter);
    } while (1);

   #endif

    return rval;
}


fixed8_16 operator/ _xdiv8_16( fixed8_16 arg1, fixed8_16 arg2)
{
    fixedU8_16 rm = 0;
    char counter = sizeof(arg1)*8+16+1;
    char c2 = sizeof(arg1)*8 + 1;
    char sign = arg1.high8 ^ arg2.high8;
    if (arg1 < 0)  {
       INVERT:
        arg1 = -arg1;
        if (c2 & 0x80)
            goto EXIT;
    }
    if (arg2 < 0)
        arg2 = -arg2;
    goto ENTRY;
    do  {
        c2 = decsz(c2);
        btsc(c2.7);
        Carry = 0;
        rm = rl( rm);
        W = rm.low8 - arg2.low8;
        genSubW( rm.mid8, arg2.mid8);
        genSubW( rm.high8, arg2.high8);
        if (!Carry)
            goto ENTRY;
        rm.high8 = W;
        rm.low8 -= arg2.low8;
        genSub( rm.mid8, arg2.mid8);
        Carry = 1;
       ENTRY:
        arg1 = rl( arg1);
        counter = decsz(counter);
    } while (1);
    if (sign & 0x80)
        goto INVERT;
   EXIT:
    return arg1;
}



fixedU8_16 operator/ _xdivU8_16( fixedU8_16 arg1, fixedU8_16 arg2)
{
    fixedU8_16 rm = 0;
    char counter = sizeof(arg1)*8+16+1;
    char c2 = sizeof(arg1)*8 + 1;
    goto ENTRY;
    do  {
        c2 = decsz(c2);
        btsc(c2.7);
        Carry = 0;
        rm = rl( rm);
        if (Carry)
            goto SUBTRACT;
        W = rm.low8 - arg2.low8;
        genSubW( rm.mid8, arg2.mid8);
        genSubW( rm.high8, arg2.high8);
        if (!Carry)
            goto ENTRY;
       SUBTRACT:
        rm.low8 -= arg2.low8;
        genSub( rm.mid8, arg2.mid8);
        genSub( rm.high8, arg2.high8);
        Carry = 1;
       ENTRY:
        arg1 = rl( arg1);
        counter = decsz(counter);
    } while (1);
    return arg1;
}

#ifdef hw_mult8x8
 #undef hw_mult8x8
 #undef loRES
 #undef hiRES
#endif

#undef genAdd
#undef genSub
#undef genAddW
#undef genSubW

#pragma library 0