lec.c

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

/*-------------------------------------------------------------------------*
 *                                                                         *
 *   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
 *   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
 *      "COPYRIGHT 2001 MICHAEL TSIROULNIKOV, ALL RIGHTS RESERVED"         *
 *                                                                         *
 *-------------------------------------------------------------------------*/


#if !defined _dsp
#include <memory.h>
#include <math.h>
#include <stdlib.h>
#else
#include <string.h> /* memcpy() */
#endif

#include "stddefs.h"
#include "leci.h"

/*--------------------- local defs ----------------------------------------*/


/* the min erl we expect to be at least is 3 dB  */
#define _ERL_OK_CNT             (3)
#define _ERL_MIN                LEC_DB(3)
#define _ERL_MAX                LEC_DB(25)
#define _ERL_DIFF               LEC_DB(0.5)
#define _ERL_STAB_CNT           (5)

#define _MSE_MIN                LEC_DB(-8)

/* for delay upon transition into fe state */
#define _TX_STATE_DELAY         3
#define _TX_COUNTER_MAX         (200 *2)

#define _EN_VLOW                LEC_DB(25)
#define _EN_VHIGH               LEC_DB(-17)

/* only reused constants */
#define _ZC_NOISE_START 		LEC_DB(15)

/* definition of events */
#define _VOICE_OFF              0 /* activity definetely ended */
#define _VOICE_ON               1 /* activity definetely started */
#define _VOICE_MAYBE            2 /* suspicious activity started */

#define abs(x)                  (((x)>0)? (x):(-x))


/*--------------------- public vars ---------------------------------------*/

/*--------------------- local vars ----------------------------------------*/

#if defined (_dsp)
#endif

/*--------------------- local functions -----------------------------------*/
/*-------------------------------------------------------------------------*/
void                        _vad_init
/*-------------------------------------------------------------------------*/
(
LEC_tVad *p,
S16 sEnNoise
)
{
    p->sZcNoise = _ZC_NOISE_START;
	p->sEnNoise = sEnNoise;
    p->sEnNoiseMax = sEnNoise;
}
/*-------------------------------------------------------------------------*/
S16                         _vad_process
/*-------------------------------------------------------------------------*/
(
LEC_tVad *pVad,
S16 *psData
)
{
    S16 sState = pVad->sState;

    pVad->slEnergy = lec_pkt_energy(psData);
    pVad->sEn = lec_en2log(pVad->slEnergy, -1024);
	pVad->sZc = lec_pkt_zc(psData);
    
    lec_vad_logic(pVad);

    return (pVad->sState - sState);
}

/*-------------------------------------------------------------------------*/
static void                     _detect_high_tx
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    if (pDb->Cfg.uControl & ILEC_CMD_DETECT_HITX_MASK)
    {
//        S16 sTxMax = lec_pkt_peak(pDb->psHst + (LEC_HST_SZ - ILEC_FR_SZ));
        S16 sTxMax = lec_pkt_peak(pSc->psTx);

	    if (sTxMax > pDb->Cfg.sTxMax)
        {
            pDb->sTxMaxCnt = 6;
        }
        else
        {
            pDb->sTxMaxCnt--;
            if (pDb->sTxMaxCnt < 0)
            {
                pDb->sTxMaxCnt = 0;
            }
        }
    }
    else
    {
        pDb->sTxMaxCnt = 0;
    }

    if (pDb->sTxMaxCnt > 0)
        pSc->uFlags |= LEC_FLAG_HITX;
}
/*-------------------------------------------------------------------------*/
static void                     _detect_tone
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    S16 sState = pDb->VadTx.sState;
    S16 sLastTxEn = lec_en2log(lec_pkt_energy(pSc->psTx), -1024);
    U16 uLow, uHigh, uEven, uEdge, uStateChange;

    _vad_process(&pDb->VadTx, 
                 &pDb->psHst[LEC_HST_SZ-2*ILEC_FR_SZ-10]);

    uLow =  ((sLastTxEn - pDb->asTxEnLog[6]) > _EN_VLOW) &&
	    	    ((sLastTxEn - pDb->asTxEnLog[5]) > _EN_VLOW) &&
		        ((sLastTxEn - pDb->asTxEnLog[4]) > _EN_VLOW);

    uHigh = (pDb->asTxEnLog[1] > _EN_VHIGH-LEC_DB(6)) &&
                (pDb->asTxEnLog[0] > _EN_VHIGH) &&
				(sLastTxEn         > _EN_VHIGH);

    uEven = (abs(sLastTxEn - pDb->asTxEnLog[0]) < LEC_DB(6)) &&
                (abs(sLastTxEn - pDb->asTxEnLog[1]) < LEC_DB(12));

    uEdge = (uLow && uHigh && uEven);

    uStateChange = (pDb->VadTx.sState == LEC_VAD_ST_ACTIVE) &&
                       (sState != LEC_VAD_ST_ACTIVE);

    /* singularity test */
    if (pSc->sJ0Tx < pDb->Cfg.sToneDetectThr)
    {
        if (pSc->asK[2] < Q15(-0.65))
        {
            pSc->uFlags |= LEC_FLAG_SINGULAR;
        }
        else
        {
            if ((pSc->asK[4] < Q15(-0.65)) &&
                (pSc->asK[1] < Q15(0.5)))
            {
                pSc->uFlags |= LEC_FLAG_SINGULAR;
            }
        }
    }

    /* if state change or a sharp edge */
    if (uStateChange || uEdge)
    {
        pDb->uIsTone = (pSc->uFlags & LEC_FLAG_SINGULAR)? TRUE : FALSE;
    }

    /* when to release */
    if (pDb->uIsTone)
    {
        if (pSc->sJ0Tx > pDb->Cfg.sToneReleaseThr)
        {
            pDb->uIsTone = FALSE;
        }

        if (sState != LEC_VAD_ST_ACTIVE)
        {
            pDb->uIsTone = FALSE;
        }
    }
    /* delay energy array */
    {
        S16 i;
        S16 *p1 = &(pDb->asTxEnLog[6]);
        S16 *p2 = p1 - 1;
    	for (i = 6; i > 0; i--)
    	{
            *p1-- = *p2--;
    	}
        *p1 = sLastTxEn;
    }
}
/*-------------------------------------------------------------------------*/
static void                     _analyse_tx
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
	lec_tx_moments(pDb, pSc);

    lec_ts_moments(pDb, pSc);
    lec_normalise_rk(pDb, pSc, -7, (1<<2));
    pSc->sJ0Tx = lec_durbin(pDb, pSc);
