lecaa.s54

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

; block	4
	ld  	*ar3, 14, a
	add 	#1, 15, a
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L44-1
		macar 	*ar4-
		st		b, *ar5+ || ld *ar2+, b
L44		
; block	5
	ld  	*ar3, 13, a
	add 	#1, 15, a
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L45-1
		macar 	*ar4-
		st		b, *ar5+ || ld *ar2+, b
L45
; block	6,7,8,9
	ld  	*ar3, 12, a
	add 	#1, 15, a
	stm		#ILEC_FR_SZ*2-1, brc
	rptb	L46-1
		macar 	*ar4-
		st		b, *ar5+ || ld *ar2+, b
L46
; last cycle includes extra load... Flt2 shall not be last in data mem.
	rsbx	frct
	ret
;-------------------------------------------------------------------------
	.global               		_lec_adapt_fast_low
	.sym _lec_adapt_fast_low, _lec_adapt_fast_low, 32, 2, 0
_lec_adapt_fast_low
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= sp(1)
;	S16 sBase		= sp(2) = ar0
;	S16 Step		= sp(3)
;
; ar3 -> asNormErr[base]
; ar4 = pData
; ar5 = 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
	ld 		a, 8, a
	ld 		a, 8, 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
	ssbx	frct
	ld  	*ar3, t
; blocks 0, 1	
	stm		#ILEC_FR_SZ-1, brc
	rptb	L51-1
		ldr		*ar5, a
		mpy 	*ar4-, b
		add 	b, 4, a
		sth		a, *ar5+
L51		
; block	2
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L52-1
		ldr		*ar5, a
		mpy 	*ar4-, b
		add 	b, 6, a
		sth		a, *ar5+
L52		
; block	3
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L53-1
		ldr		*ar5, a
		mpy 	*ar4-, b
		add 	b, 5, a
		sth		a, *ar5+
L53		
; block	4
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L54-1
		ldr		*ar5, a
		mpy 	*ar4-, b
		add 	b, 4, a
		sth		a, *ar5+
L54		
; block	5
	stm		#ILEC_FR_SZ/2-1, brc
	rptb	L55-1
		ldr		*ar5, a
		mpy 	*ar4-, b
		add 	b, 3, a
		sth		a, *ar5+
L55
	rsbx	frct
	ret

;-------------------------------------------------------------------------
	.global               		_lec_adapt_slow_high
	.sym _lec_adapt_slow_high, _lec_adapt_slow_high, 32, 2, 0
