lecaa.s54

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

	.mmregs
	.include "leci.h54"
	.text

;-------------------------------------------------------------------------
	.global				_lec_dummy2
	.sym _lec_dummy2, _lec_dummy2, 32, 2, 0
;-------------------------------------------------------------------------
_lec_dummy2
	ret
	.sect 	".lectxt1"
;-------------------------------------------------------------------------
	.global               		_lec_cancel_pkt
	.sym _lec_cancel_pkt, _lec_cancel_pkt, 32, 2, 0
_lec_cancel_pkt
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= sp(1)
;
	stlm	a, ar2						; pDb
	ssbx	sxm
	ssbx	frct
	mvdk	*ar2(#(LEC_tDb.psHst)), ar3
	mar		*+ar3(#LEC_FLT_SZ)
	mvmm	ar2, ar4
	mar		*+ar4(#(LEC_tDb.asAdf1+ILEC_FR_SZ))
	ld		*ar2(#(LEC_tDb.Adf1.sNegShift)), asm	; shall be negative ...
	nop
	nop
	mvdk	*sp(1), ar2					; pSc
	mvdk	*ar2(#(LEC_tSc.psRx)), ar5
	mar		*+ar2(#(LEC_tSc.asErr1))
	stm		#(1-LEC_FLT_SZ), ar0
	stm		#0, bk
;	
; ar0 = 1-FLT_SZ
; ar2 = psErr
; ar3 = psHst	
; ar4 = psFlt
; ar5 = psRx
;	
	stm		#ILEC_FR_SZ-1, brc
	rptb	L11-1
		rptz	a, #LEC_FLT_SZ-2
			mas		*ar3-, *ar4+, a
		mas		*ar3+, *ar4+0%, a
		mar		*ar3-0
		ldr		*ar5+, b
		add		a, asm, b
		sth		b, *ar2+
L11									; [sz+6]
	rsbx	frct
	ret	
;-------------------------------------------------------------------------
	.global               		_lec_cancel
	.sym _lec_cancel, _lec_cancel, 32, 2, 0
_lec_cancel
;-------------------------------------------------------------------------
;	S16 *psHst	= a
; 	S16 *psFlt	= sp(1)
;	S16  sRxIn	= sp(2)
;
	stlm	a, ar2
	ssbx	sxm
	ssbx	frct
	mvdk	*sp(1), ar3
	ld		*sp(2), 16, a
	rpt		#LEC_FLT2_SZ-2
		mas		*ar2-, *ar3+, a
	masr	*ar2-, *ar3+, a
	ld   	a, -16, a
	rsbx	frct
	ret
	
;-------------------------------------------------------------------------
	.global               		_lec_cancel_shifted
	.sym _lec_cancel_shifted, _lec_cancel_shifted, 32, 2, 0
_lec_cancel_shifted
;-------------------------------------------------------------------------
;	S16 *psHst	= a
; 	S16 *psFlt	= sp(1)
;	S16  sRxIn	= sp(2)
;	S16  sShift = sp(3)
;
	stlm	a, ar2
	ld 		*sp(3), asm
	ssbx	sxm
	ssbx	frct
	mvdk	*sp(1), ar3
	ld		*sp(2), 16, a
	ld 		a, asm, a
	rpt		#LEC_FLT2_SZ-2
		mas		*ar2-, *ar3+, a
	masr	*ar2-, *ar3+, a
	ld   	a, -16, a
	rsbx	frct
	ret
;-------------------------------------------------------------------------
	.global               		_lec_get_xz
	.sym _lec_get_xz, _lec_get_xz, 32, 2, 0
_lec_get_xz
;-------------------------------------------------------------------------
;	S16 *psX	= a
; 	S16 *psZ	= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	ld 		#0, a
	mvdk	*sp(1), ar3
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -4, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -4, a
;	
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -2, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -3, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -4, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -5, a
;	
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -6, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -6, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -6, a
	rptz	b, #ILEC_FR_SZ/2-1
		mac		*ar2-, *ar3-, b
	add 	b, -6, a
	
	ret
;-------------------------------------------------------------------------
	.global               		_lec_get_xz_slow
	.sym _lec_get_xz_slow, _lec_get_xz_slow, 32, 2, 0
_lec_get_xz_slow
;-------------------------------------------------------------------------
;	S16 *psX	= a
; 	S16 *psZ	= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	ld 		#0, a
	mvdk	*sp(1), ar3
	stm		#(LEC_HST_SZ/(ILEC_FR_SZ/2))-1, brc
	rptb	L15-1
		rptz	b, #ILEC_FR_SZ/2-1
			mac		*ar2+, *ar3+, b
		add 	b, -2, a
L15	
	ret
	.sect	".lectxt0"
;-------------------------------------------------------------------------
	.global               		_lec_update_xz
	.sym _lec_update_xz, _lec_update_xz, 32, 2, 0
_lec_update_xz
;-------------------------------------------------------------------------
;	S32 lsEn	= a
;	S16 *psX	= sp(1) = ar2
; 	S16 *psZ	= sp(2)	= ar4
;
	ssbx	sxm
	mvdk	*sp(1), ar2
	mvdk	*sp(2), ar4
	stm		#-ILEC_FR_SZ/2, ar0
	stm		#0, bk
	nop
	mpy		*ar2+0%, *ar4+0%, b
	add 	b, -4, a
	mpy		*ar2+0%, *ar4+0%, b
	mpy		*ar2+0%, *ar4+0%, b
	sub		b, -4, a
	add     b, -2, a
	mpy		*ar2+0%, *ar4+0%, b
	sub		b, -2, a
	add     b, -3, a
	mpy		*ar2+0%, *ar4+0%, b
	sub		b, -3, a
	add     b, -4, a
	mpy		*ar2+0%, *ar4+0%, b
	sub		b, -4, a
	add     b, -5, a
	mpy		*ar2+0%, *ar4+0%, b
	sub		b, -5, a
	add     b, -6, a
	mpy		*ar2+0%, *ar4+0%, b
	mpy		*ar2+0%, *ar4+0%, b
	mpy		*ar2+0%, *ar4+0%, b
	mpy		*ar2+0%, *ar4+0%, b
	sub		b, -6, a
	ret
	.sect	".lectxt1"
;-------------------------------------------------------------------------
	.global               		_lec_shift_adf2
	.sym _lec_shift_adf2, _lec_shift_adf2, 32, 2, 0
_lec_shift_adf2
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	ld 		#0, b
	mvmm	ar2, ar4
	mar 	*+ar4(#(LEC_tDb.asAdf2))
	stm		#LEC_FLT2_SZ-1, brc
	rptb	L21-1
		ld 		*ar4+, a
		abs		a
		max		b
L21	
	add 	b, -1, b

	mvmm	ar2, ar3
	mar		*+ar3(#(LEC_tDb.Adf2.sShift))
	st 		#4, *ar3
	
	sub		#1, 11, b, a
	nop
	nop
	xc		2, agt
		st		#3, *ar3
	sub		#1, 12, b, a
	nop
	nop
	xc		2, agt
		st		#2, *ar3
	sub		#1, 13, b, a
	nop
	nop
	xc		2, agt
		st		#1, *ar3
	sub		#1, 14, b, a
	nop
	nop
	xc		2, agt
		st		#0, *ar3
	ld 		*ar3, asm
	
	ld 		*ar3, a
	neg		a
	stl		a, *ar2(#(LEC_tDb.Adf2.sNegShift))
	
	mar		*ar4-
	stm		#LEC_FLT2_SZ-1, brc
	rptb	L22-1
		ld 		*ar4, a
		stl		a, asm, *ar4-
L22		
	ret
;-------------------------------------------------------------------------
	.global               		_lec_rshift_adf
	.sym _lec_rshift_adf, _lec_rshift_adf, 32, 2, 0
_lec_rshift_adf
;-------------------------------------------------------------------------
;	S16 *psAdf 		= a
;	S16 sNegShift	= sp(1)
;
	stlm	a, ar2
	ssbx	sxm
	ssbx	frct
; t = (0x8000L << sNegShift)
	ld 		*sp(1), t
	ld 		#1, 15, b
	norm 	b
	stlm 	b, t
; loop for all coeffs	
	stm 	#(LEC_FLT2_SZ-1), brc
	rptb	L23-1
		mpyr	*ar2, b
		sth 	b, *ar2+
L23	
	rsbx	frct
	ret
;-------------------------------------------------------------------------
	.global               		_lec_step_size
	.sym _lec_step_size, _lec_step_size, 32, 2, 0
_lec_step_size
;-------------------------------------------------------------------------
;	S32	slEn		= a
;	S16 *psStepSize	= sp(1)
;
	ssbx	sxm
	ld 		a, b
	mvdk	*sp(1), ar2
	sub		#0x10, 15, b, a
	bc		L31, agt
	sub		#1, 9, b, a
	bcd		L32, agt
	ld 		#2, a
	nop
; energy is too low	---------- 
	st 		#0, *ar2
	ld 		#0, a
	ret
L31	
	ld 		b, -10, b
	ld 		#1, a
L32
	ld 		b, -7, b
	add 	#1, b
	stl		b, *sp(1)
	ld 		#1, 15, b
	rpt		#16-1
		subc	*sp(1), b
	stl		b, *ar2
	addm	#-1, *ar2
	ret
	.sect	".lectxt0"
;-------------------------------------------------------------------------
	.global               		_lec_adapt_fast_high
	.sym _lec_adapt_fast_high, _lec_adapt_fast_high, 32, 2, 0
_lec_adapt_fast_high
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= sp(1)
;	S16 sBase		= sp(2) = ar0
;	S16 Step		= sp(3)
;
; ar3 -> asNormErr[base]
; ar4 = pData
; ar5, ar2 = pFlt
;
	stlm	a, ar2
	ssbx	sxm
	nop
	mvdk	*sp(1), ar3
	mvdk	*sp(2), ar0
; normalize & save error	
	mar		*+ar3(#(LEC_tSc.asErr2))
	mar		*ar3+0
	ld		*sp(3), t
	mpy 	*ar3, a
	add 	#8, a
	ld 		a, 12, a
	sat 	a
	mvdk	*sp(1), ar3
	mar 	*+ar3(#(LEC_tSc.asNormErr))
	mar 	*ar3+0
	sth 	a, *ar3
; set-up pData & pFlt
	mvmm	ar2, ar5
	mar 	*+ar5(#(LEC_tDb.asAdf2));
	mvdk	*ar2(#(LEC_tDb.psTxF)), ar4
	mar 	*+ar4(#LEC_FLT2_SZ)
	mar 	*ar4+0
	mvmm	ar5, ar2
	ld 		#0, asm
	ssbx	frct
	ld 		*ar2+, 16, b
; blocks 0, 1	
	ld  	*ar3, 14, a
	add 	#1, 15, a
	stm		#ILEC_FR_SZ-1, brc
	rptb	L41-1
		macar 	*ar4-
		st		b, *ar5+ || ld *ar2+, b
L41		
; block	2
	ld  	*ar3, 16, a
	add 	#1, 15, a
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L42-1
		macar 	*ar4-
		st		b, *ar5+ || ld *ar2+, b
L42		
; block	3
	ld  	*ar3, 15, a
	add 	#1, 15, a
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L43-1
		macar 	*ar4-
		st		b, *ar5+ || ld *ar2+, b
L43