owng729fp.c

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

               max = fIntCorr;
               lFracPart = i;
            }
         }
      } else {
         midLag = maxLag - 4;
         if ((lBestLag == midLag - 1) || lBestLag == midLag) {
            max  = Interpolation_3(&pCorr[lBestLag], -2);
            lFracPart = -2;

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

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

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

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

   /* limit the fraction value in the interval [-1,0,1] */

   if (lFracPart == -2) {
      lFracPart = 1;
      lBestLag -= 1;
   }
   if (lFracPart == 2) {
      lFracPart = -1;
      lBestLag += 1;
   }

   *pDstFracPitch = lFracPart;

   return lBestLag;
}

void CNGGetSize(Ipp32s *pDstSize)
{
   *pDstSize = sizeof(CNGmemory);
   return;
}

void CNGInit(Ipp8s *cngMem)
{
   CNGmemory *cngState = (CNGmemory *)cngMem;
   ippsZero_16s((Ipp16s*)cngState,sizeof(CNGmemory)>>1) ;

   ippsZero_32f(cngState->SumAutoCorrs,SUMAUTOCORRS_SIZE);
   ippsZero_32f(cngState->AutoCorrs,AUTOCORRS_SIZE);
   ippsZero_32f(cngState->Energies,GAINS_NUM);

   cngState->fCurrGain = 0.;
   cngState->lAutoCorrsCounter = 0;
   cngState->lFltChangeFlag = 0;
}

static Ipp32f gaussGen(Ipp16s *sCNGSeed)
{
    Ipp32s i, lTmp;

    lTmp = 0;
    for(i=0; i<12; i++) {
        lTmp += Rand_16s(sCNGSeed);
    }
    lTmp >>= 7;
    return((Ipp32f)lTmp * 0.001953125f);
}

void ComfortNoiseExcitation_G729(Ipp32f fCurrGain, Ipp32f *exc, Ipp16s *sCNGSeed, Ipp32s flag_cod, Ipp32f *ExcitationError, Ipp8s *phdMem, Ipp8s *pExtBuff)
{
   Ipp32f *pGaussianExc;
   Ipp32s   *pos;
   Ipp32f *sign;
   Ipp32f *CurrentExcitation;
   Ipp32f fgp, fEner, fFact, fInterExc, k, discriminant, x1, x2, absMinRoot;
   Ipp32s   i, n, pitchDelay, lFrac;
   Ipp16s sGp, sTmp1, sTmp2;
   Ipp32s *delayLine;
   //Ipp64f ener_tmp;

   pGaussianExc = (Ipp32f *)pExtBuff;
   pos = (Ipp32s *)(pExtBuff + SUBFR_LEN*sizeof(Ipp32f));
   sign = (Ipp32f *)((Ipp8s *)pos + 4*sizeof(Ipp32s));
   delayLine = (Ipp32s *)((Ipp8s *)sign + 4*sizeof(Ipp32f));

   /* Loop on subframes */
   CurrentExcitation = exc;
   for (n = 0;  n < NUN_SUBFRAMES; n++) {
      /* Fenerate random adaptive codebook and fixed codebook parameters */
      sTmp1   = Rand_16s(sCNGSeed);
      lFrac    = (Ipp32s)(sTmp1 & (Ipp16s)0x0003) - 1;
      if(lFrac == 2) lFrac = 0;
      sTmp1 >>= 2;
      pitchDelay      = (Ipp32s)(sTmp1 & (Ipp16s)0x003F) + 40;
      sTmp1 >>= 6;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0007);
      pos[0]  = 5 * (Ipp32s)sTmp2;
      sTmp1 >>= 3;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0001);
      sign[0] = 2.f * (Ipp32f)sTmp2 - 1.f;
      sTmp1 >>= 1;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0007);
      pos[1]  = 5 * (Ipp32s)sTmp2 + 1;
      sTmp1 >>= 3;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0001);
      sign[1] = 2.f * (Ipp32f)sTmp2 - 1.f;
      sTmp1   = Rand_16s(sCNGSeed);
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0007);
      pos[2]  = 5 * (Ipp32s)sTmp2 + 1;
      sTmp1 >>= 3;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0001);
      sign[2] = 2.f * (Ipp32f)sTmp2 - 1.f;
      sTmp1 >>= 1;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x000F);
      pos[3]  = (Ipp32s)(sTmp2 & (Ipp16s)0x0001) + 3; /* j+3*/
      sTmp2 >>= 1;
      sTmp2  &= (Ipp16s)0x0007;
      pos[3] += 5 * (Ipp32s)sTmp2;
      sTmp1 >>= 4;
      sTmp2   = (Ipp16s)(sTmp1 & (Ipp16s)0x0001);
      sign[3] = 2.f * (Ipp32f)sTmp2 - 1.f;
      sGp      = (Ipp16s)(Rand_16s(sCNGSeed) & (Ipp16s)0x1FFF); /* < 0.5  */
      fgp      = (Ipp32f)sGp / 16384.f;

      /* Generate gaussian excitation */
      /*ippsRandomNoiseExcitation_G729B_16s32f(sCNGSeed, excg, SUBFR_LEN);
      ippsDotProd_32f64f(excg, excg, SUBFR_LEN,&ener_tmp);*/
      fEner = 0.f;
      for(i=0; i<SUBFR_LEN; i++) {
         pGaussianExc[i] = gaussGen(sCNGSeed);
         fEner += pGaussianExc[i] * pGaussianExc[i];
      }

      /*  Compute fFact = 0.5 x fCurrGain * sqrt(40 / fEner) */
      fFact = NORM_GAUSS * fCurrGain;
      fFact /= (Ipp32f)sqrt(fEner);
      /* Multiply excg by fFact */
      for(i=0; i<SUBFR_LEN; i++) {
         pGaussianExc[i] *= fFact;
      }
      //ippsMulC_32f_I(fFact, excg, SUBFR_LEN);

      /* generate random  adaptive excitation */
      delayLine[0] = pitchDelay;
      delayLine[1] = lFrac;
      ippsDecodeAdaptiveVector_G729_32f_I(delayLine, CurrentExcitation);

      /* CurrentExcitation is an adaptive + gaussian excitation */
      fEner = 0.f;
      for(i=0; i<SUBFR_LEN; i++) {
         CurrentExcitation[i] *= fgp;
         CurrentExcitation[i] += pGaussianExc[i];
         fEner += CurrentExcitation[i] * CurrentExcitation[i];
      }
      /* Compute fixed code gain */
      /* Solve EQ(X) = 4 X**2 + 2 b X + c, where b = fInterExc*/
      fInterExc = 0.f;
      for(i=0; i<4; i++) {
         fInterExc += CurrentExcitation[pos[i]] * sign[i];
      }

      /* Compute k = fCurrGain x fCurrGain x SUBFR_LEN */
      k = fCurrGain * fCurrGain * SUBFR_LEN;

      /* Compute discriminant = b^2 - 4*c, where c=fEner-k*/
      discriminant = fInterExc * fInterExc - 4.f * (fEner - k);

      if(discriminant < 0.f) {
         /* adaptive excitation = 0 */
         ippsCopy_32f(pGaussianExc, CurrentExcitation, SUBFR_LEN);
         fInterExc = 0.f;
         for(i=0; i<4; i++) {
            fInterExc += CurrentExcitation[pos[i]] * sign[i];
         }
         /* Compute discriminant = b^2 - 4*c, where c = - k x (1- alpha^2)*/
         discriminant = fInterExc * fInterExc + K_MUL_COEFF * k;
         fgp = 0.f;
      }

      discriminant = (Ipp32f)sqrt(discriminant);
      /* First root*/
      x1 = (discriminant - fInterExc) * 0.25f;
      /* Second root*/
      x2 = - (discriminant + fInterExc) * 0.25f;
      absMinRoot = ((Ipp32f)fabs(x1) < (Ipp32f)fabs(x2)) ? x1 : x2;
      if(absMinRoot >= 0.f) {
         CLIP_TO_UPLEVEL(absMinRoot,CNG_MAX_GAIN);
      } else {
         CLIP_TO_LOWLEVEL(absMinRoot,(-CNG_MAX_GAIN));
      }

      /* Update cur_exc with ACELP excitation */
      for(i=0; i<4; i++) {
         CurrentExcitation[pos[i]] += absMinRoot * sign[i];
      }

      if(flag_cod != DECODER) UpdateExcErr_G729(fgp, pitchDelay,ExcitationError);
      else {
         if(absMinRoot >= 0.f) PhaseDispersionUpdate_G729D(fgp,absMinRoot,phdMem);
         else PhaseDispersionUpdate_G729D(fgp,-absMinRoot,phdMem);
      }
      CurrentExcitation += SUBFR_LEN;
   } /* end of loop on subframes */

   return;
}



