gaeca4s.c

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"          *
 *                                                                         *
 *-------------------------------------------------------------------------*/

#include "gaeci.h"

/*--------------------- local defs ----------------------------------------*/

#define _ADJ            (4)
#define _ADJSS          (_ADJ+2)
#define _DEXP           (Q15(0.9))

/*--------------------- public vars ---------------------------------------*/
/*--------------------- local vars ----------------------------------------*/
/*--------------------- local functions -----------------------------------*/
/*--------------------- public  functions ---------------------------------*/
/*-------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------*/
#if defined(_dsp) 
void                        gaec_get_energy4C 
#else 
void                        gaec_get_energy4 
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb
)
{
    int iRcv = GAEC_ADF_BSZ - GAEC_OFF4;
    int blk;
    int band;

    S16 *psRcvI  = &(pDb->psRcv[iRcv]);
    S16 *psRcvQ = psRcvI + (GAEC_BANDS * GAEC_RCV_SZ);
    S32 ac0;
    S32 ac1;

    for (band = 0; band < GAEC_BANDS+1; band++)
    {
        ac0 = pDb->aslEn4[band];

        // 1.375 / (1 - (1-1/64)). gain = 38.9 dB (~2^6.5)
        // 
        // k = 16*2=32 = 2^5;
        // max en = 2^(13*2+1+6.5)=2^(33.5)
        // we need shift en by 4 down, to fit into 2^30.

        for (blk = 0; blk < GAEC_BLKS; blk++)
        {
            ac0 -= ac0 >> 6;
            ac1 = (*psRcvI * (S32)*psRcvI);
            if ((band != 0) && (band != GAEC_BANDS))
            {
                ac1 += (*psRcvQ * (S32)*psRcvQ);
            }
            ac1 += ac1 >> 2;
            ac1 += ac1 >> 3;
            ac0 += ac1 >> _ADJ;
            psRcvI++;
            psRcvQ++;
        }
        pDb->aslEn4[band] = ac0;
        psRcvI += GAEC_RCV_SZ-GAEC_BLKS;
        psRcvQ += GAEC_RCV_SZ-GAEC_BLKS;
    }
}
/*-------------------------------------------------------------------------*/
#if defined(_dsp) 
void                        gaec_adapt_dc4pC
#else
void                        gaec_adapt_dc4p
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 band
)
{
    GAEC_tScA4 *pA = pSc->u.aA4;
    S32 ac0;
    S32 ac1;
    S16 tx;
    int blk;

    S16 sStepSz = pSc->asSSC[band];

    if (sStepSz == 0)
        return;
        
    ac0 = pDb->aslEn4[band];
    gaec_add_energy(pDb, band, &ac0);
    tx = _invert(&ac0, sStepSz);
    tx -= (15 - _ADJSS); // step size = 0.25
	tx += pDb->asShft[band];

    if (tx > 15)
    {
        pA->sTx = 15;
        for (blk = 0; blk < GAEC_BLKS; blk++)
        {
            ac1 = ac0 * pSc->aasErr  [band           ][blk];
            pA->asErr[blk]  = _Srnd(ac1, 15-tx);
        }
    }
    else
    {
        if (tx > 0)
        {
            pA->sTx = tx;
            for (blk = 0; blk < GAEC_BLKS; blk++)
            {
                ac1 = ac0 * pSc->aasErr  [band           ][blk];
                pA->asErr[blk]  = _Srnd(ac1, 0);
            }
        }
        else
        {
            pA->sTx = 0;
            for (blk = 0; blk < GAEC_BLKS; blk++)
            {
                ac1 = ac0 * pSc->aasErr  [band           ][blk];
                pA->asErr[blk]  = _Srnd(ac1, -tx);
            }
        }
    }
}

