gaeca2s.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)
#define _DEXP           (Q15(0.7))

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

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_get_energy2
#else
void                        gaec_get_energy2C
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
S16 blk
)
{
    int band;
    int iRcv = GAEC_ADF_BSZ - GAEC_OFF2 + 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->aslEn2[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->aslEn2[band] = ac0;

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

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_dc2p
#else
void                        gaec_adapt_dc2pC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 blk,
S16 band
)
{
    GAEC_tScA2 *pSca = pSc->u.aA2;

    S32 ac0;
    S32 ac1;
    S16 tx;
    S16 sStepSz = pSc->asSSC[band];

    if (sStepSz == 0) 
    {
//        pSca->sErrI = 0;
        return;
    }

    ac0 = pDb->aslEn2[band];
    gaec_add_energy(pDb, band, &ac0);
    tx = _invert(&ac0, sStepSz);
    
    // decrease _ADJSS for DC bands ... i.e. step size/2.
    tx -= (14-_ADJSS); // used for loading *psAdf
	tx += pDb->asShft[band];
    // tx should be: 0 <= tx <= 15

    if (tx > 15)
    {
        ac1 = ac0 * pSc->aasErr[band][blk];;
        pSca->sErrI = _Srnd(ac1,15-tx);
        tx = 15;
    }
    else
    {
        if (tx > 0)
        {
            ac1 = ac0 * pSc->aasErr[band][blk];;
            pSca->sErrI = _Srnd(ac1,0);
        }
        else
        {
            ac1 = ac0 * pSc->aasErr[band][blk];;
            pSca->sErrI = _Srnd(ac1,-tx);
            tx = 0;
        }
    }
    pSca->sTx = tx;
}

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_dc2d
#else
void                        gaec_adapt_dc2dC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 blk,
S16 band
)
{
    GAEC_tScA2 *pSca = pSc->u.aA2;
    S32 ac0;
    S16 tx = pSca->sTx;
    S16 *psErr = &pSca->sErrI;

    int iRcv = GAEC_ADF_BSZ - GAEC_OFF2 + blk;
    S16 *psRcvI = &(pDb->psRcv[GAEC_RCV_SZ*band + iRcv]);
    S16 *psAdfI = &(pDb->psAdf[GAEC_OFF2*2]);
    int tap;
    int section;

    if (band != 0) 
        psAdfI++;

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

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

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

            psAdfI += 2;
		}

        *psErr = _Smpy(_DEXP, *psErr);
    }
}

/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_ac2p
#else
void                        gaec_adapt_ac2pC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 blk
)
{
    GAEC_tScA2 *pSca = pSc->u.aA2;
    S32 ac0;
    S32 ac1;
    S16 tx;
    int band;

    	
	for (band = 1; band < GAEC_BANDS; band++, pSca++)
	{
        S16 sStepSz = pSc->asSSC[band];
        if (sStepSz == 0)
        {
        	continue;
        }
        
        ac0 = pDb->aslEn2[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)
        {
            ac1 = ac0 * pSc->aasErr[band][blk];;
            pSca->sErrI = _Srnd(ac1,15-tx);
            ac1 = ac0 * pSc->aasErr[band+GAEC_BANDS][blk];
            pSca->sErrQ = _Srnd(ac1,15-tx);
            tx = 15;
        }
        else
        {
            if (tx > 0)
            {
                ac1 = ac0 * pSc->aasErr[band][blk];;
                pSca->sErrI = _Srnd(ac1,0);
                ac1 = ac0 * pSc->aasErr[band+GAEC_BANDS][blk];
                pSca->sErrQ = _Srnd(ac1,0);
            }
            else
            {
                ac1 = ac0 * pSc->aasErr[band][blk];;
                pSca->sErrI = _Srnd(ac1,-tx);
                ac1 = ac0 * pSc->aasErr[band+GAEC_BANDS][blk];
                pSca->sErrQ = _Srnd(ac1,-tx);
                tx = 0;
            }
        }

        pSca->sTx = tx;
        pSca->sDexp = _DEXP;
    }
}    
/*-------------------------------------------------------------------------*/
#if !defined(_dsp)
void                        gaec_adapt_ac2d
#else
void                        gaec_adapt_ac2dC
#endif
/*-------------------------------------------------------------------------*/
(
GAEC_tDb *pDb,
GAEC_tSc *pSc,
S16 blk
)
{
    GAEC_tScA2 *pSca = pSc->u.aA2;
    S32 ac0;
    S32 ac1;
    int band;

    for (band = 1; band < GAEC_BANDS; band++)
	{
        int iRcv = GAEC_ADF_BSZ - GAEC_OFF2 + blk;
        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_OFF2*2]);
        S16 *psAdfQ = psAdfI + 1;
        S16 tx = pSca->sTx; 
        S16 *psErr = &pSca->sErrI;
        S16 sDexp = pSca->sDexp;

        int tap;
        int section;


        for (section = 0; section < GAEC_SECTIONS2; 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--;

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

                psAdfI += 2;
                psAdfQ += 2;
		    }

            *psErr++ = _Smpy(sDexp, *psErr);
            *psErr-- = _Smpy(sDexp, *psErr);
        }
        pSca++;
	}
}