Embedded Systems Code Snippets

Popular
Recent

All
ASM
C
C++
Matlab

Arithmetic Operations

● April 15, 2013 ● Coded in ASM for the Microchip PIC16

UDATA
HIBYTE	RES	1	
LOBYTE	RES	1
COUNTX	RES	1
MULCND	RES	1
MULPLR	RES	1
BCD	RES	2
ACCaLO	res	1
ACCaHI	res	1
ACCbLO	res	1
ACCbHI	res	1
ACCcLO	res	1
ACCcHI	res	1
ACCdLO	res	1
ACCdHI	res	1
R2	res	1
R1	res	1
R0	res	1
TEMPX	res	1
L_temp	res	1
H_temp	res	1
w_save	res	1
RandHi	res	1
RandLo	res	1
parity	res	1

;*************************************************************************
; Multiplication MULPLR(8 bit) x MULCND(8 bit) -->HIBYTE(msb),LOBYTE(lsb)*  
; a) Load the multiplier in the location MULPLR				 *
; b) Load the multiplicant in the location MULCND			 *
; c) Call Mpy8x8						         * 
; d) Msb is in the location HIBYTE					 *
; e) Lsb is in the location LOBYTE					 *
;*************************************************************************
Mpy8x8	 	 	 
 	clrf	HIBYTE	 
 	clrf	LOBYTE	 
 	clrf	COUNTX	 
 	bsf	COUNTX, 3
		 
 	movf	MULCND, W	 
LoopX
	bcf	STATUS, C	 
 	btfsc	MULPLR, 0	 
 	addwf	HIBYTE, f	 
 	rrf	HIBYTE, f	 
 	rrf	LOBYTE, f	 
 	bcf	STATUS, C	 
 	rrf	MULPLR, f	 
 	decfsz	COUNTX, f	 
 	goto	LoopX	 
 	return	
;*******************************************************************
;Multiplication: ACCb(16 bits)*ACCa(16 bits) -> ACCb,ACCc (32 bits)*	
;(a) Load the 1st operand in location ACCaLO & ACCaHI (16 bits)	   *	
;(b) Load the 2nd operand in location ACCbLO & ACCbHI (16 bits)    *
;(c) CALL Mpy_16bit						   * 
;(d) The 32 bit result is in location (ACCbHI,ACCbLO,ACCcHI,ACCcLO)*
;*******************************************************************
Mpy_16bit
	movlw	.16 		; for 16 shifts
	movwf	temp
	movf	ACCbHI,W	; move ACCb to ACCd
	movwf	ACCdHI
	movf	ACCbLO,W
	movwf	ACCdLO
	clrf	ACCbHI
	clrf	ACCbLO
Mloop
	rrf	ACCdHI, F	 ;rotate d right
	rrf	ACCdLO, F
	btfsc	STATUS,C	 ;need to add?
	call	Add_16bit
	rrf	ACCbHI, F
	rrf	ACCbLO, F
	rrf	ACCcHI, F
	rrf	ACCcLO, F
	decfsz	temp, F 	;loop until all bits checked
	goto	Mloop
	return 	 

;******************************************************************
;This routine convert the hex value present in the WREG to decimal* 
;and store the results in the reg: BCD and BCD+1  		  *
;******************************************************************
BinBCD	 	 	 
 	clrf	BCD	 
	clrf	BCD+1
Again1
	addlw	0x9C	;subtract 100 and check for borrow
 	btfss	STATUS, C	 
 	goto	add100	 
 	incf	BCD+1, f
	goto	Again1
add100	 
	addlw	0x64
Again
	addlw	0xF6	;subtract 10 and check for borrow
 	btfss	STATUS, C	 
 	goto	SwapBCD	 
 	incf	BCD, f
	goto	Again
 	 	 	 
SwapBCD
	addlw	0x0A	 
 	swapf	BCD, f	 
 	iorwf	BCD, f	 
 	return	 	 

;***************************************************************
;This routine find the square of the number present in the WREG*
;The hex result is stored in WREG and the decimal result is    *  
;stored in GPRs BCD and BCD+1                                  * 
;***************************************************************
square
	movwf	COUNTX	 
	movlw	0x01
	movwf	TEMPX
	clrw
