gaecrcva.s55

TI公司DSP/tms320c55x/tms320c54x系列的声学回声消除代码

*-------------------------------------------------------------------------*
*                                                                         *
*   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
*   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
*      "COPYRIGHT 2002 MIKET DSP SOLUTIONS, ALL RIGHTS RESERVED"          *
*                                                                         *
*-------------------------------------------------------------------------*/
	.mmregs
    .cpl_on
    .arms_on
	.include "gaeci.h55"
	.sect ".ftext2"

;-------------------------------------------------------------------------
;-------------------------------------------------------------------------
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					_gaec_rcv_durbin
;	.sym _gaec_durbin, _gaec_durbin, 32, 2, 0
;-------------------------------------------------------------------------
_gaec_rcv_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(#(GAEC_tScRcv.asF)))
	repeat	(#(6-1))
		*psF+ = ac0;
	mar(psF - #6)
;
; zero asK
	xar3 = mar(*ar1(#(GAEC_tScRcv.asK)))
	repeat	(#(6-1))
		*psK+ = ac0;
	mar(psK - #6)
;
; zero aslG 
	xar4 = mar(*ar1(#(GAEC_tScRcv.aslG)))
	repeat	(#(6-1))
		dbl(*pslG+) = ac0;
	mar(pslG - #((6-1)*2))
;
; set asRk ptr
	xar5 = mar(*ar1(#(GAEC_tScRcv.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					_gaec_rcv_normalise_rk
;-------------------------------------------------------------------------
_gaec_rcv_normalise_rk
; pDb		= xar0 : not used
; pSc		= xar1
; sShift 	= t0
; sAdd 		= t1
;
	xar2 = mar(*ar1(#(GAEC_tScRcv.asRk)));
	xar3 = mar(*ar1(#(GAEC_tScRcv.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					_gaec_rcv_moments
;-------------------------------------------------------------------------
_gaec_rcv_moments
; GAEC_tDb *pDb	= ar0
; GAEC_tScRcv *pSc = ar1
; S16 *psRcv	= ar2
;
; MIPS : 200*40*6 = 50,000 = 0.05 MIPS
;
	xar3 = mar(*ar0(#(GAEC_tDb.asTdSav)))
	xar4 = mar(*ar1(#(GAEC_tScRcv.asTdBuf)))
	repeat (#(5-1))
		*ar4+ = *ar3+;
	brc0 = #(GAEC_FR_SZ-1)
	localrepeat {
		ac0 = *ar2+ << #-2;
		*ar4+ = ac0;
	}
	xar3 = mar(*ar0(#(GAEC_tDb.asTdSav)))
	xar4 = mar(*ar1(#(GAEC_tScRcv.asTdBuf+GAEC_FR_SZ)))
	repeat (#(5-1))
		*ar3+ = *ar4+;
	
	xar2 = mar(*ar1(#(GAEC_tScRcv.asTdBuf + 5)));
	xar3 = mar(*ar1(#(GAEC_tScRcv.asTdBuf + 5)));
	xar4 = mar(*ar1(#(GAEC_tScRcv.aslRkTx)));
	brc0 = #(6-1)
	t0 =  #(1-GAEC_FR_SZ);
	localrepeat {
		ac0 = *ar2+ * *ar3+;
		|| repeat (#(GAEC_FR_SZ-3))	
			ac0 = ac0 + (*ar2+ * *ar3+); 
		ac0 = ac0 + (*(ar2+t0) * *(ar3+t0)); 
		mar (*ar3-);
		|| dbl(*ar4+) = ac0;
	}
	return