gaeca3s.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            (2)
#define _ADJSS          (_ADJ+1)
#define _DEXP           (Q15(0.8))

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

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_get_energy3
#else
void                        gaec_get_energy3C
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
S16 blk
)
{
    int band;
    int iRcv = GAEC_ADF_BSZ - GAEC_OFF3 + blk;
    S16 *psRcvI  = &(pDb->psRcv[iRcv]);
    S16 *psRcvQ = psRcvI + (GAEC_BANDS * GAEC_RCV_SZ);

    S32 ac0;
    S32 ac1;

    // max gain of 1.25 / [1 -1+1/32] is 32 dB (~2^5)
    // max Rcv = 2^13, Rcv^2 = 2^26.
    // max En = 2^(26+5) = 2^(31); need to scale it 2 down
    // ... if we scale Rcv up, we should scale En down...
    for (band = 0; band < GAEC_BANDS+1; band++)
    {
        ac0 = pDb->aslEn3[band];
        ac0 -= ac0 >> 5;
        ac1 = (*psRcvI * (S32)*psRcvI);

        if ((band != 0) && (band != GAEC_BANDS))
        {
            ac1 += (*psRcvQ * (S32)*psRcvQ);
        }
        ac1 += ac1 >> 2; // * 1.25
        ac0 += ac1 >> _ADJ;
        pDb->aslEn3[band] = ac0;

        psRcvI += GAEC_RCV_SZ;
        psRcvQ += GAEC_RCV_SZ;
    }
}

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_dc3p
#else
void                        gaec_adapt_dc3pC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 sBlk,
S16 band
)
{
    GAEC_tScA3 *pA = pSc->u.aA3;
    S32 ac0;
    S32 ac1;
    int blk;
    S16 tx;

    S16 sStepSz = pSc->asSSC[band];

    if (sStepSz == 0)
    {
        pA->asErr[0] = 0;
        pA->asErr[1] = 0;
        return;
    }
        
    ac0 = pDb->aslEn3[band];
    gaec_add_energy(pDb, band, &ac0);
    tx = _invert(&ac0, sStepSz);

    tx -= (15-_ADJSS); // used for loading *psAdf
	tx += pDb->asShft[band];


    if (tx > 15)
    {
        pA->sTx = 15;
        for (blk = 0; blk < 2; blk++)
        {
            ac1 = ac0 * pSc->aasErr[band][sBlk+blk];;
            pA->asErr[blk] = _Srnd(ac1,15-tx);
        }
    }
    else
    {
        if (tx > 0)
        {
            pA->sTx = tx;
            for (blk = 0; blk < 2; blk++)
            {
                ac1 = ac0 * pSc->aasErr[band][sBlk+blk];;
                pA->asErr[blk] = _Srnd(ac1,0);
            }
        }
        else
        {
            pA->sTx = 0;
            for (blk = 0; blk < 2; blk++)
            {
                ac1 = ac0 * pSc->aasErr[band][sBlk+blk];;
                pA->asErr[blk] = _Srnd(ac1,-tx);
            }
        }
    }
}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_dc3d
#else
void                        gaec_adapt_dc3dC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 sBlk,
S16 band
)
{
	int iRcv = GAEC_ADF_BSZ - GAEC_OFF3 + sBlk;
    S16 *psRcvI = &(pDb->psRcv[GAEC_RCV_SZ*band + iRcv]);
    S16 *psAdfI = &(pDb->psAdf[GAEC_OFF3*2]);
    GAEC_tScA3 *pA = pSc->u.aA3;
    S16 *psErr = pA->asErr;
    S16 tx = pA->sTx;
    S16 sDexp = _DEXP;
    int tap;
    int section;

    if (band != 0) 
        psAdfI++;
        
    for (section = 0; section < GAEC_SECTIONS3; section++)
    {
		for (tap = 0; tap < GAEC_SECTION_SZ; tap++)
		{
            S32 ac0 = ((S32)*psAdfI)<<tx;

            ac0 = ac0 + *psRcvI++ * (S32)*psErr++;
            ac0 = ac0 + *psRcvI-- * (S32)*psErr--;

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

            psAdfI += 2;
            psRcvI --;
		}

        *psErr++ = _Smpy(sDexp, *psErr);
        *psErr-- = _Smpy(sDexp, *psErr);
    }
}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_ac3p
#else
void                        gaec_adapt_ac3pC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 sBlk
)
{
    GAEC_tScA3 *pA = pSc->u.aA3;
    S32 ac0;
    S32 ac1;
    int blk;
    S16 tx;
    int band;
    
	for (band = 1; band < GAEC_BANDS; band++, pA++)
	{
        S16 sStepSz = pSc->asSSC[band];

        if (sStepSz != 0)
        {
            ac0 = pDb->aslEn3[band];
            gaec_add_energy(pDb, band, &ac0);
            tx = _invert(&ac0, sStepSz);

            tx -= (15-_ADJSS); // stepSize = 0.5; used for loading *psAdf
			tx += pDb->asShft[band];

            if (tx > 15)
            {
        	    pA->sTx = 15;
                for (blk = 0; blk < 2; blk++)
                {
                    ac1 = ac0 * pSc->aasErr[band][sBlk+blk];
                    pA->asErr[blk*2+0] = _Srnd(ac1,15-tx);
                    ac1 = ac0 * pSc->aasErr[band+GAEC_BANDS][sBlk+blk];
                    pA->asErr[blk*2+1] = _Srnd(ac1,15-tx);
                }
            }
            else
            {
                if (tx > 0)
                {
            	    pA->sTx = tx;
                    for (blk = 0; blk < 2; blk++)
                    {
                        ac1 = ac0 * pSc->aasErr[band][sBlk+blk];
                        pA->asErr[blk*2+0] = _Srnd(ac1,0);
                        ac1 = ac0 * pSc->aasErr[band+GAEC_BANDS][sBlk+blk];
                        pA->asErr[blk*2+1] = _Srnd(ac1,0);
                    }
                }
                else
                {
            	    pA->sTx = 0;
                    for (blk = 0; blk < 2; blk++)
                    {
                        ac1 = ac0 * pSc->aasErr[band][sBlk+blk];
                        pA->asErr[blk*2+0] = _Srnd(ac1,-tx);
                        ac1 = ac0 * pSc->aasErr[band+GAEC_BANDS][sBlk+blk];
                        pA->asErr[blk*2+1] = _Srnd(ac1,-tx);
                    }
                }
            }
            pA->sDexp = _DEXP;
        }
    }
}
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_ac3d
#else
void                        gaec_adapt_ac3dC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 sBlk
)
{
    GAEC_tScA3 *pA = pSc->u.aA3;
    S32 ac0;
    S32 ac1;
    int iRcv = GAEC_ADF_BSZ - GAEC_OFF3 + sBlk;
    int band;

    for (band = 1; band < GAEC_BANDS; band++, pA++)
	{
        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_OFF3*2]);
        S16 *psAdfQ = psAdfI + 1;

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

    	int tap;
        int section;


        for (section = 0; section < GAEC_SECTIONS3; section++)
        {
		    for (tap = 0; tap < GAEC_SECTION_SZ; tap++)
		    {
                ac0 = ((S32)*psAdfI)<<tx;
                ac1 = ((S32)*psAdfQ)<<tx;

                ac0 = ac0 + *psRcvI * (S32)*psErr;
                ac1 = ac1 + *psRcvQ * (S32)*psErr++;

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

                ac0 = ac0 + *psRcvI * (S32)*psErr;
                ac1 = ac1 + *psRcvQ * (S32)*psErr++;

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

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

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

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

	} // for - bands
}