r_square	
	addwf	TEMPX,W
	incf	TEMPX,F
	incf	TEMPX,F
	decfsz	COUNTX,F
	goto	r_square
	movwf	w_save
	call	BinBCD	
	movf	w_save,W
	return
;*******************************************************************
;This routine find the square root of a number which is stored in  *
;WREG.The result is stored in WREG.If the number hasn't a finite   * 
;square root this function returns an error value EE in WREG       *
;*******************************************************************
square_root
	movwf	w_save
	movlw	0x01
	movwf	TEMPX
	movwf	COUNTX
loop
	movf	TEMPX,W
	subwf	w_save,f
	btfsc	STATUS,Z
	goto	zero
	btfss	STATUS,C
	goto	no_root
	incf	COUNTX,F
	incf	TEMPX,F
	incf	TEMPX,F
	goto	loop
zero
	movf	COUNTX,W
	return
no_root
	movlw	0XEE
	return
	
;********************************************************************
; Binary To BCD Conversion Routine				    *
; This routine converts a 16 Bit binary Number to a 5 Digit	    *
; BCD Number.							    *
; The 16 bit binary number is input in locations ACCaHI and         *
; ACCaLO with the high byte in ACCaHI.				    *
; The 5 digit BCD number is returned in R0, R1 and R2 with R0	    *
; containing the MSD in its right most nibble.			    *
;********************************************************************
Hex_to_Dec	 	 	 
 	bcf	STATUS, C	 
 	clrf	COUNTX	 
 	bsf	COUNTX, 4	;set count to 16
 	clrf	R0	 
 	clrf	R1	 
 	clrf	R2	 
Loop16a
	rlf	ACCaLO, f	 
 	rlf	ACCaHI, f	 
 	rlf	R2, f	 
 	rlf	R1, f	 
 	rlf	R0, f	 
 	decfsz	COUNTX, f	 
 	goto	Adjdec	 
 	return	 	 
Adjdec
	movlw	R2	;load as indirect address pointer
 	movwf	FSR	 
 	call	AdjBCD	 
  	incf	FSR, f	 
 	call	AdjBCD	 
 	incf	FSR, f	 
 	call	AdjBCD	 
 	goto	Loop16a	 
 	 	 	 
AdjBCD
	movf	INDF, w	 
 	addlw	0x03	 
 	movwf	TEMPX	 
 	btfsc	TEMPX,3;test if result > 7
 	movwf	INDF	 
 	movf	INDF, w	 
 	addlw	0x30	 
 	movwf	TEMPX	 
 	btfsc	TEMPX, 7	;test if result > 7
 	movwf	INDF	;save as MSD
 	return	 	 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Division : ACCb(16 bits) / ACCa(16 bits) -> ACCd(16 bits) with  ;
; Remainder in ACCc (16 bits)					  ;
; (a) Load the Denominator in location ACCaHI & ACCaLO ( 16 bits );
; (b) Load the Numerator in location ACCbHI & ACCbLO ( 16 bits )  ;
; (c) CALL Division						  ;
; (d) The 16 bit result is in location ACCdHI & ACCdLO		  ;
; (e) The 16 bit Remainder is in locations ACCcHI & ACCcLO	  ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
Division
	clrf	COUNTX
	bsf	COUNTX,4 ; set count = 16
	clrf	ACCcHI
	clrf	ACCcLO
	clrf	ACCdLO
	clrf	ACCdHI
divLoop
	bcf	STATUS,C
	rlf	ACCbLO,F
	rlf	ACCbHI,F
	rlf	ACCcLO,F
	rlf	ACCcHI,F
	movf	ACCaHI,W
	subwf	ACCcHI,W ; check if a>c
	btfss	STATUS,Z
	goto	notz
	movf	ACCaLO,W
	subwf	ACCcLO,W ; if msb equal then check lsb
notz
	btfss	STATUS,C ; carry set if c>a
	goto	nosub	 ; if c < a
subca
	movf	ACCaLO,W ; c-a into c
	subwf	ACCcLO, F
	movf	ACCaHI,W
	subwf	ACCcHI, F
	bsf	STATUS,C ; shift a 1 into d (result)
