sqrt.asm

包含几个高效的矢量运算的数学函数

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

 .text
;;============================================================================
;;
;; Usage ASM:		
;;		.bss	sqrt_X,1 	; 0000h to 7FFFh (Q0.15 format)
;;		.bss	sqrt_Y,1 	; 0000h to 7FFFh (Q0.15 format)
;;		.bss	sqrt_Xnorm,1	; Temporary working register
;;
;;		call	Sqrt
;;
;;----------------------------------------------------------------------------
;;
;; Assumptions:	AR_STACK = Stack pointer, grows low to high, points to empty loc.
;;
;; Input:	sqrt_X 
;;
;; Modifies:	ARP
;;		AR2
;;		AR3
;;		P
;;		T
;;		DP
;;		SPM
;;		sqrt_Xnorm
;;
;; Output:	sqrt_Y = sqrt(X)          
;;
;;----------------------------------------------------------------------------
;;
;; Algorithm:	1) Normalize X		(I0Q15 format)
;;
;;		2) X = Xnorm/2^n	n = 0,1,2,...,14
;;					Xnorm = 0.5 to 0.99997
;;
;;		3) sqrt(X) = sqrt(Xnorm/2^n) = sqrt(Xnorm) * 1/sqrt(2^n)
;;
;;		4) Ytable = 1/sqrt(2^n)	 Note: obtain values from table
;;
;;		5) Ynorm = sqrt(Xnorm)	
;;
;;		   To calculate Ynorm, repeat the following N times:
;;
;;			Ynorm(new) = Ynorm(old) - (Ynorm(old)^2 - Xnorm)/2
;;
;;		   Use an initial value of Ynorm(old) = sqrt(0.53931).
;;
;;		6) Y = round(Ynorm * Ytable)
;;	
;;============================================================================

TRUE		.set	1
FALSE		.set	0

;
; To Test Code, Set The Following Flag To TRUE:
;

OPTIMIZE_SPEED	.set	TRUE
OPTIMIZE_CODE	.set	FALSE

;
; Set Number Of Iterations Of Sqrt Algorithm (N-1):
;
; Note: N = 5, accuracy = 100 * 2/32767 = 0.0061%
;

N		.set	5
ITERATIONS	.set	N-1	

; Ytable = 1/sqrt(2^n) values:
;

SqrtTable:			
	.word	7FFFh		; 1/sqrt(2^0)  = 0.99997 
	.word	5A82h		; 1/sqrt(2^1)  = 0.70711 
	.word	4000h		; 1/sqrt(2^2)  = 0.50000 
	.word	2D41h		; 1/sqrt(2^3)  = 0.35355 
	.word	2000h		; 1/sqrt(2^4)  = 0.25000 
	.word	16A1h		; 1/sqrt(2^5)  = 0.17678
	.word	1000h		; 1/sqrt(2^6)  = 0.12500 
	.word	0B50h		; 1/sqrt(2^7)  = 0.08839
	.word	0800h		; 1/sqrt(2^8)  = 0.06250 
	.word	05A8h		; 1/sqrt(2^9)  = 0.04419 
	.word	0400h		; 1/sqrt(2^10) = 0.03125 
	.word	02D4h		; 1/sqrt(2^11) = 0.02210 
	.word	0200h		; 1/sqrt(2^12) = 0.01563 
	.word	016Ah		; 1/sqrt(2^13) = 0.01105 
	.word	0100h		; 1/sqrt(2^14) = 0.00781 
	.word	0000h	

;
; Start Of Main Code:
;
	.include "ccall.asm"

	.def	_ti_sqrt
_ti_sqrt:
	pre_ccall 5,AR_X,AR_XEXP,AR_Z,AR_ZEXP,AR_N

AR_TABLE .set AR_XEXP

	sar AR_XEXP,*
	lacc *,AR_ZEXP ; XEXP
	sfr
	sacl *,AR_N
	mar *-,AR_X ; AR_N--

LOOP:
	lar	AR_TABLE,#SqrtTable 	; Get Ytable and calculate Xnorm
;	lacc	sqrt_X,16
	lacc *+,16,AR_TABLE
	add	#01h
	rpt	#14
	norm	*+
	nop ; pipeline
	nop ; pipeline

	mar	*,AR_Z
;	splk	#5E00h,sqrt_Y	; Initial value for Ynorm
	splk	#5E00h,*,AR_STACK	; Initial value for Ynorm
;	sach	sqrt_Xnorm
 	sach *,AR_Z

;	.if	OPTIMIZE_CODE
;	lar	AR3,#ITERATIONS	; Set N-1
;	mar	*,AR3
;SqrtLoop:
;	sqra	sqrt_Y		; Ynorm = Ynorm - (Ynorm^2 - Xnorm)/2
;	lacc	sqrt_Xnorm,15
;	spac
;	add	sqrt_Y,16
;	sach	sqrt_Y
;	banz	SqrtLoop,*-
;	.endif

;	.if	OPTIMIZE_SPEED

	.loop	N
;	sqra	sqrt_Y		; Ynorm = Ynorm - (Ynorm^2 - Xnorm)/2
	sqra	*,AR_STACK
;	lacc	sqrt_Xnorm,15
	lacc *,15,AR_Z
	spac
;	add	sqrt_Y,16
	add *,16
;	sach	sqrt_Y
	sach *
	.endloop
;	.endif

	mar *,AR_STACK
;	sar	AR_TABLE,sqrt_temp	; AR2 = Index into Ytable
	sar	AR_TABLE,*
;	lacl	sqrt_temp
	lacl *
;	tblr	sqrt_temp
	tblr *
;	lt	sqrt_temp	; T = Ytable
	lt *,AR_Z
;	mpy	sqrt_Y
	mpy *
	pac
	add	#04000h		; round
;	sach	sqrt_Y,1	; Y = Ynorm*Ytable
 	sach *+,1,AR_N
 	banz LOOP,AR_X
 	
DONE:
	post_ccall 5