owng729fp.c

G.729 and G.723.1 codecs x86 (and x86_64) Linux and FreeBSD source code for Asterisk open source PBX

   0.0471f, -0.0308f, 0.0131f, -0.0052f, 0.0144f, -0.0386f, 0.0664f,
   -0.0826f, 0.0770f, -0.0495f, 0.0105f, 0.0252f, -0.0467f, 0.0526f,
   -0.0506f, 0.0519f, -0.0630f, 0.0807f, -0.0934f, 0.0884f, -0.0604f,
   0.0170f, 0.0238f, -0.0418f, 0.0257f, 0.0200f
};

static __ALIGN32 CONST Ipp32f ImpHigh[SUBFR_LEN]={
   1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
   0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
   0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
   0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
};


void PHDGetSize(Ipp32s *pDstSize)
{
   *pDstSize = sizeof(PHDmemory);
   return;
}

void PHDInit(Ipp8s *phdMem)
{
   PHDmemory *phdState = (PHDmemory *)phdMem;

   ippsZero_32f(phdState->gainMem,6);
   phdState->prevDispState = 0;
   phdState->prevCbGain = 0.;
   phdState->onset = 0;
}

void PhaseDispersionUpdate_G729D(Ipp32f valPitchGain, Ipp32f valCodebookGain, Ipp8s *phdMem)
{
    Ipp32s i;
    PHDmemory *phdState = (PHDmemory *)phdMem;

    for (i = 5; i > 0; i--) phdState->gainMem[i] = phdState->gainMem[i-1];
    phdState->gainMem[0] = valPitchGain;
    phdState->prevDispState = 2;
    phdState->prevCbGain = valCodebookGain;
    phdState->onset = 0;

    return;
}

void PhaseDispersion_G729D(Ipp32f *pSrcExcSignal, Ipp32f *pDstFltExcSignal, Ipp32f valCodebookGain,
                           Ipp32f valPitchGain, Ipp32f *pSrcDstInnovation, Ipp8s *phdMem,Ipp8s *pExtBuff)
{
    Ipp32s  i;
    PHDmemory *phdState = (PHDmemory *)phdMem;

    Ipp32f *pScaledLTP;
    Ipp32f *pMemory;
    Ipp32s *pPos;
    Ipp32s numNonZeroElem, nPulse, i1, lPos;
    Ipp32s phDispState;
    const Ipp32f *pTable=NULL;

    pScaledLTP = (Ipp32f *)pExtBuff;
    pMemory = (Ipp32f *)(pExtBuff + SUBFR_LEN*sizeof(Ipp32f));
    pPos = (Ipp32s *)(pMemory + SUBFR_LEN*sizeof(Ipp32f));

    /* anti-sparseness post-processing */
    ippsAdaptiveCodebookContribution_G729_32f(valCodebookGain, pSrcDstInnovation, pSrcExcSignal, pScaledLTP);
    ippsCopy_32f(pSrcDstInnovation,pMemory,SUBFR_LEN);
    ippsZero_32f(pSrcDstInnovation,SUBFR_LEN);

    numNonZeroElem=0;
    for (i=0; i<SUBFR_LEN; i++) {
        if (pMemory[i]) /*Can't change to if(fabs(pMemory[i]) < IPP_MINABS_32F)*/
            pPos[numNonZeroElem++] = i;
    }
    if (valPitchGain <= 0.6f) {
        phDispState = 0;
    } else if ( (valPitchGain > 0.6f) && (valPitchGain < 0.9f) ) {
        phDispState = 1;
    } else {
        phDispState = 2;
    }

    for (i = 5; i > 0; i--) {
        phdState->gainMem[i]=phdState->gainMem[i-1];
    }
    phdState->gainMem[0] = valPitchGain;

    if (valCodebookGain > 2.0f * phdState->prevCbGain)
        phdState->onset = 2;
    else {
        if (phdState->onset) phdState->onset -= 1;
    }

    i1=0;
    for (i = 0; i < 6; i++) {
        if (phdState->gainMem[i] < 0.6f) i1 += 1;
    }
    if (i1 > 2 && !phdState->onset) phDispState = 0;

    if (phDispState - phdState->prevDispState > 1 && !phdState->onset) phDispState -= 1;

    if (phdState->onset) {
        if (phDispState < 2) phDispState++;
    }

    phdState->prevDispState=phDispState;
    phdState->prevCbGain = valCodebookGain;

    if (phDispState == 0) {
       pTable = ImpLow;
    } else if (phDispState == 1) {
       pTable = ImpMiddle;
    } else if (phDispState == 2) {
       pTable = ImpHigh;
    }

    for (nPulse=0; nPulse<numNonZeroElem; nPulse++) {
      lPos = pPos[nPulse];
      for (i=lPos; i<SUBFR_LEN; i++)
         pSrcDstInnovation[i] += pMemory[lPos] * pTable[i-lPos];
      for (i=0; i < lPos; i++)
         pSrcDstInnovation[i] += pMemory[lPos] * pTable[SUBFR_LEN-lPos+i];
    }

    ippsAdaptiveCodebookContribution_G729_32f(-valCodebookGain, pSrcDstInnovation, pScaledLTP, pDstFltExcSignal);

    return;
}