static __ALIGN32 CONST Ipp32f fFact[GAINS_NUM+1] =
        {(Ipp32f)0.003125, (Ipp32f)0.00078125, (Ipp32f)0.000390625};

static Ipp32s quantEnergy(Ipp32f fEner, Ipp32f *pDstQEnergy)
{
    Ipp32f fEnergydB;
    Ipp32s index;

    if(fEner <= MIN_ENER) {  /* MIN_ENER <=> -8dB */
        *pDstQEnergy = (Ipp32f)-12.;
        return(0);
    }

    fEnergydB = (Ipp32f)10. * (Ipp32f)log10(fEner);

    if(fEnergydB <= (Ipp32f)-8.) {
        *pDstQEnergy = (Ipp32f)-12.;
        return(0);
    }

    if(fEnergydB >= (Ipp32f)65.) {
        *pDstQEnergy = (Ipp32f)66.;
        return(31);
    }

    if(fEnergydB <= (Ipp32f)14.) {
        index = (Ipp32s)((fEnergydB + (Ipp32f)10.) * 0.25);
        if (index < 1) index = 1;
        *pDstQEnergy = (Ipp32f)4. * (Ipp32f)index - (Ipp32f)8.;
        return(index);
    }

    index = (Ipp32s)((fEnergydB - (Ipp32f)3.) * 0.5);
    if (index < 6) index = 6;
    *pDstQEnergy = (Ipp32f)2. * (Ipp32f)index + (Ipp32f)4.;
    return(index);
}

