lecas.cpp

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

#if ! defined(_dsp)
#include <stdio.h>
#include <conio.h>
#include <math.h>
#endif

#include "leci.h"

/*-------------------------------------------------------------------------*/
void                        lec_cancel_pkt
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc,
S16 *psTo
)
{
	U16 Sample;
	S16 Tap;
   	S16 *pData = pDb->psHst + LEC_FLT_SZ;
	for (Sample = 0; Sample < ILEC_FR_SZ; Sample++)
	{
		long Sum = 0;
		S16 *pFlt = pDb->asAdf1 + ILEC_FR_SZ;
		for(Tap = 0; Tap < LEC_FLT_SZ; Tap++)
		{
			long Data = (*pData--);
			Sum -= Data * *pFlt++;
		}
        Sum >>= pDb->Adf1.sShift;
        Sum += ((S32)pSc->psRx[Sample]) << 15;
		psTo[Sample] = (S16)((Sum + 0x4000 ) >> 15);
		pData += LEC_FLT_SZ+1; /* return, and next sample */
	}
}

/*-------------------------------------------------------------------------*/
S16                         lec_cancel
/*-------------------------------------------------------------------------*/
(
S16 *psHst,
S16 *psFlt,
S16  sRxIn
)
{
	S32 ac0 = ((S32)(sRxIn))<<15;
    S16 k;
    for(k = 0; k < LEC_FLT2_SZ; k++)
	{
	    ac0 -= (*psHst--) * (S32) *psFlt++;
    }
    ac0 += 0x4000;
    return (S16)(ac0 >> 15);
}

/*-------------------------------------------------------------------------*/
S16                         lec_cancel_shifted
/*-------------------------------------------------------------------------*/
(
S16 *psHst,
S16 *psFlt,
S16  sRxIn,
S16 sShift
)
{
	S32 ac0 = ((S32)(sRxIn))<<(15+sShift);
    S16 k;
    for(k = 0; k < LEC_FLT2_SZ; k++)
	{
	    ac0 -= (*psHst--) * (S32) *psFlt++;
    }
    ac0 += 0x4000;
    return (S16)(ac0 >> 15);
}


/*-------------------------------------------------------------------------*/
S32                         lec_get_xz
/*-------------------------------------------------------------------------*/
(
S16 *psX,
S16 *psZ
)
{
    S32 lsEn = 0;
    S32 acc;
    S16 k;
    S16 i;
    for (i = 0; i < 2; i++)
    {
	    acc = 0;
	    for (k = 0; k < ILEC_FR_SZ/2; k++, psX--, psZ--)
	    {
	        acc += *psX * (S32) *psZ;
	    }
	    lsEn += acc>>4;
    }
    
    acc = 0;
    for (k = 0; k < ILEC_FR_SZ/2; k++, psX--, psZ--)
    {
        acc += *psX * (S32) *psZ;
    }
    lsEn += acc>>2;
    acc = 0;
    for (k = 0; k < ILEC_FR_SZ/2; k++, psX--, psZ--)
    {
        acc += *psX * (S32) *psZ;
    }
    lsEn += acc>>3;
    acc = 0;
    for (k = 0; k < ILEC_FR_SZ/2; k++, psX--, psZ--)
    {
        acc += *psX * (S32) *psZ;
    }
    lsEn += acc>>4;
    acc = 0;
    for (k = 0; k < ILEC_FR_SZ/2; k++, psX--, psZ--)
    {
        acc += *psX * (S32) *psZ;
    }
    lsEn+= acc>>5;
	for (i = 0; i < 4; i++)
	{
		acc = 0;
	    for (k = 0; k < ILEC_FR_SZ/2; k++, psX--, psZ--)
	    {
	        acc += *psX * (S32) *psZ;
	    }
	    lsEn += acc>>6;
    }
    return lsEn;
}
/*-------------------------------------------------------------------------*/
S32                         lec_get_xz_slow
/*-------------------------------------------------------------------------*/
(
S16 *psX,
S16 *psZ
)
{
    S32 slEn = 0;
    S16 k;
    S16 n;
    
    for (n = 0; n < LEC_HST_SZ/(ILEC_FR_SZ/2);n++)
    {
    	S32 slAcc = 0;
    	for (k = 0; k < ILEC_FR_SZ/2; k++)
    	{
        	slAcc += *psX++ * (S32) *psZ++;
    	}
    	slEn += slAcc>>2;
    }
    return slEn;
}