static void GlobalStationnarityAdaptation_G729E(G729FPEncoder_Obj* encoderObj, Ipp32f valBackwardPredGain, Ipp32f valForwardPredGain, Ipp32s valLPCMode)
{
    Ipp16s sTmp;
    /* First adaptation based on previous backward / forward decisions */
    if (valLPCMode == 1) { /* Backward stationnary mode */

        (encoderObj->sBWDStatInd)++;
        CLIP_TO_UPLEVEL(encoderObj->sBWDStatInd,21);
        if(encoderObj->sValBWDStatInd < 32517) encoderObj->sValBWDStatInd  += 250;
        else encoderObj->sValBWDStatInd = 32767;

        /* after 20 backward frames => increase stat */
        if (encoderObj->sBWDStatInd == 20) {
            if(encoderObj->sGlobalStatInd < 30267) encoderObj->sGlobalStatInd += 2500;
            else encoderObj->sGlobalStatInd = 32767;
        }
        else if (encoderObj->sBWDStatInd > 20) encoderObj->sGlobalStatInd += 500;

    }

    else if ((valLPCMode == 0)&&(encoderObj->prevLPCMode == 1)) { /* Backward -> Forward transition */

        /* Transition occurs after less than 20 backward frames => decrease stat */
        if (encoderObj->sBWDStatInd < 20) {
            sTmp = (Ipp16s)(5000 - encoderObj->sValBWDStatInd);
            encoderObj->sGlobalStatInd = (Ipp16s)(encoderObj->sGlobalStatInd-sTmp);
        }

        /* Reset consecutive backward frames counter */
        encoderObj->sBWDStatInd = 0;
        encoderObj->sValBWDStatInd = 0;

    }

    /* Second adaptation based on prediction gains */

    if (encoderObj->sGlobalStatInd < 13000) {

        if      (valBackwardPredGain > valForwardPredGain + TH4) encoderObj->sGlobalStatInd += 3200;
        else if (valBackwardPredGain > valForwardPredGain + TH3) encoderObj->sGlobalStatInd += 2400;
        else if (valBackwardPredGain > valForwardPredGain + TH2) encoderObj->sGlobalStatInd += 1600;
        else if (valBackwardPredGain > valForwardPredGain + TH1) encoderObj->sGlobalStatInd +=  800;
        else if (valBackwardPredGain > valForwardPredGain)       encoderObj->sGlobalStatInd +=  400;

    }

    if      (valBackwardPredGain < valForwardPredGain -  TH5) encoderObj->sGlobalStatInd -= 6400;
    else if (valBackwardPredGain < valForwardPredGain -  TH4) encoderObj->sGlobalStatInd -= 3200;
    else if (valBackwardPredGain < valForwardPredGain -  TH3) encoderObj->sGlobalStatInd -= 1600;
    else if (valBackwardPredGain < valForwardPredGain -  TH2) encoderObj->sGlobalStatInd -=  800;
    else if (valBackwardPredGain < valForwardPredGain -  TH1) encoderObj->sGlobalStatInd -=  400;

    CLIP_TO_UPLEVEL(encoderObj->sGlobalStatInd,32000);
    CLIP_TO_LOWLEVEL(encoderObj->sGlobalStatInd,0);
    return;
}

