;; Vectorized and modified by: Jeff Axelrod
;; Original Version:	Alex Tessarolo

;;===========================================================================
;;
;; Contents:	InvQ15YmYe	; Y = 1/X, X = Q15, Y  = Ym * Ye, 
;;				;                   Ym = Q15, 
;;				;	            Ye = 2^n, n = 1 to 15
;;
;;===========================================================================

;; Usage ASM:	
;;		.bss	inv_X,1		;  8000h to 7FFFh (Q0.15 format)
;;		.bss	inv_Ym,1	;    0.5 to   1	  (Q0.15 format)
;;					; or -1  to -0.5  (Q0.15 format)
;;		.bss	inv_Ye,1	; Ye = 2^n, n = 1,2,...,13,14,15
;;
;;		call	InvQ15YmYe
;;
;;---------------------------------------------------------------------------
;;
;; Input:	inv_X
;;
;; Modifies:	DP
;;		SXM
;;		OVM	
;;		ACC
;;		P
;;		T
;;
;; Output:	inv_Ym
;;		inv_Ye		
;;
;;---------------------------------------------------------------------------
;;
;; Algorithm:	The most optimal method for calculating the inverse of a
;;		fractional number (Y=1/X) is to normalize the number first. 
;;		This limits the range of the number as follows:	
;;
;;			0.5 <= Xnorm <   1		
;;			-1  <= Xnorm <= -0.5			(1)
;;
;;		The resulting equation becomes:
;;
;;			Y = 1/(Xnorm*2^-n)
;;
;;		or	Y = 2^n/Xnorm				(2)
;;
;;			where n = 1,2,3,...,14,15 
;;
;;		Letting Ye = 2^n:
;;
;;			Ye = 2^n				(3)
;;
;;		Substituting (3) into equation (2):
;;
;;			Y = Ye * 1/Xnorm			(4)
;;
;;		Letting Ym = 1/Xnorm:
;;
;;			Ym = 1/Xnorm				(5)
;;
;;		Substituting (5) into equation (4):
;;
;;			Y = Ye * Ym				(6)
;; 	
;;		For the given range of Xnorm, the range of Ym is:
;;
;;			 1 <= Ym <   2
;;			-2 <= Ym <= -1				(7)
;;
;;		To calculate the value of Ym, various options are possible:
;;
;;		(a) Taylor Series Expansion
;;		(b) 2nd,3rd,4th,.. Order Polynomial (Line Of Best Fit)
;;		(c) Successive Approximation
;;
;;		The method chosen in this example is (c). Successive 
;;		approximation yields the most optimum code versus speed 
;;		versus accuracy option. The method outlined below yilds an 
;;		accuracy of 15 bits.
;;
;;		Assume Ym(new) = exact value of 1/Xnorm:
;;		
;;			Ym(new) = 1/Xnorm			(c1)
;;		
;;		or	Ym(new)*X = 1				(c2)
;;
;;		Assume Ym(old) = estimate of value 1/X:
;;
;;			Ym(old)*Xnorm = 1 + Dyx 		
;;
;;		or	Dxy = Ym(old)*Xnorm - 1			(c3)
;;
;;			where Dyx = error in calculation
;;
;;		Assume that Ym(new) and Ym(old) are related as follows:
;;
;;			Ym(new) = Ym(old) - Dy			(c4)
;;
;;			where Dy = difference in values 
;;
;;		Substituting (c2) and (c4) into (c3):
;;		
;;			Ym(old)*Xnorm = Ym(new)*Xnorm + Dxy
;;
;;			(Ym(new) + Dy)*Xnorm = Ym(new)*Xnorm + Dxy
;;
;;			Ym(new)*Xnorm + Dy*Xnorm = Ym(new)*Xnorm + Dxy
;;		
;;			Dy*Xnorm = Dxy			
;;
;;			Dy = Dxy * 1/Xnorm			(c5)
;;
;;		Assume that 1/Xnorm is approximately equal to Ym(old):
;;
;;			Dy = Dxy * Ym(old) (approx)		(c6)
;;
;;		Substituting (c6) into (c4):
;;
;;			Ym(new) = Ym(old) - Dxy*Ym(old)		(c7)
;;
;;		Substituting for Dxy from (c3) into (c7):
;;
;;			Ym(new) = Ym(old) - (Ym(old)*Xnorm - 1)*Ym(old)
;;
;;			Ym(new) = Ym(old) - Ym(old)^2*Xnorm + Ym(old)
;;
;;		     	Ym(new) = 2*Ym(old) - Ym(old)^2*Xnorm 	(c8) 
;;
;;		If after each calculation we equate Ym(old) to Ym(new):
;;
;;			Ym(old) = Ym(new) = Ym
;;		
;;		Then equation (c8) evaluates to:
;;
;;		     +--------------------------+
;;		     |	Ym = 2*Ym - Ym^2*Xnorm 	| 		(c9) 
;;		     +--------------------------+
;;
;;		If we start with an initial estimate of Ym, then equation
;;		(c9) will converge to a solution very rapidly (typically
;;		3 iterations for 16-bit resolution).
;;
;;		The initial estimate can either be obtained from a look up 
;;		table, or from choosing a mid-point, or simply from linear 
;;		interpolation. The method chosen for this problem is the
;;		latter. This is simply accomplished by taking the complement
;;		of the least significant bits of the Xnorm value.
;;
;;===========================================================================


 .text		
				    
