gaecrcva.s54

TI公司DSP/tms320c55x/tms320c54x系列的声学回声消除代码

*-------------------------------------------------------------------------*
*                                                                         *
*   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
*   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
*      "COPYRIGHT 2003 MICHAEL TSIROULNIKOV, ALL RIGHTS RESERVED"         *
*                                                                         *
*-------------------------------------------------------------------------*/
	.mmregs
	.include "gaeci.h54"
	.sect ".ftext3"

	
;-------------------------------------------------------------------------
prepare		.macro	times
;-------------------------------------------------------------------------
	mar		*+psRk(times)
	ld		*psRk-, 16, a	
	.loop	(times - 1)
	mac		*psRk-, *psF+, a
	.endloop
	macr	*psRk,  *psF+, a
	.endm
;-------------------------------------------------------------------------
get_kj0		.macro		
;-------------------------------------------------------------------------
	abs		a, b					; a=abs(ac0), shall be < J0.
	sub		*psJ0, 16, b
	bc		L02, bgeq
	add		*psJ0, 16, b
;
; get and save new sK
	ld      b, -1, b
	rpt		#16-1
		subc	*psJ0, b
	and		*pOne, b
	xc		1, alt
		neg		b
	stl		b, *psK					; asK[2]
;
; get and save new J0
	ld		*pOne, 16, a
	squrs	*psK, a
	adds	*pRnd, a
	mpya	*psJ0
	adds	*pRnd, b
	sth		b, *psJ0					; new J0 saved
	.endm	

;-------------------------------------------------------------------------
get_lG	.macro	times
;-------------------------------------------------------------------------
	mar		*+psF(-times)					; psF -> asF[1]
	.loop	times
	mpy		*psF+, *psK, a
	dst		a, *pslG+
	.endloop
	mar		*+pslG(-times*2)
	.endm
	
;-------------------------------------------------------------------------
update_f	.macro	times
;-------------------------------------------------------------------------
; update f[last]
	ld		*psK+, a				; advance psK
	neg		a
	stl		a, *psF-
; update f[prevs]
	.loop	times
	ld		*psF, 16, a
	dsub	*pslG+, a
	adds	*pRnd, a
	sat		a
	sth		a, *psF-
	.endloop
	mar		*psF+
	ld		*psJ0, a
	sub		*pJ0min, a
	bc		L01, alt
	mar		*+pslG(-times*2)
	.endm

	.sect	".ftext3"
	
;-------------------------------------------------------------------------
	.global					_gaec_rcv_durbin
;-------------------------------------------------------------------------
_gaec_rcv_durbin
;
; LEC_tDb *pDb		= a
; LEC_tScRcv *pScr	= sp(1)
;
SJ0_MIN 	.set	(2048/16)
			.asg	ar2, psF
			.asg	ar3, psK
			.asg	ar4, pslG
			.asg	ar5, psRk
			.asg	sp(3), pRnd
			.asg	sp(2), pJ0min
			.asg	sp(1), pOne
			.asg	sp(0), psJ0
	
	frame	#-4	
	ssbx	sxm
	ssbx	frct
	ld		#0, a
;
; zero asF
	mvdk	*sp(5), psF
	mar		*+psF(#(GAEC_tScRcv.asF))
	rpt		#(6-1)
		stl	a, *psF+
	mar		*+psF(-6)
;
; zero asK
	mvdk	*sp(5), psK
	mar		*+psK(#(GAEC_tScRcv.asK))
	rpt		#(6-1)
		stl	a, *psK+
	mar		*+psK(-6)
;
; zero aslG 
	mvdk	*sp(5), pslG
	mar		*+pslG(#(GAEC_tScRcv.aslG))
	rpt		#(2*6-1)
		stl		a, *pslG+
	mar		*+pslG(#-((6-1)*2))
;
; set asRk ptr
	mvdk	*sp(5), psRk
	mar		*+psRk(#(GAEC_tScRcv.asRk+1))
;
; iteration 0
;	
	st		#32767, *psF+	; F[1]
	st		#32767, *psK+	; K[1]
	st		#32767, *psJ0	; pJ0
	st		#32767, *pOne	; 1.0
	st		#SJ0_MIN, *pJ0min; SJ0_MIN
	st		#0x8000, *pRnd	; rounding
; 
; iteration 1
;
	ld		*psRk, a
	stl		a, *psK+
	neg		a
	stl		a, *psF
	ld		*pOne, 16, a
	squrs	*psRk+, a
	adds	*pRnd, a
	sth		a, *psJ0
	
	sub		*pJ0min, 16, a
	bc		L01, alt
;
; iteration 2
;
	ld		*psRk-, 16, a
	macr	*psRk, *psF+, a
	
	get_kj0	
	get_lG		1
	update_f	1
;
;	iteration 3
;
; psRk -> asRk[1]
; psF  -> asF[1]
; psK  -> asK[2]
; pslG -> aslG[1]
;
	prepare		2
	get_kj0
	get_lG		2
	update_f	2
;
;	iteration 4
;
; psRk -> asRk[1]
; psF  -> asF[1]
; psK  -> asK[3]
; pslG -> aslG[1]
;
	prepare		3	
	get_kj0
	get_lG		3
	update_f	3
;
;	iteration 5
;
; psRk -> asRk[1]
; psF  -> asF[1]
; psK  -> asK[4]
; pslG -> aslG[1]
;
	prepare		4
	get_kj0
	get_lG		4
	update_f	4
	
L01
	ld		*psJ0, a
	rsbx	frct
	frame	#4
	ret
L02
	ld		#0, a
	rsbx	frct
	frame	#4
	ret

;-------------------------------------------------------------------------
	.global					_gaec_rcv_normalise_rk
;-------------------------------------------------------------------------
_gaec_rcv_normalise_rk
; GAEC_tDb *pDb		= a : not used
; GAEC_tScRcv *pScr	= sp(1)
; S16 sShift		= sp(2)
; S16 sAdd 			= sp(3)
;
	ssbx	sxm
	mvdk	*sp(1), ar4
	mvmm	ar4, ar3
	mar		*+ar3(#(GAEC_tScRcv.asRk))
	mvmm	ar4, ar2
	mar		*+ar2(#(GAEC_tScRcv.aslRk))
	ld		*sp(2), asm
	
	dld		*ar2+, a
	add     *sp(3), a
	add		a, asm, a					; shift a by sShift
	exp		a
	nop
	norm	a
	sth		a, *ar3
	ld		#0x3fff, 15, a
	rpt		#(16-1)
		subc	*ar3, a
	and		#0xffff, a			; a = invR0
	ld		a, 8, a
	ld		a, 8, a
	
	st		t, *ar3
	ld		*ar3, asm
	st		#32767, *ar3+
	stm		#(5-1), brc
	rptb	L11-1
		dld		*ar2+, b
		ld		b, asm, b
		sth		b, *ar3
		mpya	*ar3
		sth		b, 2, *ar3+
L11	
	ret
;-------------------------------------------------------------------------
	.global					_gaec_rcv_moments
;-------------------------------------------------------------------------
_gaec_rcv_moments
; GAEC_tDb *pDb		= a, ar2
; GAEC_tScRcv *pScr	= sp(1)
; S16 *psRcv		= sp(2)
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvdk	*sp(1), ar3
; 
; shift old data	
	mvmm	ar2, ar4
	mvmm	ar3, ar5
	mar 	*+ar4(#GAEC_tDb.asTdSav)
	mar 	*+ar5(#GAEC_tScRcv.asTdBuf)
	rpt 	#5-1
		mvdd 	*ar4+, *ar5+
;		
; add new from psRcv >> 2
	mvdk 	*sp(2), ar4
	stm 	#GAEC_FR_SZ-1, brc
	rptb	L21-1
		ld 		*ar4+, 14, a
		sth 	a, *ar5+
L21
;
; save new data
	mvmm	ar2, ar4
	mvmm	ar3, ar5
	mar 	*+ar4(#GAEC_tDb.asTdSav)
	mar 	*+ar5(#GAEC_tScRcv.asTdBuf+GAEC_FR_SZ)
	rpt 	#5-1
		mvdd 	*ar5+, *ar4+
;
; set ptrs
; ar5 = psTmp
; ar4 = psTdBuf
; ar3 = pslRk
;
	mvmm	ar3, ar4
	mar		*+ar4(#GAEC_tScRcv.asTdBuf+5)
	mvmm	ar4, ar5
	mar 	*+ar3(#GAEC_tScRcv.aslRkTx)
	
; initiate loop & set Rk ptr
	stm		#(6-1), brc
	rptb	L22-1
		rptz	a, #(GAEC_FR_SZ-1)
			mac		*ar5+, *ar4+, a
		dst		a, *ar3+
		mar		*+ar5(#(-GAEC_FR_SZ))
		mar		*+ar4(#(-GAEC_FR_SZ-1))
L22	
	.if 0
	.endif
	ret