void SetLPCMode_G729FPE(G729FPEncoder_Obj* encoderObj, Ipp32f *pSrcSignal, Ipp32f *pSrcForwardLPCFilter,
                      Ipp32f *pSrcBackwardLPCFilter, Ipp32s *pDstLPCMode, Ipp32f *pSrcLSP,Ipp32f *pExtBuff)
{

    Ipp32s  i;
    Ipp32f *pLPCFlt, *PPtr;
    Ipp32f fGap, forwardPredGain, backwardPredGain, intBackwardPredGain;
    Ipp32f LSPThreshold, LSPDist, fTmp;
    Ipp32f ener_DB_pSrcSignal;

    PPtr = &pExtBuff[0]; /*FRM_LEN elements*/

    ener_DB_pSrcSignal = CalcEnergy_dB_G729(pSrcSignal, FRM_LEN);

    pLPCFlt = pSrcBackwardLPCFilter + BWD_LPC_ORDERP1;
    /* Calc backward filter prediction gain (without interpolation ) */
    ippsConvBiased_32f(pLPCFlt,BWD_LPC_ORDER+1,pSrcSignal,FRM_LEN+BWD_LPC_ORDER,PPtr,FRM_LEN,BWD_LPC_ORDER);
    backwardPredGain = ener_DB_pSrcSignal - CalcEnergy_dB_G729(PPtr, FRM_LEN);

    /* Interpolated backward filter for the first sub-frame       */
    InterpolatedBackwardFilter_G729(pSrcBackwardLPCFilter, encoderObj->PrevFlt, &encoderObj->fInterpolationCoeff);

    /* Calc interpolated backward filter prediction gain */
    ippsConvBiased_32f(pSrcBackwardLPCFilter,BWD_LPC_ORDER+1,pSrcSignal,SUBFR_LEN+BWD_LPC_ORDER,PPtr,SUBFR_LEN,BWD_LPC_ORDER);
    ippsConvBiased_32f(pLPCFlt,BWD_LPC_ORDER+1,&pSrcSignal[SUBFR_LEN],SUBFR_LEN+BWD_LPC_ORDER,&PPtr[SUBFR_LEN],SUBFR_LEN,BWD_LPC_ORDER);
    intBackwardPredGain = ener_DB_pSrcSignal - CalcEnergy_dB_G729(PPtr, FRM_LEN);

    /* Calc forward filter prediction gain */
    ippsConvBiased_32f(pSrcForwardLPCFilter,LPC_ORDER+1,pSrcSignal,SUBFR_LEN+LPC_ORDER,PPtr,SUBFR_LEN,LPC_ORDER);
    ippsConvBiased_32f(&pSrcForwardLPCFilter[LPC_ORDERP1],LPC_ORDER+1,&pSrcSignal[SUBFR_LEN],SUBFR_LEN+LPC_ORDER,&PPtr[SUBFR_LEN],SUBFR_LEN,LPC_ORDER);
    forwardPredGain = ener_DB_pSrcSignal - CalcEnergy_dB_G729(PPtr, FRM_LEN);

    /* Choose: backward/forward mode.*/
    /* 1st criterion with prediction gains. The global stationnarity index
         is used to adapt the threshold value " GAP ".*/

    /* Do the threshold adaptation according to the global stationnarity indicator */
    fGap = (Ipp32f)(encoderObj->sGlobalStatInd) * GAP_FACT;
    fGap += 1.f;

    if ( (intBackwardPredGain > forwardPredGain - fGap)&& (backwardPredGain > forwardPredGain - fGap)&&
         (backwardPredGain > 0.f)               && (intBackwardPredGain > 0.f) ) *pDstLPCMode = 1;
    else *pDstLPCMode = 0;

    if (encoderObj->sGlobalStatInd < 13000) *pDstLPCMode = 0; /* => Forward mode imposed */

    /* 2nd criterion with a distance between 2 successive LSP vectors */
    /* Computation of the LPC distance */
    LSPDist = 0;
    for(i=0; i<LPC_ORDER; i++){
        fTmp = encoderObj->OldLSP[i] - pSrcLSP[i];
        LSPDist += fTmp * fTmp;
    }

    /* Adaptation of the LSPs thresholds */
    if (encoderObj->sGlobalStatInd < 32000) {
        LSPThreshold = 0.f;
    }
    else {
        LSPThreshold = 0.03f;
    }

    /* Switching backward -> forward forbidden in case of a LPC stationnary */
    if ((LSPDist < LSPThreshold) &&(*pDstLPCMode == 0)&&(encoderObj->prevLPCMode == 1)
         &&(backwardPredGain > 0.f)&&(intBackwardPredGain > 0.f)) {
        *pDstLPCMode = 1;
    }

    /* Low energy frame => Forward mode chosen */

    if (ener_DB_pSrcSignal < THRES_ENERGY) {
        *pDstLPCMode = 0;
        if (encoderObj->sGlobalStatInd > 13000) encoderObj->sGlobalStatInd = 13000;
    }
    else isBackwardModeDominant_G729(&encoderObj->isBWDDominant, *pDstLPCMode,&encoderObj->sBWDFrmCounter,&encoderObj->sFWDFrmCounter);

    /* Adaptation of the global stationnarity indicator */
    if (ener_DB_pSrcSignal >= THRES_ENERGY) GlobalStationnarityAdaptation_G729E(encoderObj,backwardPredGain, forwardPredGain, *pDstLPCMode);
    if(*pDstLPCMode == 0) encoderObj->fInterpolationCoeff = 1.1f;
    return;
}