nosub
	rlf	ACCdLO,F
	rlf	ACCdHI,F
	decfsz	COUNTX,F
	goto	divLoop
	
	return
;*******************************************************************
; Random Number Generator					   *
; This routine generates a 16 Bit Pseudo Sequence Random Generator *
; It is based on Linear shift register feedback. The sequence	   *
; is generated by (Q15 xorwf Q14 xorwf Q12 xorwf Q3 )		   *
; The 16 bit random number is in location RandHi(high byte)	   *
; & RandLo (low byte)						   *
; Before calling this routine, make sure the initial values	   *
; of RandHi & RandLo are NOT ZERO				   *
; A good chiose of initial random number is 0x3045		   *
;*******************************************************************
Random16
	rlf	RandHi,W
	xorwf	RandHi,W
	movwf	w_save
	rlf	w_save, F ; carry bit = xorwf(Q15,14)
	swapf	RandHi, F
	swapf	RandLo,W
	movwf	w_save
	rlf	w_save, F 
	xorwf	RandHi,W ; LSB = xorwf(Q12,Q3)
	swapf	RandHi, F
	andlw	0x01
	rlf	RandLo, F
	xorwf	RandLo, F
	rlf	RandHi, F
	return

;**********************************************************************
; BCD To Binary Conversion					      *   
; This routine converts a 5 digit BCD number to a 16 bit binary	      *
; number.							      *  
; The input 5 digit BCD numbers are asumed to be in locations	      *
; R0, R1 & R2 with R0 containing the MSD in its right most nibble.    *
; The 16 bit binary number is output in registers ACCaHI & ACCaLO     *
; ( high byte & low byte repectively ).				      *
; The method used for conversion is :				      *	
; input number X = abcde ( the 5 digit BCD number )		      *
; X = abcde = 10[10[10[10a+b]+c]+d]+e				      *	
;**********************************************************************
Dec_to_Hex
	clrf	ACCaHI
	movf	R0,W
	andlw	0x0F
	movwf	ACCaLO
	call	mpy10a ; result = 10a+b
	swapf	R1,W
	call	mpy10b ; result = 10[10a+b]
	movf	R1,W
	call	mpy10b ; result = 10[10[10a+b]+c]
	swapf	R2,W
	call	mpy10b ; result = 10[10[10[10a+b]+c]+d]
	movf	R2,W
	andlw	0x0F
	addwf	ACCaLO, F
	btfsc	STATUS,C
	incf	ACCaHI, F ; result = 10[10[10[10a+b]+c]+d]+e
	return		 ; BCD to binary conversion done

mpy10b
	andlw	0x0F
	addwf	ACCaLO, F
	btfsc	STATUS,C
	incf	ACCaHI, F
mpy10a
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO,W
	movwf	L_temp
	rlf	ACCaHI,W ; (H_temp,L_temp) = 2*N
	movwf	H_temp
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO, F
	rlf	ACCaHI, F
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO, F
	rlf	ACCaHI, F
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO, F
	rlf	ACCaHI, F ; (H_byte,L_byte) = 8*N
	movf	L_temp,W
	addwf	ACCaLO, F
	btfsc	STATUS,C
	incf	ACCaHI, F
	movf	H_temp,W
	addwf	ACCaHI, F
	return		 ; (H_byte,L_byte) = 10*N
;*********************************************************************************************
;This routine is used to find the parity bit(ODD or EVEN)an 8 bit no:stored in the WREG.     *
;The parity bit is stored in the LSB of parity reg.To find EVEN parity make the EVEN_PARITY  *
;definition TRUE.To find ODD parity make the EVEN_PARITY definition FALSE.  		     *	
;*********************************************************************************************
find_parity
	movwf	TEMPX
	swapf	TEMPX,W
	xorwf	TEMPX,W
	movwf	parity
	rrf	parity, F
	rrf	parity, F
	xorwf	parity,W
	andlw	0x03
	addlw	0x01
	movwf	TEMPX
	rrf	TEMPX,F
	rrf	TEMPX,W
	movwf	parity
	#if	EVEN_PARITY
	xorlw	0x01
	movwf	parity
	#endif
	return