void QuantSIDGain_G729B(Ipp32f *fEner, Ipp32s lNumSavedEnergies, Ipp32f *enerq, Ipp32s *idx)
{
    Ipp32s i;
    Ipp32f averageEnergy;

    /* Quantize energy saved for frame erasure case*/
    if(lNumSavedEnergies == 0) {
        averageEnergy = (*fEner) * fFact[0];
    } else {
        /* Compute weighted average of energies*/
        averageEnergy = (Ipp32f)0.;
        for(i=0; i<lNumSavedEnergies; i++) {
            averageEnergy += fEner[i];
        }
        averageEnergy *= fFact[lNumSavedEnergies];
    }

    *idx = quantEnergy(averageEnergy, enerq);

    return;
}

static __ALIGN32 CONST Ipp32f coef_G729[2][2] = {
{(Ipp32f)31.134575,
(Ipp32f) 1.612322},

{(Ipp32f) 0.481389,
(Ipp32f) 0.053056}
};

static __ALIGN32 CONST Ipp32f thr1_G729[SIZECODEBOOK1-NUM_CAND1] = {
(Ipp32f)0.659681,
(Ipp32f)0.755274,
(Ipp32f)1.207205,
(Ipp32f)1.987740
};

static __ALIGN32 CONST Ipp32f thr2_G729[SIZECODEBOOK2-NUM_CAND2] = {
(Ipp32f)0.429912,
(Ipp32f)0.494045,
(Ipp32f)0.618737,
(Ipp32f)0.650676,
(Ipp32f)0.717949,
(Ipp32f)0.770050,
(Ipp32f)0.850628,
(Ipp32f)0.932089
};

static __ALIGN32 CONST Ipp32s map1_G729[SIZECODEBOOK1] = {
 5, 1, 4, 7, 3, 0, 6, 2};

static __ALIGN32 CONST Ipp32s map2_G729[SIZECODEBOOK2] = {
 4, 6, 0, 2,12,14, 8,10,15,11, 9,13, 7, 3, 1, 5};

static __ALIGN32 CONST Ipp32s imap1_G729[SIZECODEBOOK1] = {
 5, 1, 7, 4, 2, 0, 6, 3};
static __ALIGN32 CONST Ipp32s imap2_G729[SIZECODEBOOK2] = {
 2,14, 3,13, 0,15, 1,12, 6,10, 7, 9, 4,11, 5, 8};

static __ALIGN32 CONST Ipp32f gbk1_G729[SIZECODEBOOK1][2] = {
{0.000010f, 0.185084f},
{0.094719f, 0.296035f},
{0.111779f, 0.613122f},
{0.003516f, 0.659780f},
{0.117258f, 1.134277f},
{0.197901f, 1.214512f},
{0.021772f, 1.801288f},
{0.163457f, 3.315700f}};

static __ALIGN32 CONST Ipp32f gbk2_G729[SIZECODEBOOK2][2] = {
{0.050466f, 0.244769f},
{0.121711f, 0.000010f},
{0.313871f, 0.072357f},
{0.375977f, 0.292399f},
{0.493870f, 0.593410f},
{0.556641f, 0.064087f},
{0.645363f, 0.362118f},
{0.706138f, 0.146110f},
{0.809357f, 0.397579f},
{0.866379f, 0.199087f},
{0.923602f, 0.599938f},
{0.925376f, 1.742757f},
{0.942028f, 0.029027f},
{0.983459f, 0.414166f},
{1.055892f, 0.227186f},
{1.158039f, 0.724592f}};

static __ALIGN32 CONST Ipp32f coef_G729D[2][2] = {
{ 36.632507f, 2.514171f},
{  0.399259f, 0.073709f}
};

static __ALIGN32 CONST Ipp32f thr1_G729D[SIZECODEBOOK1_ANNEXD-NUM_CAND1_ANNEXD] = {
  1.210869f,
  2.401702f
};

static __ALIGN32 CONST Ipp32f thr2_G729D[SIZECODEBOOK2_ANNEXD-NUM_CAND2_ANNEXD] = {
  0.525915f,
  0.767320f
};