static void NormalizedCorrelation(Ipp32f *pSrcExc, Ipp32f *pSrcTargetVector, Ipp32f *pSrcImpulseResponse,
                                  Ipp32s len, Ipp32s lagMin, Ipp32s lagMax, Ipp32f *pDstCorr, Ipp32f *pTmpFltPastExc)
{
   Ipp32s i, k;
   Ipp64f dEnergy, dTmp;

   k = -lagMin;

   ippsConvBiased_32f( &pSrcExc[k], len , pSrcImpulseResponse, len ,  pTmpFltPastExc, len ,0);

   /* loop for every possible period */
   for (i = lagMin; i < lagMax; i++) {
      /* Compute energie of pFltExc[] */
      ippsDotProd_32f64f(pTmpFltPastExc, pTmpFltPastExc, len, &dEnergy);

      /* Compute correlation between pSrcTargetVector[] and pFltExc[] */
      ippsDotProd_32f64f(pSrcTargetVector, pTmpFltPastExc, len, &dTmp);

      /* Normalize correlation = correlation * (1/sqrt(energie)) */
      pDstCorr[i] = (Ipp32f)(dTmp)/((Ipp32f)sqrt(dEnergy+0.01));

      /* modify the filtered excitation pFltExc[] for the next iteration */
      k--;
      ippsFilteredExcitation_G729_32f( pSrcImpulseResponse, pTmpFltPastExc, len, pSrcExc[k]);
   }

   /* Compute energie of pFltExc[] */
   ippsDotProd_32f64f(pTmpFltPastExc, pTmpFltPastExc, len, &dEnergy);

   /* Compute correlation between pSrcTargetVector[] and pFltExc[] */
   ippsDotProd_32f64f(pSrcTargetVector, pTmpFltPastExc, len, &dTmp);

   /* Normalize correlation = correlation * (1/sqrt(energie)) */
   pDstCorr[lagMax] = (Ipp32f)(dTmp)/((Ipp32f)sqrt(dEnergy+0.01));

   return;

}