;************************************************************************
; Subtraction : ACCb(16 bits) - ACCa(16 bits) -> ACCb(16 bits)		*
; (a) Load the 1st operand in location ACCaLO & ACCaHI ( 16 bits )	*
; (b) Load the 2nd operand in location ACCbLO & ACCbHI ( 16 bits )	*
; (c) CALL Sub_16bit							*
; (d) The result is in location ACCbLO & ACCbHI ( 16 bits )		*
;************************************************************************
Sub_16bit
	call	Neg_16bit
	call	Add_16bit
	return		

;************************************************************************	
; Addition : ACCb(16 bits) + ACCa(16 bits) -> ACCb(16 bits)		*		
; (a) Load the 1st operand in location ACCaLO & ACCaHI ( 16 bits )	*
; (b) Load the 2nd operand in location ACCbLO & ACCbHI ( 16 bits )	*
; (c) CALL Add_16bit							*
; (d) The result is in location ACCbLO & ACCbHI ( 16 bits )		*
;************************************************************************
Add_16bit
	movf	ACCaLO,W
	addwf	ACCbLO, F ; add lsb
	btfsc	STATUS,C ; add in carry
	incf	ACCbHI, F
	movf	ACCaHI,W
	addwf	ACCbHI, F ; add msb
	return 
;************************************************************************
; 2's Compliment: negate ACCa ( -ACCa -> ACCa )				*
; (a) Load the operand in location ACCaLO & ACCaHI ( 16 bits )		*
; (b) CALL Neg_16bit							*
; (c) The result is in location ACCaLO & ACCaHI ( 16 bits )		*
;************************************************************************
Neg_16bit
	comf	ACCaLO, F ; 
	incf	ACCaLO, F
	btfsc	STATUS,Z
	decf	ACCaHI, F
	comf	ACCaHI, F
	return

Fast non-754 floating point library for Freescale 56F800E series

● March 23, 2013 ● Coded in ASM for the Freescale DSP56F8xx

#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))) );
}

Software SPI

● May 6, 2013●1 comment ● Coded in ASM for the Microchip PIC16

#include<p16f877a.inc>

#define	s_data_o	PORTC,5 ;serial data out
#define	s_data_i	PORTC,4	;serial data in
#define	s_clock		PORTC,3 ;clock out

	udata_shr
tx_reg	res	1
rx_reg	res	1

	code

;************************
;Configure I/O Ports
;Load data in WREG
;Call soft_spi_write
;************************
soft_spi_write
	global	soft_spi_write
	banksel	tx_reg
	movwf	tx_reg  	;store W = tx_reg
	banksel	PORTC 		;Bank 0
	bsf	STATUS,C 	;Set Carry Flag=1
send_next_bit
	rlf	tx_reg,F	;rotate left
	movf	tx_reg,F	;Check wheter 8 bit transmitted or not
	btfsc	STATUS,Z 	;If no ,send next bit
	return			;if yes,return

	bcf	s_data_o	;data line low
	btfsc	STATUS,C	;check the bit in carry,      	
	bsf	s_data_o	;if high,s_data_o =1
	fill	(nop),3
	bsf	s_clock		;s_clock=1	|		     _
	fill	(nop),5		;		|clock high to low _| |_		
	bcf	STATUS,C	;clear carry	|
	bcf	s_clock 	;S_clock=0  	|
	fill	(nop),3
	goto	send_next_bit	; looping process...........

;**************************************************
;Configure I/O Ports
;Call soft_spi_read	
;This fuction returns the received data is in WREG
;**************************************************
soft_spi_read			;subroutine for receive
	global	soft_spi_read
	movlw	0x01		;eight bit reception
	movwf	rx_reg
read_next_bit
	rlf	rx_reg,f	;rotating the rx_reg register to store the received bit
	bsf	s_clock 
	fill	(nop),5
	btfsc	s_data_i
	bsf	rx_reg,0	;receiving the data
	bcf	s_clock 
	fill	(nop),3
	btfss	STATUS,C	;testing whether the reception is compleate or not
	goto	read_next_bit	;if not compleated do the process again
	movf	rx_reg,W	;restore data in WREG
	return

	end

All
ASM
C
C++
Matlab

Software SPI

● May 6, 2013●1 comment ● Coded in ASM for the Microchip PIC16

#include<p16f877a.inc>

