lecpa.s54

The line echo canceller (LEC) is designed to provide the maximum attainable transparent voice qualit

	.mmregs
	.include	"leci.h54"
	
  		
	.text
;-------------------------------------------------------------------------
	.global				_lec_dummy0
	.sym _lec_dummy0, _lec_dummy0, 32, 2, 0
;-------------------------------------------------------------------------
_lec_dummy0
	ret
	
;-------------------------------------------------------------------------
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	".lectxt1"
	
;-------------------------------------------------------------------------
	.global					_lec_durbin
	.sym _lec_durbin, _lec_durbin, 32, 2, 0
;-------------------------------------------------------------------------
_lec_durbin
;
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= 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(#(LEC_tSc.asF))
	rpt		#(6-1)
		stl	a, *psF+
	mar		*+psF(-6)
;
; zero asK
	mvdk	*sp(5), psK
	mar		*+psK(#(LEC_tSc.asK))
	rpt		#(6-1)
		stl	a, *psK+
	mar		*+psK(-6)
;
; zero aslG 
	mvdk	*sp(5), pslG
	mar		*+pslG(#(LEC_tSc.aslG))
	rpt		#(2*6-1)
		stl		a, *pslG+
	mar		*+pslG(#-((6-1)*2))
;
; set asRk ptr
	mvdk	*sp(5), psRk
	mar		*+psRk(#(LEC_tSc.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					_lec_normalise_rk
	.sym _lec_normalise_rk, _lec_normalise_rk, 32, 2, 0
;-------------------------------------------------------------------------
_lec_normalise_rk
; pDb		= a : not used
; pSc		= sp(1)
; sShift 	= sp(2)
; sAdd 		= sp(3)
;
	ssbx	sxm
	mvdk	*sp(1), ar4
	mvmm	ar4, ar3
	mar		*+ar3(#(LEC_tSc.asRk))
	mvmm	ar4, ar2
	mar		*+ar2(#(LEC_tSc.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					_lec_tx_moments
	.sym _lec_tx_moments, _lec_tx_moments, 32, 2, 0
;-------------------------------------------------------------------------
_lec_tx_moments
; pDb 	= a, ar2
; pSc	= sp(1)
	stlm	a, ar2
	ssbx	sxm
	nop
; ar3 = &pDb->psHst[LEC_HST_SZ-5]	
	mvdk	*ar2(#(LEC_tDb.psHst)), ar3
	mar 	*+ar3(#(LEC_HST_SZ-5))
; ar4 = pSc->asTmp	
	mvdk	*sp(1), ar4
	mar		*+ar4(#(LEC_tSc.asTmp))
; copy 5 samples	
	rpt		#(5-1)
		mvdd	*ar3+, *ar4+
; ar3 = pSc->psTx
	mvdk	*sp(1), ar5
	mvdk	*ar5(#(LEC_tSc.psTx)), ar3
; add new data from psTx
	rpt		#(ILEC_FR_SZ-1)
		mvdd	*ar3+, *ar4+
; return psTx, psSav pointers back			
	mar		*+ar3(#(-ILEC_FR_SZ))
	mar		*+ar4(#(-ILEC_FR_SZ))
; initiate loop & set Rk ptr
	stm		#(6-1), brc
	mar		*+ar5(#(LEC_tSc.aslRkTx))
	rptb	L17-1
		rptz	a, #(ILEC_FR_SZ/2-1)
			mac		*ar3+, *ar4+, a
		rptz	b, #(ILEC_FR_SZ/2-1)
			mac		*ar3+, *ar4+, b
		ld 		a, -1, a
		add 	b, -1, a
		dst		a, *ar5+
		mar		*+ar3(#(-ILEC_FR_SZ))
		mar		*+ar4(#(-ILEC_FR_SZ-1))
L17	
	ret

;-------------------------------------------------------------------------
	.global					_lec_gs_moments
	.sym _lec_gs_moments, _lec_gs_moments, 32, 2, 0
;-------------------------------------------------------------------------
_lec_gs_moments
; pDb		= a : ar2
; pSc		= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	nop
; ar2 = pDb->aslRk	
	mar		*+ar2(#(LEC_tDb.aslRk))
; ar3 = pSc->aslRk	
	mvdk	*sp(1), ar3
	mar 	*+ar3(#(LEC_tSc.aslRk))
; ar4 = pSc->aslRkTx
	mvdk	*sp(1), ar4
	mar		*+ar4(#(LEC_tSc.aslRkTx))
	stm		#(6-1), brc
	rptb	L21-1
		dld 	*ar4+, a	
		dsub	*ar2, a
		ld 		a, -4, a
		dadd 	*ar2, a
		dst 	a, *ar2+
		dst		a, *ar3+
L21
	ret			
;-------------------------------------------------------------------------
	.global					_lec_ts_moments
	.sym _lec_ts_moments, _lec_ts_moments, 32, 2, 0
;-------------------------------------------------------------------------
_lec_ts_moments
; pDb		= a : ar2
; pSc		= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	nop
; ar3 = pSc->aslRkTx	
	mvdk	*sp(1), ar3
	mar 	*+ar3(#(LEC_tSc.aslRkTx))
; ar4 = pSc->aslRk	
	mvdk	*sp(1), ar4
	mar 	*+ar4(#(LEC_tSc.aslRk))
; ar2 = pDb->aslRkSav	
	mar		*+ar2(#(LEC_tDb.aslRkSav))
; start looping	
	stm		#(6-1), brc
	rptb	L31-1
		dld		*ar3+, a
		dld		*ar2, b
		dst 	a, *ar2+
		add 	b, a
		dst		a, *ar4+
L31	
	ret			
	.sect	".lectxt0"
;-------------------------------------------------------------------------
	.global					_lec_gs_filter
	.sym _lec_gs_filter, _lec_gs_filter, 32, 2, 0
;-------------------------------------------------------------------------
_lec_gs_filter
; pDb		= a : ar2
; pSc		= sp(1)
;
; ar5 = psHst
; ar3 = psTxF
; ar4 = psF
; ar0 = -(sizeof(asF)-1)
;
	stlm	a, ar2
	ssbx	sxm
	ssbx	frct
; ar3 = pDb->psTxF	
	mvdk	*ar2(#(LEC_tDb.psTxF)), ar3
; ar4 = &pDb->psTxF[ILEC_FR_SZ]	
	mvmm	ar3, ar4
	mar		*+ar4(#(ILEC_FR_SZ))
; copy data	
	rpt		#(LEC_HST_SZ-ILEC_FR_SZ-1)
		mvdd	*ar4+, *ar3+
; ar3 -> right location in TxF already
; ar2 = &pDb->psHst[];
	mvdk	*ar2(#(LEC_tDb.psHst)), ar4
	mar		*+ar4(#(LEC_HST_SZ-ILEC_FR_SZ))
; set up psF
	mvdk	*sp(1), ar5
	mar 	*+ar5(#(LEC_tSc.asF))
; set up loop
	stm		#(1-6), ar0
	stm		#0, bk
	stm		#(ILEC_FR_SZ-1), brc
; filter it	
	rptb	L41-1
		rptz	a, #(6-2)
			mac		*ar4-, *ar5+, a	; 5 times; ar4-=5;
		macr	*ar4+, *ar5+0%, a	; ar5 is back, ar4+=1
		sth		a, *ar3+
		mar		*ar4-0				; ar4+=5
L41 			
	rsbx	frct
	ret	
	.sect	".lectxt1"
;-------------------------------------------------------------------------
	.global				_lec_lp_residual_error
	.sym _lec_lp_residual_error, _lec_lp_residual_error, 32, 2, 0
;-------------------------------------------------------------------------
_lec_lp_residual_error
; pDb		= a
; pSc		= sp(1)
; pCoeff	= sp(2)
;
	stlm	a, ar4
	ssbx	sxm
	ssbx	frct
	mvdk	*sp(1), ar2
	mar		*+ar4(#(LEC_tDb.asLpSav))
	mar		*+ar2(#(LEC_tSc.asTmp))
;
; copy data from Db.Sav
	rpt		#(5-1)
		mvdd	*ar4+, *ar2+
;
; append data from Err1
	mvdk	*sp(1), ar3
	mar		*+ar3(#(LEC_tSc.asErr1))
	rpt		#(ILEC_FR_SZ-1)
		mvdd	*ar3+, *ar2+			; at the end, ar2->lpTmp[SZ+5]
	mar		*+ar3(#(-ILEC_FR_SZ))			; return pErr1 ptr back
;
; update Db.Sav
	mar		*ar2-
	mar		*ar4-
	rpt		#(5-1)
		mvdd	*ar2-, *ar4-			; ar2->lpTmp[SZ-2]
;
; filter
	mar		*+ar2(#(6-ILEC_FR_SZ))		; points to lpTmp[5+0]
	stm		#1-6, ar0
	stm		#(ILEC_FR_SZ-1), brc
	stm		#0, bk
	mvdk	*sp(2), ar5
	rptb	L51-1
		mpy		*ar2-, *ar5+, a
		rpt		#(6-3)
			mac		*ar2-, *ar5+, a
		macr	*ar2+, *ar5+0%, a
		mar		*ar2-0					; ar2->next: lpTmp[5+k] 
		sth		a, *ar3+
L51
	rsbx	frct
	ret