lecus.c

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

#include "leci.h"

/* ------------------------------------------------------------------------ */
void		                    lec_avrgC
/* ------------------------------------------------------------------------ */
(
S16 *psVal, 
S16 sNew,
S16 sCoeff
)
/* Vk+1 = Vk + (y - Vk) * coeff/32768; */
{
    S32 acc;

    acc = (sNew - *psVal);
    acc *= sCoeff;
    acc += ((S32)(*psVal))<<15;
    acc += 0x4000;
    acc >>= 15;
    *psVal = (S16)acc;
}

/* ------------------------------------------------------------------------ */
void		                    lec_pkts_cpyC
/* ------------------------------------------------------------------------ */
(
S16 *psTo, 
S16 *psFrom,
S16 uPktsToCopy
)
{
     S16 k;
     for(k = 0; k < uPktsToCopy; k++)
     {
          U16 n; /* --- rpt reg */
          for(n = 0; n < ILEC_FR_SZ; n++)
               *psTo++ = *psFrom++;
     }
}   

#define MULT    (31821)
#define INC     (13849)
/* ------------------------------------------------------------------------ */
void                            lec_pkt_randC
/* ------------------------------------------------------------------------ */
(
S16 *psTo, 
S16 *psSeed
)
{
    S16 k;
    S16 t = MULT;
    S16 b = INC;
    for (k = 0; k < ILEC_FR_SZ; k++)
    {
        S32 acc = *psSeed * (S32) t;
        acc += b;
        *psTo++ = (S16) acc;
        *psSeed = (S16)acc;
    }
}
/* ------------------------------------------------------------------------ */
S32                             lec_pkt_energyC
/* ------------------------------------------------------------------------ */
(
S16 *psIn /* --- ar2, pointer to the start of array */
)
{
     S32 a = 0; /* --- acc */
     S32 b = 0; /* --- accb */
     U16 k;

     for (k = 0; k < ILEC_FR_SZ/2; k++, psIn++)
     {
          b += *psIn * (S32)*psIn;
     }
     for (k = 0; k < ILEC_FR_SZ/2; k++, psIn++)
     {
          a += *psIn * (S32)*psIn;
     }
     a >>= 2;
     a += (b>>2);
     return a;
}
/*-------------------------------------------------------------------------*/
S16                             lec_en2logC
/*-------------------------------------------------------------------------*/
(
S32 ac0,    /* but it is positive */
S16 sCorr
)
{
    S16 t0 = 0;

    ac0 += 1; // min val -13952
    // mant aco :: nexp ac0, t0
    for (;;)
    {
        if (ac0 &0x40000000L)
            break;
        ac0 <<= 1;
        t0--;
    }
    ac0 >>= 21;
    ac0 += 0x780;
    ac0 += t0*512;
    return (S16)(ac0 + sCorr); 
}
#if 0
/* ------------------------------------------------------------------------ */
S16                             lec_dB2Lin
/* ------------------------------------------------------------------------ */
(
S16 level /* in DB*100, shall be negative  */
)
{
	
    /* linear approximation, max err ~ 6% */
    if (level > 0) return 4096;
    if (level < -73*100) return 0;
    S32 acc = -level*109L;
	 /* number of right shifts in accH */
    /* reminder in accL */

    S16 rem = (S16)((acc & 0xffff)>>2);
    S16 val = 4096>>((S16)(acc >> 16));
    S16 mult = 0x7fff - rem;
    return (S16)((val * (S32)mult + 0x4000)>>15);

}
#endif
/* ------------------------------------------------------------------------ */
S16                             lec_expC
/* ------------------------------------------------------------------------ */
(
S16 log /* in dBm0*170.1 */
)
{
    S16 x, exp;
    S32 a = ((S32)log)<<2;
    S16 t = (S16)(a>>12);
    S16 d = a&4095;
    a = (d * (S32) d)>>13;
    a += d;
    a *= 21845;
    a >>= 15;
    a += 4096;
    x = (S16) a;/* 4096 + (d + (S16)((d*(S32)d)>>13))*0.66667; */

    if (t > 0) exp = x<<t; else exp = x>>(-t);
    return exp; 
}  

static S16 sign(S16 x){return (x>=0)?0:1;}
//static S16 abs(S16 x){return (x>0)?x:-x;}
/* ------------------------------------------------------------------------ */
S16                             lec_pkt_zcC
/* ------------------------------------------------------------------------ */
(
S16 *p
)
{
    S16 i;
    S16 Zc = 0;
    for(i=0; i < ILEC_FR_SZ-1; i++)
    {
        S16 s0 = sign(p[i]);
        S16 s1 = sign(p[i+1]);
        Zc += (s0 ^ s1);
    }
    Zc *= 170;
    return Zc;
}
/* ------------------------------------------------------------------------ */
S16                             lec_pkt_peakC
/* ------------------------------------------------------------------------ */
(
S16 *p
)
{
#define abs(x) (((x)>0)?(x):-(x))
     S16 i;
     S16 max = 0;
     for (i = 0; i < ILEC_FR_SZ; i++,p++)
     {
          S16 t = abs(*p);
          if(t > max) max = t;
     }
     return max;

}
/*-------------------------------------------------------------------------*/
S16                         lec_pkt_excessC
/*-------------------------------------------------------------------------*/
(
S16 *pError,
S16 ClipThreshold
)
{
	S16 i;
	S16 excess = 0;
	for (i = 0; i < ILEC_FR_SZ; i++)
	{
		if ((abs(pError[i])) > ClipThreshold)
			excess++;
	}
	return excess;
}
#if 0
S16 _asNoiseCoeff[] = {
 1863,    8263,    9761,    6297,    3846
/*2949,    8192,    14090,    8847, 0 */
};
#else
S16 _asNoiseCoeff[] = {
 12000,   9000,    6000,    3000,    5000
/*2949,    8192,    14090,    8847, 0 */
};
#endif
/*-------------------------------------------------------------------------*/
void                    lec_make_noiseC
/*-------------------------------------------------------------------------*/
(
S16 *psTo,
S16 *psFrom,
S16 sNoiseLevel
)
{
	S16 i;
	S32 acc = 0;
	S16 *psCoeff = &_asNoiseCoeff[0];

	for ( i = 0; i < ILEC_FR_SZ; i++ )
	{
		int k;

		acc = 0;
		for (k = 0; k < 5; k++)
		{
			acc += *psFrom++ * (S32) *psCoeff++;
		}
		psCoeff -= 5;
		psFrom -= 4;
		acc >>= 15;

		acc *= sNoiseLevel;
		acc += 0x4000;
		acc >>= 15;

		*psTo++ = (S16)acc;
	 }
}

/*-------------------------------------------------------------------------*/
void                lec_central_clippingC
/*-------------------------------------------------------------------------*/
(
S16 *pTo,
S16 *pFrom,
S16 Threshold
)
{
	S16 i;

	for ( i = 0; i < ILEC_FR_SZ; i++ )
	{
		S16 IsNegative = (*pFrom < 0) ? 1 : 0;
		S16 acc = *pFrom++;

		if (IsNegative) acc = -acc; /* abs */

        acc -= Threshold;
        if (acc < 0)
        {
            acc = 0;
        }
        if (IsNegative)
        {
            acc = -acc; /* neg */
        }
        acc += *pTo; /* noise in *pTo */
        *pTo++ = acc;
    }   /* end-of-for */
}