#define	s_data_o	PORTC,5 ;serial data out
#define	s_data_i	PORTC,4	;serial data in
#define	s_clock		PORTC,3 ;clock out

	udata_shr
tx_reg	res	1
rx_reg	res	1

	code

;************************
;Configure I/O Ports
;Load data in WREG
;Call soft_spi_write
;************************
soft_spi_write
	global	soft_spi_write
	banksel	tx_reg
	movwf	tx_reg  	;store W = tx_reg
	banksel	PORTC 		;Bank 0
	bsf	STATUS,C 	;Set Carry Flag=1
send_next_bit
	rlf	tx_reg,F	;rotate left
	movf	tx_reg,F	;Check wheter 8 bit transmitted or not
	btfsc	STATUS,Z 	;If no ,send next bit
	return			;if yes,return

	bcf	s_data_o	;data line low
	btfsc	STATUS,C	;check the bit in carry,      	
	bsf	s_data_o	;if high,s_data_o =1
	fill	(nop),3
	bsf	s_clock		;s_clock=1	|		     _
	fill	(nop),5		;		|clock high to low _| |_		
	bcf	STATUS,C	;clear carry	|
	bcf	s_clock 	;S_clock=0  	|
	fill	(nop),3
	goto	send_next_bit	; looping process...........

;**************************************************
;Configure I/O Ports
;Call soft_spi_read	
;This fuction returns the received data is in WREG
;**************************************************
soft_spi_read			;subroutine for receive
	global	soft_spi_read
	movlw	0x01		;eight bit reception
	movwf	rx_reg
read_next_bit
	rlf	rx_reg,f	;rotating the rx_reg register to store the received bit
	bsf	s_clock 
	fill	(nop),5
	btfsc	s_data_i
	bsf	rx_reg,0	;receiving the data
	bcf	s_clock 
	fill	(nop),3
	btfss	STATUS,C	;testing whether the reception is compleate or not
	goto	read_next_bit	;if not compleated do the process again
	movf	rx_reg,W	;restore data in WREG
	return

	end

Arithmetic Operations

● April 15, 2013 ● Coded in ASM for the Microchip PIC16

UDATA
HIBYTE	RES	1	
LOBYTE	RES	1
COUNTX	RES	1
MULCND	RES	1
MULPLR	RES	1
BCD	RES	2
ACCaLO	res	1
ACCaHI	res	1
ACCbLO	res	1
ACCbHI	res	1
ACCcLO	res	1
ACCcHI	res	1
ACCdLO	res	1
ACCdHI	res	1
R2	res	1
R1	res	1
R0	res	1
TEMPX	res	1
L_temp	res	1
H_temp	res	1
w_save	res	1
RandHi	res	1
RandLo	res	1
parity	res	1

;*************************************************************************
; Multiplication MULPLR(8 bit) x MULCND(8 bit) -->HIBYTE(msb),LOBYTE(lsb)*  
; a) Load the multiplier in the location MULPLR				 *
; b) Load the multiplicant in the location MULCND			 *
; c) Call Mpy8x8						         * 
; d) Msb is in the location HIBYTE					 *
; e) Lsb is in the location LOBYTE					 *
;*************************************************************************
Mpy8x8	 	 	 
 	clrf	HIBYTE	 
 	clrf	LOBYTE	 
 	clrf	COUNTX	 
 	bsf	COUNTX, 3
		 
 	movf	MULCND, W	 
LoopX
	bcf	STATUS, C	 
 	btfsc	MULPLR, 0	 
 	addwf	HIBYTE, f	 
 	rrf	HIBYTE, f	 
 	rrf	LOBYTE, f	 
 	bcf	STATUS, C	 
 	rrf	MULPLR, f	 
 	decfsz	COUNTX, f	 
 	goto	LoopX	 
 	return	
;*******************************************************************
;Multiplication: ACCb(16 bits)*ACCa(16 bits) -> ACCb,ACCc (32 bits)*	
;(a) Load the 1st operand in location ACCaLO & ACCaHI (16 bits)	   *	
;(b) Load the 2nd operand in location ACCbLO & ACCbHI (16 bits)    *
;(c) CALL Mpy_16bit						   * 
;(d) The 32 bit result is in location (ACCbHI,ACCbLO,ACCcHI,ACCcLO)*
;*******************************************************************
Mpy_16bit
	movlw	.16 		; for 16 shifts
	movwf	temp
	movf	ACCbHI,W	; move ACCb to ACCd
	movwf	ACCdHI
	movf	ACCbLO,W
	movwf	ACCdLO
	clrf	ACCbHI
	clrf	ACCbLO
