gaeca2a.s55

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

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

_ADJ            		.set (2)
_ADJSS          		.set (_ADJ)
_DEXP           		.set (22937) ; Q15(0.7)

	.sect ".ftext1"
;-------------------------------------------------------------------------

;-------------------------------------------------------------------------
	.global 	_gaec_get_energy2
;-------------------------------------------------------------------------
_gaec_get_energy2
; GAEC_tDb *pDb = ar0
; S16 blk		= t0
;
; input parameters are not modified
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	
	xar2 = dbl(*ar0(#(GAEC_tDb.psRcv)))
;	mar (ar2 + #(GAEC_ADF_BSZ - GAEC_OFF2))
	mar (*+ar2(#(GAEC_ADF_BSZ - GAEC_OFF2)))
	mar (ar2 + t0);
	xar3 = mar(*ar2(#((GAEC_BANDS+1)* GAEC_RCV_SZ)))
	xar4 = mar(*ar0(#(GAEC_tDb.aslEn2)));
	t1 = #(GAEC_RCV_SZ);
;	
; band 0	
	ac0 = dbl(*ar4);
	ac0 = ac0 - (ac0 << #-5);
	ac1 = *(ar2+t1) * *(ar2+t1);
	ac1 = ac1 + (ac1 << #-2)
	ac0 = ac0 + (ac1 << #(-_ADJ));
	dbl(*ar4+) = ac0;
;
; complex bands
	brc0 = #(GAEC_BANDS-2);
	localrepeat {
		ac0 = dbl(*ar4);
		ac0 = ac0 - (ac0 << #-5);
		ac1 = *(ar2+t1) * *(ar2+t1);
		ac1 = ac1 + (*(ar3+t1) * *(ar3+t1));
		ac1 = ac1 + (ac1 << #-2);
		ac0 = ac0 + (ac1 << #(-_ADJ));
		dbl(*ar4+) = ac0;
	}	
; last band	
	ac0 = dbl(*ar4);
	ac0 = ac0 - (ac0 << #-5);
	ac1 = *(ar2+t1) * *(ar2+t1);
	ac1 = ac1 + (ac1 << #-2);
	ac0 = ac0 + (ac1 << #(-_ADJ));
	dbl(*ar4+) = ac0;
	
    .arms_on
	bit(ST2, #ST2_ARMS) = #1;
	return	
	
	
;-------------------------------------------------------------------------
	.global 	__invert
;-------------------------------------------------------------------------
__invert
; S32 *psl		= ar0
; S16 sStepSz	= t0
; return t0
;
	ac0 = dbl(*ar0);
	ac0 = mant(ac0), t1 = -exp(ac0) || nop;
	.if 1
	ac1 = #1 << #15
	ac1 = ac1 - (ac0 << #-16);
	ac0 = #16020;
	ac0 = ac0 + (ac1 << #-1);
	ac1 = ac1 << #16;
	.else
	ac1 = ac0 - (#32767 << #16);
;	ac1 = ac0 + (#32768 << #16);
	ac0 = #16020;
	ac0 = ac0 - (ac1 << #-17);
	.endif
	ac1 = ac1 * ac1;
	ac0 = ac0 + (ac1 << #-15);
	ac0 = ac0 << #16;
	ac0 = ac0 * t0;
	ac0 = ac0 + (#1 << #11);
	ac0 = ac0 << #-12;
	dbl(*ar0) = ac0;

	t0 = #30 + t1;
	
	return


;-------------------------------------------------------------------------
	.global 	_gaec_adapt_pre
;-------------------------------------------------------------------------
_gaec_adapt_pre
; in:
; ac3 = _ADJSS
; ar2 = pSc->asSSC
; ar3 = pDb->aslEnX
; ar4 = pDb->aslAdEn
; ar6 = pSc->u.aA2;
; out:
; ac0 = corr step size
; t1 = shift
	ac0 = dbl(*ar3)
	ac1 = dbl(*ar4)
	ac1 = ac1 + (ac0 << #-4)
	ac2 = #GAEC_ADEN_BASE << #GAEC_ADEN_SHFT;
	ac1 = min(ac2, ac1);
	dbl(*ar4) = ac1;
	ac0 = ac0 + ac1;
	
; invert here, put it into t2
	ac0 = mant(ac0), t1 = -exp(ac0) || nop;
	.if 1
	ac1 = #1 << #15
	ac1 = ac1 - (ac0 << #-16);
	ac0 = #16020;
	ac0 = ac0 + (ac1 << #-1);
	.else
	ac1 = ac0 - (#32767 << #16);
	ac0 = #16020;
	ac0 = ac0 - (ac1 << #-17);
	.endif
	ac1 = ac1 << #16;
	ac1 = ac1 * ac1;
	ac0 = ac0 + (ac1 << #-15);
	ac0 = ac0 << #16;
	
	ac0 = ac0 * *ar2;
	ac0 = ac0 + (#1 << #11);
	ac0 = ac0 << #4;

	t1 = #(30-15) + t1;
	t1 = t1 + ac3;
	t0 = t1 - #15;
	if (t0 <= #0) goto A10
		t0 = -t0;
		*ar6 = #15
		return
A10	
	if (t1 < #0) goto A11
		t0 = #0
		*ar6 = t1
		return
A11	
		t0 = -t1;
		*ar6 = 0;	
		return
		
;-------------------------------------------------------------------------
	.global 	_gaec_adapt_dc2p
;-------------------------------------------------------------------------
_gaec_adapt_dc2p
; GAEC_tDb *pDb = ar0
; GAEC_tSc *pSc = ar1
; S16 blk		= t0
; S16 band		= t1
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	pshboth(xar5);
	pshboth(xar6);
	pshboth(xar7);
;	
; set pointers
	xar2 = mar(*ar1(#(GAEC_tSc.asSSC)))
	xar3 = mar(*ar0(#(GAEC_tDb.aslEn2)))
	xar4 = mar(*ar0(#(GAEC_tDb.aslAdEn)))
	xar5 = mar(*ar1(#(GAEC_tSc.aasErr)))
	mar (ar5+t0)
	xar6 = mar(*ar1(#(GAEC_tSc.u.aA2)))
	xar7 = mar(*ar0(#(GAEC_tDb.asShft)))
;	
; adjust ptrs if band == BANDS	
	if (t1 != #0) execute(AD_unit) ||
		mar (ar2+#GAEC_BANDS)
	if (t1 != #0) execute(AD_unit) ||
		mar (ar3+#(GAEC_BANDS*2))
	if (t1 != #0) execute(AD_unit) ||
		mar (ar4+#(GAEC_BANDS*2))
	if (t1 != #0) execute(AD_unit) ||
		mar (ar5+#(GAEC_BANDS*GAEC_BLKS))
	if (t1 != #0) execute(AD_unit) ||
		mar (ar7+#(GAEC_BANDS))
	
	ac3 = *ar2;
	if (ac3 == #0) goto L03;
		ac3 = #(_ADJSS+1)
		ac3 = ac3 + *ar7
		call _gaec_adapt_pre
		t0 = t0 + #1;
;		bit(ST1, #ST1_FRCT) = 1;
		ac1 = *ar5 * ac0
		*ar6(#(GAEC_tScA2.sErrI)) = HI(saturate(rnd(ac1 << t0)))
;		bit(ST1, #ST1_FRCT) = 0;
L03
	xar7 = popboth();
	xar6 = popboth();
	xar5 = popboth();
    .arms_on
	bit(ST2, #ST2_ARMS) = #1;
	return		
	
;-------------------------------------------------------------------------
	.global 	_gaec_adapt_dc2d
;-------------------------------------------------------------------------
_gaec_adapt_dc2d
; GAEC_tDb *pDb = ar0
; GAEC_tSc *pSc = ar1
; S16 blk		= t0
; S16 band		= t1
;
	push(t2)
	push(t3)
; set up ptrs
	xar2 = dbl(*ar0(#(GAEC_tDb.psAdf)))	
	mar(ar2 + #(GAEC_OFF2*2))
	if (t1 != #0) execute (AD_unit) ||
		mar(*ar2+)
	xar3 = dbl(*ar0(#(GAEC_tDb.psRcv)));
;	mar(ar3 + #(GAEC_ADF_BSZ - GAEC_OFF2));
	mar(*+ar3(#(GAEC_ADF_BSZ - GAEC_OFF2)));
	mar(ar3 + t0);
	if (t1 != #0) execute(AD_unit) ||
		ar3 = ar3 + #(GAEC_RCV_SZ*GAEC_BANDS)
	xar4 = mar(*ar1(#(GAEC_tSc.u.aA2)));
;	
; set up shifts	
	t0 = #2;		; t0 = moving between Adf
	t2 = *ar4+		; t2 = adf loading shift
	t3 = t2- #16;
	t3 = -t3;		; t3 = adf storing shift
;
; setup repeat counters	
	brc0 = #(GAEC_SECTIONS2-1);
	brc1 = #(GAEC_SECTION_SZ-1);
	ac2 = #_DEXP << #16;
;
; the loop
	localrepeat {
		localrepeat {
			ac0 = *ar2 << t2;
			ac0 = ac0 + (*ar3- * *ar4);
			*(ar2+t0) = HI(saturate(rnd(ac0 << t3)));
		}
		ac0 = *ar4 * ac2;
		*ar4 = HI(rnd(ac0 << #1));
;		nop; let it stored before next read
	}
	t3 = pop();
	t2 = pop();
	return;	
	
;-------------------------------------------------------------------------
	.global 	_gaec_adapt_ac2p
;-------------------------------------------------------------------------
_gaec_adapt_ac2p
; GAEC_tDb *pDb = ar0
; GAEC_tSc *pSc = ar1
; S16 blk		= t0
;
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	pshboth(xar5);
	pshboth(xar6);
	pshboth(xar7);
	
	
; let's preset pointers
	xar2 = mar(*ar1(#(GAEC_tSc.asSSC+1)))
	xar3 = mar(*ar0(#(GAEC_tDb.aslEn2+2)))
	xar4 = mar(*ar0(#(GAEC_tDb.aslAdEn+2)))
	xar5 = mar(*ar1(#(GAEC_tSc.aasErr+GAEC_BLKS)))
	mar (ar5 + t0);
	xar6 = mar(*ar1(#(GAEC_tSc.u.aA2))) 
	xar7 = mar(*ar0(#(GAEC_tDb.asShft+1)))
	
	ac2 = #GAEC_ADEN_BASE << #GAEC_ADEN_SHFT;
	t1 = #(GAEC_BLKS*GAEC_BANDS);

	brc0 = #(GAEC_BANDS-2);
	
	blockrepeat {
		ac3 = *ar2;

		if (ac3 == #0) goto L23;
			ac3 = #_ADJSS
			ac3 = ac3 + *ar7
			call _gaec_adapt_pre
			t0 = t0 + #1;
		
			ac1 = *ar5 * ac0
			*ar6(#(GAEC_tScA2.sErrI)) = HI(saturate(rnd(ac1 << t0)))
			ac1 = *ar5(#(GAEC_BLKS*GAEC_BANDS)) * ac0;
			*ar6(#(GAEC_tScA2.sErrQ)) = HI(saturate(rnd(ac1 << t0)))
			*ar6(#(GAEC_tScA2.sDexp)) = #(0x5999);
L23
; if step size if == 0; just advance pointers
		mar(ar2 + #1)
		mar(ar3 + #2);
		mar(ar4 + #2);
		mar(ar5 + #GAEC_BLKS);
		mar(ar6 + #GAEC_tScA2_Sz);	
		mar(ar7 + #1)
	}		
	
	xar7 = popboth();
	xar6 = popboth();
	xar5 = popboth();
    .arms_on
	bit(ST2, #ST2_ARMS) = #1;
	return;	
	
;-------------------------------------------------------------------------
	.global 	_gaec_adapt_ac2d
;-------------------------------------------------------------------------
_gaec_adapt_ac2d
; GAEC_tDb *pDb = ar0
; GAEC_tSc *pSc = ar1
; S16 blk		= t0
    .arms_off
	bit(ST2, #ST2_ARMS) = #0;
	pshboth(xar5);
	pshboth(xar6);
	push(t2, t3);
;
;	
; setup ptrs initially
;
; ar2 = band counter
; ar3 -> Adf
; ar4 -> RcvI
; ar5 -> RcvQ
; ar6 -> Tmp (precalculated& stored vars)
;
	xar2 = #(GAEC_BANDS-2);
	xar3 = dbl(*ar0(#(GAEC_tDb.psAdf)));
	mar(*+ar3(#(GAEC_ADF_SZ*2 + GAEC_OFF2*2)))	
	xar4 = dbl(*ar0(#(GAEC_tDb.psRcv)));			
	mar (*+ar4(#(GAEC_RCV_SZ + GAEC_ADF_BSZ - GAEC_OFF2)));
	mar(ar4 + t0);
	xar5 = mar(*ar4(#(GAEC_RCV_SZ*GAEC_BANDS)))
	xar6 = mar(*ar1(#(GAEC_tSc.u.aA2)));
	
L33
;	
; set up shifts	
		t2 = *ar6;		; t2 = adf loading shift
		t3 = t2 - #16;
		t3 = -t3;		; t3 = adf storing shift
;
; setup block repeat counters	
		|| brc0 = #(GAEC_SECTIONS2-1);
		brc1 = #(GAEC_SECTION_SZ-1);
		xcdp = mar (*ar6(#(GAEC_tScA2.sErrI)));
;
; the loop
		localrepeat {
			localrepeat {
				ac0 = *ar3+ << t2;
				ac1 = *ar3- << t2;
				ac0 = ac0 + (*ar4  * coef(*cdp+)),
				ac1 = ac1 + (*ar5  * coef(*cdp+));
				ac1 = ac1 - (*ar4- * coef(*cdp-)),
				ac0 = ac0 + (*ar5- * coef(*cdp-));
				*ar3+ = HI(saturate(rnd(ac0 << t3)));
				*ar3+ = HI(saturate(rnd(ac1 << t3)));
			}
			ac0 = *ar6(#(GAEC_tScA2.sDexp)) * coef(*cdp);
			*cdp+ = HI(rnd(ac0 << #1));
			ac0 = *ar6(#(GAEC_tScA2.sDexp)) * coef(*cdp);
			*cdp- = HI(rnd(ac0 << #1));
		}
		; advance to the next band
;		mar (ar3 + #(GAEC_ADF_SZ*2 - GAEC_SECTIONS2*GAEC_SECTION_SZ*2)) ; psAdf
		mar (*+ar3(#(GAEC_ADF_SZ*2 - GAEC_SECTIONS2*GAEC_SECTION_SZ*2))) ; psAdf
;		mar (ar4 + #(GAEC_RCV_SZ + GAEC_SECTIONS2*GAEC_SECTION_SZ)); psRcvI
		mar (*+ar4(#(GAEC_RCV_SZ + GAEC_SECTIONS2*GAEC_SECTION_SZ))); psRcvI
;		mar (ar5 + #(GAEC_RCV_SZ + GAEC_SECTIONS2*GAEC_SECTION_SZ)); psRcvI
		mar (*+ar5(#(GAEC_RCV_SZ + GAEC_SECTIONS2*GAEC_SECTION_SZ))); psRcvI
		mar (ar6 + #(GAEC_tScA2_Sz));
	if (*ar2- != #0) goto L33
	
	t2, t3 = pop();
	xar6 = popboth();
	xar5 = popboth();
    .arms_on
	bit(ST2, #ST2_ARMS) = #1;
	return;