/*-------------------------------------------------------------------------*/
S32                         lec_update_xz
/*-------------------------------------------------------------------------*/
(
S32 lsEn,
S16 *psX,
S16 *psZ
)
{
    lsEn += (*psX * (S32) *psZ)>>4;
    psX -= ILEC_FR_SZ;
    psZ -= ILEC_FR_SZ;
    lsEn -= (*psX * (S32) *psZ)>>4;

    lsEn += (*psX * (S32) *psZ)>>2;
    psX -= ILEC_FR_SZ/2;
    psZ -= ILEC_FR_SZ/2;
    lsEn -= (*psX * (S32) *psZ)>>2;

    lsEn += (*psX * (S32) *psZ)>>3;
    psX -= ILEC_FR_SZ/2;
    psZ -= ILEC_FR_SZ/2;
    lsEn -= (*psX * (S32) *psZ)>>3;

    lsEn += (*psX * (S32) *psZ)>>4;
    psX -= ILEC_FR_SZ/2;
    psZ -= ILEC_FR_SZ/2;
    lsEn -= (*psX * (S32) *psZ)>>4;

    lsEn += (*psX * (S32) *psZ)>>5;
    psX -= ILEC_FR_SZ/2;
    psZ -= ILEC_FR_SZ/2;
    lsEn -= (*psX * (S32) *psZ)>>5;

    lsEn += (*psX * (S32) *psZ)>>6;
    psX -= ILEC_FR_SZ*2;
    psZ -= ILEC_FR_SZ*2;
    lsEn -= (*psX * (S32) *psZ)>>6;
    return lsEn;
}
/*-------------------------------------------------------------------------*/
void                        lec_shift_adf2
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    S16 *psFlt = pDb->asAdf2;
    S16 sMax = 0;
    S16 k;

    for (k = 0; k < LEC_FLT2_SZ; k++)
    {
        if (sMax < abs(psFlt[k]))
            sMax = abs(psFlt[k]);
    }

    sMax += (sMax>>1);

         if (sMax > (1<<14)) pDb->Adf2.sShift = 0;
    else if (sMax > (1<<13)) pDb->Adf2.sShift = 1;
    else if (sMax > (1<<12)) pDb->Adf2.sShift = 2;
    else if (sMax > (1<<11)) pDb->Adf2.sShift = 3;
    else pDb->Adf2.sShift = 4;
    
    pDb->Adf2.sNegShift = -pDb->Adf2.sShift;

    for (k = 0; k < LEC_FLT2_SZ; k++)
    {
        psFlt[k] <<= pDb->Adf2.sShift;
    }
}
/*-------------------------------------------------------------------------*/
S16                         lec_step_size
/*-------------------------------------------------------------------------*/
(
S32 slEn,
S16 *psStepSize
)
{
	S16 uEn = 0;
	
    if (slEn > (1L<<19))
    {
        *psStepSize = ((S16)(1*0x8000L/((slEn>>17) + 1)) - 1);
        uEn = 1;
    }
    else
    {

        if (slEn > (1L<<9))
        {
            *psStepSize = (S16)(1*0x8000L/((slEn>>7) + 1)) - 1;
            uEn = 2;
        }
        else
        {
            *psStepSize = 0;
            uEn = 0;
        }
    }
    return uEn;
}
/*-------------------------------------------------------------------------*/
void                        lec_adapt_fast_high
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc,
S16 base,
S16 SSS
)
{
	S16 *pData;
	S16 *psFlt;
	S16 Tap;
	S16 k;
	S32 slNormErr;
	
/* NORMALIZE ERROR */
    S32 Tmp = ((pSc->asErr2[base] * (long)SSS + 0x8) >> 4);
    if (Tmp > 32767) Tmp = 32767;
    if (Tmp < -32768) Tmp = -32768;
    pSc->asNormErr[base] = (S16)Tmp;
/* ADAPT */
    pData = pDb->psTxF+ LEC_FLT2_SZ + base;
    psFlt = pDb->asAdf2;

    slNormErr = ((S32)pSc->asNormErr[base])<<(16-2);
    slNormErr += 0x8000L;
    slNormErr >>= 16;

    for (Tap = 0; Tap < ILEC_FR_SZ; Tap += 1)
    {
	    S32 Sum = (((S32)*psFlt) << 15) + 0x4000L;
    	    Sum += slNormErr * *pData--;

    /*    *pFilter += (S16)(Sum/ 0x8000L);  correct coeff */

        *psFlt++ = (S16)(Sum >> 15);
    }

    
    for (k = 0; k < 4; k++)
    {
	    slNormErr = ((S32)pSc->asNormErr[base])<<(16-k);
	    slNormErr += 0x8000L;
	    slNormErr >>= 16;
        for (Tap = 0; Tap < ILEC_FR_SZ/2; Tap += 1)
        {
	        S32 Sum = (((S32)*psFlt) << 15) + 0x4000L;
    	        Sum += slNormErr * *pData--;

        /*    *pFilter += (S16)(Sum/ 0x8000L);  correct coeff */

            *psFlt++ = (S16)(Sum >> 15);
        }
        
    }
    slNormErr = ((S32)pSc->asNormErr[base])<<(16-4);
    slNormErr += 0x8000L;
    slNormErr >>= 16;
    for (Tap = 0; Tap < ILEC_FR_SZ*2; Tap += 1)
    {
	    S32 Sum = (((S32)*psFlt) << 15) + 0x4000L;
    	    Sum += slNormErr * *pData--;

    /*    *pFilter += (S16)(Sum/ 0x8000L);  correct coeff */

        *psFlt++ = (S16)(Sum >> 15);
    }
}
/*-------------------------------------------------------------------------*/
void                        lec_adapt_fast_low
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc,
S16 base,
S16 SSS
)
{
	S16 *pData;
	S16 *psFlt;
	S16 Tap;
	S16 k;
	S32 slNormErr;
	
    S32 Tmp = pSc->asErr2[base] * (long)SSS;
    if (Tmp > 32767) Tmp = 32767;