Mloop
	rrf	ACCdHI, F	 ;rotate d right
	rrf	ACCdLO, F
	btfsc	STATUS,C	 ;need to add?
	call	Add_16bit
	rrf	ACCbHI, F
	rrf	ACCbLO, F
	rrf	ACCcHI, F
	rrf	ACCcLO, F
	decfsz	temp, F 	;loop until all bits checked
	goto	Mloop
	return 	 

;******************************************************************
;This routine convert the hex value present in the WREG to decimal* 
;and store the results in the reg: BCD and BCD+1  		  *
;******************************************************************
BinBCD	 	 	 
 	clrf	BCD	 
	clrf	BCD+1
Again1
	addlw	0x9C	;subtract 100 and check for borrow
 	btfss	STATUS, C	 
 	goto	add100	 
 	incf	BCD+1, f
	goto	Again1
add100	 
	addlw	0x64
Again
	addlw	0xF6	;subtract 10 and check for borrow
 	btfss	STATUS, C	 
 	goto	SwapBCD	 
 	incf	BCD, f
	goto	Again
 	 	 	 
SwapBCD
	addlw	0x0A	 
 	swapf	BCD, f	 
 	iorwf	BCD, f	 
 	return	 	 

;***************************************************************
;This routine find the square of the number present in the WREG*
;The hex result is stored in WREG and the decimal result is    *  
;stored in GPRs BCD and BCD+1                                  * 
;***************************************************************
square
	movwf	COUNTX	 
	movlw	0x01
	movwf	TEMPX
	clrw
r_square	
	addwf	TEMPX,W
	incf	TEMPX,F
	incf	TEMPX,F
	decfsz	COUNTX,F
	goto	r_square
	movwf	w_save
	call	BinBCD	
	movf	w_save,W
	return
;*******************************************************************
;This routine find the square root of a number which is stored in  *
;WREG.The result is stored in WREG.If the number hasn't a finite   * 
;square root this function returns an error value EE in WREG       *
;*******************************************************************
square_root
	movwf	w_save
	movlw	0x01
	movwf	TEMPX
	movwf	COUNTX
loop
	movf	TEMPX,W
	subwf	w_save,f
	btfsc	STATUS,Z
	goto	zero
	btfss	STATUS,C
	goto	no_root
	incf	COUNTX,F
	incf	TEMPX,F
	incf	TEMPX,F
	goto	loop
zero
	movf	COUNTX,W
	return
no_root
	movlw	0XEE
	return
	
;********************************************************************
; Binary To BCD Conversion Routine				    *
; This routine converts a 16 Bit binary Number to a 5 Digit	    *
; BCD Number.							    *
; The 16 bit binary number is input in locations ACCaHI and         *
; ACCaLO with the high byte in ACCaHI.				    *
; The 5 digit BCD number is returned in R0, R1 and R2 with R0	    *
; containing the MSD in its right most nibble.			    *
;********************************************************************
Hex_to_Dec	 	 	 
 	bcf	STATUS, C	 
 	clrf	COUNTX	 
 	bsf	COUNTX, 4	;set count to 16
 	clrf	R0	 
 	clrf	R1	 
 	clrf	R2	 
Loop16a
	rlf	ACCaLO, f	 
 	rlf	ACCaHI, f	 
 	rlf	R2, f	 
 	rlf	R1, f	 
 	rlf	R0, f	 
 	decfsz	COUNTX, f	 
 	goto	Adjdec	 
 	return	 	 
Adjdec
	movlw	R2	;load as indirect address pointer
 	movwf	FSR	 
 	call	AdjBCD	 
  	incf	FSR, f	 
 	call	AdjBCD	 
 	incf	FSR, f	 
 	call	AdjBCD	 
 	goto	Loop16a	 
 	 	 	 
