;**************************************************************
;	FAST IEEE FP Routines for Atmel AVR RISC
;	1998,1999 by Jack Tidwell <jackt@igalaxy.net>
;	These routines may be used for personal and educational
;	purposes, and are free to download.
;	Work is still in progress (smaller & FASTER!)
;	Trig funcs comming soon
;	I would like feedback.
;	Most problems fixed, converting to Decimal seems to be
;    	the only one left?
;	Added some 'C' support
;	Work on preservation of r9 to r25 is needed
; 	
;	NOTE: All funcs w/xxxMEM: i.e; FDIVMEM: 
;	enter w/r30,r31 as a pointer to 32bit SRAM FPvalue.
;	You still have to preserv r9-r25
;	It's a start toward Rons' SmallC.
;***************************************************************

		.include "8515def.inc"	; (Any AVR except 1200)
		.def	QUOT=r9		; partial product scratch area
		.def	QUOTM=r10
		.def	QUOTH=r11
		.def	DEXP=r12	; Decimal Exp for 'ftoa(), ecvt()' funcs
		.def	MANT1=r13	; Accumulator 1
		.def	MANT1m=r14
		.def	MANT1h=r15
		.def	EXP1=r16
		.def	SIGN1=r17
		.def	MANT2=r18	; Accumulator 2
		.def	MANT2m=r19
		.def	MANT2h=r20
		.def	EXP2=r21
		.def	SIGN2=r22
		.def	MANT1T=r23	; used to extend 24bit to 32bit ops
		.def	count=r24	; GP loop counter
		.def	temp=r25	; scratch 'ram'?
		.dseg

DECBUFF:	.byte	10		; some place to hold Decimal Digits
MATHTMP:	.byte	8		; Accumulator 'Holding' registers.
		.cseg
		.org	$000
		rjmp	RESET
		.org	$00b
	
		;** Avr Studio test code **

RESET:		ldi	temp,low(RAMEND)
		out	SPL,temp
		ldi	temp,high(RAMEND)
		out	SPH, temp
Main:		ldi	zl,low(k1e7*2)
		ldi	zh,high(k1e7*2)
		rcall	KTOAC1
		rcall	fdiv10
		rcall	fdiv10
		rcall	fdiv10
		rcall	fdiv10
		rcall	faddp5
		rcall	fsqrt
		rcall	fltneg
main1:		rjmp	main1

;*******************************************************
;	FP Math Routines
;	Very similar to _ecvt(), (still a little buggy?)
;*******************************************************

FLTDEC:		rcall	PUSH1
		push	sign1
		push	exp1		; FP to 7 digit dec # +exp
		tst	exp1
		brpl	FLTD1
		rcall	FLTNEG
FLTD1:		ldi	temp,0x7f
		sub	temp,exp1
		mov	DEXP,temp
FLTD2:		ldi	zl,low(k1E8 *2)
		ldi	zh,high(k1E8 *2)
		rcall	KTOAC2
		rcall	FLTCP
		brpl	FLTD5
		ldi	zl,low(k10*2)
		ldi	zh,high(k10*2)
		rcall	KTOAC2
		rcall	FMUL
		inc	DEXP
		rjmp	FLTD2
FLTD5:		pop	temp		; orig exp1
		mov	count,temp
		neg	temp
		add	temp,DEXP
		subi	temp,0x7f
		mov	DEXP,temp
		andi	count,0x80
		rcall	FADDp5
		pop	sign1
ITOD:		rcall	FTOI
		ldi	yl,low(DECBUFF)
		ldi	yh,high(DECBUFF)
		tst	sign1
		brpl	ITOD1
		ldi	temp,'-'
		st	y+,temp
		rcall	FLTNEG
ITOD1:		ldi	zl,low(I1E6*2)	; decimal 'lookup' table
		ldi	zh,high(I1E6*2)
		ldi	count,6
ITODLP:		push	count
		rcall	KTOAC2
		clr	temp
