lec.c

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

                // flt 2 is much better than flt 1 in terms of ERLE
                if (pDb->sErrOverrideCnt > LEC_DB(10))
                {
                    pSc->uFltCpyMode = 1;
                }
                // flt 2 is somehow better than flt 1 in terms of ERLE
                else if ((pDb->sErrOverrideCnt > LEC_DB(5)) &&
                         // even if MSE1 and MSE 2 are about the same
                         (pDb->Adf2.sMSE < (pDb->Adf1.sMSE + LEC_DB(0.5))) && 
                         // current MSE is alright
                         (pSc->sMSE < pDb->Adf2.sMSE))
                {
                    pSc->uFltCpyMode = 1;
                }
                // flt 2 is better than flt 1 in terms of ERLE,
                // continuous update
                else if ((pDb->sErrOverrideCnt > LEC_DB(0.5)) &&
                         // and MSE
                         (pDb->Adf2.sMSE < pDb->Adf1.sMSE) && 
                         // and current MSE is alright
                         (pSc->sMSE < pDb->Adf2.sMSE))
                {
                    pSc->uFltCpyMode = 1;
                }
                else // no visible improvements
                {
                    // keep adapting
                    if (pSc->sMSE < (pDb->Adf1.sMSE + LEC_DB(5)))
                    {
                        pSc->uFltCpyMode = 3;
                    }
                    else // or reset back to 1: screwed up...
                    {
                        pSc->uFltCpyMode = 2;
                    }
                }
            } /* if : DT */
        } /* if : converged */
    } /* if: ERL stabilized */
}


/*-------------------------------------------------------------------------*/
void                        _convergence_control
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    pSc->sErle = pSc->sRxEn - pDb->VadErr.sEn;
    if (pSc->sErle > 0 && (!pDb->uIsDT))
    {
        S32 sl = pSc->sErle * (S32) (pSc->sJ0 - 5500);//4096);
        sl >>= (15+2);
        if (sl < 0) sl = 0;
        pDb->Adf2.slAcc +=  sl;

        if (pDb->Adf2.slAcc > LEC_SLACC_MAX/2)
        {
            if (pDb->Adf2.slAcc >= LEC_SLACC_MAX)
            {
                pDb->Adf2.slAcc = LEC_SLACC_MAX;
                if (pDb->Adf2.uConverged == 1) 
                {
                    lec_shift_adf2(pDb, pSc);
                }
                pDb->Adf2.uConverged = 2;
            }
            else
            {
                pDb->Adf2.uConverged = 1;
            }
        }
    }
}
/*-------------------------------------------------------------------------*/
void                            _compare_adfs
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    // if any adaptation happened
    if (pSc->uAdaptMode & 0x000c )
    {
        pSc->sErr2En = lec_en2log(lec_pkt_energy(pSc->asErr2), -1024);

        pDb->sErrOverrideCnt += (pSc->sErr1En - pSc->sErr2En)>>5;

        if (pSc->sErr2En < pSc->sErr1En)
        {
            pSc->uFlags |= LEC_FLAG_ERROVERIDE;
            /* prevent wrap-around */
            if (pDb->sErrOverrideCnt > LEC_DB(70))
                pDb->sErrOverrideCnt = LEC_DB(70);
            /* if DT, do not make actual copy...
               flt2 can get pretty crazy, doing all kinds of crazy stuff
               we collect ErrOverrideCnt and that's it. 
             */
            if (!pDb->uIsDT)
                lec_pkts_cpy(pSc->asErr1, pSc->asErr2, 1);
        }
        else  /* use error from cancellation */
        {
            /* limit from below, by a small negative value,
               to prevent spurious triggering of 2-1 copy.
             */
            if (pDb->sErrOverrideCnt < LEC_DB(-6))
                pDb->sErrOverrideCnt = LEC_DB(-6);
        }

        _set_update_flt_mode(pDb, pSc);
    }
    else ; // adaptation was off
}
/*-------------------------------------------------------------------------*/
void                            _update_flt
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    switch(pSc->uFltCpyMode)
    {
    case 1: // flt1 (main filter) update
        pDb->Adf1 = pDb->Adf2;
        pDb->sErrOverrideCnt = 0;
	    lec_pkts_cpy(
            pDb->psFlt1, 
            pDb->psFlt2, 
            LEC_FLT2_SZ/ILEC_FR_SZ
            );

        break;
    case 2: // flt2 override - it got corrupted 
        pDb->Adf2 = pDb->Adf1;
        // enforce re-test that ERL is OK, and insert a waiting period.
        pDb->Adf2.sStabCnt = -5;
        pDb->sErrOverrideCnt = 0;
	    lec_pkts_cpy(
            pDb->psFlt2, 
            pDb->psFlt1, 
            LEC_FLT2_SZ/ILEC_FR_SZ
            );
        break;
    case 3:
    default:
        // do not copy, but continue to adapt
        break;
    }
}
/*-------------------------------------------------------------------------*/
void                            _update_history
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
	/* shift old data in delay line one section down */
	lec_pkts_cpy(
        pDb->psHst, 
        pDb->psHst + ILEC_FR_SZ, 
        LEC_HST_FRAMES - 1);

	/* add the new section to the end of the delay line */
	lec_pkts_cpy(
        pDb->psHst + (LEC_HST_SZ - ILEC_FR_SZ), 
        pSc->psTx, 
        1);
}