AdjBCD
	movf	INDF, w	 
 	addlw	0x03	 
 	movwf	TEMPX	 
 	btfsc	TEMPX,3;test if result > 7
 	movwf	INDF	 
 	movf	INDF, w	 
 	addlw	0x30	 
 	movwf	TEMPX	 
 	btfsc	TEMPX, 7	;test if result > 7
 	movwf	INDF	;save as MSD
 	return	 	 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; Division : ACCb(16 bits) / ACCa(16 bits) -> ACCd(16 bits) with  ;
; Remainder in ACCc (16 bits)					  ;
; (a) Load the Denominator in location ACCaHI & ACCaLO ( 16 bits );
; (b) Load the Numerator in location ACCbHI & ACCbLO ( 16 bits )  ;
; (c) CALL Division						  ;
; (d) The 16 bit result is in location ACCdHI & ACCdLO		  ;
; (e) The 16 bit Remainder is in locations ACCcHI & ACCcLO	  ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
Division
	clrf	COUNTX
	bsf	COUNTX,4 ; set count = 16
	clrf	ACCcHI
	clrf	ACCcLO
	clrf	ACCdLO
	clrf	ACCdHI
divLoop
	bcf	STATUS,C
	rlf	ACCbLO,F
	rlf	ACCbHI,F
	rlf	ACCcLO,F
	rlf	ACCcHI,F
	movf	ACCaHI,W
	subwf	ACCcHI,W ; check if a>c
	btfss	STATUS,Z
	goto	notz
	movf	ACCaLO,W
	subwf	ACCcLO,W ; if msb equal then check lsb
notz
	btfss	STATUS,C ; carry set if c>a
	goto	nosub	 ; if c < a
subca
	movf	ACCaLO,W ; c-a into c
	subwf	ACCcLO, F
	movf	ACCaHI,W
	subwf	ACCcHI, F
	bsf	STATUS,C ; shift a 1 into d (result)
nosub
	rlf	ACCdLO,F
	rlf	ACCdHI,F
	decfsz	COUNTX,F
	goto	divLoop
	
	return
;*******************************************************************
; Random Number Generator					   *
; This routine generates a 16 Bit Pseudo Sequence Random Generator *
; It is based on Linear shift register feedback. The sequence	   *
; is generated by (Q15 xorwf Q14 xorwf Q12 xorwf Q3 )		   *
; The 16 bit random number is in location RandHi(high byte)	   *
; & RandLo (low byte)						   *
; Before calling this routine, make sure the initial values	   *
; of RandHi & RandLo are NOT ZERO				   *
; A good chiose of initial random number is 0x3045		   *
;*******************************************************************
Random16
	rlf	RandHi,W
	xorwf	RandHi,W
	movwf	w_save
	rlf	w_save, F ; carry bit = xorwf(Q15,14)
	swapf	RandHi, F
	swapf	RandLo,W
	movwf	w_save
	rlf	w_save, F 
	xorwf	RandHi,W ; LSB = xorwf(Q12,Q3)
	swapf	RandHi, F
	andlw	0x01
	rlf	RandLo, F
	xorwf	RandLo, F
	rlf	RandHi, F
	return

;**********************************************************************
; BCD To Binary Conversion					      *   
; This routine converts a 5 digit BCD number to a 16 bit binary	      *
; number.							      *  
; The input 5 digit BCD numbers are asumed to be in locations	      *
; R0, R1 & R2 with R0 containing the MSD in its right most nibble.    *
; The 16 bit binary number is output in registers ACCaHI & ACCaLO     *
; ( high byte & low byte repectively ).				      *
; The method used for conversion is :				      *	
; input number X = abcde ( the 5 digit BCD number )		      *
; X = abcde = 10[10[10[10a+b]+c]+d]+e				      *	
;**********************************************************************
Dec_to_Hex
	clrf	ACCaHI
	movf	R0,W
	andlw	0x0F
	movwf	ACCaLO
	call	mpy10a ; result = 10a+b
	swapf	R1,W
	call	mpy10b ; result = 10[10a+b]
	movf	R1,W
	call	mpy10b ; result = 10[10[10a+b]+c]
	swapf	R2,W
	call	mpy10b ; result = 10[10[10[10a+b]+c]+d]
	movf	R2,W
	andlw	0x0F
	addwf	ACCaLO, F
	btfsc	STATUS,C
	incf	ACCaHI, F ; result = 10[10[10[10a+b]+c]+d]+e
	return		 ; BCD to binary conversion done

