lecpa.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"
	
	.sect ".leca"
	
__asBandPassFlt
	.word 29527, -9592, -7602, -5666, -3858 
	.word -2204,  -441,  -147,  -26,    5 
	
	.text
;-------------------------------------------------------------------------
	.global				_lec_dummy0
	.sym _lec_dummy0, _lec_dummy0, 32, 2, 0
;-------------------------------------------------------------------------
_lec_dummy0
	return
	
;-------------------------------------------------------------------------
	.global				_lec_lp_residual_error
	.sym _lec_lp_residual_error, _lec_lp_residual_error, 32, 2, 0
;-------------------------------------------------------------------------
_lec_lp_residual_error
; pDb		= xar0
; pSc		= xar1
; pCoeff	= xar2
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	bit(ST1, #ST1_FRCT) = #1;
	xcdp = xar2;
	xar4 = mar(*ar0(#(LEC_tDb.asLpSav)))
	xar3 = mar(*ar1(#(LEC_tSc.asTmp)))
;
; copy data from Db.Sav
	repeat	(#(5-1))
		*ar3+ = *ar4+;
;
; append data from Err1
	xar2 = mar(*ar1(#(LEC_tSc.asErr1)))
	repeat	(#(ILEC_FR_SZ-1))
		*ar3+ = *ar2+			; at the end, ar2->lpTmp[SZ+5]
;
; update Db.Sav
	mar(*ar3-), mar (*ar4-), mar(coef(*cdp))
	|| repeat	(#(5-1))
		*ar4- = *ar3-			; ar2->lpTmp[SZ-2]
;
; filter
	xar2 = mar(*ar1(#(LEC_tSc.asTmp + 5)));
	xar3 = mar(*ar1(#(LEC_tSc.asTmp + 6)));
	xar4 = mar(*ar1(#(LEC_tSc.asErr1)));
	t0 = #(1-6);
	brc0 = #(ILEC_FR_SZ/2-1);
	t1 = #(1+6);
	|| localrepeat {
		ac0 = *ar2- * coef(*cdp+),
		ac1 = *ar3- * coef(*cdp+);
		|| repeat	(#(6-3))
			ac0 = ac0 + (*ar2- * coef(*cdp+)),
			ac1 = ac1 + (*ar3- * coef(*cdp+));
		ac0 = rnd(ac0 + (*(ar2+t1) * coef(*(cdp+t0)))),
		ac1 = rnd(ac1 + (*(ar3+t1) * coef(*(cdp+t0))));
		*ar4+ = pair(HI(ac0));
	}
	.arms_on
	bit(ST2, #ST2_ARMS) = #1;
	bit(ST1, #ST1_FRCT) = #0;
	return
;-------------------------------------------------------------------------
	.global				_lec_band_pass_rx
	.sym _lec_band_pass_rx, _lec_band_pass_rx, 32, 2, 0
;-------------------------------------------------------------------------
_lec_band_pass_rx
; pDb		= xar0
; pSc		= xar1
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	bit(ST1, #ST1_FRCT) = #1;
	xar2 = mar(*ar0(#(LEC_tDb.asBandPassSav)))
	xar4 = mar(*ar1(#(LEC_tSc.asTmp)))
;
; copy data from Db.Sav
	repeat		(#(LEC_BP_SSZ-1))
		*ar4+ = *ar2+;
;
; append data from In
	xar3 = dbl(*ar1(#(LEC_tSc.psRx)))
	repeat		(#(ILEC_FR_SZ-1))
		*ar4+ = *ar3+;						; at the end, 
	mar (ar3 - #ILEC_FR_SZ)					; return pRx ptr back
;
; update Db.Sav
	mar (*ar2-), mar(*ar4-), mar(coef(*cdp))
	|| repeat	(#(LEC_BP_SSZ-1))
		*ar2- = *ar4-						;
;
; filter
	xar2 = mar(*ar1(#(LEC_tSc.asTmp + LEC_BP_FSZ - 1)))
	xar4 = mar(*ar1(#(LEC_tSc.asTmp + LEC_BP_FSZ - 0)))
	t1 = #-2;
	t0 = #(1-LEC_BP_HSZ)
	brc0 = #(ILEC_FR_SZ/2-1)
	xcdp = #__asBandPassFlt;
	localrepeat {
		ac0 = *(ar2+t1) * coef(*cdp+),
		ac1 = *(ar4+t1) * coef(*cdp+);
		|| repeat (#(LEC_BP_HSZ-3))
			ac0 = ac0 + (*(ar2+t1) * coef(*cdp+)),
			ac1 = ac1 + (*(ar4+t1) * coef(*cdp+));
		ac0 = rnd(ac0 + (*(ar2+t1) * coef(*(cdp+t0)))),
		ac1 = rnd(ac1 + (*(ar4+t1) * coef(*(cdp+t0))));
		mar (ar2 + #(2+LEC_BP_HSZ*2))
		mar (ar4 + #(2+LEC_BP_HSZ*2))
		*ar3+ = pair(HI(ac0));
	}
	.arms_on
	bit(ST2, #ST2_ARMS) = #1;
	bit(ST1, #ST1_FRCT) = #0;
	return

;-------------------------------------------------------------------------
prepare		.macro	times
;-------------------------------------------------------------------------
	mar(psRk + #times)
	ac0 = *psRk- << #16
	.loop	(times - 1)
	ac0 = ac0 + (*psRk- * *psF+);
	.endloop
	ac0 = rnd(ac0 + (*psRk * *psF+));
	.endm
	
;-------------------------------------------------------------------------
get_kj0		.macro		
;-------------------------------------------------------------------------
	ac1 =  | ac0 |					; a=abs(ac0), shall be < J0.
	ac1 = ac1 - (*psJ0 << #16)
	if (ac1 >= #0) goto	L02
	ac1 = ac1 + (*psJ0 << #16);
;
; get and save new sK
	ac1 = ac1 << #-1;
	|| repeat	(#(16-1))
		subc(*psJ0, ac1, ac1);
	ac1 = ac1 & #0x7fff;
	if (ac0 < #0) execute (D_unit)
		|| ac1 = -ac1;
	*psK = ac1;				; asK[2]
;
; get and save new J0
	ac0 = #0x7fff << #16;
	ac0 = rnd(ac0 - (*psK * *psK));
	ac1 = rnd(ac0 * *psJ0);
	*psJ0 = HI(ac1)							; new J0 saved
	.endm	

;-------------------------------------------------------------------------
get_lG	.macro	times
;-------------------------------------------------------------------------
	mar(psF - #times)						; psF -> asF[1]
	.loop	times
	ac0 = *psF+ * *psK
	dbl(*pslG+) = ac0;
	.endloop
	mar(pslG - #(times*2))
	.endm
	
;-------------------------------------------------------------------------
update_f	.macro	times
;-------------------------------------------------------------------------
; update f[last]
	ac0 = *psK+								; advance psK
	ac0 = -ac0
	*psF- = ac0
; update f[prevs]
	.loop	times
	ac0 = *psF << #16
	ac0 = ac0 - dbl(*pslG+)
	*psF- = HI(saturate(rnd(ac0)));	
;	ac0 = rnd(ac0)
;	ac0 = saturate(ac0)
;	*psF- = HI(ac0)
	.endloop
	mar(*psF+)
	ac0 = *psJ0
	ac0 = ac0 - *pJ0min
	if (ac0 < #0) goto	L01
	mar(pslG - #(times*2))
	.endm
	
;-------------------------------------------------------------------------
	.global					_lec_durbin
	.sym _lec_durbin, _lec_durbin, 32, 2, 0
;-------------------------------------------------------------------------
_lec_durbin
;
;	LEC_tDb *pDb	= xar0
;	LEC_tSc *pSc	= xar1
;
SJ0_MIN 	.set	(2048/16)
			.asg	ar2, psF
			.asg	ar3, psK
			.asg	ar4, pslG
			.asg	ar5, psRk
			.asg	sp(1), pJ0min
			.asg	sp(0), psJ0

	pshboth(xar5)	
	bit(ST1, #ST1_FRCT) = #1;
	ac0 = #0;
;
; zero asF
	xar2 = mar(*ar1(#(LEC_tSc.asF)))
	repeat	(#(6-1))
		*psF+ = ac0;
	mar(psF - #6)
;
; zero asK
	xar3 = mar(*ar1(#(LEC_tSc.asK)))
	repeat	(#(6-1))
		*psK+ = ac0;
	mar(psK - #6)
;
; zero aslG 
	xar4 = mar(*ar1(#(LEC_tSc.aslG)))
	repeat	(#(6-1))
		dbl(*pslG+) = ac0;
	mar(pslG - #((6-1)*2))
;
; set asRk ptr
	xar5 = mar(*ar1(#(LEC_tSc.asRk+1)))
;
	sp = sp + #-2
;
; iteration 0
;	
	*psF+ = #32767			; F[1]
	*psK+ = #32767			; K[1]
	*psJ0 = #32767			; pJ0
	*pJ0min = #SJ0_MIN		; SJ0_MIN
; 
; iteration 1
;
	ac0 = *psRk;
	*psK+ = ac0;
	ac0 = -ac0;
	*psF = ac0;
	ac0 = #32767 << #16;
	ac0 = rnd(ac0 - (*psRk+ * *psRk+));
	*psJ0 = HI(ac0)
	
	ac0 = ac0 - (*pJ0min << #16)
	if (ac0 < #0) goto L01
;
; iteration 2
;
	ac0 = *psRk- << #16
	ac0 = rnd(ac0 + (*psRk * *psF+));	
	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
	t0 = *psJ0
	bit(ST1, #ST1_FRCT) = #0;
	sp = sp + #2
	xar5 = popboth()
	return
L02
	t0 = #0;
	bit(ST1, #ST1_FRCT) = #0;
	sp = sp + #2
	xar5 = popboth()
	return

;-------------------------------------------------------------------------
	.global					_lec_normalise_rk
	.sym _lec_normalise_rk, _lec_normalise_rk, 32, 2, 0
;-------------------------------------------------------------------------
_lec_normalise_rk
; pDb		= xar0 : not used
; pSc		= xar1
; sShift 	= t0
; sAdd 		= t1
;
	xar2 = mar(*ar1(#(LEC_tSc.asRk)));
	xar3 = mar(*ar1(#(LEC_tSc.aslRk)));
	
	ac0 = dbl(*ar3+);
	ac0 = ac0 + t1;
	ac0 = ac0 + (ac0 << t0);
	ac0 = mant(ac0), t0 = -exp(ac0);
	*ar2 = HI(ac0);
	ac0 = #0x3fff << #15;
	repeat (#(16-1))
		subc(*ar2, ac0, ac0);
	t1 = ac0;
	t0 = -t0;
	*ar2+ = #32767;
	brc0 = #(6-2);
	localrepeat {
		ac0 = dbl(*ar3+);
		ac0 = ac0 << t0;
		ac0 = ac0 * t1;
		*ar2+ = HI(ac0 << 2);
	}
	return
;-------------------------------------------------------------------------
	.global					_lec_tx_moments
	.sym _lec_tx_moments, _lec_tx_moments, 32, 2, 0
;-------------------------------------------------------------------------
_lec_tx_moments
; pDb		= xar0
; pSc		= xar1
;
    .arms_off
    push(t2)
	|| bit(ST2, #ST2_ARMS) = #0;
	xar2 = dbl(*ar0(#(LEC_tDb.psHst)));
	mar (ar2 + #(LEC_HST_SZ - 5));
	xar3 = mar(*ar1(#(LEC_tSc.asTmp)));
	mar(t2 = #(-2));
; get stored data
	ac0 = *ar2+ << 16;
	|| repeat(#(5-2))
		ac0 = *ar2+ << #16, *ar3+ = HI(ac0 << t2); 
	*ar3+ = HI(ac0 << t2);
; append new data	
	xar2 = dbl(*ar1(#(LEC_tSc.psTx)))
	ac0 = *ar2+ << 16;
	|| repeat(#(ILEC_FR_SZ-2))
		ac0 = *ar2+ << #16, *ar3+ = HI(ac0 << t2); 
	*ar3+ = HI(ac0 << t2);
; cdp -> psTx;
; ar2 -> asTmp[5];
; ar3 -> asTmp[5-1];	
; ar4 -> aslRkTx
	xcdp = dbl(*ar1(#(LEC_tSc.psTx)));
	xar2 = mar(*ar1(#(LEC_tSc.asTmp + 5)));
	xar3 = mar(*ar1(#(LEC_tSc.asTmp + 4)));
	xar4 = mar(*ar1(#(LEC_tSc.aslRkTx)))
	brc0 = #(3-1);
	t0 = #(1-ILEC_FR_SZ);
	t1 = #(-1-ILEC_FR_SZ);
	localrepeat {
		ac0 = *ar2+ * coef(*cdp+),
		ac1 = *ar3+ * coef(*cdp+);
		|| repeat (#(ILEC_FR_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)));
		dbl(*ar4+) = ac0;
		dbl(*ar4+) = ac1;
	}
    .arms_on
    t2 = pop();
	|| bit(ST2, #ST2_ARMS) = #1;
	return
;-------------------------------------------------------------------------
	.global					_lec_gs_moments
	.sym _lec_gs_moments, _lec_gs_moments, 32, 2, 0
;-------------------------------------------------------------------------
_lec_gs_moments
; pDb		= xar0
; pSc		= xar1
;
	xar2 = mar(*ar1(#(LEC_tSc.aslRkTx)));
	xar3 = mar(*ar0(#(LEC_tDb.aslRk)));
	xar4 = mar(*ar1(#(LEC_tSc.aslRk)));
	brc0 = #(6-1);
	localrepeat {
		ac0 = dbl(*ar2+);
		ac0 = ac0 - dbl(*ar3);
		ac0 = ac0 << #-4;
		ac0 = ac0 + dbl(*ar3);
		dbl(*ar3+) = ac0;
		dbl(*ar4+) = ac0;
	}
	return;
;-------------------------------------------------------------------------
	.global					_lec_gs_filter
	.sym _lec_gs_filter, _lec_gs_filter, 32, 2, 0
;-------------------------------------------------------------------------
_lec_gs_filter
; pDb		= xar0
; pSc		= xar1
;
; ar2,3 = psHst
; ar4 = psTxF
; cdp = psF
; t0 = -(sizeof(asF)-1)
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	bit(ST1, #ST1_FRCT) = #1;
; move data	
	xar2 = dbl(*ar0(#(LEC_tDb.psTxF)));
	xar3 = mar(*ar2(#(ILEC_FR_SZ)));
	repeat (#(LEC_HST_SZ-ILEC_FR_SZ-1))
		*ar2+ = *ar3+;
; setup ptrs		
	xar3 = dbl(*ar0(#(LEC_tDb.psHst)))
	mar (ar3 + #(LEC_HST_SZ-ILEC_FR_SZ))
	xar4 = mar(*ar3(#(1)));
	xcdp = mar(*ar1(#(LEC_tSc.asF)));
	mar (t0 = #(1-6));
	mar (t1 = #(1+6));
	brc0 = #(ILEC_FR_SZ/2-1)
; filtering
	localrepeat {
		ac0 = *ar3- * coef(*cdp+), 
		ac1 = *ar4- * coef(*cdp+);
		|| repeat (#(6-3))
			ac0 = ac0 + (*ar3- * coef(*cdp+)),
			ac1 = ac1 + (*ar4- * coef(*cdp+));
		ac0 = rnd(ac0 + (*(ar3+t1) * coef(*(cdp+t0)))),
		ac1 = rnd(ac1 + (*(ar4+t1) * coef(*(cdp+t0))));
		*ar4+ = pair(HI(ac0));
	}
    .arms_on
	bit(ST2, #ST2_ARMS) = #1;
	bit(ST1, #ST1_FRCT) = #0;
	return
;-------------------------------------------------------------------------
	.global					_lec_ts_moments
	.sym _lec_ts_moments, _lec_ts_moments, 32, 2, 0
;-------------------------------------------------------------------------
_lec_ts_moments
; pDb		= xar0
; pSc		= xar1
;
	xar2 = mar(*ar0(#(LEC_tDb.aslRkSav)));
	xar3 = mar(*ar1(#(LEC_tSc.aslRkTx)));
	xar4 = mar(*ar1(#(LEC_tSc.aslRk)));
	brc0 = #(6-1);
	localrepeat {
		ac0 = dbl(*ar3+);
		ac1 = dbl(*ar2);
		dbl(*ar2+) = ac0;
		ac0 = ac0 + ac1;
		dbl(*ar4+) =  ac0;
	}
	return;
;-------------------------------------------------------------------------