main.c

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

#include "gaeci.h"
#include "spbpf.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "dbgcase.h"

S16 asTx[GAEC_FR_SZ];
S16 asRxC[GAEC_FR_SZ];
S16 asRxA[GAEC_FR_SZ];
extern S16 _asCnl[GAEC_FR_SZ];
S16 aS[GAEC_FR_SZ*2];
S16 aC[GAEC_FR_SZ*2];


#pragma DATA_ALIGN(asHstC, 2);
#pragma DATA_ALIGN(asHstA, 2);
#pragma DATA_SECTION(asHstA, ".gaech");
#pragma DATA_SECTION(asHstC, ".gaechx");
S16 asHstC[GAEC_RCV_SZ*GAEC_BANDS*2];
S16 asHstA[GAEC_RCV_SZ*GAEC_BANDS*2];

#pragma DATA_SECTION(asAdfA, ".gaecaf");
#pragma DATA_SECTION(asAdfC, ".gaecafx");
#pragma DATA_SECTION(asAdfMA, ".gaecafm");
#pragma DATA_SECTION(asAdfMC, ".gaecafmx");
S16 asAdfC[GAEC_ADF_SZ*GAEC_BANDS*2];
S16 asAdfA[GAEC_ADF_SZ*GAEC_BANDS*2];
S16 asAdfMC[GAEC_ADF_SZ*GAEC_BANDS*2];
S16 asAdfMA[GAEC_ADF_SZ*GAEC_BANDS*2];

#pragma DATA_SECTION(GaecDbA, ".gaecdb");
#pragma DATA_SECTION(GaecDbC, ".gaecdbx");
GAEC_tDb GaecDbA, GaecDbC;

#pragma DATA_SECTION(GaecScA, ".gaecsc");
#pragma DATA_SECTION(GaecScC, ".gaecscx");
GAEC_tSc GaecScA, GaecScC;

#pragma DATA_SECTION(SpBpfDbA, ".bpfa");
#pragma DATA_SECTION(SpBpfDbC, ".bpfax");
SPBPF_tDb SpBpfDbA, SpBpfDbC;

#pragma DATA_SECTION(SpBpfScA, ".bpfsc");
#pragma DATA_SECTION(SpBpfScC, ".bpfscx");
SPBPF_tSc SpBpfScA, SpBpfScC;


volatile S16 sErr = 0;
volatile S16 sTrap = 0;
volatile S32 slA, slC;
volatile S16 sA, sC;
volatile S32 slCntr = 0;
volatile S16 k;

S16 asErrPosDb[32];
S16 asErrPosSc[32];
S16 asErrPos[32];

#if !defined(_dsp)
#include "igaec.cpp"
#else
#include "igaec.c"
#endif

S16 asTmr[1000];
S16 sTmrMax;
volatile S16 sStackUseMax = 0;

/*-------------------------------------------------------------------------*/
void 						tmr_start
/*-------------------------------------------------------------------------*/
(
)
{
	*(volatile unsigned int *)0x26 = 0x0010;
	*(volatile unsigned int *)0x25 = 0xffff;
	*(volatile unsigned int *)0x26 = 0x002f;
}
/*-------------------------------------------------------------------------*/
S16 						tmr_get
/*-------------------------------------------------------------------------*/
(
)
{
	S16 s;
	U16 u = *(volatile unsigned int *)0x24;
	s = _smpy(0xffff - u, Q15(0.32));
	*(volatile unsigned int *)0x26 = 0x0010;
	return s;
}

#if 0

S16 GAEC_aasFBX[GAEC_BANDS*2][GAEC_FBZ_SZ];
/*-------------------------------------------------------------------------*/
void 						_shaffle_fbz
/*-------------------------------------------------------------------------*/
(
)
{
	int k;
	S16 *ps1 = &GAEC_aasFBX[0][0];
	S16 *ps2 = &GAEC_aasFBX[GAEC_BANDS][0];
	
    for (k = 0; k < GAEC_BLK_SZ/2; k++)
    {
        int band;
	    for (band = 0; band < 2*GAEC_BANDS; band++)
	    {
		    int i0 = GAEC_FBZ_SZ - GAEC_BLK_SZ + k*2+0;
		    int i1 = GAEC_FBZ_SZ - GAEC_BLK_SZ + k*2+1;
            int n;
		    for(n = 0; n < GAEC_LFCTR; n++)
		    {
			    *ps1++ = GAEC_aasFBZ[band][i0];
			    *ps2++ = GAEC_aasFBZ[band][i1];
			    i0 -= GAEC_BLK_SZ;
			    i1 -= GAEC_BLK_SZ;
		    }
	    }
    }
}
#endif