ITOD2:		cp	mant1,mant2
		cpc	mant1m,mant2m
		cpc	mant1h,mant2h
		brlo	ITOD3
		sub	mant1,mant2
		sbc	mant1m,mant2m
		sbc	mant1h,mant2h
		inc	temp
		brne	ITOD2
ITOD3:		ori	temp,'0'	; convert to Ascii char
		st	y+,temp		; store the dig in the SRAM Buffer, and post-inc 'y' reg
		pop	count
		dec	count		; have we processed all tables yet?
		brne	ITODLP
		mov	temp,mant1
		ori	temp,'0'
		st	y+,temp		; * count = 0 here, use it to add ending 'NULL' char.
		st	y+,count	; * DEXP has dp position -1
		st	y+,DEXP
		rjmp	POP1		; restore ac1
FTOIMEM:	rcall	FSRAMA1
FTOI:		rcall	UNPACK
		brne	FTOI1
		rjmp	MINRES
FTOI1:		ldi	temp,22
		sub	temp,exp1
		brpl	FTOI2
		rjmp	MAXRES
FTOI2:		cpi	temp,24
		brlo	FTOI3
		rjmp	MINRES
FTOI3:		cpi	temp,8		; use fast byte-move, if possible
		brlo	FTOI4
		mov	mant1,mant1m
		mov	mant1m,mant1h
		mov	mant1h,quot
		clr	quot
		subi	temp,8
		rjmp	FTOI3
FTOI4:		tst	temp
		breq	FTOI6
FTOI5:		lsr	quot
		ror	mant1h
		ror	mant1m
		ror	mant1
		dec	temp
		brne	FTOI5
FTOI6:		;	tst	sign1
		;	brpl	FTOIX
		;	rjmp	NEGMANT
FTOIX:		ret

		;** ACC1 -= *r30 **

FSUBMEM:	 rcall	FSRAMA2		; Acc1 -= *r30

		;** ACC1 -= ACC2 **

FSUB:		rcall	UNPACK
		cpi	exp2,0x80
		breq	FSADX
		ldi	temp,0x80
		eor	sign2,temp
		rjmp	FADD1
FSUNDER:	brmi	FSADX
FRET2:		rcall	SWAPACC
FSADX:		rjmp	REPACK

		;** ACC1 += 0.5f **

FADDp5:		ldi	exp2,0x3f
		clr	mant2
		clr	mant2m
		clr	mant2h
		rjmp	FADD
FADDMEM:	rcall	FSRAMA2		; Acc1 += *r30

		;** ACC1 += ACC2 **

FADD:		rcall	UNPACK
		cpi	exp2,0x80
		breq	FSADX
FADD1:		cpi	exp1,0x80
		breq	FRET2
FADD2:		mov	temp,exp1
		sub	temp,exp2
		brvs	FSADX
		brpl	FADD3
		rcall	SWAPACC
		rjmp	FADD2
FADD3:		cpi	temp,24		; are we more than 24 bits diff?
		brlo	FADD3a		; no, we can add it.
		clr	mant2		; yes, acc2 is to small
		clr	mant2m
		clr	mant2h
FADD3a:		cpi	temp,8
		brlo	FADD3b
		mov	mant2,mant2m
		mov	mant2m,mant2h
		clr	mant2h
		subi	temp,8
		rjmp	FADD3a
FADD3b: 	tst	temp
		breq	FADD3d
FADD3c:		lsr	mant2h
		ror	mant2m
		ror	mant2
		dec	temp
		brne	FADD3c
FADD3d:		mov	temp,sign1
		eor	temp,sign2
		brmi	FADD4
		rcall	UADD
		brcc	FSADX
		ror	mant1h
		ror	mant1m
		ror	mant1
		subi	exp1,-1		; Add +1
		brvc	FSADX
		rjmp	MAXRES
FADD4:		rcall	USUB
		breq	FCLR
		brcc	FADD5
		rcall	NEGMANT