_lec_adapt_slow_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 = pFlt
;
	stlm	a, ar2
	ssbx 	sxm
	nop
	mvdk 	*sp(2), ar0
	mvdk	*sp(1), ar4
	mar		*+ar4(#(LEC_tSc.asErr2))
	mar 	*ar4+0
	mvdk	*sp(1), ar3
	mar		*+ar3(#(LEC_tSc.asNormErr))
	mar 	*ar3+0
	ld 		*sp(3), t
	stm		#LEC_SECTION_SZ-1, brc
	rptb	L61-1
		mpy		*ar4+, a
		add 	#8, a
		ld 		a, 12, a
		sat 	a
		sth		a, *ar3+
L61	
	ssbx	frct
	mar 	*+ar3(#(-LEC_SECTION_SZ))
	mvdk	*ar2(#(LEC_tDb.psTxF)), ar4
	mar		*+ar4(#LEC_FLT2_SZ)
	mar 	*ar4+0
	mvmm	ar2, ar5
	mar		*+ar5(#(LEC_tDb.asAdf2))
	stm		#(-LEC_SECTION_SZ), ar0
	stm		#LEC_FLT2_SZ-1, brc
	ld 		#0, asm
	rptb	L62-1
		ldr 	*ar5, a
		mpy 	*ar3+, *ar4+, b
		mac 	*ar3+, *ar4+, b
		mac 	*ar3+, *ar4+, b
		mac 	*ar3+0%, *ar4+0%, b
		add 	b, -2, a
		st  	a, *ar5+ || ld *ar3+, b
			; this || ld only to move ar3 to the proper position
L62
	rsbx	frct
	ret
	
;-------------------------------------------------------------------------
	.global               		_lec_adapt_slow_low
	.sym _lec_adapt_slow_low, _lec_adapt_slow_low, 32, 2, 0
_lec_adapt_slow_low
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= sp(1)
;	S16 sBase		= sp(2) = ar0
;	S16 Step		= sp(3)
;
; ar3 -> asNormErr[base]
; ar4 = pData
; ar5 = pFlt
;
	stlm	a, ar2
	ssbx 	sxm
	nop
	mvdk 	*sp(2), ar0
	mvdk	*sp(1), ar4
	mar		*+ar4(#(LEC_tSc.asErr2))
	mar 	*ar4+0
	mvdk	*sp(1), ar3
	mar		*+ar3(#(LEC_tSc.asNormErr))
	mar 	*ar3+0
	ld 		*sp(3), t
	stm		#LEC_SECTION_SZ-1, brc
	rptb	L71-1
		mpy		*ar4+, a
		ld 		a, 8, a
		ld 		a, 8, a
		sat 	a
		sth		a, *ar3+
L71	
	ssbx	frct
	mar 	*+ar3(#(-LEC_SECTION_SZ))
	mvdk	*ar2(#(LEC_tDb.psTxF)), ar4
	mar		*+ar4(#(LEC_FLT2_SZ))
	mar 	*ar4+0
	mvmm	ar2, ar5
	mar		*+ar5(#(LEC_tDb.asAdf2))
	stm		#(-LEC_SECTION_SZ), ar0
	stm		#LEC_FLT2_SZ-1, brc
	ld 		#0, asm
	rptb	L72-1
		ldr 	*ar5, a
		mpy 	*ar3+, *ar4+, b
		mac 	*ar3+, *ar4+, b
		mac 	*ar3+, *ar4+, b
		mac 	*ar3+0%, *ar4+0%, b
		add 	b, 4, a
		st  	a, *ar5+ || ld *ar3+, b
			; this || ld only to move ar3 to the proper position
L72
	rsbx	frct
	ret
	.sect	".lectxt1"
;-------------------------------------------------------------------------
	.global               		_lec_shift_err2
	.sym _lec_shift_err2, _lec_shift_err2, 32, 2, 0
_lec_shift_err2
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= a
;	LEC_tSc *pSc	= sp(1)
	stlm	a, ar2
	ssbx	sxm
	ld  	#1, 15, b
	ld  	*ar2(#(LEC_tDb.Adf2.sShift)), a
	bc 		L81, aeq
; if shift != 0	
	mvdk	*sp(1), ar3
	mar		*+ar3(#(LEC_tSc.asErr2))
	ld  	*ar2(#(LEC_tDb.Adf2.sShift)), asm
	ld 		b, asm, b
	mvmm	ar3, ar4
	stm		#ILEC_FR_SZ-1, brc
	ld 		*ar3+, 16, a
	ld  	*ar2(#(LEC_tDb.Adf2.sNegShift)), asm
	rptb	L81-1
		add 	b, a
		st  	a, *ar4+ || ld 	*ar3+, a
L81	
	ret
	
;-------------------------------------------------------------------------
	.global               		_lec_step_size_corr_slow
	.sym _lec_step_size_corr_slow, _lec_step_size_corr_slow, 32, 2, 0
_lec_step_size_corr_slow
;-------------------------------------------------------------------------
;	LEC_tDb *pDb 	= a
;	LEC_tSc *pSc	= sp(1)
; 	S16 sBase		= sp(2)
	stlm	a, ar2
	ssbx	sxm
	nop
; setting pointers	
	mvdk	*sp(1), ar3
	mvmm 	ar3, ar4
	mar		*+ar3(#(LEC_tSc.asErr2))
	mvdk 	*sp(2), ar0
	mar 	*ar3+0
; calculating and shifting energy	
	squr	*ar3+, b
	squra	*ar3+, b
	squra	*ar3+, b
	squra	*ar3+, b
	ld 		*ar2(#(LEC_tDb.Adf2.sNegShift)), 1, a
	stlm 	a, t
	ld 		b, -2, b
	norm 	b, a
; taking log2(energy)	
	add		#1, a
	exp		a
	ld		#0x780+512*(5-2)+125-1024, b
	norm	a
	ld		a, -16, a
	add		a, -5, b
	mac		#-512, b, a				; a = sEn
	
	sub 	*ar2(#(LEC_tDb.sErrAvrEn)), a, b
	bc		L91, bgt
		add 	#4, b
		ld 		b, -3, b
		b 		L92
L91	
		add 	#1, b
		ld 		b, -1, b
L92
	add 	*ar2(#(LEC_tDb.sErrAvrEn)), b
	stl 	b, *ar2(#(LEC_tDb.sErrAvrEn))
; a = sErleLoss = pDb->sErrAvrEn - pSc->sExpectedErrorEn;
	sub		*ar4(#(LEC_tSc.sExpectedErrorEn)), b, a
	
	ld 		*ar4(#(LEC_tSc.sExpectedErrorEn)), b
	sub 	*ar2(#(LEC_tDb.VadErr.sEnNoise)), b
; sp(2) = pSc->sExpectedErrorEn < pDb->VadErr.sEnNoise;
	stl  	b, *sp(2)
	bc		L93, bgeq
		add 	#LEC_1DB*6, b
		nop
		nop
		xc 		1, bgt
			add 	*sp(2), a
		xc 		2, bleq
			sub 	#LEC_1DB*6, a
L93
;    if (sErleLoss < 0) sErleLoss = 0;
	ld 		#0, b
	max 	a
	neg 	a
	.global	_lec_exp
	b 		_lec_exp
	
;-------------------------------------------------------------------------
	.global               		_lec_step_size_corr_fast2
	.sym _lec_step_size_corr_fast2, _lec_step_size_corr_fast2, 32, 2, 0
_lec_step_size_corr_fast2
;-------------------------------------------------------------------------
;	LEC_tDb *pDb 	= a
;	LEC_tSc *pSc	= sp(1)
; 	S16 sBase		= sp(2)
	stlm	a, ar2
	ssbx	sxm
	nop
; setting pointers	
	mvdk	*sp(1), ar3
	mvmm 	ar3, ar4
	mar		*+ar3(#(LEC_tSc.asErr2))
	mvdk 	*sp(2), ar0
	mar 	*ar3+0
; calculating and shifting energy	
	squr	*ar3, a
; taking log2(energy)	
	add		#1, a
	exp		a
	ld		#0x780+512*(5-2)+125-1024, b
	norm	a
	ld		a, -16, a
	add		a, -5, b
	mac		#-512, b, a				; a = sEn
	
	sub 	*ar2(#(LEC_tDb.sErrAvrEn)), a, b
	bc		L101, bgt
		add 	#16, b
		ld 		b, -5, b
		b 		L102
L101	
		add 	#4, b
		ld 		b, -3, b
L102
	add 	*ar2(#(LEC_tDb.sErrAvrEn)), b
	stl 	b, *ar2(#(LEC_tDb.sErrAvrEn))
; a = sErleLoss = pDb->sErrAvrEn - pSc->sExpectedErrorEn;
	sub		*ar4(#(LEC_tSc.sExpectedErrorEn)), b, a
;    if (sErleLoss < 0) sErleLoss = 0;
	ld 		#0, b
	max 	a
	neg 	a
	ld 		a, 2, a
	ld 		a, -12, b
	stlm 	b, t
	and 	#4095, a
	add 	#4096, a
	norm	a
	ret
	
;-------------------------------------------------------------------------
	.global _dummy2
	.sym _dummy2, _dummy2, 32, 2, 0
;-------------------------------------------------------------------------
_dummy2
	ret
;-------------------------------------------------------------------------
	.global _dummy
	.sym _dummy, _dummy, 32, 2, 0
;-------------------------------------------------------------------------
_dummy
	call _dummy2
	ret