Fast non-754 floating point library for Freescale 56F800E series
#ifndef ALREADY_READ_FFLOATV2A_H
#define ALREADY_READ_FFLOATV2A_H
/*******************************************************************************
FFloat Number Definitions
*******************************************************************************/
#define MINUSONEFF 0x00008000 //FFloat number -1
#define ZEROFF 0xFF800000 //FFloat number 0
#define ONEFF 0x00014000 //FFloat number 1
#define TWOFF 0x00024000 //FFloat number 2
#define THREEFF 0x00026000 //FFloat number 3
#define FOURFF 0x00034000 //FFloat number 4
#define FIVEFF 0x00035000 //FFloat number 5
#define SIXFF 0x00036000 //FFloat number 6
#define SEVENFF 0x00037000 //FFloat number 7
#define EIGHTFF 0x00044000 //FFloat number 8
#define NINEFF 0x00044800 //FFloat number 9
#define TENFF 0x00045000 //FFloat number 10
#define ELEVENFF 0x00045800 //FFloat number 11
#define TWELVEFF 0x00046000 //FFloat number 12
#define NOMINALBATVOLTAGE 0x00044800
/*******************************************************************************
FFloat Data Type Definition
*******************************************************************************/
typedef unsigned char bool;
typedef long unsigned int ffloat;
/*******************************************************************************
FFloat FUNCTION PROTOTYPES
*******************************************************************************/
asm ffloat FFabs(register ffloat ffnum);
asm ffloat FFneg(register ffloat ffnum);
asm ffloat S16int2FFloat(register short int inum);
asm ffloat S32int2FFloat(register long int inum);
asm ffloat U32int2FFloat(register long unsigned int unum);
asm ffloat FFadd(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat FFdiv(register ffloat ffnum1,register ffloat ffnum2);
asm short int FFloatTrunc2S16int(register ffloat ffnum);
asm short int FFloatRnd2S16int(register ffloat ffnum);
asm ffloat FFmult(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat FFsub(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat IEEE2FFloat(register float fnum);
float FFloat2IEEE(ffloat ffnum);
asm bool FFgt(register ffloat ffnum1, register ffloat ffnum2);
asm bool FFlt(register ffloat ffnum1, register ffloat ffnum2);
asm bool FFgte(register ffloat a, register ffloat b);
asm bool FFlte(register ffloat a, register ffloat b);
asm bool FFgtz(register ffloat ffnum);
asm bool FFltz(register ffloat ffnum);
asm bool FFeqz(register ffloat ffnum);
ffloat FFatan(ffloat xin);
ffloat FFsin(ffloat xin);
ffloat FFcos(ffloat xin);
#endif
**********************************************************
Function code begins below
**********************************************************
#include "FFloatV2A.h"
ffloat FFatan(ffloat xin)
{
int k,klo,khi;
ffloat xdiff0, xdiff1;
ffloat x=xin;
static ffloat xlo = 0x0005b000;
static ffloat xhi = 0x00055000;
static ffloat ya[151] = {0x00019eaa, 0x00019eb5, 0x00019ec0, 0x00019ecc, 0x00019ed8, 0x00019ee4, 0x00019ef1, 0x00019efe, 0x00019f0c, 0x00019f19, 0x00019f28, 0x00019f36, 0x00019f46, 0x00019f55, 0x00019f66, 0x00019f76, 0x00019f88, 0x00019f99, 0x00019fac, 0x00019fbf, 0x00019fd3, 0x00019fe8, 0x00019ffd, 0x0001a013, 0x0001a02a, 0x0001a042, 0x0001a05b, 0x0001a075, 0x0001a090, 0x0001a0ac, 0x0001a0ca, 0x0001a0e9, 0x0001a109, 0x0001a12b, 0x0001a14e, 0x0001a173, 0x0001a19a, 0x0001a1c3, 0x0001a1ee, 0x0001a21c, 0x0001a24c, 0x0001a27f, 0x0001a2b5, 0x0001a2ef, 0x0001a32c, 0x0001a36d, 0x0001a3b3, 0x0001a3fd, 0x0001a44d, 0x0001a4a2, 0x0001a4ff, 0x0001a563, 0x0001a5d0, 0x0001a646, 0x0001a6c7, 0x0001a754, 0x0001a7f0, 0x0001a89d, 0x0001a95d, 0x0001aa33, 0x0001ab25, 0x0001ac37, 0x0001ad71, 0x0001aeda, 0x0001b07f, 0x0001b26e, 0x0001b4bc, 0x0001b785, 0x0001baf1, 0x0001bf38, 0x0000894e, 0x00009757, 0x0000a9a2, 0xffff8292, 0xffffbd49, 0xff800000, 0xffff42b6, 0xffff7d6d, 0x0000565d, 0x000068a8, 0x000076b1, 0x000140c7, 0x0001450e, 0x0001487a, 0x00014b43, 0x00014d91, 0x00014f80, 0x00015125, 0x0001528e, 0x000153c8, 0x000154da, 0x000155cc, 0x000156a2, 0x00015762, 0x0001580f, 0x000158ab, 0x00015938, 0x000159b9, 0x00015a2f, 0x00015a9c, 0x00015b00, 0x00015b5d, 0x00015bb2, 0x00015c02, 0x00015c4c, 0x00015c92, 0x00015cd3, 0x00015d10, 0x00015d4a, 0x00015d80, 0x00015db3, 0x00015de3, 0x00015e11, 0x00015e3c, 0x00015e65, 0x00015e8c, 0x00015eb1, 0x00015ed4, 0x00015ef6, 0x00015f16, 0x00015f35, 0x00015f53, 0x00015f6f, 0x00015f8a, 0x00015fa4, 0x00015fbd, 0x00015fd5, 0x00015fec, 0x00016002, 0x00016017, 0x0001602c, 0x00016040, 0x00016053, 0x00016066, 0x00016077, 0x00016089, 0x00016099, 0x000160aa, 0x000160b9, 0x000160c9, 0x000160d7, 0x000160e6, 0x000160f3, 0x00016101, 0x0001610e, 0x0001611b, 0x00016127, 0x00016133, 0x0001613f, 0x0001614a, 0x00016155};
static ffloat y2a[151] = {0xff800000, 0xfff443e4, 0xfff446b6, 0xfff449b0, 0xfff44cd5, 0xfff45029, 0xfff453af, 0xfff4576a, 0xfff45b5f, 0xfff45f92, 0xfff46408, 0xfff468c6, 0xfff46dd1, 0xfff47331, 0xfff478ec, 0xfff47f0a, 0xfff542c9, 0xfff54648, 0xfff54a06, 0xfff54e0a, 0xfff55259, 0xfff556fa, 0xfff55bf6, 0xfff56156, 0xfff56722, 0xfff56d66, 0xfff5742f, 0xfff57b8a, 0xfff641c3, 0xfff6461c, 0xfff64ad8, 0xfff65004, 0xfff655ac, 0xfff65be0, 0xfff662b0, 0xfff66a30, 0xfff67278, 0xfff67ba1, 0xfff742e5, 0xfff7488b, 0xfff74ed9, 0xfff755e6, 0xfff75dd0, 0xfff766ba, 0xfff770cc, 0xfff77c39, 0xfff8449e, 0xfff84c0f, 0xfff8549c, 0xfff85e7b, 0xfff869ef, 0xfff8774e, 0xfff9437f, 0xfff94cc5, 0xfff957cc, 0xfff96504, 0xfff974fc, 0xfffa4439, 0xfffa5032, 0xfffa5f16, 0xfffa71cd, 0xfffb44d0, 0xfffb542e, 0xfffb684a, 0xfffc4182, 0xfffc538f, 0xfffc6c5c, 0xfffd4779, 0xfffd5fe2, 0xfffe4133, 0xfffe5918, 0xfffe77b6, 0xffff4b62, 0xffff503a, 0xfffe707d, 0xff800000, 0xfffe8f82, 0xffffafc5, 0xffffb49d, 0xfffe8849, 0xfffea6e7, 0xfffebecc, 0xfffda01d, 0xfffdb886, 0xfffc93a3, 0xfffcac70, 0xfffcbe7d, 0xfffb97b5, 0xfffbabd1, 0xfffbbb2f, 0xfffa8e32, 0xfffaa0e9, 0xfffaafcd, 0xfffabbc6, 0xfff98b03, 0xfff99afb, 0xfff9a833, 0xfff9b33a, 0xfff9bc80, 0xfff888b1, 0xfff89610, 0xfff8a184, 0xfff8ab63, 0xfff8b3f0, 0xfff8bb61, 0xfff783c6, 0xfff78f33, 0xfff79945, 0xfff7a22f, 0xfff7aa19, 0xfff7b126, 0xfff7b774, 0xfff7bd1a, 0xfff6845e, 0xfff68d87, 0xfff695cf, 0xfff69d4f, 0xfff6a41f, 0xfff6aa53, 0xfff6affb, 0xfff6b527, 0xfff6b9e3, 0xfff6be3c, 0xfff58475, 0xfff58bd0, 0xfff59299, 0xfff598dd, 0xfff59ea9, 0xfff5a409, 0xfff5a905, 0xfff5ada6, 0xfff5b1f5, 0xfff5b5f9, 0xfff5b9b7, 0xfff5bd36, 0xfff480f5, 0xfff48713, 0xfff48cce, 0xfff4922e, 0xfff49739, 0xfff49bf7, 0xfff4a06d, 0xfff4a4a0, 0xfff4a895, 0xfff4ac50, 0xfff4afd6, 0xfff4b32a, 0xfff4b64f, 0xfff4b949, 0xfff4bc1b, 0xfff4bc1b};
static int numpoints = 151;
static ffloat h = 0xffff4444;
static ffloat hinv = 0x00027800;
klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
khi=klo+1;
if(FFlt(x,xlo)){
return(ya[0]);
}else if(FFgt(x,xhi)){
return(ya[numpoints-1]);
}
xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
xdiff1 = FFsub(xdiff0, h);
return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}
ffloat FFcos(ffloat xin)
{
int k,klo,khi;
ffloat xdiff0, xdiff1;
ffloat x=xin;
static ffloat xlo = 0x00029b78;
static ffloat xhi = 0x00026487;
static ffloat ya[31] = {0x00008000, 0x000082cc, 0x00008b10, 0x00009872, 0x0000aa59, 0xffff8000, 0xffffb0e4, 0xfffd94f6, 0xfffd6b09, 0xffff4f1b, 0x00004000, 0x000055a6, 0x0000678d, 0x000074ef, 0x00007d33, 0x00014000, 0x00007d33, 0x000074ef, 0x0000678d, 0x000055a6, 0x00004000, 0xffff4f1b, 0xfffd6b09, 0xfffd94f6, 0xffffb0e4, 0xffff8000, 0x0000aa59, 0x00009872, 0x00008b10, 0x000082cc, 0x00008000};
static ffloat y2a[31] = {0xff800000, 0xffff7cbe, 0xffff7481, 0xffff672d, 0xffff5556, 0xfffe7f88, 0xfffe4ed1, 0xfffc6aa5, 0xfffc955a, 0xfffeb12e, 0xfffe8077, 0xffffaaa9, 0xffff98d2, 0xffff8b7e, 0xffff8341, 0xffff8077, 0xffff8341, 0xffff8b7e, 0xffff98d2, 0xffffaaa9, 0xfffe8077, 0xfffeb12e, 0xfffc955a, 0xfffc6aa5, 0xfffe4ed1, 0xfffe7f88, 0xffff5556, 0xffff672d, 0xffff7481, 0xffff7cbe, 0xffff7cbe};
static int numpoints = 31;
static ffloat h = 0xfffe6b3b;
static ffloat hinv = 0x00034c64;
static ffloat pi2=0x00036487;
static ffloat pi2inv=0xfffe517c;
if(FFlt(xin,xlo)){
x=FFadd(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xhi,xin),
pi2inv
)
)
),
pi2
)
);
}else if(FFgt(xin,xhi)){
x=FFsub(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xin,xlo),
pi2inv
)
)
),
pi2
)
);
}
klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
khi=klo+1;
xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
xdiff1 = FFsub(xdiff0, h);
return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}
//Return the negative of ffnum
asm ffloat FFneg(register ffloat ffnum)
{
move.w A1,Y0 //store ffnum exp in Y0
move.w A0,A //A holds mantissa of ffnum
neg A //full 36-bit negate
asr A //shift right to prevent overflow of clb
jeq Zero //Don't normalize if zero
//ffnum != 0
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
sub X0,Y0 //Adjust exponent
inc.w Y0 //Return to normal scale
clb Y0,X0 //check number of sign bits in exponent
cmp.w #8,X0 //If less than 8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rtsd //delayed return from subroutine
move.w A1,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main neg function
Zero:
rtsd //Delayed return from subroutine - will execute next three words
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //Delay slot filler
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three cycles
move.w #$8000,A0 //Most negative mantissa
nop //Delay slot filler
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
//Return the absolute value of ffnum
asm ffloat FFabs(register ffloat ffnum)
{
move.w A1,Y0 //store ffnum exp in Y0
move.w A0,A //A holds mantissa of ffnum
abs A //full-width absolute value
asr A //shift right to prevent overflow of clb
jeq Zero //Don't normalize if zero
//ffnum != 0
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
sub X0,Y0 //Adjust exponent
inc.w Y0 //Return to normal scale
clb Y0,X0 //check number of sign bits in exponent
cmp.w #8,X0 //If less than 8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rtsd //delayed return from subroutine
move.w A,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main abs function
Zero:
rtsd //Delayed return from subroutine - will execute next three words
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //Delay slot filler
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three cycles
move.w #$8000,A0 //Most negative mantissa
nop //Delay slot filler
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
//convert an int16 to an ffloat value
asm ffloat S16int2FFloat(register short int inum)
{
tst.w Y0
jeq Zero
//inum != 0
clb Y0,X0
asll.l X0,Y0 //normalize inum
neg X0 //set exponent
rtsd
add.w #15,X0
move.w X0,A //exponent
move.w Y0,A0 //mantissa
//FFloat zero = 0xFF800000
Zero:
rtsd
move.w #$FF80,A
clr.w A0
}
asm ffloat FFadd(register ffloat ffnum1,register ffloat ffnum2)
{
move.w A0,X0 //Store ffnum1 mantissa temporarily in X0
move.w B0,Y0 //Store ffnum2 mantissa temporarily in Y0
move.w A1,Y1 //Put ffnum1 exponent (exp1) in Y1
sub B,Y1 //Y1 = exp1 - exp2
//Setup: Larger ffnum exponent goes in Y0; mantissa to be shifted goes in B1;
//mantissa to stay the same goes in A1; abs exp difference goes in Y1
tlt B,A //Move ffnum2 (mantissa and exp) to A (not shifted) if Y1 neg
tlt X0,B //Move ffnum1 mantissa to B1 for shifting if Y1 neg
tge Y0,B //Move ffnum2 mantissa to B1 for shifting if Y1 not negative
abs Y1 //positive shift values
cmp.w #15,Y1 //More than 15-bit shift (ASRAC only works to 15 bits)?
jgt Neglect //If yes, an input ffnum will go to zero if shifted
move.w A1,Y0 //Move larger exp to Y0 for shifting
move.w A0,A //Move mantissa A0 to A1 for adding
asrac B1,Y1,A //Extend B1 to 36 bits, shift right by Y1, and add to A
asr A //Shift right to prevent overflow of CLB (next)
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
tst.w A1 //Check if relevant part of result is zero
jeq Zero //Result is zero
sub X0,Y0 //Adjust exponent of exp1
inc.w Y0 //Return to normal scale
clb Y0,X0 //check number of sign bits in exponent
cmp.w #8,X0 //If less than 8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rnd A //round to 16 bits in A1
rtsd //delayed return from subroutine
move.w A,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main add function
Zero:
rtsd //Delayed return from subroutine - will execute next three words
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //Delay slot filler
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three cycles
move.w #$8000,A0 //Most negative mantissa
nop //Delay slot filler
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
Neglect:
rts //The input with the larger exp becomes the output
}
asm ffloat FFdiv(register ffloat ffnum1, register ffloat ffnum2)
{
move.w A1,X0 //Move exponent of ffnum1 to X0
move.w B1,Y0 //Move exponent of ffnum2 to Y0
move.w A0,Y1 //Move mantissa of ffnum1 to Y1 for sign check
move.w A0,A //Move mantissa of ffnum1 to A1
move.w B0,B //Move mantissa of ffnum2 to B1
eor.w B,Y1 //Calculate sign of final result
//(sign bit of result will be 1=negative if inputs signs differ)
abs A
abs B
jeq DivZero //ffnum2 cannot be zero
L1:
cmp A,B //Check result of B - A
bgt L2 //Ready to divide
brad L1 //Recheck (delayed branch)
asr A //Reduce ffnum1 mantissa by factor of 2
inc.w X0 //Increase ffnum1 exponent by one
//end
L2:
//Division of Positive Fractional Data (A1:A0 / B1)
BFCLR #$0001,SR //Clear carry bit: required for 1st DIV instruction
//REP #16
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
move.w A0,A //Move A0 to A1
tst.w Y1 //Check sign needed for final result
BGE L3 //Branch if final sign is non-neg
NEG A //Negate mantissa if result is neg
L3:
clb A,Y1 //Count sign bits
asll.l Y1,A //Normalize
tst A //Check if relevant part of result is zero
jeq Zero //Result is zero
sub Y0,X0 //Adjust exponent of exp1
sub Y1,X0
clb X0,Y0 //check size of exponent word
cmp.w #8,Y0
jlt Exp_Err
Continue:
RTSD
MOVE.W A,A0
MOVE.W X0,A1
sxt.l A //Sign-extend A to 36 bits
//END
DivZero:
//Call error handler here
MOVE.W #$007F,A //Needs work here
RTSD
MOVE.W #$7FFF,A0
NOP
//END
Zero:
RTSD
MOVE.W #$FF80,A
CLR.W A0
//END
Exp_Err:
cmp.w #$007F,X0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //filler for third delay slot
//end
Underflow:
cmp.w #$FF80,X0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
asm short int FFloatRnd2S16int(register ffloat ffnum)
{
move.w A1,Y0
move.w A0,A
//Scale so that exponent = 15; converts mantissa to integer scale
//Check if resulting mantissa is in range -32768 to 32767 (16 bit signed int)
sub.w #15,Y0
jgt Over //Number is outside range -32768 to 32767
cmp.w #-17,Y0
jlt Zero //Number is small and rounds to zero
rtsd
asll.l Y0,A //Scale to exponent = 15 (one word, two cycles)
rnd A //Convergent rounding (round down boundary case if even)
move.w A1,Y0
//end
Zero:
rtsd
clr.w Y0 //Result is zero
nop
nop
//end
Over:
tst A
blt Neg //branch to Neg: if number is below -32768
rtsd
move.w #$7FFF,Y0 //Set to most positive 16-bit value
nop //Filler for third delay slot
//end
Neg:
rtsd
move.w #$8000,Y0 //Set to most negative 16-bit value
nop //Filler for third delay slot
//end
}
asm short int FFloatTrunc2S16int(register ffloat ffnum)
{
move.w A1,Y0
move.w A0,A
//Scale so that exponent = 15; converts mantissa to integer scale
//Check if resulting mantissa is in range -32768 to 32767 (16 bit signed int)
sub.w #15,Y0
jgt Over //Number is outside range -32768 to 32767
cmp.w #-17,Y0
jlt Zero //Number is small and rounds to zero
rtsd
asll.l Y0,A //Scale to exponent = 15 (one word, two cycles)
move.w A1,Y0
nop //Filler for third delay slot
//end
Zero:
rtsd
clr.w Y0 //Result is zero
nop
nop
//end
Over:
tst A
blt Neg //branch to Neg: if number is below -32768
rtsd
move.w #$7FFF,Y0 //Set to most positive 16-bit value
nop //Filler for third delay slot
//end
Neg:
rtsd
move.w #$8000,Y0 //Set to most negative 16-bit value
nop //Filler for third delay slot
//end
}
//convert an unsigned int32 to an ffloat value
asm ffloat U32int2FFloat(register long unsigned int unum)
{
tst.l A
jeq Zero //unum = 0
jlt LongUnsigned //If 2^31 <= unum <= 2^32-1, unum will
//be a negative number
//unum <= 2^31 - 1
clb A,X0
asll.l X0,A //normalize unum
neg X0 //set exponent
add.w #31,X0
rtsd
move.w A1,A0 //mantissa
move.w X0,A1 //exponent
sxt.l A //sign-extend A to 36 bits
//FFloat zero = 0xFF800000
Zero:
rtsd
move.w #$FF80,A
clr.w A0
//If unum is between 2^31 and 2^32-1
LongUnsigned:
lsr.w A //divide mantissa by 2
move.w A1,A0 //move mantissa to its right place
//divide the mantissa by two and increase the exponent by 1
//this will correct the sign of A while keeping the absolute
//value of a the same
rtsd
move.w #32,A1 //exponent will always be 32 for this case
sxt.l A //sign-extend A to 36 bits
}
//convert an int32 to an ffloat value
asm ffloat S32int2FFloat(register long int inum)
{
//inum = 0
tst.l A
jeq Zero
//inum != 0
clb A,X0
asll.l X0,A //normalize inum
neg X0 //set exponent
add.w #31,X0
rtsd
move.w A1,A0 //mantissa
move.w X0,A1 //exponent
sxt.l A //sign-extend A to 36 bits
//FFloat zero = 0xFF800000
Zero:
rtsd
move.w #$FF80,A
clr.w A0
}
//typedef long unsigned int ffloat;
asm ffloat FFmult(register ffloat ffnum1, register ffloat ffnum2)
{
move.w B1,Y1 //This is to save exp2, use B for mult, and prepare for exp add
move.w A0,X0 //Can't multiply A0,B0 directly
move.w B0,Y0
mpyr X0,Y0,B //Multiply with round; result unlikely to differ from mpy, since truncated later
asr B //Shift right, so CLB can give correct count
clb B,X0 //Count sign bits for normalization
asll.l X0,B //Normalize
tst.w B1 //Check if relevant part of result is zero
jeq Zero //Go to zero handler
add A,Y1 //add A1 to Y1
sub X0,Y1 //Update exponent after normalization
inc.w Y1 //Return to normal scale
clb Y1,Y0 //count sign bits in exponent word
cmp.w #8,Y0 //If <8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rtsd //return with 3-cyle delay
move.w Y1,A //Put exp in return register
rnd B //Round to 16 bits in B1
move.w B1,A0 //Move mantissa to A0
//end of mult routine
Zero:
rtsd //return with 3-cyle delay
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y1 //Check for overflow
jle Underflow //If not overflow, go to underflow check
tst.w B1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$7FFF,A0 //Max out mantissa
rtsd //Delayed return - will execute next three words
nop //Filler for third delay slot
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //Filler for third delay slot
//end
Underflow:
cmp.w #$FF80,Y1 //Check for underflow
jge Continue //Not an error - continue normal code
tst.w B1 //Positive or negative overflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return - will execute next three words
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //Filler for third delay slot
//end of Exp_Err
}
asm ffloat FFsub(register ffloat ffnum1,register ffloat ffnum2)
{
move.w A0,X0 //Store ffnum1 mantissa temporarily in X0
move.w B1,Y1 //Store ffnum2 mantissa temporarily in Y1
move.w B0,B //Prepare to negate B
asr B //Prevent overflow
inc.w Y1 //Adjust exponent
neg B //Negate
clb B,Y0 //Count leading bits
asll.l Y0,B //rescale
sub Y0,Y1 //adjust exponent
move.w B1,Y0
move.w Y1,B
move.w Y0,B0
move.w A1,Y1 //Put ffnum1 exponent (exp1) in Y1
sub B,Y1 //Y1 = exp1 - exp2
//Setup: Larger ffnum exponent goes in Y0; mantissa to be shifted goes in B1;
//mantissa to stay the same goes in A1; abs exp difference goes in Y1
tlt B,A //Move ffnum2 (mantissa and exp) to A (not shifted) if Y1 neg
tlt X0,B //Move ffnum1 mantissa to B1 for shifting if Y1 neg
tge Y0,B //Move ffnum2 mantissa to B1 for shifting if Y1 not negative
abs Y1 //positive shift values
cmp.w #15,Y1 //More than 15-bit shift (ASRAC only works to 15 bits)?
jgt Neglect //If yes, an input ffnum will go to zero if shifted
move.w A1,Y0 //Move larger exp to Y0 for shifting
move.w A0,A //Move mantissa A0 to A1 for adding
asrac B1,Y1,A //Extend B1 to 36 bits, shift right by Y1, and add to A
asr A //Shift right to prevent overflow of CLB (next)
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
tst.w A1 //Check if relevant part of result is zero
jeq Zero //Result is zero
sub X0,Y0 //Adjust exponent of exp1
inc.w Y0 //Return to normal scale
clb Y0,X0 //check size of exponent word
cmp.w #8,X0
jlt Exp_Err
Continue:
rnd A //Round to 16 bits
rtsd //delayed return from subroutine
move.w A,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main add function
Zero:
rtsd //Delayed return from subroutine - will execute next three inst.
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //filler for third delay slot
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //filler for third delay slot
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three inst.
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three inst.
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
Neglect:
rts //The input with the larger exp becomes the output
}
asm ffloat IEEE2FFloat(register float fnum)
{
bftstl #$7F80,A1
jcs Zero //For IEEE, zero is indicated by zero exp.
move.w A1,Y0
bfclr #$FF00,A1
sxt.l A //Sign-extend A to 36 bits
bfset #$0080,A1
brclr #$8000,Y0,L1 //Branch if sign bit is positive
neg A //Negate mantissa if sign bit is negative
L1:
clb A,X0 //Normalize mantissa
asll.l X0,A
bfclr #$807F,Y0
lsrr.w #7,Y0
sub.w #119,Y0
sub X0,Y0 //FFloat exponent is ready
clb Y0,X0 //Check for overflow/underflow
cmp.w #8,X0
jlt Exp_Err
Continue:
rnd A
rtsd
move.w A,A0
move.w Y0,A1
sxt.l A //Sign-extend A to 36 bits
//end
Zero:
RTSD
MOVE.W #$FF80,A
CLR.W A0
//END
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //filler for third delay slot
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //filler for third delay slot
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
//A not very good C function. Ok for testing other functions in simulation.
//Converts an FFloat number to an IEEE 754-compatible single precision floating point number.
//typedef long unsigned int ffloat;
float FFloat2IEEE(ffloat ffnum)
{
float fout = 0;
long int iexp = 0;
long unsigned int tempout = 0, sign = 0, mantissa = 0, exp = 0;
void *VoidPointer;
float *FloatPointer;
long unsigned int *LintPointer;
if (ffnum&0xFFFF) //ffnum is not zero
{
mantissa = ffnum & 0x0000FFFF;
exp = ffnum&0xFFFF0000;
iexp = (long int)exp;
iexp += 0x007F0000; //Bias exponent positive by 127
if (iexp < 0x00010000) //Limit exponent size to allowed IEEE range
{
iexp = 0x00010000;
}
else if (iexp > 0x00FE0000)
{
iexp = 0x00FE0000;
}
if (mantissa&0x00008000) //ffnum is negative
{
sign = 0x80000000;
mantissa ^= 0x0000FFFF; //Negate
mantissa++;
}
while (!(mantissa&0x8000)) //normalize
{
mantissa <<= 1;
iexp -= 0x00010000;
}
if (iexp < 0x00010000) //Limit exponent size to allowed IEEE range
{
iexp = 0x00010000;
}
else if (iexp > 0x00FE0000)
{
iexp = 0x00FE0000;
}
exp = (long unsigned int)iexp;
exp <<= 7; //Shift exponent to correct position
mantissa <<= 8; //Shift to correct IEEE position
mantissa &= 0x007FFFFF; //Clear leading one
tempout = sign | exp | mantissa;
}
else exp = 0x00000000; //zero
VoidPointer = &(tempout); //obtain pointer to unsigned long int tempout
FloatPointer = VoidPointer; //convert to float
fout = *FloatPointer;
return(fout);
}
//return true if ffnum1>ffnum2, false otherwise
asm bool FFgt(register ffloat ffnum1, register ffloat ffnum2)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, a>b
rtsd
move.w #1,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #0,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aGTb //if(expB<expA) then a>b
bgt aNotGTb //if(expB>expA) then !(a>b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
blt aGTb //if(mantissaB<mantissaA) then a>b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aGTb //if(expB>expA) then a>b
blt aNotGTb //if(expB<expA) then !(a>b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
blt aGTb //if(mantissaB<mantissaA) then a>b
rtsd
move.w #0,Y0
nop
nop
//if a>b, go here
aGTb:
rtsd
move.w #1,Y0
nop
nop
//if a<=b, go here
aNotGTb:
rtsd
move.w #0,Y0
nop
nop
}
//return true if ffnum>0, false otherwise
asm bool FFgtz(register ffloat ffnum)
{
//Test ffnum mantissa
tst.w A0
bgt Positive
//ffnum <= 0
rtsd //delayed return
clr.w Y0 //return value 0
nop //first filler instruction
nop //second filler instruction
//end
Positive:
//ffnum > 0
rtsd //delayed return
move.w #1,Y0 //return value 1
nop //first filler instruction
nop //second filler instruction
//end
}
//return true if ffnum<0, false otherwise
asm bool FFltz(register ffloat ffnum)
{
//Test ffnum mantissa
tst.w A0
blt Negative
//ffnum >= 0
rtsd //delayed return
clr.w Y0 //return value 0
nop //first filler instruction
nop //second filler instruction
//end
Negative:
//ffnum < 0
rtsd //delayed return
move.w #1,Y0 //return value 1
nop //first filler instruction
nop //second filler instruction
//end
}
//return true if ffnum=0, false otherwise
asm bool FFeqz(register ffloat ffnum)
{
//Test ffnum mantissa
tst.w A0
beq Zero
//ffnum != 0
rtsd //delayed return
clr.w Y0 //return value 0
nop //first filler instruction
nop //second filler instruction
//end
Zero:
//ffnum < 0
rtsd //delayed return
move.w #1,Y0 //return value 1
nop //first filler instruction
nop //second filler instruction
//end
}
//return true if ffnum1<ffnum2, false otherwise
asm bool FFlt(register ffloat ffnum1, register ffloat ffnum2)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, !(a<b)
rtsd
move.w #0,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #1,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aLTb //if(expB>expA) then a<b
blt aNotLTb //if(expB<expA) then !(a<b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bgt aLTb //if(mantissaB>mantissaA) then a<b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aLTb //if(expB<expA) then a<b
bgt aNotLTb //if(expB>expA) then !(a<b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bgt aLTb //if(mantissaB>mantissaA) then a<b
rtsd
move.w #0,Y0
nop
nop
//if a<b, go here
aLTb:
rtsd
move.w #1,Y0
nop
nop
//if a>=b, go here
aNotLTb:
rtsd
move.w #0,Y0
nop
nop
}
//return true if a>=b, false otherwise
asm bool FFgte(register ffloat a, register ffloat b)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, a>=b
rtsd
move.w #1,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #0,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aGTEb //if(expB<expA) then a>=b
bgt aNotGTEb //if(expB>expA) then !(a>=b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
ble aGTEb //if(mantissaB<=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aGTEb //if(expB>expA) then a>b
blt aNotGTEb //if(expB<expA) then !(a>b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
ble aGTEb //if(mantissaB<=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//if a>=b, go here
aGTEb:
rtsd
move.w #1,Y0
nop
nop
//if a<b, go here
aNotGTEb:
rtsd
move.w #0,Y0
nop
nop
}
//return true if a<=b, false otherwise
asm bool FFlte(register ffloat a, register ffloat b)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, !(a<=b)
rtsd
move.w #0,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #1,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aLTEb //if(expB>expA) then a<=b
blt aNotLTEb //if(expB>expA) then !(a<=b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bge aLTEb //if(mantissaB>=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aLTEb //if(expB<expA) then a<=b
bgt aNotLTEb //if(expB>expA) then !(a<=b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bge aLTEb //if(mantissaB>=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//if a<=b, go here
aLTEb:
rtsd
move.w #1,Y0
nop
nop
//if a>b, go here
aNotLTEb:
rtsd
move.w #0,Y0
nop
nop
}
ffloat FFsin(ffloat xin)
{
int k,klo,khi;
ffloat xdiff0, xdiff1;
ffloat x=xin;
static ffloat xlo = 0x00029b78;
static ffloat xhi = 0x00026487;
static ffloat ya[31] = {0xffccb968, 0xfffe958c, 0xffff97e0, 0x0000b4c3, 0x0000a0e0, 0x00009126, 0x00008643, 0x000080b3, 0x000080b3, 0x00008643, 0x00009126, 0x0000a0e0, 0x0000b4c3, 0xffff97e0, 0xfffe958c, 0xff800000, 0xfffe6a73, 0xffff681f, 0x00004b3c, 0x00005f1f, 0x00006ed9, 0x000079bc, 0x00007f4c, 0x00007f4c, 0x000079bc, 0x00006ed9, 0x00005f1f, 0x00004b3c, 0xffff681f, 0xfffe6a73, 0xffcc4698};
static ffloat y2a[31] = {0xff800000, 0xfffd6a0f, 0xfffe67be, 0xffff4af6, 0xffff5ec6, 0xffff6e72, 0xffff794a, 0xffff7ed5, 0xffff7ed5, 0xffff794a, 0xffff6e72, 0xffff5ec6, 0xffff4af6, 0xfffe67be, 0xfffd6a0f, 0xff800000, 0xfffd95f0, 0xfffe9841, 0xffffb509, 0xffffa139, 0xffff918d, 0xffff86b5, 0xffff812a, 0xffff812a, 0xffff86b5, 0xffff918d, 0xffffa139, 0xffffb509, 0xfffe9841, 0xfffd95f0, 0xfffd95f0};
static int numpoints = 31;
static ffloat h = 0xfffe6b3b;
static ffloat hinv = 0x00034c64;
static ffloat pi2=0x00036487;
static ffloat pi2inv=0xfffe517c;
if(FFlt(xin,xlo)){
x=FFadd(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xhi,xin),
pi2inv
)
)
),
pi2
)
);
}else if(FFgt(xin,xhi)){
x=FFsub(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xin,xlo),
pi2inv
)
)
),
pi2
)
);
}
klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
khi=klo+1;
xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
xdiff1 = FFsub(xdiff0, h);
return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}Fast non-754 floating point library for Freescale 56F800E series
#ifndef ALREADY_READ_FFLOATV2A_H
#define ALREADY_READ_FFLOATV2A_H
/*******************************************************************************
FFloat Number Definitions
*******************************************************************************/
#define MINUSONEFF 0x00008000 //FFloat number -1
#define ZEROFF 0xFF800000 //FFloat number 0
#define ONEFF 0x00014000 //FFloat number 1
#define TWOFF 0x00024000 //FFloat number 2
#define THREEFF 0x00026000 //FFloat number 3
#define FOURFF 0x00034000 //FFloat number 4
#define FIVEFF 0x00035000 //FFloat number 5
#define SIXFF 0x00036000 //FFloat number 6
#define SEVENFF 0x00037000 //FFloat number 7
#define EIGHTFF 0x00044000 //FFloat number 8
#define NINEFF 0x00044800 //FFloat number 9
#define TENFF 0x00045000 //FFloat number 10
#define ELEVENFF 0x00045800 //FFloat number 11
#define TWELVEFF 0x00046000 //FFloat number 12
#define NOMINALBATVOLTAGE 0x00044800
/*******************************************************************************
FFloat Data Type Definition
*******************************************************************************/
typedef unsigned char bool;
typedef long unsigned int ffloat;
/*******************************************************************************
FFloat FUNCTION PROTOTYPES
*******************************************************************************/
asm ffloat FFabs(register ffloat ffnum);
asm ffloat FFneg(register ffloat ffnum);
asm ffloat S16int2FFloat(register short int inum);
asm ffloat S32int2FFloat(register long int inum);
asm ffloat U32int2FFloat(register long unsigned int unum);
asm ffloat FFadd(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat FFdiv(register ffloat ffnum1,register ffloat ffnum2);
asm short int FFloatTrunc2S16int(register ffloat ffnum);
asm short int FFloatRnd2S16int(register ffloat ffnum);
asm ffloat FFmult(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat FFsub(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat IEEE2FFloat(register float fnum);
float FFloat2IEEE(ffloat ffnum);
asm bool FFgt(register ffloat ffnum1, register ffloat ffnum2);
asm bool FFlt(register ffloat ffnum1, register ffloat ffnum2);
asm bool FFgte(register ffloat a, register ffloat b);
asm bool FFlte(register ffloat a, register ffloat b);
asm bool FFgtz(register ffloat ffnum);
asm bool FFltz(register ffloat ffnum);
asm bool FFeqz(register ffloat ffnum);
ffloat FFatan(ffloat xin);
ffloat FFsin(ffloat xin);
ffloat FFcos(ffloat xin);
#endif
**********************************************************
Function code begins below
**********************************************************
#include "FFloatV2A.h"
ffloat FFatan(ffloat xin)
{
int k,klo,khi;
ffloat xdiff0, xdiff1;
ffloat x=xin;
static ffloat xlo = 0x0005b000;
static ffloat xhi = 0x00055000;
static ffloat ya[151] = {0x00019eaa, 0x00019eb5, 0x00019ec0, 0x00019ecc, 0x00019ed8, 0x00019ee4, 0x00019ef1, 0x00019efe, 0x00019f0c, 0x00019f19, 0x00019f28, 0x00019f36, 0x00019f46, 0x00019f55, 0x00019f66, 0x00019f76, 0x00019f88, 0x00019f99, 0x00019fac, 0x00019fbf, 0x00019fd3, 0x00019fe8, 0x00019ffd, 0x0001a013, 0x0001a02a, 0x0001a042, 0x0001a05b, 0x0001a075, 0x0001a090, 0x0001a0ac, 0x0001a0ca, 0x0001a0e9, 0x0001a109, 0x0001a12b, 0x0001a14e, 0x0001a173, 0x0001a19a, 0x0001a1c3, 0x0001a1ee, 0x0001a21c, 0x0001a24c, 0x0001a27f, 0x0001a2b5, 0x0001a2ef, 0x0001a32c, 0x0001a36d, 0x0001a3b3, 0x0001a3fd, 0x0001a44d, 0x0001a4a2, 0x0001a4ff, 0x0001a563, 0x0001a5d0, 0x0001a646, 0x0001a6c7, 0x0001a754, 0x0001a7f0, 0x0001a89d, 0x0001a95d, 0x0001aa33, 0x0001ab25, 0x0001ac37, 0x0001ad71, 0x0001aeda, 0x0001b07f, 0x0001b26e, 0x0001b4bc, 0x0001b785, 0x0001baf1, 0x0001bf38, 0x0000894e, 0x00009757, 0x0000a9a2, 0xffff8292, 0xffffbd49, 0xff800000, 0xffff42b6, 0xffff7d6d, 0x0000565d, 0x000068a8, 0x000076b1, 0x000140c7, 0x0001450e, 0x0001487a, 0x00014b43, 0x00014d91, 0x00014f80, 0x00015125, 0x0001528e, 0x000153c8, 0x000154da, 0x000155cc, 0x000156a2, 0x00015762, 0x0001580f, 0x000158ab, 0x00015938, 0x000159b9, 0x00015a2f, 0x00015a9c, 0x00015b00, 0x00015b5d, 0x00015bb2, 0x00015c02, 0x00015c4c, 0x00015c92, 0x00015cd3, 0x00015d10, 0x00015d4a, 0x00015d80, 0x00015db3, 0x00015de3, 0x00015e11, 0x00015e3c, 0x00015e65, 0x00015e8c, 0x00015eb1, 0x00015ed4, 0x00015ef6, 0x00015f16, 0x00015f35, 0x00015f53, 0x00015f6f, 0x00015f8a, 0x00015fa4, 0x00015fbd, 0x00015fd5, 0x00015fec, 0x00016002, 0x00016017, 0x0001602c, 0x00016040, 0x00016053, 0x00016066, 0x00016077, 0x00016089, 0x00016099, 0x000160aa, 0x000160b9, 0x000160c9, 0x000160d7, 0x000160e6, 0x000160f3, 0x00016101, 0x0001610e, 0x0001611b, 0x00016127, 0x00016133, 0x0001613f, 0x0001614a, 0x00016155};
static ffloat y2a[151] = {0xff800000, 0xfff443e4, 0xfff446b6, 0xfff449b0, 0xfff44cd5, 0xfff45029, 0xfff453af, 0xfff4576a, 0xfff45b5f, 0xfff45f92, 0xfff46408, 0xfff468c6, 0xfff46dd1, 0xfff47331, 0xfff478ec, 0xfff47f0a, 0xfff542c9, 0xfff54648, 0xfff54a06, 0xfff54e0a, 0xfff55259, 0xfff556fa, 0xfff55bf6, 0xfff56156, 0xfff56722, 0xfff56d66, 0xfff5742f, 0xfff57b8a, 0xfff641c3, 0xfff6461c, 0xfff64ad8, 0xfff65004, 0xfff655ac, 0xfff65be0, 0xfff662b0, 0xfff66a30, 0xfff67278, 0xfff67ba1, 0xfff742e5, 0xfff7488b, 0xfff74ed9, 0xfff755e6, 0xfff75dd0, 0xfff766ba, 0xfff770cc, 0xfff77c39, 0xfff8449e, 0xfff84c0f, 0xfff8549c, 0xfff85e7b, 0xfff869ef, 0xfff8774e, 0xfff9437f, 0xfff94cc5, 0xfff957cc, 0xfff96504, 0xfff974fc, 0xfffa4439, 0xfffa5032, 0xfffa5f16, 0xfffa71cd, 0xfffb44d0, 0xfffb542e, 0xfffb684a, 0xfffc4182, 0xfffc538f, 0xfffc6c5c, 0xfffd4779, 0xfffd5fe2, 0xfffe4133, 0xfffe5918, 0xfffe77b6, 0xffff4b62, 0xffff503a, 0xfffe707d, 0xff800000, 0xfffe8f82, 0xffffafc5, 0xffffb49d, 0xfffe8849, 0xfffea6e7, 0xfffebecc, 0xfffda01d, 0xfffdb886, 0xfffc93a3, 0xfffcac70, 0xfffcbe7d, 0xfffb97b5, 0xfffbabd1, 0xfffbbb2f, 0xfffa8e32, 0xfffaa0e9, 0xfffaafcd, 0xfffabbc6, 0xfff98b03, 0xfff99afb, 0xfff9a833, 0xfff9b33a, 0xfff9bc80, 0xfff888b1, 0xfff89610, 0xfff8a184, 0xfff8ab63, 0xfff8b3f0, 0xfff8bb61, 0xfff783c6, 0xfff78f33, 0xfff79945, 0xfff7a22f, 0xfff7aa19, 0xfff7b126, 0xfff7b774, 0xfff7bd1a, 0xfff6845e, 0xfff68d87, 0xfff695cf, 0xfff69d4f, 0xfff6a41f, 0xfff6aa53, 0xfff6affb, 0xfff6b527, 0xfff6b9e3, 0xfff6be3c, 0xfff58475, 0xfff58bd0, 0xfff59299, 0xfff598dd, 0xfff59ea9, 0xfff5a409, 0xfff5a905, 0xfff5ada6, 0xfff5b1f5, 0xfff5b5f9, 0xfff5b9b7, 0xfff5bd36, 0xfff480f5, 0xfff48713, 0xfff48cce, 0xfff4922e, 0xfff49739, 0xfff49bf7, 0xfff4a06d, 0xfff4a4a0, 0xfff4a895, 0xfff4ac50, 0xfff4afd6, 0xfff4b32a, 0xfff4b64f, 0xfff4b949, 0xfff4bc1b, 0xfff4bc1b};
static int numpoints = 151;
static ffloat h = 0xffff4444;
static ffloat hinv = 0x00027800;
klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
khi=klo+1;
if(FFlt(x,xlo)){
return(ya[0]);
}else if(FFgt(x,xhi)){
return(ya[numpoints-1]);
}
xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
xdiff1 = FFsub(xdiff0, h);
return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}
ffloat FFcos(ffloat xin)
{
int k,klo,khi;
ffloat xdiff0, xdiff1;
ffloat x=xin;
static ffloat xlo = 0x00029b78;
static ffloat xhi = 0x00026487;
static ffloat ya[31] = {0x00008000, 0x000082cc, 0x00008b10, 0x00009872, 0x0000aa59, 0xffff8000, 0xffffb0e4, 0xfffd94f6, 0xfffd6b09, 0xffff4f1b, 0x00004000, 0x000055a6, 0x0000678d, 0x000074ef, 0x00007d33, 0x00014000, 0x00007d33, 0x000074ef, 0x0000678d, 0x000055a6, 0x00004000, 0xffff4f1b, 0xfffd6b09, 0xfffd94f6, 0xffffb0e4, 0xffff8000, 0x0000aa59, 0x00009872, 0x00008b10, 0x000082cc, 0x00008000};
static ffloat y2a[31] = {0xff800000, 0xffff7cbe, 0xffff7481, 0xffff672d, 0xffff5556, 0xfffe7f88, 0xfffe4ed1, 0xfffc6aa5, 0xfffc955a, 0xfffeb12e, 0xfffe8077, 0xffffaaa9, 0xffff98d2, 0xffff8b7e, 0xffff8341, 0xffff8077, 0xffff8341, 0xffff8b7e, 0xffff98d2, 0xffffaaa9, 0xfffe8077, 0xfffeb12e, 0xfffc955a, 0xfffc6aa5, 0xfffe4ed1, 0xfffe7f88, 0xffff5556, 0xffff672d, 0xffff7481, 0xffff7cbe, 0xffff7cbe};
static int numpoints = 31;
static ffloat h = 0xfffe6b3b;
static ffloat hinv = 0x00034c64;
static ffloat pi2=0x00036487;
static ffloat pi2inv=0xfffe517c;
if(FFlt(xin,xlo)){
x=FFadd(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xhi,xin),
pi2inv
)
)
),
pi2
)
);
}else if(FFgt(xin,xhi)){
x=FFsub(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xin,xlo),
pi2inv
)
)
),
pi2
)
);
}
klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
khi=klo+1;
xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
xdiff1 = FFsub(xdiff0, h);
return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}
//Return the negative of ffnum
asm ffloat FFneg(register ffloat ffnum)
{
move.w A1,Y0 //store ffnum exp in Y0
move.w A0,A //A holds mantissa of ffnum
neg A //full 36-bit negate
asr A //shift right to prevent overflow of clb
jeq Zero //Don't normalize if zero
//ffnum != 0
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
sub X0,Y0 //Adjust exponent
inc.w Y0 //Return to normal scale
clb Y0,X0 //check number of sign bits in exponent
cmp.w #8,X0 //If less than 8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rtsd //delayed return from subroutine
move.w A1,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main neg function
Zero:
rtsd //Delayed return from subroutine - will execute next three words
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //Delay slot filler
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three cycles
move.w #$8000,A0 //Most negative mantissa
nop //Delay slot filler
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
//Return the absolute value of ffnum
asm ffloat FFabs(register ffloat ffnum)
{
move.w A1,Y0 //store ffnum exp in Y0
move.w A0,A //A holds mantissa of ffnum
abs A //full-width absolute value
asr A //shift right to prevent overflow of clb
jeq Zero //Don't normalize if zero
//ffnum != 0
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
sub X0,Y0 //Adjust exponent
inc.w Y0 //Return to normal scale
clb Y0,X0 //check number of sign bits in exponent
cmp.w #8,X0 //If less than 8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rtsd //delayed return from subroutine
move.w A,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main abs function
Zero:
rtsd //Delayed return from subroutine - will execute next three words
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //Delay slot filler
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three cycles
move.w #$8000,A0 //Most negative mantissa
nop //Delay slot filler
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
//convert an int16 to an ffloat value
asm ffloat S16int2FFloat(register short int inum)
{
tst.w Y0
jeq Zero
//inum != 0
clb Y0,X0
asll.l X0,Y0 //normalize inum
neg X0 //set exponent
rtsd
add.w #15,X0
move.w X0,A //exponent
move.w Y0,A0 //mantissa
//FFloat zero = 0xFF800000
Zero:
rtsd
move.w #$FF80,A
clr.w A0
}
asm ffloat FFadd(register ffloat ffnum1,register ffloat ffnum2)
{
move.w A0,X0 //Store ffnum1 mantissa temporarily in X0
move.w B0,Y0 //Store ffnum2 mantissa temporarily in Y0
move.w A1,Y1 //Put ffnum1 exponent (exp1) in Y1
sub B,Y1 //Y1 = exp1 - exp2
//Setup: Larger ffnum exponent goes in Y0; mantissa to be shifted goes in B1;
//mantissa to stay the same goes in A1; abs exp difference goes in Y1
tlt B,A //Move ffnum2 (mantissa and exp) to A (not shifted) if Y1 neg
tlt X0,B //Move ffnum1 mantissa to B1 for shifting if Y1 neg
tge Y0,B //Move ffnum2 mantissa to B1 for shifting if Y1 not negative
abs Y1 //positive shift values
cmp.w #15,Y1 //More than 15-bit shift (ASRAC only works to 15 bits)?
jgt Neglect //If yes, an input ffnum will go to zero if shifted
move.w A1,Y0 //Move larger exp to Y0 for shifting
move.w A0,A //Move mantissa A0 to A1 for adding
asrac B1,Y1,A //Extend B1 to 36 bits, shift right by Y1, and add to A
asr A //Shift right to prevent overflow of CLB (next)
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
tst.w A1 //Check if relevant part of result is zero
jeq Zero //Result is zero
sub X0,Y0 //Adjust exponent of exp1
inc.w Y0 //Return to normal scale
clb Y0,X0 //check number of sign bits in exponent
cmp.w #8,X0 //If less than 8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rnd A //round to 16 bits in A1
rtsd //delayed return from subroutine
move.w A,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main add function
Zero:
rtsd //Delayed return from subroutine - will execute next three words
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //Delay slot filler
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three cycles
move.w #$8000,A0 //Most negative mantissa
nop //Delay slot filler
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
Neglect:
rts //The input with the larger exp becomes the output
}
asm ffloat FFdiv(register ffloat ffnum1, register ffloat ffnum2)
{
move.w A1,X0 //Move exponent of ffnum1 to X0
move.w B1,Y0 //Move exponent of ffnum2 to Y0
move.w A0,Y1 //Move mantissa of ffnum1 to Y1 for sign check
move.w A0,A //Move mantissa of ffnum1 to A1
move.w B0,B //Move mantissa of ffnum2 to B1
eor.w B,Y1 //Calculate sign of final result
//(sign bit of result will be 1=negative if inputs signs differ)
abs A
abs B
jeq DivZero //ffnum2 cannot be zero
L1:
cmp A,B //Check result of B - A
bgt L2 //Ready to divide
brad L1 //Recheck (delayed branch)
asr A //Reduce ffnum1 mantissa by factor of 2
inc.w X0 //Increase ffnum1 exponent by one
//end
L2:
//Division of Positive Fractional Data (A1:A0 / B1)
BFCLR #$0001,SR //Clear carry bit: required for 1st DIV instruction
//REP #16
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
DIV B1,A //Form positive quotient in A0
move.w A0,A //Move A0 to A1
tst.w Y1 //Check sign needed for final result
BGE L3 //Branch if final sign is non-neg
NEG A //Negate mantissa if result is neg
L3:
clb A,Y1 //Count sign bits
asll.l Y1,A //Normalize
tst A //Check if relevant part of result is zero
jeq Zero //Result is zero
sub Y0,X0 //Adjust exponent of exp1
sub Y1,X0
clb X0,Y0 //check size of exponent word
cmp.w #8,Y0
jlt Exp_Err
Continue:
RTSD
MOVE.W A,A0
MOVE.W X0,A1
sxt.l A //Sign-extend A to 36 bits
//END
DivZero:
//Call error handler here
MOVE.W #$007F,A //Needs work here
RTSD
MOVE.W #$7FFF,A0
NOP
//END
Zero:
RTSD
MOVE.W #$FF80,A
CLR.W A0
//END
Exp_Err:
cmp.w #$007F,X0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //filler for third delay slot
//end
Underflow:
cmp.w #$FF80,X0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
asm short int FFloatRnd2S16int(register ffloat ffnum)
{
move.w A1,Y0
move.w A0,A
//Scale so that exponent = 15; converts mantissa to integer scale
//Check if resulting mantissa is in range -32768 to 32767 (16 bit signed int)
sub.w #15,Y0
jgt Over //Number is outside range -32768 to 32767
cmp.w #-17,Y0
jlt Zero //Number is small and rounds to zero
rtsd
asll.l Y0,A //Scale to exponent = 15 (one word, two cycles)
rnd A //Convergent rounding (round down boundary case if even)
move.w A1,Y0
//end
Zero:
rtsd
clr.w Y0 //Result is zero
nop
nop
//end
Over:
tst A
blt Neg //branch to Neg: if number is below -32768
rtsd
move.w #$7FFF,Y0 //Set to most positive 16-bit value
nop //Filler for third delay slot
//end
Neg:
rtsd
move.w #$8000,Y0 //Set to most negative 16-bit value
nop //Filler for third delay slot
//end
}
asm short int FFloatTrunc2S16int(register ffloat ffnum)
{
move.w A1,Y0
move.w A0,A
//Scale so that exponent = 15; converts mantissa to integer scale
//Check if resulting mantissa is in range -32768 to 32767 (16 bit signed int)
sub.w #15,Y0
jgt Over //Number is outside range -32768 to 32767
cmp.w #-17,Y0
jlt Zero //Number is small and rounds to zero
rtsd
asll.l Y0,A //Scale to exponent = 15 (one word, two cycles)
move.w A1,Y0
nop //Filler for third delay slot
//end
Zero:
rtsd
clr.w Y0 //Result is zero
nop
nop
//end
Over:
tst A
blt Neg //branch to Neg: if number is below -32768
rtsd
move.w #$7FFF,Y0 //Set to most positive 16-bit value
nop //Filler for third delay slot
//end
Neg:
rtsd
move.w #$8000,Y0 //Set to most negative 16-bit value
nop //Filler for third delay slot
//end
}
//convert an unsigned int32 to an ffloat value
asm ffloat U32int2FFloat(register long unsigned int unum)
{
tst.l A
jeq Zero //unum = 0
jlt LongUnsigned //If 2^31 <= unum <= 2^32-1, unum will
//be a negative number
//unum <= 2^31 - 1
clb A,X0
asll.l X0,A //normalize unum
neg X0 //set exponent
add.w #31,X0
rtsd
move.w A1,A0 //mantissa
move.w X0,A1 //exponent
sxt.l A //sign-extend A to 36 bits
//FFloat zero = 0xFF800000
Zero:
rtsd
move.w #$FF80,A
clr.w A0
//If unum is between 2^31 and 2^32-1
LongUnsigned:
lsr.w A //divide mantissa by 2
move.w A1,A0 //move mantissa to its right place
//divide the mantissa by two and increase the exponent by 1
//this will correct the sign of A while keeping the absolute
//value of a the same
rtsd
move.w #32,A1 //exponent will always be 32 for this case
sxt.l A //sign-extend A to 36 bits
}
//convert an int32 to an ffloat value
asm ffloat S32int2FFloat(register long int inum)
{
//inum = 0
tst.l A
jeq Zero
//inum != 0
clb A,X0
asll.l X0,A //normalize inum
neg X0 //set exponent
add.w #31,X0
rtsd
move.w A1,A0 //mantissa
move.w X0,A1 //exponent
sxt.l A //sign-extend A to 36 bits
//FFloat zero = 0xFF800000
Zero:
rtsd
move.w #$FF80,A
clr.w A0
}
//typedef long unsigned int ffloat;
asm ffloat FFmult(register ffloat ffnum1, register ffloat ffnum2)
{
move.w B1,Y1 //This is to save exp2, use B for mult, and prepare for exp add
move.w A0,X0 //Can't multiply A0,B0 directly
move.w B0,Y0
mpyr X0,Y0,B //Multiply with round; result unlikely to differ from mpy, since truncated later
asr B //Shift right, so CLB can give correct count
clb B,X0 //Count sign bits for normalization
asll.l X0,B //Normalize
tst.w B1 //Check if relevant part of result is zero
jeq Zero //Go to zero handler
add A,Y1 //add A1 to Y1
sub X0,Y1 //Update exponent after normalization
inc.w Y1 //Return to normal scale
clb Y1,Y0 //count sign bits in exponent word
cmp.w #8,Y0 //If <8 (exp > 8 bits),
jlt Exp_Err //jump to exponent exception handler
Continue:
rtsd //return with 3-cyle delay
move.w Y1,A //Put exp in return register
rnd B //Round to 16 bits in B1
move.w B1,A0 //Move mantissa to A0
//end of mult routine
Zero:
rtsd //return with 3-cyle delay
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y1 //Check for overflow
jle Underflow //If not overflow, go to underflow check
tst.w B1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$7FFF,A0 //Max out mantissa
rtsd //Delayed return - will execute next three words
nop //Filler for third delay slot
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //Filler for third delay slot
//end
Underflow:
cmp.w #$FF80,Y1 //Check for underflow
jge Continue //Not an error - continue normal code
tst.w B1 //Positive or negative overflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return - will execute next three words
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //Filler for third delay slot
//end of Exp_Err
}
asm ffloat FFsub(register ffloat ffnum1,register ffloat ffnum2)
{
move.w A0,X0 //Store ffnum1 mantissa temporarily in X0
move.w B1,Y1 //Store ffnum2 mantissa temporarily in Y1
move.w B0,B //Prepare to negate B
asr B //Prevent overflow
inc.w Y1 //Adjust exponent
neg B //Negate
clb B,Y0 //Count leading bits
asll.l Y0,B //rescale
sub Y0,Y1 //adjust exponent
move.w B1,Y0
move.w Y1,B
move.w Y0,B0
move.w A1,Y1 //Put ffnum1 exponent (exp1) in Y1
sub B,Y1 //Y1 = exp1 - exp2
//Setup: Larger ffnum exponent goes in Y0; mantissa to be shifted goes in B1;
//mantissa to stay the same goes in A1; abs exp difference goes in Y1
tlt B,A //Move ffnum2 (mantissa and exp) to A (not shifted) if Y1 neg
tlt X0,B //Move ffnum1 mantissa to B1 for shifting if Y1 neg
tge Y0,B //Move ffnum2 mantissa to B1 for shifting if Y1 not negative
abs Y1 //positive shift values
cmp.w #15,Y1 //More than 15-bit shift (ASRAC only works to 15 bits)?
jgt Neglect //If yes, an input ffnum will go to zero if shifted
move.w A1,Y0 //Move larger exp to Y0 for shifting
move.w A0,A //Move mantissa A0 to A1 for adding
asrac B1,Y1,A //Extend B1 to 36 bits, shift right by Y1, and add to A
asr A //Shift right to prevent overflow of CLB (next)
clb A,X0 //Count sign bits
asll.l X0,A //Normalize
tst.w A1 //Check if relevant part of result is zero
jeq Zero //Result is zero
sub X0,Y0 //Adjust exponent of exp1
inc.w Y0 //Return to normal scale
clb Y0,X0 //check size of exponent word
cmp.w #8,X0
jlt Exp_Err
Continue:
rnd A //Round to 16 bits
rtsd //delayed return from subroutine
move.w A,A0 //Move mantissa of sum to lower word of ffnum1 (return value)
move.w Y0,A1 //Move exponent to upper word of ffnum1 (return value)
sxt.l A //Sign-extend A to 36 bits
//end of main add function
Zero:
rtsd //Delayed return from subroutine - will execute next three inst.
move.w #$FF80,A //Set exp of sum to minimum
clr.w A0 //Set mantissa of sum to 0
//end of zero handler
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //filler for third delay slot
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //filler for third delay slot
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU //If negative, go to negative handler
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three inst.
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three inst.
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
Neglect:
rts //The input with the larger exp becomes the output
}
asm ffloat IEEE2FFloat(register float fnum)
{
bftstl #$7F80,A1
jcs Zero //For IEEE, zero is indicated by zero exp.
move.w A1,Y0
bfclr #$FF00,A1
sxt.l A //Sign-extend A to 36 bits
bfset #$0080,A1
brclr #$8000,Y0,L1 //Branch if sign bit is positive
neg A //Negate mantissa if sign bit is negative
L1:
clb A,X0 //Normalize mantissa
asll.l X0,A
bfclr #$807F,Y0
lsrr.w #7,Y0
sub.w #119,Y0
sub X0,Y0 //FFloat exponent is ready
clb Y0,X0 //Check for overflow/underflow
cmp.w #8,X0
jlt Exp_Err
Continue:
rnd A
rtsd
move.w A,A0
move.w Y0,A1
sxt.l A //Sign-extend A to 36 bits
//end
Zero:
RTSD
MOVE.W #$FF80,A
CLR.W A0
//END
Exp_Err:
cmp.w #$007F,Y0
jle Underflow //If not overflow, go to underflow check
tst.w A1 //Positive or negative overflow?
jlt NegO //If negative, go to negative handler
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$7FFF,A0 //Max out mantissa
nop //filler for third delay slot
//end
NegO:
move.w #$007F,A //Max out exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$8000,A0 //Most negative mantissa
nop //filler for third delay slot
//end
Underflow:
cmp.w #$FF80,Y0 //Check for underflow
jge Continue //Not an error
tst.w A1 //Positive or negative underflow?
jlt NegU
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$4000,A0 //Minimum normalized positive mantissa
nop //Filler for third delay slot
//end
NegU:
move.w #$FF80,A //Minimum exponent
rtsd //Delayed return from subroutine - will execute next three words
move.w #$BFFF,A0 //Minimum (abs) normalized negative mantissa
nop //filler for third delay slot
//end of E_Err
}
//A not very good C function. Ok for testing other functions in simulation.
//Converts an FFloat number to an IEEE 754-compatible single precision floating point number.
//typedef long unsigned int ffloat;
float FFloat2IEEE(ffloat ffnum)
{
float fout = 0;
long int iexp = 0;
long unsigned int tempout = 0, sign = 0, mantissa = 0, exp = 0;
void *VoidPointer;
float *FloatPointer;
long unsigned int *LintPointer;
if (ffnum&0xFFFF) //ffnum is not zero
{
mantissa = ffnum & 0x0000FFFF;
exp = ffnum&0xFFFF0000;
iexp = (long int)exp;
iexp += 0x007F0000; //Bias exponent positive by 127
if (iexp < 0x00010000) //Limit exponent size to allowed IEEE range
{
iexp = 0x00010000;
}
else if (iexp > 0x00FE0000)
{
iexp = 0x00FE0000;
}
if (mantissa&0x00008000) //ffnum is negative
{
sign = 0x80000000;
mantissa ^= 0x0000FFFF; //Negate
mantissa++;
}
while (!(mantissa&0x8000)) //normalize
{
mantissa <<= 1;
iexp -= 0x00010000;
}
if (iexp < 0x00010000) //Limit exponent size to allowed IEEE range
{
iexp = 0x00010000;
}
else if (iexp > 0x00FE0000)
{
iexp = 0x00FE0000;
}
exp = (long unsigned int)iexp;
exp <<= 7; //Shift exponent to correct position
mantissa <<= 8; //Shift to correct IEEE position
mantissa &= 0x007FFFFF; //Clear leading one
tempout = sign | exp | mantissa;
}
else exp = 0x00000000; //zero
VoidPointer = &(tempout); //obtain pointer to unsigned long int tempout
FloatPointer = VoidPointer; //convert to float
fout = *FloatPointer;
return(fout);
}
//return true if ffnum1>ffnum2, false otherwise
asm bool FFgt(register ffloat ffnum1, register ffloat ffnum2)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, a>b
rtsd
move.w #1,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #0,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aGTb //if(expB<expA) then a>b
bgt aNotGTb //if(expB>expA) then !(a>b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
blt aGTb //if(mantissaB<mantissaA) then a>b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aGTb //if(expB>expA) then a>b
blt aNotGTb //if(expB<expA) then !(a>b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
blt aGTb //if(mantissaB<mantissaA) then a>b
rtsd
move.w #0,Y0
nop
nop
//if a>b, go here
aGTb:
rtsd
move.w #1,Y0
nop
nop
//if a<=b, go here
aNotGTb:
rtsd
move.w #0,Y0
nop
nop
}
//return true if ffnum>0, false otherwise
asm bool FFgtz(register ffloat ffnum)
{
//Test ffnum mantissa
tst.w A0
bgt Positive
//ffnum <= 0
rtsd //delayed return
clr.w Y0 //return value 0
nop //first filler instruction
nop //second filler instruction
//end
Positive:
//ffnum > 0
rtsd //delayed return
move.w #1,Y0 //return value 1
nop //first filler instruction
nop //second filler instruction
//end
}
//return true if ffnum<0, false otherwise
asm bool FFltz(register ffloat ffnum)
{
//Test ffnum mantissa
tst.w A0
blt Negative
//ffnum >= 0
rtsd //delayed return
clr.w Y0 //return value 0
nop //first filler instruction
nop //second filler instruction
//end
Negative:
//ffnum < 0
rtsd //delayed return
move.w #1,Y0 //return value 1
nop //first filler instruction
nop //second filler instruction
//end
}
//return true if ffnum=0, false otherwise
asm bool FFeqz(register ffloat ffnum)
{
//Test ffnum mantissa
tst.w A0
beq Zero
//ffnum != 0
rtsd //delayed return
clr.w Y0 //return value 0
nop //first filler instruction
nop //second filler instruction
//end
Zero:
//ffnum < 0
rtsd //delayed return
move.w #1,Y0 //return value 1
nop //first filler instruction
nop //second filler instruction
//end
}
//return true if ffnum1<ffnum2, false otherwise
asm bool FFlt(register ffloat ffnum1, register ffloat ffnum2)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, !(a<b)
rtsd
move.w #0,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #1,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aLTb //if(expB>expA) then a<b
blt aNotLTb //if(expB<expA) then !(a<b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bgt aLTb //if(mantissaB>mantissaA) then a<b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aLTb //if(expB<expA) then a<b
bgt aNotLTb //if(expB>expA) then !(a<b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bgt aLTb //if(mantissaB>mantissaA) then a<b
rtsd
move.w #0,Y0
nop
nop
//if a<b, go here
aLTb:
rtsd
move.w #1,Y0
nop
nop
//if a>=b, go here
aNotLTb:
rtsd
move.w #0,Y0
nop
nop
}
//return true if a>=b, false otherwise
asm bool FFgte(register ffloat a, register ffloat b)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, a>=b
rtsd
move.w #1,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #0,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aGTEb //if(expB<expA) then a>=b
bgt aNotGTEb //if(expB>expA) then !(a>=b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
ble aGTEb //if(mantissaB<=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aGTEb //if(expB>expA) then a>b
blt aNotGTEb //if(expB<expA) then !(a>b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
ble aGTEb //if(mantissaB<=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//if a>=b, go here
aGTEb:
rtsd
move.w #1,Y0
nop
nop
//if a<b, go here
aNotGTEb:
rtsd
move.w #0,Y0
nop
nop
}
//return true if a<=b, false otherwise
asm bool FFlte(register ffloat a, register ffloat b)
{
//First compare signs of numbers
tst.w A0
blt CheckSignANeg
//a is nonnegative
tst.w B0
//Both numbers are nonnegative - nonnegative exponents case
bge CasePNumExp
//If b is negative, !(a<=b)
rtsd
move.w #0,Y0
nop
nop
//a is negative
CheckSignANeg:
tst.w B0
//Both numbers are negative - negative exponents case
blt CaseNNumExp
//If b is nonnegative, a<b
rtsd
move.w #1,Y0
nop
nop
//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
bgt aLTEb //if(expB>expA) then a<=b
blt aNotLTEb //if(expB>expA) then !(a<=b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bge aLTEb //if(mantissaB>=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
//move exponent data to X0 and Y0 registers for comparison
move.w A1,X0
move.w B1,Y0
cmp.w X0,Y0
blt aLTEb //if(expB<expA) then a<=b
bgt aNotLTEb //if(expB>expA) then !(a<=b)
//If exponents are equal, check mantissas
move.w A0,X0
move.w B0,Y0
cmp.w X0,Y0
bge aLTEb //if(mantissaB>=mantissaA) then a>=b
rtsd
move.w #0,Y0
nop
nop
//if a<=b, go here
aLTEb:
rtsd
move.w #1,Y0
nop
nop
//if a>b, go here
aNotLTEb:
rtsd
move.w #0,Y0
nop
nop
}
ffloat FFsin(ffloat xin)
{
int k,klo,khi;
ffloat xdiff0, xdiff1;
ffloat x=xin;
static ffloat xlo = 0x00029b78;
static ffloat xhi = 0x00026487;
static ffloat ya[31] = {0xffccb968, 0xfffe958c, 0xffff97e0, 0x0000b4c3, 0x0000a0e0, 0x00009126, 0x00008643, 0x000080b3, 0x000080b3, 0x00008643, 0x00009126, 0x0000a0e0, 0x0000b4c3, 0xffff97e0, 0xfffe958c, 0xff800000, 0xfffe6a73, 0xffff681f, 0x00004b3c, 0x00005f1f, 0x00006ed9, 0x000079bc, 0x00007f4c, 0x00007f4c, 0x000079bc, 0x00006ed9, 0x00005f1f, 0x00004b3c, 0xffff681f, 0xfffe6a73, 0xffcc4698};
static ffloat y2a[31] = {0xff800000, 0xfffd6a0f, 0xfffe67be, 0xffff4af6, 0xffff5ec6, 0xffff6e72, 0xffff794a, 0xffff7ed5, 0xffff7ed5, 0xffff794a, 0xffff6e72, 0xffff5ec6, 0xffff4af6, 0xfffe67be, 0xfffd6a0f, 0xff800000, 0xfffd95f0, 0xfffe9841, 0xffffb509, 0xffffa139, 0xffff918d, 0xffff86b5, 0xffff812a, 0xffff812a, 0xffff86b5, 0xffff918d, 0xffffa139, 0xffffb509, 0xfffe9841, 0xfffd95f0, 0xfffd95f0};
static int numpoints = 31;
static ffloat h = 0xfffe6b3b;
static ffloat hinv = 0x00034c64;
static ffloat pi2=0x00036487;
static ffloat pi2inv=0xfffe517c;
if(FFlt(xin,xlo)){
x=FFadd(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xhi,xin),
pi2inv
)
)
),
pi2
)
);
}else if(FFgt(xin,xhi)){
x=FFsub(
xin,
FFmult(
S16int2FFloat(
FFloatTrunc2S16int(
FFmult(
FFsub(xin,xlo),
pi2inv
)
)
),
pi2
)
);
}
klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
khi=klo+1;
xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
xdiff1 = FFsub(xdiff0, h);
return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}