/*-------------------------------------------------------------------------*/
#if defined(_dsp) 
void                        gaec_adapt_dc4dC
#else
void                        gaec_adapt_dc4d
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 band
)
{
    GAEC_tScA4 *pA = pSc->u.aA4;
    int iRcv = GAEC_ADF_BSZ - GAEC_OFF4;
    S32 ac0;

    S16 *psRcvI = &(pDb->psRcv[GAEC_RCV_SZ*band + iRcv]);
    S16 *psAdfI = &(pDb->psAdf[GAEC_OFF4*2]);

	S16 tx = pA->sTx;
    S16 *psErr = pA->asErr;
    S16 sDexp = _DEXP;
	
	int tap;
    int section;
    int blk;

    if (band != 0)
        psAdfI++;

    for (section = 0; section < GAEC_SECTIONS4; section++)
    {
		for (tap = 0; tap < GAEC_SECTION_SZ; tap++)
		{
            ac0 = ((S32)*psAdfI)<<tx;
        
            for (blk = 0; blk < GAEC_BLKS; blk++)
            {
                ac0 = ac0 + *psRcvI++ * (S32)*psErr++;   // ErrI
            }

            *psAdfI = _Srnd(ac0,15-tx);

            psAdfI += 2;
            psErr  -= GAEC_BLKS;
            psRcvI -= GAEC_BLKS + 1;
		}

        for (blk = 0; blk < GAEC_BLKS; blk++)
        {
            *psErr++ = _Smpy(sDexp, *psErr);
        }
        psErr -= GAEC_BLKS;
    }
}
/*-------------------------------------------------------------------------*/
#if defined(_dsp) 
void                        gaec_adapt_ac4pC
#else
void                        gaec_adapt_ac4p
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc
)
{
    GAEC_tScA4 *pA = pSc->u.aA4;
    S32 ac0;
    S32 ac1;
    S16 tx;

    int blk;
    int band;

	for (band = 1; band < GAEC_BANDS; band++, pA++)
	{
        S16 sStepSz = pSc->asSSC[band];
        if (sStepSz != 0)
        {
            ac0 = pDb->aslEn4[band];
            gaec_add_energy(pDb, band, &ac0);
            tx = _invert(&ac0, sStepSz);
            tx -= (15 - _ADJSS); // step size = 0.25
			tx += pDb->asShft[band];

            if (tx > 15)
            {
        	    pA->sTx = 15;
                for (blk = 0; blk < GAEC_BLKS; blk++)
                {
                    ac1 = ac0 * pSc->aasErr  [band           ][blk];
                    pA->asErr[2*blk+0] = _Srnd(ac1, 15-tx);
                    ac1 = ac0 * pSc->aasErr  [band+GAEC_BANDS][blk];
                    pA->asErr[2*blk+1] = _Srnd(ac1, 15-tx);
                }
            }
            else
            {
                if (tx > 0)
                {
        		    pA->sTx = tx;
                    for (blk = 0; blk < GAEC_BLKS; blk++)
                    {
                        ac1 = ac0 * pSc->aasErr  [band           ][blk];
                        pA->asErr[2*blk+0] = _Srnd(ac1, 0);
                        ac1 = ac0 * pSc->aasErr  [band+GAEC_BANDS][blk];
                        pA->asErr[2*blk+1] = _Srnd(ac1, 0);
                    }
                }
                else
                {
	        	    pA->sTx = 0;
                    for (blk = 0; blk < GAEC_BLKS; blk++)
                    {
                        ac1 = ac0 * pSc->aasErr  [band           ][blk];
                        pA->asErr[2*blk+0] = _Srnd(ac1, -tx);
                        ac1 = ac0 * pSc->aasErr  [band+GAEC_BANDS][blk];
                        pA->asErr[2*blk+1] = _Srnd(ac1, -tx);
                    }
                }
            }
            pA->sDexp = _DEXP;
        }
    }
}
    
/*-------------------------------------------------------------------------*/
#if defined(_dsp) 
void                        gaec_adapt_ac4dC
#else
void                        gaec_adapt_ac4d
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc
)
{
    GAEC_tScA4 *pA = pSc->u.aA4;
    S32 ac0;
    S32 ac1;

    int blk;
    int band;

	for (band = 1; band < GAEC_BANDS; band++, pA++)
	{
	    int iRcv = GAEC_ADF_BSZ - GAEC_OFF4;
        S16 *psRcvI = &(pDb->psRcv[GAEC_RCV_SZ*band + iRcv]);
        S16 *psRcvQ = psRcvI + (GAEC_BANDS * GAEC_RCV_SZ);
        S16 *psAdfI = &(pDb->psAdf[band *GAEC_ADF_SZ*2+GAEC_OFF4*2]);
        S16 *psAdfQ = psAdfI + 1;
    	int tap;
        int section;

        S16 tx     = pA->sTx;
        S16 *psErr = pA->asErr;
        S16 sDexp  = pA->sDexp;

        for (section = 0; section < GAEC_SECTIONS4; section++)
        {
		    for (tap = 0; tap < GAEC_SECTION_SZ; tap++)
		    {
                ac0 = ((S32)*psAdfI)<<tx;
                ac1 = ((S32)*psAdfQ)<<tx;
            
                for (blk = 0; blk < GAEC_BLKS; blk++)
                {
                    ac0 = ac0 + *psRcvI * (S32)*psErr;   // ErrI
                    ac1 = ac1 + *psRcvQ * (S32)*psErr++;

                    ac0 = ac0 + *psRcvQ++ * (S32)*psErr; // ErrQ
                    ac1 = ac1 - *psRcvI++ * (S32)*psErr++;
                }

                *psAdfI = _Srnd(ac0,15-tx);
                *psAdfQ = _Srnd(ac1,15-tx);

                psAdfI += 2;
                psAdfQ += 2;
                psErr  -= GAEC_BLKS * 2;
                psRcvI -= GAEC_BLKS + 1;
                psRcvQ -= GAEC_BLKS + 1;
		    }

            for (blk = 0; blk < GAEC_BLKS*2; blk++)
            {
                *psErr++ = _Smpy(sDexp, *psErr);
            }
            psErr -= GAEC_BLKS*2;
        } // for: sections
	}
}