lecps.c

The line echo canceller (LEC) is designed to provide the maximum attainable transparent voice qualit

#include <math.h>

#include "leci.h"


#define SJ0_MIN (2048/16)



#if ! defined (_dsp)
S16 _asKk[6];
S16 _asJk[6];
S16 _asJkS[6];
S16 _asJkW[6];
S16 _asKkS[6];
S16 _asKkW[6];

/*-------------------------------------------------------------------------*/
void                            lec_JKS()
/*-------------------------------------------------------------------------*/
{
    S16 k;
    for (k = 0; k < 6; k++)
    {
        _asJkS[k] = _asJk[k];
        _asKkS[k] = _asKk[k];
    }
}
/*-------------------------------------------------------------------------*/
void                            lec_JKW()
/*-------------------------------------------------------------------------*/
{
    S16 k;
    for (k = 0; k < 6; k++)
    {
        _asJkW[k] = _asJk[k];
        _asKkW[k] = _asKk[k];
    }
}
#endif

/*-------------------------------------------------------------------------*/
S16                            lec_durbinC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    S16 k;
    S16 i;
    S16 sK;
    S32 ac0;
    S16 sJ0;

    for (k = 0; k < 6; k++)
    {
        pSc->asF[k] = 0;
        pSc->asK[k] = 0;
        pSc->aslG[k] = 0;
#if ! defined (_dsp)
        _asKk[k] = 0;
        _asJk[k] = 0;
#endif        
    }

    /* durbin iterations */

    /* iteration 0 */
    pSc->asF[0] = 32767;
    pSc->asK[0] = 32767;
    sJ0 = 32767;

#if ! defined (_dsp)
    _asJk[0] = 32767;
    _asKk[0] = 32767;
#endif
    /* iteration 1 */
    {
        S16 sK = pSc->asRk[1];
        ac0 = ((32767L<<15) - (sK * (S32) sK) + 0x4000)>>15;
        sJ0 = (S16)(ac0);
        pSc->asF[1] = -sK;
        pSc->asK[1] = sK;

#if ! defined (_dsp)
        _asKk[1] = sK;
        _asJk[1] = sJ0;
#endif        
        if (sJ0 < SJ0_MIN) return sJ0;
    }

    for (i = 2; i < 6; i++)
    {
        ac0  = ((S32)pSc->asRk[i])<<15;
        for (k = i-1; k > 0; k--)
        {
        	ac0 += pSc->asRk[k] * (S32)pSc->asF[i-k];
        }
        ac0 += 0x4000;
        ac0 >>= 15;
        if (abs(ac0) >= sJ0) return 0;

        sK = (ac0 * 0x8000L)/sJ0;
        pSc->asK[i] = sK;
        
        ac0 = ((32767L<<15) - sK * (S32) sK + 0x4000)>>15;
        sJ0 = (ac0 * sJ0 + 0x4000)>>15;

        for(k = 1; k < i; k++) 
            pSc->aslG[k] = pSc->asF[k] * (S32)sK;
        pSc->asF[i]  = -sK;
        for(k = 1; k < i; k++) 
        {
            ac0 = ((S32)pSc->asF[i-k])<<15;
            ac0 -= pSc->aslG[k];
            ac0 += 0x4000;
            ac0 >>= 15; 
            if (ac0 > 32767) ac0 = 32767;
            if (ac0 < -32768) ac0 = -32768;
            pSc->asF[i-k] = (S16)ac0;
        }

#if ! defined (_dsp)
        _asKk[i] = sK;
        _asJk[i] = sJ0;
#endif
		if (sJ0 < SJ0_MIN) return sJ0;
    }
    return sJ0;
}


