📄 qsqrt.asm
字号:
;===========================================================
;
; File Name :qsqrt.asm
;
; Originator :Digital Control Systems Group
; Texas Instruments
;
; Description :This file contain source code for Fixed point square root
;
; Date : 30/10/2000
;==========================================================
;
;
; Routine Name : Generic Function
; Routine Type : C Callable
;
; Description :
; unsigned int qsqrt(unsigned long int x)
;
; Algorithm :
; sqrt(x): where 'x' is in 16.16 format
;
; The following equation approximates the sqrt(x), where 0.5<= x <=1.0
; 0.5*sqrt(x)= 0.7274475*x-0.672455*x^2+0.553406*x^3-0.2682495*x^4+0.0560605*x^5+0.1037903
;
; To determine the sqrt of an input value outside the range from 0.5 to 1.0, the input
; should be scaled to a number within the range. After computing the sqrt of the
; scaled number, it has to be multipied by the sqrt of the scaling value to produce
; the sqrt of the original number. Scaling is done by normalising the number, hence the
; scaling will be interge power of 2.
;
;====================================================================
; Function Local Frame
;======================================================================
; |_______|
; |_______|<- Stack Pointer (FP+2) <---AR1
; |_______|<- Computation (FP+1) <---AR2
; |_______|<- Register to register transfer (FP) <---AR0
; |_______|<- Old FP (FP-1)
; |_______|<- Return Address of the Caller (FP-2)
; |_______|<- MSW of Formal parameter x (FP-3) <---AR3
; |_______|<- LSW of Formal Parameter (FP-4)
;=============================================================================
; Module definition for external referance
.def _qsqrt
__sqrt_frs .set 00002h ; Local frame size for this routine
__a0 .set 06a48h ; 0.1037903 scaled by 2^18
__a1 .set 05d1dh ; 0.7274475 scaled by 2^15
__a2 .set 0a9edh ; -0.672455 scaled by 2^15
__a3 .set 046d6h ; 0.553406 scaled by 2^15
__a4 .set 0bb54h ; -0.2682495 scaled by 2^16
__a5 .set 00e5ah ; 0.0560605 scaled by 2^16 for Immediate MPY
__isqt2 .set 00b50h ;(1/sqrt(2))=0.707106 in scaled Q12 format
; Also sqrt(2) in scaled Q11 format
__isqt2d .set 05a82h ;(1/sqrt(2)) in Q15 format
; Also sqrt(2) in Q14 format
_qsqrt:
POPD *+ ; Store the Return Address in stack
SAR AR0,*+ ; Store the Caller's Frame Pointer
SAR AR1,*
LAR AR0,#__sqrt_frs
LAR AR0,*0+,AR2 ; Create Local frame for SQRT function
SETC SXM
LAR AR2,#01h ; AR2=#1
MAR *0+,AR3 ; AR2=FP+1
LAR AR3,#0FFFDh ; AR3=-3
MAR *0+ ; AR3=FP-3 to point to argument x (MSW)
LACC *-,16 ; Load the MSW of x in ACCL
ADDS *+,AR4 ; Combine with the LSB
; ACC=x
BCND calc,GEQ
ROR ; If MSB is '1' then x=x/2
calc: LAR AR4,#0fff1h ; AR4=-15
RPT #29 ; Scale the X to the range 0.5 to 1.0
NORM *+ ; AR4=E-15, Max value of E will be 30
; The scaling=2^(E-15)
NOP
NOP
BCND return,EQ ; If ACC=0, then return with ZERO
MAR *,AR2 ; AR2=FP+1
SACH * ; Scaled value of 'x'(Range 0.5 to 1.0) in Q15
LT * ; TREG=x in Q15
MPY #__a5 ; P=x*a5 in Q31
LACC #__a4,15 ; ACC=a4 in Q31
APAC ; ACC=a4+x*a5 in Q31
SACH * ; Store a4+x*a5 in Q15
MPY * ; P=x*(a4+x*a5) in Q30
LACC #__a3,15 ; ACC=a3 in Q30
APAC ; ACC=a3+x*(a4+x*a5) in Q30
SACH *,1 ; Store a3+x*(a4+x*a5) in Q15
MPY * ; P=x*(a3+(a4+x*a5)) in Q30
LACC #__a2,15 ; ACC=a2 in Q30 format
APAC ; ACC=a2+x*(a3+(a4+x*a5)) in Q30
SACH *,1 ; Store a2+x*(a3+(a4+x*a5)) in Q15
MPY * ; P=x*(a2+x*(a3+(a4+x*a5))) in Q30
LACC #__a1,15 ; ACC=a1 in Q30 format
APAC ; ACC=a1+x*(a2+x*(a3+(a4+x*a5))) in Q30
SACH *,1 ; Store a1+x*(a2+x*(a3+(a4+x*a5))) in Q15
MPY * ; P=x*(a1+x*(a2+x*(a3+(a4+x*a5)))) in Q30
LACC #__a0,12 ; ACC=a0 in Q30
APAC ; ACC=a0+x*(a1+x*(a2+x*(a3+(a4+x*a5)))) in Q30
; 0.5*sqrt(scaled x) is in ACC in Q30
SACH *,1,AR0 ; Store sqrt(scaled x) in Q14 format
SAR AR4,* ; (FP)=E-15
LACC * ; Load the exponent of the scaler
BCND noscal, EQ ; if E-15 is zero, scaling is 1.
;------------------------------------------------------------------
; Calculating the square root of the scaler 2^(E-15)
;------------------------------------------------------------------
BCND scal_gt_1,GT ; If GT, the original number is scaled up
ABS ; If LT, then the original number is scaled down
ADD #06h ; offset the shift by (6/2)=3
SFR ; LSBit is shifted to C
SACL *,0,AR2 ; (FP)=(|E-15|+6)/2, the Bit 0 stored in C bit
BCND lsskipmul,NC ; Skip the sqrt(2) multiplication for even exp
LT * ; sqrt of approximation in Q14
;==============================================================
; This code is commented, because if we use Q11 format for sqrt(2)
; then, the sqrt(65535) gives 28 counts error
; The Actual result in 8.8 format should be 65535
; What we get is 65507
;==============================================================
; MPY #__isqt2 ; sqrt(2) in Q11 format
; PAC ; Result in Q25 format
; SACH *,5 ; Store it in Q14 format
;==============================================================
; Q14 format of sqrt(5) reduces the error to 15 counts
;==============================================================
LACC #__isqt2d ; sqrt(2) in Q14 format
SACL *
MPY *
PAC ; Result in Q28 format
SACH *,2 ; Store it in Q14 format
;----------------------------------------------------------------
lsskipmul: MAR *,AR0
LT *,AR2 ; TREG=(|E-15|/2)+3
LACT *,AR0 ; ACC in Q17 due to the shift offset of 3
SACH *,7,AR3 ; 8.8 format
B modify
scal_gt_1: NEG
ADD #0fh
SFR
SACL *,0,AR2 ; (FP)=(15-|E-15|)/2, bit zero is in C bit
BCND gskipmul,C ; skip 1/sqrt(2) multiplication for even exp
LT * ; TREG=sqrt of approximation in Q14
;==============================================================
; This code is commented, because if we use Q12 format for 1/sqrt(2)
; then, the sqrt(65535) gives 28 counts error
; The Actual result in 8.8 format should be 65535
; What we get is 65507
;==============================================================
; MPY #__isqt2 ; 1/sqrt(2) in Q12 format
; PAC ; Result in Q26
; SACH *,4 ; Store it in Q14 format
;==============================================================
; Q15 format of 1/sqrt(5) reduces the error to 15 counts
;==============================================================
LACC #__isqt2d ; 1/sqrt(2) in Q15 format
SACL *
MPY *
PAC ; Result in Q29
SACH *,1 ; Store it in Q14 format
;-------------------------------------------------------------
gskipmul: MAR *,AR0
LT *,AR2 ; Load the Exp in TREG
LACT *,AR0 ; (Q14/2^7)=Q21 multiplied by 2^exp
SACH *,3,AR3 ; Store in 8.8 format
B modify
noscal: MAR *,AR2 ; Convert the Q14 result to Q8
LACC *,10,AR0
SACH *,0,AR3 ; Store result in 8.8 format
modify:
BIT *,0,AR0 ; is MSB of the input argument '1'
BCND nomul,NTC ; (MSBit is 0, then return the result)
LT * ; TREG=8.8 format
;==============================================================
; This code is commented, because if we use Q11 format for sqrt(2)
; then, the sqrt(65535) gives 28 counts error
; The Actual result in 8.8 format should be 65535
; What we get is 65507
;==============================================================
; LACC #__isqt2
; SACL * ; sqrt(2) in Q11 format
; MPYU *
; PAC ; Q19 format
; SACH *,5 ; Store it in 8.8 format
;==============================================================
; Q14 format of sqrt(5) reduces the error to 15 counts
;==============================================================
LACC #__isqt2d ; sqrt(2) in Q14 format
SACL *
MPYU *
PAC ; Q22 format
SACH *,2 ; Store it in 8.8 format
;---------------------------------------------------------------
nomul: LACL * ; Return result in 8.8 format
return: MAR *,AR1
SBRK #(__sqrt_frs+1) ; Clear the local frame
LAR AR0,*- ; Retrive Caller's frame pointer
PSHD * ; Push the return address to TOS
RET ; Return to the caller
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -