gaecza.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 ".ftext2"

_SHFT			.set 	2
;-------------------------------------------------------------------------
;-------------------------------------------------------------------------
;-------------------------------------------------------------------------
	.global				_gaec_loopback
;-------------------------------------------------------------------------
_gaec_loopback
; GAEC_tDb *pDb	= a
; S16 *psSend 	= sp(1)
	stlm	a, ar2
	ssbx	sxm
	ssbx	frct
	mvdk	*sp(1), ar3
	mar		*+ar2(#GAEC_tDb.asRbuf+5)
	stm		#18425, t		; = Q15(0.5623)
	stm		#GAEC_FR_SZ-1, brc
	rptb	I01-1
		mpyr	*ar2+, a
		sth		a, *ar3+
I01	
	rsbx	frct
	ret
;-------------------------------------------------------------------------
	.global				_gaec_max_coef
;-------------------------------------------------------------------------
_gaec_max_coef
; GAEC_tDb *pDb	= a
;
	stlm 	a, ar2
	ssbx	sxm
	nop
	ld 		*ar2(#(GAEC_tDb.sMaxCoef)), a
	mpy		*ar2(#(GAEC_tDb.sMaxCoefBand)), #GAEC_ADF_SZ, b	
	add		*ar2(#(GAEC_tDb.psAdf)), b
	stlm	b, ar3
	stm		#GAEC_ADF_SZ-1, brc
	rptb	I11-1
		ld 		*ar3+, b
		abs		b
		max		a
I11	
	stl 	a, *ar2(#(GAEC_tDb.sMaxCoef))
	addm	#1, *ar2(#(GAEC_tDb.sMaxCoefBand))
	ld		*ar2(#(GAEC_tDb.sMaxCoefBand)), a
	sub		#(GAEC_BANDS*2), a
	bc		I12, alt
		ld 		*ar2(#(GAEC_tDb.sMaxCoef)), a
		ld 		*ar2(#(GAEC_tDb.sMaxCoefAv)), b
		sub		b, a
		add		a, -2, b
		stl		b, *ar2(#(GAEC_tDb.sMaxCoefAv))
		st		#0, *ar2(#(GAEC_tDb.sMaxCoefBand))
		st		#0, *ar2(#(GAEC_tDb.sMaxCoef))
I12
	ret	
	
	
	
	.if 0
;-------------------------------------------------------------------------
	.global				_gaec_shift_err
;-------------------------------------------------------------------------
_gaec_shift_err
; GAEC_tDb *pDb	= a
;
	stlm	a, ar2
	ssbx	sxm
	nop
	stm 	#GAEC_BLKS+1, ar0
	stm 	#0, bk
	mvmm	ar2, ar3
	mar		*+ar2(#GAEC_tDb.aasErrSav)
	mar		*+ar3(#GAEC_tDb.aasErrSav+GAEC_BLKS)
	stm		#2*GAEC_BANDS-1, brc
	rptb	I10-1
		rpt		#GAEC_ERR_SZ-GAEC_BLKS-2
			mvdd	*ar3+, *ar2+
		mvdd	*ar3+0%, *ar2+0%
I10	
	ret
	.endif
;-------------------------------------------------------------------------
	.global				_gaec_update_rbuf
;-------------------------------------------------------------------------
_gaec_update_rbuf
; GAEC_tDb *pDb	= a
; S16 *psRcv	= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvmm	ar2, ar4
	mar		*+ar4(#GAEC_tDb.asRbuf)
	mvmm	ar2, ar5
	mar		*+ar5(#GAEC_tDb.asRbuf+GAEC_FR_SZ)
	rpt		#(GAEC_RBUF_SZ-GAEC_FR_SZ-1)
		mvdd	*ar5+, *ar4+
	mvdk	*sp(1), ar5
	rpt		#GAEC_FR_SZ-1
		mvdd	*ar5+, *ar4+
	ret
		
;-------------------------------------------------------------------------
	.global				_gaec_shift_rcv
;-------------------------------------------------------------------------
_gaec_shift_rcv
; GAEC_tDb *pDb	= a
;
	stlm	a, ar4
	ssbx	sxm
	nop
	stm 	#GAEC_BLKS+1, ar0
	stm 	#0, bk
	mvdk 	*ar4(#GAEC_tDb.psRcv), ar2
	mvmm	ar2, ar3
	mar		*+ar3(#GAEC_BLKS)
	stm		#2*GAEC_BANDS-1, brc
	rptb	I20-1
		rpt		#GAEC_RCV_SZ-GAEC_BLKS-2
			mvdd	*ar3+, *ar2+
		mvdd	*ar3+0%, *ar2+0%
I20	
	ret
	
;-------------------------------------------------------------------------
	.global				_gaec_update_err_sav
;-------------------------------------------------------------------------
_gaec_update_err_sav
; GAEC_tDb *pDb	= a
; GAEC_tSc *pSc = sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvdk	*sp(1), ar3
	mvmm	ar2, ar4
	mar		*+ar4(#GAEC_tDb.aasErrSav+GAEC_BLKS)
	mar		*+ar2(#GAEC_tDb.aasErrSav)
	mar		*+ar3(#GAEC_tSc.aasErr)
	stm		#2*GAEC_BANDS-1, brc
	rptb	I30-1
		rpt 	#GAEC_ERR_SZ-GAEC_BLKS-1
			mvdd 	*ar4+, *ar2+
		rpt		#GAEC_BLKS-1
			mvdd	*ar3+, *ar2+
		mar 	*+ar4(#GAEC_BLKS)
I30	
	ret
;-------------------------------------------------------------------------
	.global				_gaec_copy_adf
;-------------------------------------------------------------------------
_gaec_copy_adf
; S16 *psTo		= a
; S16 *psFrom 	= sp(1)
; S16 band		= sp(2)
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvdk	*sp(1), ar3
	cmpm 	*sp(2), #0
	bc		I41, tc
	cmpm	*sp(2), #GAEC_BANDS
	bc		I42, tc
		mpy		*sp(2), #GAEC_ADF_SZ*2, a
		stlm	a, ar0
		nop
		nop
		mar		*ar2+0
		mar		*ar3+0
		rpt		#GAEC_ADF_SZ*2-1
			mvdd	*ar3+, *ar2+
		ret
I42
	mar		*ar2+
	mar		*ar3+
I41
	stm 	#2, ar0
	stm		#0, bk
	rpt		#GAEC_ADF_SZ-1
		mvdd	*ar3+0%, *ar2+0%
	ret	

;-------------------------------------------------------------------------
	.global				_gaec_copy_err
;-------------------------------------------------------------------------
_gaec_copy_err
; GAEC_tSc *pSc	= a
; S16 band		= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvmm	ar2, ar3
	mar		*+ar3(#GAEC_tSc.aasErr)
	mvmm	ar2, ar4
	mar		*+ar4(#GAEC_tSc.aasErrM)
;	
; copy real	
	mpy		*sp(1), #GAEC_BLKS, a
	stlm	a, ar0
	nop
	nop
	mar		*ar3+0
	mar		*ar4+0
	rpt		#GAEC_BLKS-1
		mvdd	*ar4+, *ar3+
;		
; copy imag		
	cmpm 	*sp(1), #0
	rc		tc
	nop
	cmpm	*sp(1), #GAEC_BANDS
	rc		tc
; 			
	mar		*+ar3(#GAEC_BLKS*(GAEC_BANDS-1))
	mar		*+ar4(#GAEC_BLKS*(GAEC_BANDS-1))
	rpt		#GAEC_BLKS-1
		mvdd	*ar4+, *ar3+
	ret
;-------------------------------------------------------------------------
	.global				_gaec_in_corr
;-------------------------------------------------------------------------
_gaec_in_corr
; S16 *psSav	= a, ar2
; S16 *psIO		= sp(1), ar3
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvdk	*sp(1), ar3
	stm		#GAEC_FR_SZ-1, brc
	rptb	I61-1
		sub		*ar3, *ar2, a
		add 	*ar2, (16-_SHFT), a
		add		#1, 15, a
		mvdd 	*ar3, *ar2
		sth		a, *ar3+ 
I61	
	ret
;-------------------------------------------------------------------------
	.global				_gaec_out_corr
;-------------------------------------------------------------------------
_gaec_out_corr
; S32 *pslSav	= a, ar2
; S16 *psIO		= sp(1), ar3
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvdk	*sp(1), ar3
	stm		#GAEC_FR_SZ-1, brc
	rptb	I71-1
		ld 		*ar3, 15, a
		dld		*ar2, b
		add		b, a
		sub		b, -_SHFT, a
		dst		a, *ar2
		
		add		#1, 14, a
		sth		a, 1, *ar3+
I71
	ret