/*-------------------------------------------------------------------------*/
void                            _ec_process
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    _analyse_tx(pDb, pSc);
    _test_erl(pDb, pSc);

    lec_cancel_pkt(pDb, pSc);
    pSc->sErr1En = lec_en2log(lec_pkt_energy(pSc->asErr1), -1024);

    _estimate_expected_error(pDb, pSc);

    pSc->sStepSize = 0;
   	pSc->uAdaptMode = 0;
    pSc->uFltCpyMode = 3;

    if ((!(pDb->Cfg.uControl & ILEC_CMD_NOADAPT_MASK)) &&
        (pSc->uFlags & LEC_FLAG_ERL_OK) &&
        (!pDb->uIsTone) &&
        (!(pSc->uFlags & LEC_FLAG_HITX)))
    {
        pSc->uFlags |= LEC_FLAG_ADAPT;

        if (!pDb->Adf2.uConverged)
        {
            pSc->uAdaptMode = 1;
            lec_adapt_fast(pDb, pSc);
        }
        else
        {
            pSc->uAdaptMode = 2;
            lec_adapt_slow(pDb, pSc);
        }
    }

    _compare_adfs(pDb, pSc);

	_vad_process(&pDb->VadErr, pSc->asErr1);
    _convergence_control(pDb, pSc);

    /* RxNoiseLevel is slowly rising from -80dBm */
    if (pDb->VadErr.sState == LEC_VAD_ST_IDLE)
    {
        // noise level shall not be higher than backgrownd noise,
        // the constant is empiric
        lec_avrg(&pDb->sRxNoiseLevel, pDb->VadErr.sEnNoise, Q15(0.01));
    }

    lec_double_talk_detector(pDb, pSc);
    lec_non_linear_processor(pDb, pSc);

    _update_flt(pDb, pSc);
    _update_history(pDb, pSc);
}

/*-------------------------------------------------------------------------*/
static void                     _update_statistics
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
LEC_tSc *pSc
)
{
    LEC_MIKET_tStts *pStts = pDb->pStts;
    U16 uFlags = pSc->uFlags;

    pStts->slFrames += 1;

    if (pDb->VadErr.sState >= LEC_VAD_ST_ACTIVE)
    {
        pStts->slRxFrames += 1;
        uFlags |= (1<<8);
    }

    if (pDb->VadTx.sState >= LEC_VAD_ST_ACTIVE)
    {
        pStts->slTxFrames += 1;
        uFlags |= (1<<9);
    }

    if (pDb->uIsDT)
    {
        pStts->slDtFrames += 1;
        uFlags |= (1<<10);
    }

    uFlags |= (pSc->uNlpMode << 12);
    uFlags |= (pSc->uAdaptMode << 14);

    pStts->uFlags = uFlags;
    pStts->sTxEn = pDb->VadTx.sEn;
    pStts->sRxInEn = pSc->sRxEn;
    pStts->sErrEn = pDb->VadErr.sEn;
    pStts->sRxOutEn = pDb->VadErr.sEn;
    pStts->sErl = pDb->Adf1.sErlAveraged;
    pStts->sErle = pDb->Adf1.sErleAveraged;
}