static __ALIGN32 CONST Ipp32f ReorderInter_3[2*3*INTERPOL4_LEN] = {
 0.015738f,-0.047624f, 0.084078f, 0.900839f,
 0.084078f,-0.047624f, 0.015738f, 0.000000f,
 0.000000f, 0.016285f,-0.105570f, 0.760084f,
 0.424082f,-0.121120f, 0.031217f,-0.005925f,
-0.005925f, 0.031217f,-0.121120f, 0.424082f,
 0.760084f,-0.105570f, 0.016285f, 0.000000f
};

static Ipp32f Interpolation_3(Ipp32f *pSrc, Ipp32s lFrac)
{
   Ipp32s i;
   Ipp32f sum, *x1;
   const Ipp32f *c;

   x1 = &pSrc[-(INTERPOL4_LEN-1)];
   if (lFrac < 0) {
      x1--;
      lFrac += UP_SAMPLING;
   }
   c = &ReorderInter_3[2*INTERPOL4_LEN*lFrac];

   sum = 0.0f;
   for(i=0; i< 2*INTERPOL4_LEN; i++)
      sum+= x1[i] * c[i];

   return sum;
}

Ipp32s AdaptiveCodebookSearch_G729_32f(Ipp32f *pSrcExc, Ipp32f *pSrcTargetVector, Ipp32f *pSrcImpulseResponse, Ipp32s len,
                        Ipp32s minLag, Ipp32s maxLag, Ipp32s valSubframeNum, Ipp32s *pDstFracPitch, G729Codec_Type codecType,Ipp32f *pExtBuff)
{
   Ipp32s i, lFracPart;
   Ipp32s lBestLag, lMin, lMax;
   Ipp32f max;
   Ipp32f fIntCorr;
   Ipp32f *pFltExc;     /* filtered past excitation */
   Ipp32f *pCorr;
   Ipp32s midLag;

   pFltExc = &pExtBuff[0];

   /* Find interval to compute normalized correlation */
   lMin = minLag - INTERPOL4_LEN;
   lMax = maxLag + INTERPOL4_LEN;

   pCorr = &pExtBuff[SUBFR_LEN - lMin]; /*Actually (10+2*INTERPOL4_LEN) from [SUBFR_LEN]. pCorr[lMin:lMax]*/

   /* Compute normalized correlation between target and filtered excitation */
   NormalizedCorrelation(pSrcExc, pSrcTargetVector, pSrcImpulseResponse, len, lMin, lMax, pCorr,pFltExc);

   /* find integer pitch */
   max = pCorr[minLag];
   lBestLag  = minLag;

   for(i= minLag+1; i<=maxLag; i++) {
      if( pCorr[i] >= max) {
         max = pCorr[i];
         lBestLag = i;
      }
   }

   /* If first subframe and lBestLag > 84 do not search fractionnal pitch */

   if( (valSubframeNum == 0) && (lBestLag > 84) ) {
      *pDstFracPitch = 0;
      return(lBestLag);
   }

   /* test the fractions around lBestLag and choose the one which maximizes the interpolated normalized correlation */

   if (codecType == G729D_CODEC) {    /* 6.4 kbps */
      if (valSubframeNum == 0) {
         max  = Interpolation_3(&pCorr[lBestLag], -2);
         lFracPart = -2;

         for (i = -1; i <= 2; i++) {
            fIntCorr = Interpolation_3(&pCorr[lBestLag], i);
            if(fIntCorr > max) {