mpy10b
	andlw	0x0F
	addwf	ACCaLO, F
	btfsc	STATUS,C
	incf	ACCaHI, F
mpy10a
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO,W
	movwf	L_temp
	rlf	ACCaHI,W ; (H_temp,L_temp) = 2*N
	movwf	H_temp
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO, F
	rlf	ACCaHI, F
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO, F
	rlf	ACCaHI, F
	bcf	STATUS,C ; multiply by 2
	rlf	ACCaLO, F
	rlf	ACCaHI, F ; (H_byte,L_byte) = 8*N
	movf	L_temp,W
	addwf	ACCaLO, F
	btfsc	STATUS,C
	incf	ACCaHI, F
	movf	H_temp,W
	addwf	ACCaHI, F
	return		 ; (H_byte,L_byte) = 10*N
;*********************************************************************************************
;This routine is used to find the parity bit(ODD or EVEN)an 8 bit no:stored in the WREG.     *
;The parity bit is stored in the LSB of parity reg.To find EVEN parity make the EVEN_PARITY  *
;definition TRUE.To find ODD parity make the EVEN_PARITY definition FALSE.  		     *	
;*********************************************************************************************
find_parity
	movwf	TEMPX
	swapf	TEMPX,W
	xorwf	TEMPX,W
	movwf	parity
	rrf	parity, F
	rrf	parity, F
	xorwf	parity,W
	andlw	0x03
	addlw	0x01
	movwf	TEMPX
	rrf	TEMPX,F
	rrf	TEMPX,W
	movwf	parity
	#if	EVEN_PARITY
	xorlw	0x01
	movwf	parity
	#endif
	return

;************************************************************************
; Subtraction : ACCb(16 bits) - ACCa(16 bits) -> ACCb(16 bits)		*
; (a) Load the 1st operand in location ACCaLO & ACCaHI ( 16 bits )	*
; (b) Load the 2nd operand in location ACCbLO & ACCbHI ( 16 bits )	*
; (c) CALL Sub_16bit							*
; (d) The result is in location ACCbLO & ACCbHI ( 16 bits )		*
;************************************************************************
Sub_16bit
	call	Neg_16bit
	call	Add_16bit
	return		

;************************************************************************	
; Addition : ACCb(16 bits) + ACCa(16 bits) -> ACCb(16 bits)		*		
; (a) Load the 1st operand in location ACCaLO & ACCaHI ( 16 bits )	*
; (b) Load the 2nd operand in location ACCbLO & ACCbHI ( 16 bits )	*
; (c) CALL Add_16bit							*
; (d) The result is in location ACCbLO & ACCbHI ( 16 bits )		*
;************************************************************************
Add_16bit
	movf	ACCaLO,W
	addwf	ACCbLO, F ; add lsb
	btfsc	STATUS,C ; add in carry
	incf	ACCbHI, F
	movf	ACCaHI,W
	addwf	ACCbHI, F ; add msb
	return 
;************************************************************************
; 2's Compliment: negate ACCa ( -ACCa -> ACCa )				*
; (a) Load the operand in location ACCaLO & ACCaHI ( 16 bits )		*
; (b) CALL Neg_16bit							*
; (c) The result is in location ACCaLO & ACCaHI ( 16 bits )		*
;************************************************************************
Neg_16bit
	comf	ACCaLO, F ; 
	incf	ACCaLO, F
	btfsc	STATUS,Z
	decf	ACCaHI, F
	comf	ACCaHI, F
	return

Fast non-754 floating point library for Freescale 56F800E series

● March 23, 2013 ● Coded in ASM for the Freescale DSP56F8xx

#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))) );
}

Arithmetic Operations

Fast non-754 floating point library for Freescale 56F800E series

Software SPI

Software SPI

Arithmetic Operations

Fast non-754 floating point library for Freescale 56F800E series

Sign in

You might also like...

Search code

Blogs - Hall of Fame

Free PDF Downloads

Quick Links

About EmbeddedRelated.com

Social Networks

The Related Media Group