FADD5:		tst	mant1h		; normalize
		brmi	FSADX
		lsl	mant1
		rol	mant1m
		rol	mant1h
		subi	exp1,1
		brvc	FADD5
		rjmp	MAXRES
FCLR:		rjmp	MINRES
UADD:		add	mant1,mant2
		adc	mant1m,mant2m
		adc	mant1h,mant2h
		ret
USUB:		sub	mant1,mant2
		sbc	mant1m,mant2m
		sbc	mant1h,mant2h
		ret

		;** ACC1 = -ACC1 **

FLTNEG:		rcall	UNPACK
		com	sign1
		rjmp	REPACK
NEGMANT:	ldi	temp,-1
		sub	mant1,temp
		sbc	mant1m,temp
		sbc	mant1h,temp
		ret

		;** MANT1 <<= 1 **

RLMANT1:	lsl	mant1
		rol	mant1m
		rol	mant1h
		rol	mant1t
		ret

		;** Straight 24bit Unsigned ACC1 / ACC2 **

UDIVMANT:	clr	quot
		clr	quotm
		clr	quoth
		ldi	count,24
UDIVLP:		cp	mant1,mant2
		cpc	mant1m,mant2m
		cpc	mant1h,mant2h
		brlo	UDIV1
		sub	mant1,mant2
		sbc	mant1m,mant2m
		sbc	mant1h,mant2h
		sec
		rjmp	UDIV2
UDIV1:		clc
UDIV2:		rol	quot
		rol	quotm
		rol	quoth
		lsl	mant1
		rol	mant1m
		rol	mant1h
		dec	count
		brne	UDIVLP
		push	mant1
		mov	mant1,quot
		mov	mant1m,quotm
		mov	mant1h,quoth
		pop	temp
		lsr	temp
		brcc	udivx
		inc	mant1
		brne	udivx
		inc	mant1m
		brne	udivx
		inc	mant1h
udivx:		ret

		;** ACC1 = 1/*r30 **

F1OVERMEM:	rcall FSRAMA1
	
		;** ACC1 = 1/ACC1 **

F1OVERX:	rcall	AC1TOAC2
		clr	mant1
		clr	mant1m
		ldi	temp,0x80
		mov	mant1h,temp
		ldi	exp1,0x3f	; ieee 1.0f
		rjmp	FDIV		; do 1/x

		;** Acc1 /= *r30 **

FDIVMEM:	rcall	FSRAMA2
		rjmp	FDIV
	
		;** ACC1 /= 10.0f */
	
FDIV10:		clr	mant2		; x/10.0f  could use fmul * 0.1f?
		clr	mant2m
		ldi	mant2h,0x20
		ldi	exp2,0x41	;Put 10.0 -> Acc2

		;** ACC1 /= ACC2 **

FDIV:		tst	exp2		; test for x/0.0
		breq	FDIVZ
		tst	exp1
		breq	MINRES		; 0.0f/x = return 0.0f
FDIVa:		rcall	UNPACK
		breq	MINRES
		eor	sign1,sign2	; get result sign
		sec
		sbc	exp1,exp2	; sub exps.
		brvs	MAXRES		; overflow ?
		lsr	mant2h		; no, de-norm & div mants
		ror	mant2m
		ror	mant2
		lsr	mant1h
		ror	mant1m
		ror	mant1
		rcall	UDIVMANT
		tst	mant1h		; need norm?
		brmi	FDIVX		; no, exit
		lsl	mant1
		rol	mant1m
		rol	mant1h
		subi	exp1,1		; --exp
		brvs	MAXRES
FDIVX:		rjmp	REPACK		; re-pack
FDIVZ:		rjmp	MINRES
MAXRES:		ldi	temp,0x7f
		mov	exp1,temp
		or	mant1h,temp
		ldi	temp,0xff
		mov	mant1,temp
		mov	mant1m,temp
		ret