#if !defined(_dsp)
    {
        extern void lec_JKS(void);
        lec_JKS();
    }
#endif

    _detect_tone(pDb, pSc);
    _detect_high_tx(pDb, pSc);

    lec_gs_moments(pDb, pSc);
    lec_normalise_rk(pDb, pSc, -4, (1<<4));
    pSc->sJ0 = lec_durbin(pDb, pSc);
#if !defined(_dsp)
    {
        extern void lec_JKW(void);
        lec_JKW();
    }
#endif
	lec_gs_filter(pDb, pSc);
}
/*-------------------------------------------------------------------------*/
void                            _test_erl
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    U16 uErlOk;

    pSc->sRxEn = lec_en2log(lec_pkt_energy(pSc->psRx),-1024);
    pSc->sErl = pDb->VadTx.sEn - pSc->sRxEn;

    /*  this frame ErlOk tells us if the signal can be an echo.
        if ErlOk is false, then it is definetely not an echo.
        although, it may be DT - we do not know yet.
     */
    if (pDb->Adf1.uConverged)
    {
        pSc->sErlEst = (pDb->Adf1.sErlAveraged + pDb->Adf1.sERL)/2;
        uErlOk = ((pSc->sErl - pSc->sErlEst) > LEC_DB(-6));
    }
    else // not converged yet.
    {
        pSc->sErlEst = pDb->Cfg.sErlMin;
        if (pDb->Adf2.sERL > pSc->sErlEst)
        {
            pSc->sErlEst = (pSc->sErlEst + pDb->Adf2.sERL)/2;
        }
        uErlOk = (pSc->sErl > pSc->sErlEst);
    }
    /*  we do not want to adapt on short spikes. there are very big chances
        that it will bring no good at all, so we delay the start of
        adaptation till we know for sure that it is not a transient process.

        we end adaptation as soon as we see active RX. actually, it is
        usually preceeded by a (possibly, very short) DT period 
        as a transition between TX and RX dominances.
     */
    if (uErlOk)
    {
        pDb->sErlOkCnt++;
        if (pDb->sErlOkCnt > _ERL_OK_CNT)
        {
            pDb->sErlOkCnt = _ERL_OK_CNT;
            pSc->uFlags |= LEC_FLAG_ERL_OK;
        }
    }
    else 
    {
#if 0
        if (pDb->sErlOkCnt == _ERL_OK_CNT)
        {
            // transition to rx, test adf2.
        }
        pDb->sErlOkCnt = 0;
#else
        pDb->sErlOkCnt--;
        if (pDb->sErlOkCnt < 0)
            pDb->sErlOkCnt = 0;
#endif
    }
}

