; CC1B Ver 0.5A beta, Copyright (c) B Knudsen Data
; C compiler for the Ubicom SX family
; ************  24. Jun 2002  11:40  *************

        device SX28

FSR_7       EQU   7
Carry       EQU   0
Zero_       EQU   2
arg1_3      EQU   $10
arg2_3      EQU   $13
rm          EQU   $16
counter_3   EQU   $19
c2          EQU   $1A
sign_2      EQU   $1B
ad_data     EQU   $1C
tx          EQU   $30
av          EQU   $33
a           EQU   $39
vx          EQU   $3C
prev        EQU   $50
kp          EQU   $53

  ; FILE sx\demo-fxm.c
                        ;
                        ;#pragma chip SX28 // select device
                        ;
                        ;// FIRST ADD THE INTERRUPT ROUTINE (if any)
                        ;// #include "app-int.c"
                        ;
                        ;#pragma rambank 0
                        ;#pragma codepage 1
        ORG $0200

  ; FILE test\math24x.h
                        ;
                        ;#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);
                        ;*/
                        ;
                        ;#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
                        ;
                        ;
                        ;
                        ;fixed16_8 operator* _xmul16_8( fixed16_8 arg1, fixed16_8 arg2)
                        ;{
_xmul16_8
                        ;    fixed16_8 rval;
                        ;    char counter = sizeof(arg1)*8;
                        ;    char sign = arg1.high8 ^ arg2.high8;
                        ;    if ( arg1 < 0)
                        ;        arg1 = -arg1;
                        ;    if ( arg2 < 0)
                        ;        arg2 = -arg2;
                        ;    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++;
                        ;        }
                        ;    } while ( --counter > 0);
                        ;    if ( sign & 0x80)
                        ;        rval = -rval;
                        ;    return rval;
                        ;}
                        ;
                        ;
                        ;fixedU16_8 operator* _xmulU16_8( fixedU16_8 arg1, fixedU16_8 arg2)
                        ;{
_xmulU16_8
                        ;    fixedU16_8 rval;
                        ;    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++;
                        ;        }
                        ;    } while ( --counter > 0);
                        ;    return rval;
                        ;}
                        ;
                        ;
                        ;fixed16_8 operator/ _xdiv16_8( fixed16_8 arg1, fixed16_8 arg2)
                        ;{
_xdiv16_8
                        ;    fixedU16_8 rm = 0;
        CLR   rm
        CLR   rm+1
        CLR   rm+2
                        ;    char counter = sizeof(arg1)*8+8+1;
        MOV   W,#33
        MOV   counter_3,W
                        ;    char c2 = sizeof(arg1)*8;
        MOV   W,#24
        MOV   c2,W
                        ;    char sign = arg1.high8 ^ arg2.high8;
        MOV   W,arg2_3+2
        XOR   W,arg1_3+2
        MOV   sign_2,W
                        ;    if ( arg1 < 0)  {
        SB    arg1_3+2.7
        JMP   m003
                        ;       INVERT:
                        ;        arg1 = -arg1;
m001    NOT   arg1_3+2
        NOT   arg1_3+1
        NOT   arg1_3
        INCSZ arg1_3
        JMP   m002
        INC   arg1_3+1
        SNB   3.Zero_
        INC   arg1_3+2
                        ;        if ( c2&0x80)
m002    SNB   c2.7
                        ;            goto EXIT;
        JMP   m006
                        ;    }
                        ;    if ( arg2 < 0)
m003    SB    arg2_3+2.7
        JMP   m005
                        ;        arg2 = -arg2;
        NOT   arg2_3+2
        NOT   arg2_3+1
        NOT   arg2_3
        INCSZ arg2_3
        JMP   m005
        INC   arg2_3+1
        SNB   3.Zero_
        INC   arg2_3+2
                        ;    goto ENTRY;
        JMP   m005
                        ;    do  {
                        ;        c2 -= 1;
m004    DEC   c2
                        ;        if ( c2 & 0x80)
        SNB   c2.7
                        ;            Carry = 0;
        CLRB  3.Carry
                        ;        rm = rl( rm);
        RL    rm
        RL    rm+1
        RL    rm+2
                        ;        W = rm.low8 - arg2.low8;
        MOV   W,arg2_3
        MOV   W,rm-W
                        ;        genSubW( rm.mid8, arg2.mid8);
        MOV   W,arg2_3+1
        SB    3.Carry
        MOVSZ W,++arg2_3+1
        MOV   W,rm+1-W
                        ;        genSubW( rm.high8, arg2.high8);
        MOV   W,arg2_3+2
        SB    3.Carry
        MOVSZ W,++arg2_3+2
        MOV   W,rm+2-W
                        ;        if (!Carry)
        SB    3.Carry
                        ;            goto ENTRY;
        JMP   m005
                        ;        rm.high8 = W;
        MOV   rm+2,W
                        ;        rm.low8 -= arg2.low8;
        MOV   W,arg2_3
        SUB   rm,W
                        ;        genSub( rm.mid8, arg2.mid8);
        MOV   W,arg2_3+1
        SB    3.Carry
        MOVSZ W,++arg2_3+1
        SUB   rm+1,W
                        ;        Carry = 1;
        SETB  3.Carry
                        ;       ENTRY:
                        ;        arg1 = rl( arg1);
m005    RL    arg1_3
        RL    arg1_3+1
        RL    arg1_3+2
                        ;    } while ( --counter > 0);
        DECSZ counter_3
        JMP   m004
                        ;    if ( sign & 0x80)
        SNB   sign_2.7
                        ;        goto INVERT;
        JMP   m001
                        ;   EXIT:
                        ;    return arg1;
m006    MOV   W,arg1_3
        RET  
                        ;}
                        ;
                        ;
                        ;
                        ;fixedU16_8 operator/ _xdivU16_8( fixedU16_8 arg1, fixedU16_8 arg2)
                        ;{
_xdivU16_8
                        ;    fixedU16_8 rm = 0;
                        ;    char counter = sizeof(arg1)*8+8+1;
                        ;    char c2 = sizeof(arg1)*8;
                        ;    goto ENTRY;
                        ;    do  {
                        ;        c2 -= 1;
                        ;        if ( c2 & 0x80)
                        ;            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);
                        ;    } while ( --counter > 0);
                        ;    return arg1;
                        ;}
                        ;
                        ;
                        ;
                        ;
                        ;fixed8_16 operator* _xmul8_16( fixed8_16 arg1, fixed8_16 arg2)
                        ;{
_xmul8_16
                        ;    fixed8_16 rval;
                        ;    char counter = sizeof(arg2)*8;
                        ;    uns8 rvalH = 0;
                        ;    rval.high16 = 0;
                        ;    char sign = arg1.high8 ^ arg2.high8;
                        ;    if ( arg1 < 0)
                        ;        arg1 = -arg1;
                        ;    if ( arg2 < 0)
                        ;        arg2 = -arg2;
                        ;    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);
                        ;    } while ( --counter > 0);
                        ;    if ( sign & 0x80)
                        ;        rval = -rval;
                        ;    return rval;
                        ;}
                        ;
                        ;
                        ;fixedU8_16 operator* _xmulU8_16( fixedU8_16 arg1, fixedU8_16 arg2)
                        ;{
_xmulU8_16
                        ;    fixedU8_16 rval;
                        ;    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);
                        ;    } while ( --counter > 0);
                        ;    return rval;
                        ;}
                        ;
                        ;
                        ;fixed8_16 operator/ _xdiv8_16( fixed8_16 arg1, fixed8_16 arg2)
                        ;{
_xdiv8_16
                        ;    fixedU8_16 rm = 0;
                        ;    char counter = sizeof(arg1)*8+16+1;
                        ;    char c2 = sizeof(arg1)*8;
                        ;    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 -= 1;
                        ;        if ( c2 & 0x80)
                        ;            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);
                        ;    } while ( --counter > 0);
                        ;    if ( sign & 0x80)
                        ;        goto INVERT;
                        ;   EXIT:
                        ;    return arg1;
                        ;}
                        ;
                        ;
                        ;
                        ;fixedU8_16 operator/ _xdivU8_16( fixedU8_16 arg1, fixedU8_16 arg2)
                        ;{
_xdivU8_16
                        ;    fixedU8_16 rm = 0;
                        ;    char counter = sizeof(arg1)*8+16+1;
                        ;    char c2 = sizeof(arg1)*8;
                        ;    goto ENTRY;
                        ;    do  {
                        ;        c2 -= 1;
                        ;        if ( c2 & 0x80)
                        ;            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);
                        ;    } while ( --counter > 0);
                        ;    return arg1;

  ; FILE sx\demo-fxm.c
                        ;#include "math24x.h"
                        ;#pragma codepage 0
        ORG $0000
                        ;
                        ;uns16 ad_data;
                        ;bank1 fixed16_8  tx, av, mg, a, vx;
                        ;bank2 fixed16_8  prev, kp;
                        ;
                        ;void main(void)
                        ;{
main
                        ;    vx = 3.127;
        MOV   W,#33
        BANK  32
        CLRB  4.FSR_7
        MOV   vx,W
        MOV   W,#3
        MOV   vx+1,W
        CLR   vx+2
                        ;    tx += ad_data;    // automatic type cast
        BANK  0
        MOV   W,ad_data+1
        BANK  32
        ADD   tx+2,W
        BANK  0
        MOV   W,ad_data
        BANK  32
        ADD   tx+1,W
        SNB   3.Carry
        INC   tx+2
                        ;    ad_data = kp;     // assign integer part
        BANK  64
        MOV   W,kp+1
        BANK  0
        MOV   ad_data,W
        BANK  64
        MOV   W,kp+2
        BANK  0
        MOV   ad_data+1,W
                        ;    if ( tx < 0)
        BANK  32
        SB    tx+2.7
        JMP   m007
                        ;        tx = -tx;     // make positive
        NOT   tx+2
        NOT   tx+1
        NOT   tx
        INCSZ tx
        JMP   m007
        INC   tx+1
        SNB   3.Zero_
        INC   tx+2
                        ;    av = tx/20.0;
m007    MOV   W,tx
        BANK  0
        MOV   arg1_3,W
        BANK  32
        MOV   W,tx+1
        BANK  0
        MOV   arg1_3+1,W
        BANK  32
        MOV   W,tx+2
        BANK  0
        MOV   arg1_3+2,W
        CLR   arg2_3
        MOV   W,#20
        MOV   arg2_3+1,W
        CLR   arg2_3+2
        PAGE  512
        CALL  _xdiv16_8
        BANK  32
        MOV   av,W
        BANK  0
        MOV   W,arg1_3+1
        BANK  32
        MOV   av+1,W
        BANK  0
        MOV   W,arg1_3+2
        BANK  32
        MOV   av+2,W
                        ;    //mg = av * 1.25;
                        ;    //a = mg * 0.98;    // 0.980469: error on constant: 0.000478
                        ;    prev = vx;
        MOV   W,vx
        BANK  64
        MOV   prev,W
        BANK  32
        MOV   W,vx+1
        BANK  64
        MOV   prev+1,W
        BANK  32
        MOV   W,vx+2
        BANK  64
        MOV   prev+2,W
                        ;    vx = a/5.0 + prev;
        BANK  32
        MOV   W,a
        BANK  0
        MOV   arg1_3,W
        BANK  32
        MOV   W,a+1
        BANK  0
        MOV   arg1_3+1,W
        BANK  32
        MOV   W,a+2
        BANK  0
        MOV   arg1_3+2,W
        CLR   arg2_3
        MOV   W,#5
        MOV   arg2_3+1,W
        CLR   arg2_3+2
        CALL  _xdiv16_8
        PAGE  0
        BANK  64
        MOV   W,prev+2
        BANK  0
        ADD   W,arg1_3+2
        BANK  32
        MOV   vx+2,W
        BANK  64
        MOV   W,prev+1
        BANK  0
        ADD   W,arg1_3+1
        BANK  32
        MOV   vx+1,W
        SNB   3.Carry
        INC   vx+2
        BANK  64
        MOV   W,prev
        BANK  0
        ADD   W,arg1_3
        BANK  32
        MOV   vx,W
        SB    3.Carry
        JMP   m008
        INC   vx+1
        SNB   3.Zero_
        INC   vx+2
                        ;
                        ;    // kp = vx * 0.036;       // 0.03515626: error on constant: 0.024
                        ;    kp = vx / (1.0/0.036); // 27.7773437 error on constant: 0.0000156
m008    MOV   W,vx
        BANK  0
        MOV   arg1_3,W
        BANK  32
        MOV   W,vx+1
        BANK  0
        MOV   arg1_3+1,W
        BANK  32
        MOV   W,vx+2
        BANK  0
        MOV   arg1_3+2,W
        MOV   W,#199
        MOV   arg2_3,W
        MOV   W,#27
        MOV   arg2_3+1,W
        CLR   arg2_3+2
        PAGE  512
        CALL  _xdiv16_8
        PAGE  0
        BANK  64
        MOV   kp,W
        BANK  0
        MOV   W,arg1_3+1
        BANK  64
        MOV   kp+1,W
        BANK  0
        MOV   W,arg1_3+2
        BANK  64
        MOV   kp+2,W
                        ;}
        SLEEP

        ORG $07FF
        GOTO main
        END