MINRES:		clr	mant1		;Result = 0.0f
		clr	mant1m
		clr	mant1h
		clr	exp1
		clr	sign1
		ret
		
		;** ACC1 *= *r30 **

FMULMEM: 	rcall	FSRAMA2		; Acc1 *= *r30
		rjmp	FMUL

		;** ACC1 *= 10.0f **

FMUL10:		ldi	exp2,0x41	; IEEE 10.0f
		ldi	mant2h,0x20
		clr	mant2m
		clr	mant2
	
		;** ACC1 *= ACC2 **

FMUL:		rcall	UNPACK
		breq	MINRES
		cpi	exp2,0x80
		breq	MINRES
		eor	sign1,sign2
		sec
		adc	exp1,exp2
		brvs	MAXRES		;FMULMAX
		rcall	UMUL
		tst	mant1h
		brmi	FMULX
		lsl	quoth
		rol	mant1
		rol	mant1m
		rol	mant1h
		rjmp	REPACK
FMULX:		subi	exp1,-1
		brvs	MAXRES		;FMULMAX
		rjmp	REPACK

		;** 24bit unsigned Multiply

UMUL:		push	temp
		push	count
		clr	quot
		clr	quotm
		clr	quoth
		clr	temp
		ldi	count,24
UMULLP:		lsl	quot
		rol	quotm
		rol	quoth
		rol	mant1
		rol	mant1m
		rol	mant1h
		brcc	UMUL1
		add	quot,mant2
		adc	quotm,mant2m
		adc	quoth,mant2h
		adc	mant1,temp
		adc	mant1m,temp
		adc	mant1h,temp
UMUL1:		dec	count
		brne	UMULLP
		pop	count
		pop	temp
		ret
		
		;** ACC1 = (*r30) * (*r30) **

FSQRMEM:	 rcall	FSRAMA1

		;** ACC1 *= ACC1 **

FSQR:		rcall	AC1TOAC2
		rjmp	FMUL
sqrthalf:	clr	mant2
		clr	mant2m
		clr	mant2h
		ldi	exp2,0x40
		rjmp	FDIVa		; div by 2 to get our guess
SQERR:		rjmp	MINRES		; add your own error handler here.

		;** ACC1 = sqrt(*r30) **

FSQRTMEM:	 rcall FSRAMA1
		
		;** ACC1 = sqrt(ACC1) **
FSQRT:		tst	exp1
		breq	SQERR		; trap sqrt(0) or sqrt(-x)!
		brmi	SQERR
		mov	r5,mant1	; save org Num. user must preserve r5-r8!
		mov	r6,mant1m
		mov	r7,mant1h
		mov	r8,exp1
		rcall	sqrthalf	; get first guess
		rcall	AC1TOAC2	; put it in Acc2
		ldi	temp,5		; number of iterations
SQRTLP:		push	temp
		rcall	PUSH2		; save 'guess' results
		mov	mant1,r5	; restore orig Number
		mov	mant1m,r6
		mov	mant1h,r7
		mov	exp1,r8
		rcall	FDIV
		rcall	POP2
		rcall	FADD
		rcall	sqrthalf
		rcall	AC1TOAC2
		pop	temp
		dec	temp
		brne	SQRTLP
		ret

		;** Compare ACC1 == ACC2 **
		;** return w/temp = 0,1,or -1 (Z,N flags set) **

FLTCPFLASH:	 rcall KTOAC2
		rjmp	FLTCP
	
		;** compare two sram floats **

FLTCP2:		push	zl		; save r30,r31, (pointer to acc2 variable)
		push	zh
		mov	zl,yl
		mov	zh,yh
		rcall	FSRAMA1
		pop	zh
		pop	zl
FLTCPMEM: 	rcall FSRAMA2		; compare Acc1 w/Memory
FLTCP:		rcall	FCMP		; do compare
		tst	temp		; set status reg accordingly
		ret			; return to caller
