lecaa.s55

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

*-------------------------------------------------------------------------*
*                                                                         *
*   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
*   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
*      "COPYRIGHT 2001 MICHAEL TSIROULNIKOV, ALL RIGHTS RESERVED"         *
*                                                                         *
*-------------------------------------------------------------------------*/
	.mmregs
    .cpl_on
    .arms_on
	.include "leci.h55"
	.text

;-------------------------------------------------------------------------
macro_en2log   .macro acc, tx, sCorr
;-------------------------------------------------------------------------

    acc = acc + #1;
    acc = mant (acc), tx = -exp(acc);
    acc = acc << #-21;
    acc = acc + #(0x780 + sCorr);
    acc = acc + (tx*#512);
    .endm
;-------------------------------------------------------------------------
	.global				_lec_dummya1
	.sym _lec_dummya1, _lec_dummya1, 32, 2, 0
;-------------------------------------------------------------------------
_lec_dummya1
;	sp = sp + #-1;
;	ac0 = #0x123;
;	*sp = HI(saturate(ac0 << #8));
;	sp = sp + #1;
	return
	.if 0
;-------------------------------------------------------------------------
	.global               		_lec_cancel
	.sym _lec_cancel, _lec_cancel, 32, 2, 0
_lec_cancel
;-------------------------------------------------------------------------
;	S16 *psHst	= xar0
; 	S16 *psFlt	= xar1
;	S16  sRxIn	= t0
;
	ac0 = t0;
	ac0 = ac0<<#15;
	|| repeat (#(LEC_FLT2_SZ-1))
		ac0 = ac0 - (*ar0- * *ar1+);
	ac0 = ac0 + #0x4000;
	ac0 = ac0 << #(-15);
	t0 = ac0
	return;	(t0)
	
	.endif
;-------------------------------------------------------------------------
	.global               		_lec_cancel_shifted
	.sym _lec_cancel_shifted, _lec_cancel_shifted, 32, 2, 0
_lec_cancel_shifted
;-------------------------------------------------------------------------
;	S16 *psHst	= xar0
; 	S16 *psFlt	= xar1
;	S16  sRxIn	= t0
;	S16  sShift = t1
;
	HI(ac0) = t0;
	|| bit(ST1, #ST1_FRCT) = #1;
	ac0 = ac0 << t1;
	|| repeat (#(LEC_FLT2_SZ-2))
		ac0 = ac0 - (*ar0- * *ar1+);
	ac0 = rnd(ac0 - (*ar0- * *ar1+));
	t0 = HI(ac0);
	bit(ST1, #ST1_FRCT) = #0;
	|| return;	(t0)

;-------------------------------------------------------------------------
	.global               		_lec_cancel_pkt
	.sym _lec_cancel_pkt, _lec_cancel_pkt, 32, 2, 0
_lec_cancel_pkt
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	|| pshboth(xar5)
	bit(ST1, #ST1_FRCT) = #1;
	|| push(t2)
	xar2 = dbl(*ar0(#(LEC_tDb.psHst)));
	xar3 = dbl(*ar0(#(LEC_tDb.psHst)));
	xar4 = dbl(*ar1(#(LEC_tSc.psRx)));
	xar5 = mar(*ar1(#(LEC_tSc.asErr1)));
	xcdp = dbl(*ar0(#(LEC_tDb.psFlt1)));
	t2 = *ar0(#(LEC_tDb.Adf1.sNegShift))
	mar	(ar2 + #LEC_FLT_SZ);
	mar	(ar3 + #(LEC_FLT_SZ+1));
	mar (*+cdp(#(ILEC_FR_SZ)));
	brc0 = #(ILEC_FR_SZ/2-1);
	t0 = #(1-LEC_FLT_SZ);
	t1 = #(1+LEC_FLT_SZ);
	|| localrepeat {
      	ac0 = *ar2- *coef(*cdp+), 
       	ac1 = *ar3- *coef(*cdp+);
       	|| repeat (#(LEC_FLT_SZ-3))
           	ac0 = ac0 + (*ar2- * coef(*cdp+)), 
			ac1 = ac1 + (*ar3- * coef(*cdp+));
       	ac0 = ac0 + (*(ar2+t1) * coef(*(cdp+t0))), 
		ac1 = ac1 + (*(ar3+t1) * coef(*(cdp+t0)));
		pair(HI(ac2)) = *ar4+;
		ac2 = ac2 - (ac0 <<	t2);
		ac3 = ac3 - (ac1 <<	t2);
		*ar5+ = HI(rnd(ac2));
		*ar5+ = HI(rnd(ac3));
	}
	bit(ST1, #ST1_FRCT) = #0;
	|| t2 = pop()
	.arms_on
	bit(ST2, #ST2_ARMS) = #1;
	|| xar5=popboth()
	return;	(none)
	
	.if 0
;-------------------------------------------------------------------------
get_xz	.macro sShift
	ac1 = #0
	|| repeat	(#(ILEC_FR_SZ/2-1))
		ac1 = ac1 + (*ar0- * *ar1-);
	ac0 = ac0 + (ac1 << sShift);
	.endm
;-------------------------------------------------------------------------
	.global               		_lec_get_xz
	.sym _lec_get_xz, _lec_get_xz, 32, 2, 0
_lec_get_xz
;-------------------------------------------------------------------------
;	S16 *psX	= xar0
; 	S16 *psZ	= xar1
;
	ac0 = #0
	get_xz	-4
	get_xz	-4
	get_xz	-2
	get_xz	-3
	get_xz	-4
	get_xz	-5
	get_xz	-6
	get_xz	-6
	get_xz	-6
	get_xz	-6
	
	return; 	(ac0)
	.endif
;-------------------------------------------------------------------------
	.global               		_lec_get_xz_slow
	.sym _lec_get_xz_slow, _lec_get_xz_slow, 32, 2, 0
_lec_get_xz_slow
;-------------------------------------------------------------------------
;	S16 *psX	= xar0
; 	S16 *psZ	= xar1
;
	brc0 = #((LEC_HST_SZ/(ILEC_FR_SZ/2))-1)
	ac0 = #0;
	|| localrepeat	{
		ac1 = #0;
		|| repeat	(#(ILEC_FR_SZ/2-1));
			ac1 = ac1 + (*ar0+ * *ar1+);
		ac0 = ac0 + (ac1 << -2);
	}
	return ; (ac0)
	.if 0
;-------------------------------------------------------------------------
	.global               		_lec_update_xz
	.sym _lec_update_xz, _lec_update_xz, 32, 2, 0
_lec_update_xz
;-------------------------------------------------------------------------
;	S32 lsEn	= ac0
;	S16 *psX	= xar0
; 	S16 *psZ	= xar1
;
	t0 = #(-ILEC_FR_SZ/2)
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 + (ac1 << -4);
	
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 - (ac1 << -4);
	ac0 = ac0 + (ac1 << -2);
	
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 - (ac1 << -2);
	ac0 = ac0 + (ac1 << -3);
	
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 - (ac1 << -3);
	ac0 = ac0 + (ac1 << -4);
	
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 - (ac1 << -4);
	ac0 = ac0 + (ac1 << -5);
	
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 - (ac1 << -5);
	ac0 = ac0 + (ac1 << -6);
	
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac1 = *(ar0+t0) * *(ar1+t0)
	ac0 = ac0 - (ac1 << -6);
	
	return; 	(ac0)
	.endif
;-------------------------------------------------------------------------
	.global               		_lec_shift_adf2
	.sym _lec_shift_adf2, _lec_shift_adf2, 32, 2, 0
_lec_shift_adf2
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1 (not used)
;
	xar4 = dbl(*ar0(#(LEC_tDb.psFlt2)))
	brc0 = #(LEC_FLT2_SZ-1)
	ac1 = #0
	|| localrepeat {
		ac0 = *ar4+;
		ac0 = |ac0|;
		ac1 = max(ac0, ac1);
	}
	ac1 = ac1 + (ac1 << -1);
	|| t0 = #4;
	ac0 = ac1 - #(1<<11);
	if (ac0 > #0) execute (D_unit) 
		|| t0 = #3;
	ac0 = ac1 - #(1<<12);
	if (ac0 > #0) execute (D_unit) 
		|| t0 = #2;
	ac0 = ac1 - #(1<<13);
	if (ac0 > #0) execute (D_unit) 
		|| t0 = #1;
	ac0 = ac1 - #(1<<14);
	if (ac0 > #0) execute (D_unit) 
		|| t0 = #0;
	*ar0(#(LEC_tDb.Adf2.sShift)) = t0
	ac0 = -t0;
	*ar0(#(LEC_tDb.Adf2.sNegShift)) = ac0
	mar		(*ar4-)
	brc0 = #(LEC_FLT2_SZ-1);
	localrepeat {
		ac0 = *ar4 << t0;
		*ar4- = ac0;
	}
	return;	(none)
;-------------------------------------------------------------------------
	.global               		_lec_step_size
	.sym _lec_step_size, _lec_step_size, 32, 2, 0
_lec_step_size
;-------------------------------------------------------------------------
;	S32	slEn		= ac0
;	S16 *psStepSize	= xar0
;
	ac1 = ac0 - (#0x10 << #15)
	if (ac1 > #0) goto L31
	ac1 = ac0 - #(1<<9)
	if (ac1 > #0) goto L32
; energy is too low	---------- 
	*ar0 = #0
	t0 = #4;
	|| return
; energy is high ---------- 
L31	
	ac1 = ac0 << #-17
	ac1 = ac1 + #1
	*ar0 = ac1;
	ac1 = #1 << #15;
	|| repeat	(#(16-1))
		subc	(*ar0, ac1, ac1);
	*ar0 = ac1;
	|| t0 = #1
	*ar0 = *ar0 + #-1;
	return;	(t0)
; energy is low	---------- 
L32
	ac1 = ac0 << #-7;
	ac1 = ac1 + #1
	*ar0 = ac1;
	ac1 = #1 << #15;
	|| repeat	(#(16-1))
		subc	(*ar0, ac1, ac1);
	*ar0 = ac1;
	|| t0 = #2
	*ar0 = *ar0 + #-1;
	return;	(t0)
	.if 1
;-------------------------------------------------------------------------
blk_adapt_fh	.macro sSz, sShift
	.if (sShift != 0)
	ac3 = ac2 << #sShift;
	*sp = HI(rnd(ac3));
	.else
	*sp = HI(ac2);
	.endif
	brc0 = #(sSz/2-1);
	localrepeat {
		pair(HI(ac0)) = *ar4;
		ac0 = rnd(ac0 + (*(ar2-t0) * coef(*cdp))),
		ac1 = rnd(ac1 + (*(ar3-t0) * coef(*cdp)));
		*ar4+ = pair(HI(ac0));
	}
	.endm
;-------------------------------------------------------------------------
	.global               		_lec_adapt_fast_high
	.sym _lec_adapt_fast_high, _lec_adapt_fast_high, 32, 2, 0
_lec_adapt_fast_high
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1
;	S16 sBase		= t0
;	S16 Step		= t1
;
; normalize & save error
	sp = sp + #-1	
	xar4 = mar(*ar1(#(LEC_tSc.asErr2)))
	mar (ar4+t0)
	
	ac2 = t1 * *ar4;
	|| mar (ar4 + #(LEC_tSc.asNormErr-LEC_tSc.asErr2))
	ac2 = ac2 << #12;
	ac2 = saturate(rnd(ac2));
	|| xcdp = xsp;
	*ar4 = HI(ac2)

	
; set-up pData & pFlt
	xar4 = dbl(*ar0(#(LEC_tDb.psFlt2)))
	xar2 = dbl(*ar0(#(LEC_tDb.psTxF)))
	mar (ar2 + #LEC_FLT2_SZ)
	mar (ar2+t0)
	xar3 = mar(*ar2(#-1));
	mar (t0 = #2)
	|| bit(ST1, #ST1_FRCT) = #1;
; blocks 0, 1
	blk_adapt_fh	(ILEC_FR_SZ), -2
; blocks 2
	blk_adapt_fh	(ILEC_FR_SZ/2), 0
; blocks 3
	blk_adapt_fh	(ILEC_FR_SZ/2), -1
; blocks 4
	blk_adapt_fh	(ILEC_FR_SZ/2), -2
; blocks 5
	blk_adapt_fh	(ILEC_FR_SZ/2), -3
; blocks 6+
	blk_adapt_fh	(ILEC_FR_SZ*2), -4
	
	bit(ST1, #ST1_FRCT) = #0;
	sp = sp + #1;
	return; (none)
	.if 0
;-------------------------------------------------------------------------
blk_adapt_fl	.macro sSz, sShift
	brc0 = #((sSz)/2-1);
	localrepeat {
	.if 0
		; 3.0 clock per tap
		pair(HI(ac0)) = *ar4;			; 0.5
		ac2 = *(ar2+t0) * coef(*cdp),	; 0.5
		ac3 = *(ar3+t0) * coef(*cdp);
		ac0 = ac0 + (ac2 << #sShift);	; 1.0
		ac1 = ac1 + (ac3 << #sShift);
		*ar4+ = HI(rnd(ac0));			; 1.0
		*ar4+ = HI(rnd(ac1));
	.else
		; 2.5 clock per tap
		ac0 = *ar4+ << #(16 - sShift);			1.0
		ac1 = *ar4- << #(16 - sShift);
		ac0 = ac0 + (*(ar2+t0) * coef(*cdp)),
		ac1 = ac1 + (*(ar3+t0) * coef(*cdp));	0.5
		*ar4+ = HI(rnd(ac0 << #sShift));
		*ar4+ = HI(rnd(ac1 << #sShift));		1.0
	.endif
	}
	.endm
;-------------------------------------------------------------------------
	.global               		_lec_adapt_fast_low
	.sym _lec_adapt_fast_low, _lec_adapt_fast_low, 32, 2, 0
_lec_adapt_fast_low
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1
;	S16 sBase		= t0
;	S16 Step		= t1
;
; normalize & save error	
	xar4 = mar(*ar1(#(LEC_tSc.asErr2)))
	mar	(ar4+t0)
	ac2 = t1 * *ar4;
	|| sp = sp + #-1;
	ac2 = ac2 << #16;
	ac2 = saturate(ac2)
	|| xcdp = xsp;
	mar (ar4 + #(LEC_tSc.asNormErr-LEC_tSc.asErr2))
	
; set-up pData & pFlt
	xar2 = dbl(*ar0(#(LEC_tDb.psTxF)));
	mar (ar2+#LEC_FLT2_SZ);
	mar	(ar2+t0)
	|| *sp = HI(ac2);
	xar3 = xar2;
	mar	(ar3-#1)
	|| *ar4 = HI(ac2);
	
	bit(ST1, #ST1_FRCT) = #1;
	|| mar(t0 = #(-2));
	xar4 = dbl(*ar0(#(LEC_tDb.psFlt2)))
; blocks 0, 1	
	blk_adapt_fl	(ILEC_FR_SZ), 4
; block	2
	blk_adapt_fl	(ILEC_FR_SZ/2), 6
; block	3
	blk_adapt_fl	(ILEC_FR_SZ/2), 5
; block	4
	blk_adapt_fl	(ILEC_FR_SZ/2), 4
; block	5
	blk_adapt_fl	(ILEC_FR_SZ/2), 3
;	
	bit(ST1, #ST1_FRCT) = #0;
	sp = sp + #1;
	return
	.else
;-------------------------------------------------------------------------
blk_adapt_fl	.macro sSz, sShift
	brc0 = #((sSz)/2-1);
	*sp = #(1<<(15-sShift));
	|| localrepeat {
		ac0 = *ar4 * coef (*cdp+),
		ac1 = *ar5 * coef (*cdp+);				0.5
		ac0 = ac0 + (*(ar2-t0) * coef(*cdp-)),
		ac1 = ac1 + (*(ar3-t0) * coef(*cdp-));	0.5
		*(ar4+t0) = HI(rnd(ac0 << #sShift));
		*(ar5+t0) = HI(rnd(ac1 << #sShift));		1.0
	}
	.endm
;-------------------------------------------------------------------------
	.global               		_lec_adapt_fast_low
	.sym _lec_adapt_fast_low, _lec_adapt_fast_low, 32, 2, 0
_lec_adapt_fast_low
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1
;	S16 sBase		= t0
;	S16 Step		= t1
;
; normalize & save error	
	pshboth(xar5)
	sp = sp + #-2;

	xar4 = mar(*ar1(#(LEC_tSc.asErr2)))
	mar	(ar4+t0)
	ac2 = t1 * *ar4;
	mar (ar4 + #(LEC_tSc.asNormErr-LEC_tSc.asErr2))
	*ar4 = HI(saturate(ac2 << 16));
	*sp(#1) = HI(saturate(ac2 << 16));
	
; set-up pData & pFlt
	xar2 = dbl(*ar0(#(LEC_tDb.psTxF)));
	mar (ar2+#LEC_FLT2_SZ);
	mar	(ar2+t0)
	xar3 = mar(*ar2(#(-1)));
	
	bit(ST1, #ST1_FRCT) = #1;
	mar(t0 = #2);
	xar4 = dbl(*ar0(#(LEC_tDb.psFlt2)))
	xar5 = mar(*ar4(#1))
	xcdp = xsp;
; blocks 0, 1	
	blk_adapt_fl	(ILEC_FR_SZ), 4
; block	2
	blk_adapt_fl	(ILEC_FR_SZ/2), 6
; block	3
	blk_adapt_fl	(ILEC_FR_SZ/2), 5
; block	4
	blk_adapt_fl	(ILEC_FR_SZ/2), 4
; block	5
	blk_adapt_fl	(ILEC_FR_SZ/2), 3
;	
	bit(ST1, #ST1_FRCT) = #0;
	sp = sp + #2;
	xar5 = popboth();
	return
	.endif
	
	.endif
;-------------------------------------------------------------------------
	.global               		_lec_adapt_slow_high
	.sym _lec_adapt_slow_high, _lec_adapt_slow_high, 32, 2, 0
_lec_adapt_slow_high
;-------------------------------------------------------------------------
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1
;	S16 sBase		= t0
;	S16 Step		= t1
;
	xar2 = mar(*ar1(#(LEC_tSc.asErr2)))
	xar3 = mar(*ar1(#(LEC_tSc.asNormErr)))