InvYeTable:
	.word	0002h		; Ye = 2^1
	.word	0004h		; Ye = 2^2
	.word	0008h		; Ye = 2^3
	.word	0010h		; Ye = 2^4
	.word	0020h		; Ye = 2^5
	.word	0040h		; Ye = 2^6
	.word	0080h		; Ye = 2^7
	.word	0100h		; Ye = 2^8
	.word	0200h		; Ye = 2^9
	.word	0400h		; Ye = 2^10
	.word	0800h		; Ye = 2^11
	.word	1000h		; Ye = 2^12
	.word	2000h		; Ye = 2^13
	.word	4000h		; Ye = 2^14
	.word	8000h		; Ye = 2^15

	.include "ccall.asm"
	
	.def	_ti_recip16
_ti_recip16:
	pre_ccall 4,AR_X,AR_Ym,AR_Ye,AR_N

AR_Xnorm .set AR_Ye ; reuse Ye 
AR_Table .set ar7

	mar *,AR_N	
	mar *-,AR_X

	setc	SXM		; MUST turn sign extension mode on.
					; Note: Overflow mode is off in C.
	setc	OVM		; MUST turn overflow mode on.
	spm	1		; MUST set product shift mode to +1.				

LOOP:
;	lacc	inv_X,16	; Normalize the input value X.
	lacc *+,16,AR_Table
	lar	AR_Table,#InvYeTable
	rpt	#15
	norm *+		; ACCH = X normalized.
	nop
	nop ; pipeline
	mar *,AR_Xnorm
;	sach	inv_Xnorm  	; AR2 points to appropriate Ye value in table.
	sach *,AR_Ym

	sfr			; Estimate the first Ym value.
	xor	#01FFFh,16		
;	sach	inv_Ym
	sach *
				; First iteration:
;	lacc	inv_Ym,15	; Calculate Ym = 2*Ym - Ym^2*X
	lacc *,15
;	lt	inv_Ym
	lt *,AR_Xnorm
;	mpy	inv_Xnorm
	mpy *,AR_Ym
;	sph	inv_Ym
	sph *
;	mpy	inv_Ym
	mpy *
	spac
;	sach	inv_Ym,2
	sach *,2

				; Second iteration:
;	lacc	inv_Ym,15	; Calculate Ym = 2*Ym - Ym^2*X
	lacc *,15
;	lt	inv_Ym
	lt *,AR_Xnorm
;	mpy	inv_Xnorm
	mpy *,AR_Ym
;	sph	inv_Ym
	sph *
;	mpy	inv_Ym
	mpy *
	spac
;	sach	inv_Ym,2
	sach *,2

				; Final iteration:
;	lacc	inv_Ym,15	; Calculate Ym = 2*Ym - Ym^2*X
	lacc *,15
;	lt	inv_Ym
	lt *,AR_Xnorm
;	mpy	inv_Xnorm
	mpy *,AR_Ym
;	sph	inv_Ym
	sph *
;	mpy	inv_Ym
	mpy *
	spac

;	splk	#07000h,inv_Ym	; Make sure that 8000h <= Ym < 7FFFh
	splk	#07000h,*		; Make sure that 8000h <= Ym < 7FFFh
;	add	inv_Ym,16
;	sub	inv_Ym,16
;	sub	inv_Ym,16
;	add	inv_Ym,16
	add	*,16
	sub	*,16
	sub	*,16
	add	*,16
;	sach	inv_Ym,3
	sach *+,3,AR_Ye

	sar	AR_Table,*	; Read Ye value from table.
	lacl	*
	tblr	*+,AR_N	; Ye = *AR2
	
	banz LOOP,AR_X

DONE:
	clrc OVM ; Restore OVM	
	spm	0		; MUST restore product shift mode to 0.
	post_ccall 4