FCMP:		tst	exp1		; is acc1 pos?
		brmi	FCMP1		; no. test acc2
		tst	exp2		; yes, is acc2 pos?
		brmi	A1GTA2		; no, return Acc1 > Acc2
		cp	exp1,exp2	; both are '+'
		brlo	A1LTA2
		brne	A1GTA2
		cp	mant1,mant2	; exps are equal, are mants?
		cpc	mant1m,mant2m
		cpc	mant1h,mant2h
		brlo	A1LTA2
		breq	A1EQA2
A1GTA2:		ldi	temp,1		; ac1 > ac2
		ret
A1LTA2:		ldi	temp,0xff	; ac1 < ac2
		ret
A1EQA2:		clr	temp		; ac1 = ac2
		ret

		;** we're here, acc1 must be '-' **

FCMP1:		tst	exp2		; if acc1 is '-' & acc2 is '+'
		brpl	A1LTA2		; then acc1 is < acc2
		cp	exp2,exp1	; both are '-', is exp1 more negative?
		brlo	A1GTA2
		brne	A1LTA2
		cp	mant2,mant1
		cpc	mant2m,mant1m
		cpc	mant2h,mant1h
		brlo	A1GTA2
		breq	A1EQA2		; return acc1 == acc2
		rjmp	A1LTA2

;***********************************************************
; These routines are for future atof(), atoi(), atol(), etc.
; Fast Multiply by 10
;***********************************************************

UMUL10:		rcall	AC1TOAC2
		rcall	RLMANT1
		rcall	RLMANT1
		rcall	UADD
		sbci	mant1t,-1	; adci 0
		rjmp	RLMANT1
ADDDIG:		rcall	UMUL10
		clr	mant2m
		clr	mant2h
		mov	mant2,temp
		andi	mant2,0x0f
		rcall	UADD
		sbci	mant1t,-1	; just add 'C'
		ret

;***************************
; convert an integer to FP
;***************************

ITOFMEM: 	rcall	FSRAMA1		; psuedo 32bit long?
		mov	mant1t,exp1
ITOF:		mov	temp,mant1
		or	temp,mant1m
		or	temp,mant1h
		or	temp,mant1t
		brne	ITOF1
		rjmp	MINRES
ITOF1:		tst	mant1t
		brpl	ITOF2
		rcall	NEGMANT
ITOF2:		ldi	exp1,30
		tst	mant1t
ITOF3:		brmi	ITOF4
		dec	exp1
		lsl	mant1
		rol	mant1m
		rol	mant1h
		rol	mant1t
		rjmp	ITOF3
ITOF4:		mov	mant1,mant1m
		mov	mant1m,mant1h
		mov	mant1h,mant1t
		rjmp	REPACK

;**************************
; Memory Move funcs
; Copy SRAM Float to Acc1 
;**************************

FSRAMA1: 	ld	mant1,z+
		ld	mant1m,z+
		ld	mant1h,z+
		ld	exp1,z+
		ret

		;** Copy SRAM Float to Acc2 **
	
FSRAMA2: 	ld	mant2,z+
		ld	mant2m,z+
		ld	mant2h,z+
		ld	exp2,z+
		ret

		;** Copy ACC2 = ACC1 **

AC1TOAC2:	mov	exp2,exp1
		mov	mant2,mant1	
		mov	mant2m,mant1m
		mov	mant2h,mant1h
		mov	sign2,sign1
		ret
		
		;** ACC1 <-> ACC2 **

SWAPACC: 	push	mant1
		push	mant1m
		push	mant1h
		push	exp1
		push	sign1
		mov	mant1,mant2
		mov	mant1m,mant2m
		mov	mant1h,mant2h
		mov	exp1,exp2
		mov	sign1,sign2
		pop	sign2
		pop	exp2
		pop	mant2h
		pop	mant2m
		pop	mant2
		ret

		;** Get 1 byte from program memory & inc pointer **