/*-------------------------------------------------------------------------*/
void                            _reset_db
/*-------------------------------------------------------------------------*/
(
LEC_tDb *pDb,
U16 uZeroFilter
)
{
    memset(pDb->psHst,  0, LEC_HST_SZ*sizeof(S16));
    memset(pDb->psTxF,  0, LEC_HST_SZ*sizeof(S16));
    memset (pDb->pStts,  0, sizeof (LEC_MIKET_tStts));

   	if (uZeroFilter)
	{
		memset(pDb->psFlt1, 0, LEC_FLT2_SZ*sizeof(S16));
		memset(pDb->psFlt2, 0, LEC_FLT2_SZ*sizeof(S16));
	}

    {
        LEC_MIKET_tStts *pStts = pDb->pStts;
        S16 *psHst = pDb->psHst;
        S16 *psFlt = pDb->psFlt1;
        ILEC_tCfg Cfg = pDb->Cfg;

        memset(pDb, 0, sizeof(LEC_tDb));

        pDb->Cfg = Cfg;
        pDb->pStts = pStts;
        pDb->psHst = psHst;
        pDb->psTxF = psHst + LEC_HST_SZ;
        pDb->psFlt1 = psFlt;
        pDb->psFlt2 = psFlt + LEC_FLT2_SZ;
    }

	/* init db */

	_vad_init(
        &pDb->VadErr, // pDb
        LEC_DB(-50)   // en noise max
        );
	_vad_init(
        &pDb->VadTx,  
        LEC_DB(-40)
        );

    pDb->sRxNoiseLevel = LEC_DB(-70);
    pDb->sClipLevel = LEC_DB(-70);
    pDb->Adf2.sERL = pDb->Cfg.sErlMin;
    pDb->Adf2.sErlAveraged = pDb->Cfg.sErlMin;
}


       
/*--------------------- public  functions ---------------------------------*/


/*-------------------------------------------------------------------------*/
void                            LEC_MIKET_init_db
/*-------------------------------------------------------------------------*/
(
void *p2db,  /* data base */
/*const*/ ILEC_tCfg *pCfg,
LEC_MIKET_tStts *pStts, /* statistics */
S16 *pHst,  /* tx history */
S16 *pAdf  /* adaptive filter */
)
{
    LEC_tDb *pDb = (LEC_tDb *)p2db;

	pDb->psHst  = pHst;
    pDb->psTxF  = &pHst[LEC_HST_SZ];
	pDb->psFlt1 = pAdf;
	pDb->psFlt2 = &pAdf[LEC_FLT2_SZ];
    pDb->pStts  = pStts;
    
    pDb->Cfg = *pCfg;

	_reset_db(pDb, TRUE);

}

/*-------------------------------------------------------------------------*/
void                            LEC_MIKET_control
/*-------------------------------------------------------------------------*/
(
void *p2db,
S16 sCmd,
ILEC_Status *pStatus
)
{
    LEC_tDb *pDb = (LEC_tDb *)p2db;

	if (pDb != NULL)
	{
		if (sCmd & ILEC_CMD_RESET_MASK)
		{
			/* do NOT zero filter coeffs */
			U16 uZeroFilter = TRUE;
            if (sCmd & ILEC_CMD_PRESERVE_FLT_MASK)
            {
                uZeroFilter = FALSE;
            };

			_reset_db(pDb, uZeroFilter);

			sCmd &= ~(ILEC_CMD_RESET_MASK | ILEC_CMD_PRESERVE_FLT_MASK);
		}

		pDb->Cfg.uControl = sCmd;
	}

    pStatus->size = sizeof(ILEC_Status);
}

/*-------------------------------------------------------------------------*/
void                        LEC_MIKET_process
/*-------------------------------------------------------------------------*/
(
void *p2db,
void *p2sc,
S16 *psTx,
S16 *psRx,
S16  sIsTone
)
{
    LEC_tDb *pDb = (LEC_tDb *)p2db;
    LEC_tSc *pSc = (LEC_tSc *)p2sc;

    if (pDb->Cfg.uControl & ILEC_CMD_OFF_MASK)
    {
        return;
    }
    else
    {
        memset(pSc, 0, sizeof(LEC_tSc));
	    pSc->psTx = psTx;
	    pSc->psRx = psRx;
        if (sIsTone)
            pSc->uFlags |= LEC_FLAG_TONE;

	    if (pDb->Cfg.uControl & ILEC_CMD_LOOPBACK_MASK)
	    {
        	S16 k;
		    for (k = 0; k < ILEC_FR_SZ; k++)
		    {
			    pSc->psRx[k] = (pDb->psHst[k + 83])>>2;

		    }
	    }

    	if (pDb->Cfg.uControl & ILEC_CMD_HIPASS_MASK)
	    {
            lec_band_pass_rx(pDb, pSc);
        }

        _ec_process(pDb, pSc);

        _update_statistics(pDb, pSc);

	}
}