/*-------------------------------------------------------------------------*/
static void                     _estimate_expected_error
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    S16 sSuppression = pDb->Adf2.sErleAveraged;
    // minus effect of possible misconvergence
    // adf2.slacc is always bigger than adf1.slacc.
    S16 sConvLack = lec_en2log(LEC_SLACC_MAX,0) - // this is a const
                    lec_en2log(pDb->Adf2.slAcc,0);

    // subtraction of entire ConLack is too much...
    sSuppression -= sConvLack/4;
    if (sSuppression < 0)
        sSuppression = 0;

    pSc->sExpectedErrorEn = pSc->sRxEn - sSuppression;
}

/*-------------------------------------------------------------------------*/
void                            _set_update_flt_mode
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    S32 slEn0;
    S32 slEn1;
    S16 sEn0;
    S16 sEn1;
    S16 k;

    slEn0 = lec_pkt_energy(pDb->psFlt2);

    slEn1 = 0;
    for (k = 0; k < LEC_FRAMES; k++)
    {
        slEn1 += lec_pkt_energy(pDb->psFlt2 + k * ILEC_FR_SZ);
    }
    
    sEn0 = -lec_en2log(slEn0,-1024-350);
    sEn1 = -lec_en2log(slEn1,-1024-350); // this is ERL

    // if MSE is ok, then this sERL is better to use than sErlAveraged
    pSc->sERL = sEn1 + pDb->Adf2.sShift*1024; // <<10;

    pSc->sMSE = sEn1 - sEn0 + LEC_DB(6); // < 0 if OK, and  shall be much < 0

/// ERL stabilization 

    lec_avrg(&pDb->Adf2.sERL,pSc->sERL, Q15(0.1));

    if ((pDb->Adf2.sERL < _ERL_MAX) &&
        (pSc->sERL      < _ERL_MAX) &&
// what if double talk -- then pSc->sErl will be off... ????
        (abs(pSc->sERL - pDb->Adf2.sERL) < _ERL_DIFF))
    {
        pDb->Adf2.sStabCnt++;
        if (pDb->Adf2.sStabCnt > _ERL_STAB_CNT)
        {
            pDb->Adf2.sStabCnt = _ERL_STAB_CNT;
            // we are in ERL stable state
        }
    }
    else /// a change happened
    {
        pDb->Adf2.sStabCnt = 0;
        // we are back to ERL unstable state.
    }

// actions:
    // if ERL is still unstable
    if (pDb->Adf2.sStabCnt < _ERL_STAB_CNT)
    {
        // if erl has not stabilized yet, do not copy in any case
        pSc->uFltCpyMode = 3;
        lec_avrg(&pDb->Adf2.sMSE, pSc->sMSE, Q15(0.25));
        pSc->uMseMode = 0;
    }
    else // erl has stabilized
    {
        pSc->uMseMode = 1; // dbg

        // if not converged yet
        if (pDb->Adf1.sMSE > _MSE_MIN)
        {
            lec_avrg(&pDb->Adf2.sMSE, pSc->sMSE, Q15(0.125));

            // if MSE is somethings more or less real
            if ((pDb->Adf2.sMSE < 0) &&           
                // if flt2 is better than flt 1 in terms of MSE
                (pDb->Adf2.sMSE < pDb->Adf1.sMSE) && 
                // flt 2 is still ok
                (pSc->sMSE < pDb->Adf2.sMSE) && 
                // flt 2 is better than flt 1 in terms of ERLE
                (pDb->sErrOverrideCnt > LEC_DB(10)))
            {
                pSc->uFltCpyMode = 1;
            }
            else // keep adapting
            {
                pSc->uFltCpyMode = 3;
            }
        }
        else // may be converged 
        {
            pSc->uMseMode |= 2;
            if (pDb->uIsDT)
            {
                lec_avrg(&pDb->Adf2.sMSE, pSc->sMSE, Q15(0.25));
                pSc->uFltCpyMode = 3;
                // no copy, keep converging, unless convergence is very good.
                if (pDb->sErrOverrideCnt > LEC_DB(20))
                {
                    S16 sErleDlt = pDb->sErrOverrideCnt;
                    S16 sMseDlt = pDb->Adf2.sMSE - pDb->Adf1.sMSE;
                    sErleDlt = (S16)((Q15(0.40) * (S32)sErleDlt)>>15);

                    if ((sErleDlt - sMseDlt) > 0)
                    {
                        pSc->uFltCpyMode = 1;
                    }
                }
                else if (pDb->sErrOverrideCnt > LEC_DB(10))
                {
                    // flt2 is at least as good as flt1 in terms of MSE
                    if ((pDb->Adf2.sMSE < (pDb->Adf1.sMSE + LEC_DB(0.25))) && 
                    // current flt2 is good
                        (pSc->sMSE < pDb->Adf2.sMSE))
                    {
                        pSc->uFltCpyMode = 1;
                    }
                }
            }
            else // no double talk recognized
            {
                lec_avrg(&pDb->Adf2.sMSE, pSc->sMSE, Q15(0.05));