_lpmbyte: 	lpm
		adiw	zl,1
		ret

		;** Put a Flash const -> ACC1

KTOAC1:		rcall	_lpmbyte
		mov	mant1,r0
		rcall	_lpmbyte
		mov	mant1m,r0
		rcall	_lpmbyte
		mov	mant1h,r0
		rcall	_lpmbyte
		mov	exp1,r0
		ret

		;** Put a Flash const -> ACC2

KTOAC2:		rcall	_lpmbyte
		mov	mant2,r0
		rcall	_lpmbyte
		mov	mant2m,r0
		rcall	_lpmbyte
		mov	mant2h,r0
		rcall	_lpmbyte
		mov	exp2,r0
		ret

		;** 'Hold' Acc1 in sram **

PUSH1:		sts	MATHTMP,mant1
		sts	MATHTMP+1,mant1m
		sts	MATHTMP+2,mant1h
		sts	MATHTMP+3,exp1
		ret

		;** 'Restore' Acc1 from sram **

POP1:		lds	mant1,MATHTMP
		lds	mant1m,MATHTMP+1
		lds	mant1h,MATHTMP+2
		lds	exp1,MATHTMP+3
		ret

		;** 'Hold' Acc2 in sram **

PUSH2:		sts	MATHTMP+4,mant2
		sts	MATHTMP+5,mant2m
		sts	MATHTMP+6,mant2h
		sts	MATHTMP+7,exp2
		ret

		;** 'Restore' Acc2 from sram **

POP2:		lds	mant2,MATHTMP+4
		lds	mant2m,MATHTMP+5
		lds	mant2h,MATHTMP+6
		lds	exp2,MATHTMP+7
		ret

;*********************************************
; IEEE convertions
; Convert ACC1 & ACC2 from IEEE to work format
;*********************************************

UNPACK:		push	temp	
		mov	sign1,exp1
		ldi	temp,0x80
		lsl	mant1h
		rol	exp1
		eor	exp1,temp	; AVR has no 'eori'
		lsl	temp
		ror	mant1h		; restore Hidden '1' bit
		andi	sign1,0x80

		;** Un-Pack Acc2 **

		mov	sign2,exp2
		ldi	temp,0x80
		lsl	mant2h
		rol	exp2
		eor	exp2,temp
		lsl	temp
		ror	mant2h
		andi	sign2,0x80
		pop	temp
		cpi	exp1,0x80	; return w/ACC1=0 ?
		ret

		;** Convert Acc1 from work format to IEEE **

REPACK:		push	temp
		lsl	mant1h	
		ldi	temp,0x80
		eor	temp,exp1
		lsl	sign1
		ror	temp
		ror	mant1h
		mov	exp1,temp
		pop	temp
		ret

		;** Constants used for decimal conversion **

I1E6:		.db	0x40,0x42,0x0f,0	;  1,000,000
I1E5:		.db	0xa0,0x86,0x01,0	;    100,000
I1E4:		.db	0x10,0x27,0x00,0	;     10,000
I1E3:		.db	0xe8,0x03,0x00,0	;      1,000
I1E2:		.db	0x64,0x00,0x00,0	;        100
I1E1:		.db	0x0a,0x00,0x00,0	;         10

		;** IEEE format **

PI:		.db	0xdb,0x0f,0x49,0x40	; 3.1415927f
Kp1:		.db	0xCD,0xCC,0xCC,0x3D	; 0.1f
Kp5:		.db	0x00,0x00,0x00,0x3F	; 0.5f
K1:		.db	0x00,0x00,0x80,0x3F	; 1.0f
K10:		.db	0x00,0x00,0x20,0x41	;10.0f
K1E7:		.db	0x80,0x96,0x18,0x4b
K1E8:		.db	0x20,0xbc,0xbe,0x4c
K999999p9: 	.db	0xfe,0x23,0x74,0x49	; 999999.9f
K9999999:	.db	0x7f,0x96,0x18,0x4b	;9999999.0f