/*-------------------------------------------------------------------------*/
void                    lec_normalise_rkC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc,
S16 sShift,
S16 sAdd
)
{
    S32 ac0;
    S16 exp;
    S16 sInvR0;
    S16 k;

    /* regularization */    
    ac0 = pSc->aslRk[0];
    ac0 += sAdd;
    ac0 += ac0>>(-sShift);

    /* normalization */

    for (exp = 0; exp < 32;)
    {
        if (ac0 & 0x40000000L) break;
        ac0 <<= 1;
        exp++;
    }
	pSc->asRk[0] = (S16)(ac0>>16);
    sInvR0 = (S16)((0x3fff*0x8000L)/pSc->asRk[0]);
    pSc->asRk[0] = 32767;
    
    for (k = 1; k< 6; k++) 
    {
        ac0 = (pSc->aslRk[k]<<exp)>>16;
	    ac0 = ac0 * sInvR0;
       	pSc->asRk[k] = (S16)(ac0>>14);
    }
}

/*-------------------------------------------------------------------------*/
void                    lec_tx_momentsC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
	S16 k;
	S16 *psTx = pSc->psTx; 
    S16 *psSav = pSc->asTmp;

    for (k = 0; k < 5; k++)
    {
        psSav[k] = pDb->psHst[LEC_HST_SZ-5+k];
    }
    for (k = 0; k < ILEC_FR_SZ; k++)
    {
        psSav[5+k] = psTx[k];
    }

    /* compute moments for the current frame */
    for (k = 0; k < 6; k++)
    {
        S32 ac0 = 0;
        S32 ac1 = 0;
        S16 i;

	    for (i = 0; i < ILEC_FR_SZ/2; i++)
	    {
		    ac0 += psTx[i]*((S32)(psSav[i-k+5]));
	    }
	    for (i = 0; i < ILEC_FR_SZ/2; i++)
	    {
		    ac1 += psTx[i+ILEC_FR_SZ/2]*((S32)(psSav[i+ILEC_FR_SZ/2+5-k]));
        }
        pSc->aslRkTx[k] = (ac0>>1) + (ac1>>1);
    }
}
/*-------------------------------------------------------------------------*/
void                    lec_gs_momentsC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
	S16 k;

    /* compute moments for the current frame */
    for (k = 0; k < 6; k++)
    {
        S32 ac0 = pSc->aslRkTx[k];
        pDb->aslRk[k] += (ac0 - pDb->aslRk[k])>>4;
        pSc->aslRk[k] = pDb->aslRk[k];
    }
}
/*-------------------------------------------------------------------------*/
void                    lec_gs_filterC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
	S16 i;
    for (i = 0; i < LEC_HST_SZ-ILEC_FR_SZ; i++)
    {
        pDb->psTxF[i] = pDb->psTxF[i+ILEC_FR_SZ];
    }

	for (i = LEC_HST_SZ-ILEC_FR_SZ; i < LEC_HST_SZ; i++)
	{
		S32 ac0 = 0;
        S16 k;
        for (k = 0; k < 6; k++)
        {
            ac0 += pDb->psHst[i-k] * (S32)pSc->asF[k];
        }
        ac0 += 0x4000;
        pDb->psTxF[i] = (S16)(ac0>>15);
	}
}

/*-------------------------------------------------------------------------*/
void                    lec_ts_momentsC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
	S16 k;
    S32 ac0;
    S32 ac1;

    for (k = 0; k < 6; k++)
    {
        ac0 = pSc->aslRkTx[k];
        ac1 = pDb->aslRkSav[k];
        pDb->aslRkSav[k] = ac0;
        pSc->aslRk[k] = ac0 + ac1;
    }
}

/*-------------------------------------------------------------------------*/
void                     lec_lp_residual_errorC
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc,
const S16 *psCoeff
)
{
    S16 k;

	for (k=0; k < 5; k++)
	{
		pSc->asTmp[k] = pDb->asLpSav[k];
	}
    for (k=0; k < ILEC_FR_SZ; k++)
    {
        pSc->asTmp[k+5] = pSc->asErr1[k];
    }
	for (k=0; k < 5; k++)
	{
		pDb->asLpSav[k] = pSc->asTmp[k+ILEC_FR_SZ];
	}
    for (k = 0; k < ILEC_FR_SZ; k++)
    {
        S16 i;
        S32 ac0 = 0;
        for (i = 0; i < 6; i++)
        {
            ac0 += pSc->asTmp[k+5-i] * (S32) psCoeff[i];
        }
        ac0 += 0x4000;
        ac0 >>= 15;
        pSc->asErr1[k] = (S16)ac0;
    }
}