extern S16 * stack_ini(S16 sSize);
extern S16   stack_used (S16 *psStart, S16 sSize);

/*-------------------------------------------------------------------------*/
void 						fill_rand
/*-------------------------------------------------------------------------*/
(
S16 *pTo,
S16 sSz,
S16 sLevel
)
#if 0
;
S16 sRndSav;
#else
{
	S16 s;
	for (s = 0; s < sSz; s++)
	{
		S16 x = rand() - (RAND_MAX/2);
		x  = (S16)((x * (S32)sLevel)>>14); /* /RAND_MAX = 32767 */
		pTo[s] = x;
	}
}
#endif

/*-------------------------------------------------------------------------*/
void 						fill_cnt
/*-------------------------------------------------------------------------*/
(
S16 *pTo,
S16 sSz,
S16 sStart
)
{
	S16 s;
	S16 x = sStart;
	for (s = 0; s < sSz; s++)
	{
		pTo[s] = x;
		x += 0x100;
	}
}

/*-------------------------------------------------------------------------*/
S16 						compare
/*-------------------------------------------------------------------------*/
(
S16 *psIn,
S16 *psInC,
S16 sSz
)
{
	S16 s;     
	S16 err = 0;
	
	for (s = 0; s < 32; s++)asErrPos[s] = 0;
	for (s = 0; s < sSz; s++)
	{
		if(psIn[s] != psInC[s]) {
			err++;
			if (err < 32)
			{
				asErrPos[err] = s;
			}
		}
	}      
	asErrPos[0] = err;
	return err;
}
/*-------------------------------------------------------------------------*/
S16 						compare_db
/*-------------------------------------------------------------------------*/
(
)
{
	S16 *psC = (S16 *)&GaecDbC;
	S16 *psA = (S16 *)&GaecDbA;
	S16 s;     
	S16 err = 0;

	for (s = 0; s < 32; s++)asErrPosDb[s] = 0;
	
	for (s = 2*2; s < sizeof(GaecDbC); s++)
	{
		if(psA[s] != psC[s])
		{
			err++;
			if (err < 32)
			{
				asErrPosDb[err] = s;
			}
		}
		asErrPosDb[0] = err;
	}
	return err;
}
/*-------------------------------------------------------------------------*/
S16 						compare_sc
/*-------------------------------------------------------------------------*/
(
)
{
	S16 *psC = (S16 *)&GaecScC;
	S16 *psA = (S16 *)&GaecScA;
	S16 s;     
	S16 err = 0;
	
	for (s = 0; s < 32; s++)asErrPosSc[s] = 0;
	/* last is ptr, which is different */
	/* all structures are long-word aligned */
	for (s = 0*2; s < sizeof(GaecScC); s++)
	{
		if(psA[s] != psC[s])
		{
			err++;
			if (err < 32)
			{
				asErrPosSc[err] = s;
			}
		}
		asErrPosSc[0] = err;
	}
	return err;
}


void main ()
{
#if 0
	GAEC_tDb *pDbC = &GaecDbC;
	GAEC_tDb *pDbA = &GaecDbA;
	GAEC_tSc *pScC = &GaecScC;
	GAEC_tSc *pScA = &GaecScA;
#endif

	memset(&GaecDbA, 0, sizeof(GaecDbA));	
	memset(&GaecDbC, 0, sizeof(GaecDbC));	
	memset(&GaecScA, 0, sizeof(GaecScA));	
	memset(&GaecScC, 0, sizeof(GaecScC));	
	memset(asHstC, 0, GAEC_RCV_SZ*GAEC_BANDS*2);
	memset(asHstA, 0, GAEC_RCV_SZ*GAEC_BANDS*2);
	memset(asAdfA, 0, GAEC_ADF_SZ*GAEC_BANDS*2);
	memset(asAdfC, 0, GAEC_ADF_SZ*GAEC_BANDS*2);
	memset(asAdfMA, 0, GAEC_ADF_SZ*GAEC_BANDS*2);
	memset(asAdfMC, 0, GAEC_ADF_SZ*GAEC_BANDS*2);
	GaecDbA.psRcv = asHstA;
	GaecDbC.psRcv = asHstC;
	GaecDbA.psAdf = asAdfA;
	GaecDbC.psAdf = asAdfC;
	GaecDbA.psAdfM = asAdfMA;
	GaecDbC.psAdfM = asAdfMC;
	sErr = 0;
	
	
#if (DBG_IT == 0)
	printf("DB: sizeof %d, _LSZ %d\n", sizeof (GAEC_tDb), GAEC_MIKET_DB_LSZ*2);
	printf("Sc: sizeof %d, _LSZ %d\n", sizeof (GAEC_tSc), GAEC_MIKET_SC_LSZ*2);
	
	if (GAEC_MIKET_DB_LSZ*2 != sizeof (GAEC_tDb)) sErr++;
	if (GAEC_MIKET_SC_LSZ*2 != sizeof (GAEC_tSc)) sErr++;
	if (GAEC_MIKET_ADF_LSZ*2 != GAEC_ADF_SZ*GAEC_BANDS*2) sErr++;
	if (GAEC_MIKET_RCV_LSZ*2 != GAEC_RCV_SZ*GAEC_BANDS*2) sErr++;
		
#elif (DBG_IT == 1)
{
	extern void sp_bpfC     (SPBPF_tDb *pDb, SPBPF_tSc *pSc, S16 *psData);

	memset(&SpBpfDbA, 0xdead, sizeof(SpBpfDbA));
	memset(&SpBpfDbC, 0xdead, sizeof(SpBpfDbC));
	memset(&SpBpfScA, 0xfeed, sizeof(SpBpfScA));
	memset(&SpBpfScC, 0xfeed, sizeof(SpBpfScA));
	for (k = 0; k < 100; k++)
	{
		fill_rand(asRxC, SP_FR_SZ, 4444);
//		fill_cnt(GaecDbC.asBandPassSav, LEC_BP_SSZ, 0x12);
//		fill_cnt(asRxC, SP_FR_SZ, 0x34);
//		memset(asRxC, 0, SP_FR_SZ);		asRxC[15] = 0x7fff;
		memcpy(asRxA, asRxC, SP_FR_SZ);
		sp_bpf (&SpBpfDbA, &SpBpfScA, asRxA);
		sp_bpfC(&SpBpfDbC, &SpBpfScC, asRxC);
		sErr += compare((S16*)&SpBpfDbC, (S16*)&SpBpfDbA, sizeof(SpBpfDbC));
		sErr += compare((S16*)&SpBpfScC, (S16*)&SpBpfScA, sizeof(SpBpfScC));
		sErr += compare(asRxC, asRxA, SP_FR_SZ);
	}
}
#elif (DBG_IT == 2)
	{
		extern void _adapt_dc_core(S16 *psAdf, S16 *psRcv, S16 *psTmp, U16 uRpt);
		GaecScA.asTmp[0] = 7;
		GaecScA.asTmp[1] = 0x1234;
		GaecScA.asTmp[3] = Q15(0.75);
		GaecDbA.asAdf[0] = 0x20;
		GaecDbA.psRcv[GAEC_ADF_SZ] = 0x40;
		_adapt_dc_core(
			GaecDbA.asAdf, 
			&GaecDbA.psRcv[GAEC_ADF_SZ],
			GaecScA.asTmp,
			7-1);
	}
#elif (DBG_IT == 101)
	{
		void gaec_nrg_errC(S16 ppsErr[][GAEC_ERR_SZ], S16 *psTo);
		
		for (k = 0; k < 1001; k++)
		{
			fill_rand(&GaecScC.aasErr0[0][0], GAEC_BANDS*2*GAEC_ERR_SZ, 8200);
			memcpy(GaecScA.aasErr0, GaecScC.aasErr0, GAEC_BANDS*2*GAEC_ERR_SZ);
			gaec_nrg_errC(GaecScC.aasErr0, GaecScC.asErrEn);
			gaec_nrg_err (GaecScA.aasErr0, GaecScA.asErrEn);
			sErr += compare_db();
			sErr += compare_sc();
		}
	}
#elif (DBG_IT == 102)
	{
		void gaec_nrg_rcvC(GAEC_tDb *pDb,GAEC_tSc *pSc);
		
		for (k = 0; k < 1102; k++)
		{
			S16 amp = abs(rand()) % 8200;
			fill_rand(GaecDbC.psRcv, GAEC_BANDS*2*GAEC_RCV_SZ, amp);
			memcpy(GaecDbA.psRcv, GaecDbC.psRcv, GAEC_BANDS*2*GAEC_RCV_SZ);
			gaec_nrg_rcvC(&GaecDbC, &GaecScC);
			gaec_nrg_rcv (&GaecDbA, &GaecScA);
			sErr += compare_sc();
			sErr += compare_db();
			if (sErr)
			{
				sTrap = gaec_utl_en2log(sErr);
				sErr = 0;
			}
		}
	}
#elif (DBG_IT == 103)
	{
		void gaec_nrg_blkC(S16 *psFrom, S16 *psTo);
		
		for (k = 0; k < 1003; k++)
		{
			S16 amp = abs(rand()) % 8200;
			fill_rand(&GaecScC.aasErr[0][0], GAEC_BANDS*2*GAEC_BLKS, amp);
			memcpy(GaecScA.aasErr, GaecScC.aasErr, GAEC_BANDS*2*GAEC_BLKS);
			gaec_nrg_blkC(&GaecScC.aasErr[0][0], &GaecScC.asErrEn[0]);
			gaec_nrg_blk (&GaecScA.aasErr[0][0], &GaecScA.asErrEn[0]);
			sErr += compare_sc();
			sErr += compare_db();
			if (sErr)
			{
				sTrap = gaec_utl_en2log(sErr);
				sErr = 0;
			}
		}
	}
#elif (DBG_IT == 105)
	{
		void gaec_fb_snd_analysisC(GAEC_tDb *pDb,GAEC_tSc *pSc,S16 *psIn);
		
		fill_rand(&GaecDbC.asSbuf[0], GAEC_SBUF_SZ, 4321);
//		memset(&GaecDbC.asSbuf[0], 0x2000, GAEC_SBUF_SZ);
		memcpy(GaecDbA.asSbuf, GaecDbC.asSbuf, GAEC_SBUF_SZ);
		for (k = 0; k < 1005; k++)
		{
			S16 amp = abs(rand()) % 8200;
			fill_rand(asRxC, GAEC_FR_SZ, amp);
//			memset(asRxC, 0x2000, GAEC_SBUF_SZ);
			
			gaec_fb_snd_analysisC(&GaecDbC, &GaecScC, asRxC);
			gaec_fb_snd_analysis (&GaecDbA, &GaecScA, asRxC);
			sErr += compare_sc();
			sErr += compare_db();
			if (sErr)
			{
				sTrap = gaec_utl_en2log(sErr);
//				sErr = 0;
			}
		}
	}
#elif (DBG_IT == 106)
	{
		void gaec_fb_rcv_analysisC(GAEC_tDb *pDb,GAEC_tSc *pSc);
		
		for (k = 0; k < 1006; k++)
		{
			S16 amp = abs(rand()) % 8200;
			fill_rand(&GaecDbC.asRbuf[0], GAEC_RBUF_SZ, amp);
			fill_rand(asRxC, GAEC_FR_SZ, amp);
			memcpy(GaecDbA.asRbuf, GaecDbC.asRbuf, GAEC_RBUF_SZ);
			gaec_fb_rcv_analysisC(&GaecDbC, &GaecScC);
			gaec_fb_rcv_analysis (&GaecDbA, &GaecScA);
			sErr += compare_sc();
			sErr += compare_db();
			sErr += compare(asHstC, asHstA, GAEC_RCV_SZ*GAEC_BANDS*2);
			if (sErr)
			{
				sTrap = gaec_utl_en2log(sErr);
//				sErr = 0;
			}
		}
	}
#elif (DBG_IT == 107)
	{
		void gaec_fb_synthesisC(S16 ppsErr[][GAEC_ERR_SZ], S16 *psSin);
//		_shaffle_fbz();
		memset(asRxC, 0, GAEC_FR_SZ);
		memset(asRxA, 0, GAEC_FR_SZ);
		memset(_asCnl, 0, GAEC_FR_SZ);
				
		for (k = 0; k < 107; k++)
		{
			S16 amp = abs(rand()) % 8200;
			fill_rand(&GaecScC.aasErr0[0][0], GAEC_ERR_SZ*2*GAEC_BANDS, amp);
//			memset(&GaecScC.aasErr0[0][0], 256, 2*GAEC_BANDS*GAEC_ERR_SZ);
			
			memcpy( &GaecScA.aasErr0[0][0], 
					&GaecScC.aasErr0[0][0], 
					GAEC_ERR_SZ*2*GAEC_BANDS);
			gaec_fb_synthesisC(GaecScC.aasErr0, asRxC);
			gaec_fb_synthesis (GaecScA.aasErr0, asRxA);
			sErr += compare_sc();
			sErr += compare_db();
			sErr += compare(asRxC, asRxA, GAEC_FR_SZ);
			if (sErr)
			{
				sTrap = gaec_utl_en2log(sErr);
//				sErr = 0;
			}
		}
	}
#elif (DBG_IT == 201)
	{
		extern S16 _invert(S32 *psl,S16 sStepSz);
		extern S16 _invertC(S32 *psl,S16 sStepSz);
		
		for (k = 0; k < 201; k++)
		{
			S16 sStep = rand();
			U16 shft = ((U16)rand()) & 0x1f;