decg729fp.c

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

      decPrm[7] = ExtractBitsG729FP(&pParm,&i,6);
      decPrm[8] = ExtractBitsG729FP(&pParm,&i,4);
      decPrm[9] = ExtractBitsG729FP(&pParm,&i,9);
      decPrm[10] = ExtractBitsG729FP(&pParm,&i,2);
      decPrm[11] = ExtractBitsG729FP(&pParm,&i,6);

      rate = G729E_MODE;
   }else if(frametype == 3){ /* active frame */
      i=0;
      decPrm[1] = 3; /* FrameType=voice e */
      decPrm[0] = 0; /* bfi = 0*/
      decPrm[1+1] = ExtractBitsG729FP(&pParm,&i,1+N_BITS_1ST_STAGE);
      decPrm[1+2] = ExtractBitsG729FP(&pParm,&i,N_BITS_2ND_STAGE*2);
      decPrm[1+3] = ExtractBitsG729FP(&pParm,&i,8);
      decPrm[1+4] = ExtractBitsG729FP(&pParm,&i,1);
      decPrm[1+5] = ExtractBitsG729FP(&pParm,&i,13);
      decPrm[1+6] = ExtractBitsG729FP(&pParm,&i,4);
      decPrm[1+7] = ExtractBitsG729FP(&pParm,&i,7);
      decPrm[1+8] = ExtractBitsG729FP(&pParm,&i,5);
      decPrm[1+9] = ExtractBitsG729FP(&pParm,&i,13);
      decPrm[1+10] = ExtractBitsG729FP(&pParm,&i,4);
      decPrm[1+11] = ExtractBitsG729FP(&pParm,&i,7);
      decPrm[1+4] = (Parity(decPrm[1+3])+decPrm[1+4]) & 1; /*  parity error (1) */
      rate = G729_BASE;
   }else if(frametype == 4){ /* active frame E*/
      i=0;
      decPrm[0] = 0; /* bfi = 0*/
      decPrm[1] = 4; /* FrameType */
      if(ExtractBitsG729FP(&pParm,&i,2)==0) {
         decPrm[2] = 0; /*LPCMode*/

         decPrm[3] = ExtractBitsG729FP(&pParm,&i,1+N_BITS_1ST_STAGE);
         decPrm[4] = ExtractBitsG729FP(&pParm,&i,N_BITS_2ND_STAGE*2);
         decPrm[5] = ExtractBitsG729FP(&pParm,&i,8);
         decPrm[6] = ExtractBitsG729FP(&pParm,&i,1);
         decPrm[7] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[8] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[9] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[10] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[11] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[12] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[13] = ExtractBitsG729FP(&pParm,&i,5);
         decPrm[14] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[15] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[16] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[17] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[18] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[19] = ExtractBitsG729FP(&pParm,&i,7);

         i = decPrm[5]>>1;
         i &= 1;
         decPrm[6] += i;
         decPrm[6] = (Parity(decPrm[5])+decPrm[6]) & 1;/* parm[6] = parity error (1) */
      } else {
         decPrm[2] = 1; /*LPCMode*/

         decPrm[3] = ExtractBitsG729FP(&pParm,&i,8);
         decPrm[4] = ExtractBitsG729FP(&pParm,&i,1);
         decPrm[5] = ExtractBitsG729FP(&pParm,&i,13);
         decPrm[6] = ExtractBitsG729FP(&pParm,&i,10);
         decPrm[7] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[8] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[9] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[10] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[11] = ExtractBitsG729FP(&pParm,&i,5);
         decPrm[12] = ExtractBitsG729FP(&pParm,&i,13);
         decPrm[13] = ExtractBitsG729FP(&pParm,&i,10);
         decPrm[14] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[15] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[16] = ExtractBitsG729FP(&pParm,&i,7);
         decPrm[17] = ExtractBitsG729FP(&pParm,&i,7);

         i = decPrm[3]>>1;
         i &= 1;
         decPrm[4] += i;
         decPrm[4] = (Parity(decPrm[3])+decPrm[4]) & 1;/* parm[4] = parity error (1) */
      }
      rate = G729E_MODE;
   }
   pDecPrm = &decPrm[0];
   parm2 = pDecPrm[2];

   bfi = *pDecPrm++;
   FrameType = *pDecPrm++;

   if(bfi == 1) {
      if(decoderObj->objPrm.codecType!=G729A_CODEC) {
         FrameType = decoderObj->prevFrameType;
         if(FrameType == 1) FrameType = 0;
      } else {
           if(decoderObj->prevFrameType == 1) FrameType = 1;
          else FrameType = 0;
      }
      pDecPrm[-1] = FrameType;
   }

   Vad = FrameType;
   rate = FrameType - 2;
   /* Decoding the Backward/Forward LPC mode */
   if( rate != G729E_MODE) LPCMode = 0;
   else {
      if (bfi != 0) {
         LPCMode = decoderObj->prevLPCMode; /* Frame erased => LPCMode = previous LPCMode */
         *pDecPrm++ = LPCMode;
      } else {
         LPCMode = *pDecPrm++;
      }
      if(decoderObj->lPrevBFI != 0) decoderObj->lVoicing = decoderObj->lPrevVoicing;

      /* Backward LPC mode */
      ippsWinHybrid_G729E_32f(decoderObj->SynthBuffer, forwardAutoCorr, decoderObj->pHWState);

      /* Lag windowing    */
      ippsMul_32f_I(lagBwd, &forwardAutoCorr[1], BWD_LPC_ORDER);
      if (forwardAutoCorr[0]<1.0) forwardAutoCorr[0]=1.0;

      sts = ippsLevinsonDurbin_G729_32f(forwardAutoCorr, BWD_LPC_ORDER, &backwardLPC[BWD_LPC_ORDERP1], backwardReflectCoeff, &tmp_lev);
      if(sts == ippStsOverflow) {
         ippsCopy_32f(decoderObj->OldBackwardLPC,&backwardLPC[BWD_LPC_ORDERP1],BWD_LPC_ORDER+1);
         backwardReflectCoeff[0] = decoderObj->OldBackwardRC[0];
         backwardReflectCoeff[1] = decoderObj->OldBackwardRC[1];
      } else {
         ippsCopy_32f(&backwardLPC[BWD_LPC_ORDERP1],decoderObj->OldBackwardLPC,BWD_LPC_ORDER+1);
         decoderObj->OldBackwardRC[0] = backwardReflectCoeff[0];
         decoderObj->OldBackwardRC[1] = backwardReflectCoeff[1];
      }

      /* Tests saturation of backwardLPC */
      isSaturateAZ = 0;
      for (i=BWD_LPC_ORDERP1; i<2*BWD_LPC_ORDERP1; i++) if (backwardLPC[i] >= 8.) {isSaturateAZ = 1;break;}
      if (isSaturateAZ == 1) ippsCopy_32f(decoderObj->BackwardLPCMemory, &backwardLPC[BWD_LPC_ORDERP1], BWD_LPC_ORDERP1);
      else ippsCopy_32f(&backwardLPC[BWD_LPC_ORDERP1], decoderObj->BackwardLPCMemory, BWD_LPC_ORDERP1);

      /* Additional bandwidth expansion on backward filter */
      WeightLPCCoeff_G729(&backwardLPC[BWD_LPC_ORDERP1], BWD_GAMMA, BWD_LPC_ORDER, &backwardLPC[BWD_LPC_ORDERP1]);

      if(LPCMode == 1) {
         if (!isVarZero(decoderObj->fFEInterpolationCoeff)) {
            /* Interpolation of the backward filter after a bad frame */
            tmp = 1.f - decoderObj->fFEInterpolationCoeff;
            pLPC = backwardLPC + BWD_LPC_ORDERP1;
            ippsInterpolateC_G729_32f(pLPC, tmp, decoderObj->BackwardUnqLPC, decoderObj->fFEInterpolationCoeff, pLPC, BWD_LPC_ORDERP1);
         }
      }
   }
   if( bfi == 0) {
      decoderObj->fGainMuting = 1.;
      decoderObj->lBFICounter = 0;
   } else if((decoderObj->lPrevBFI == 0) && (decoderObj->prevFrameType >3)) {
      /* store the last good backward filter when the frame is erased */
      ippsCopy_32f(&backwardLPC[BWD_LPC_ORDERP1], decoderObj->BackwardUnqLPC, BWD_LPC_ORDERP1);
   }
   /* Update next frame synthesis signal */
   ippsCopy_32f(&decoderObj->SynthBuffer[FRM_LEN], &decoderObj->SynthBuffer[0], BWD_SYNTH_MEM);

   if(FrameType < 2) {
      /* SID or Untr or Erased Frame */
      if(pDecPrm[-1] != 0) {
         decoderObj->fSIDGain = SIDGainTbl[pDecPrm[3]];

         /* Inverse quantization of the LSP */
         ippsLSFDecode_G729B_32f(&pDecPrm[0], (Ipp32f*)decoderObj->PrevFreq, decoderObj->SIDLSP);
      } else {
         /* non SID Frame
           Case of 1st SID frame erased : quantize-decode
           energy estimate stored in fSIDGain         */
         if(decoderObj->prevFrameType > 1) {
            QuantSIDGain_G729B(&decoderObj->SID, 0, &fTmp, &i);
            decoderObj->fSIDGain = SIDGainTbl[i];
         }
      }
      if(decoderObj->prevFrameType > 1) {
         decoderObj->fCurrGain = decoderObj->fSIDGain;
      } else {
         decoderObj->fCurrGain *= GAIN_INT_FACTOR;
         decoderObj->fCurrGain += INV_GAIN_INT_FACTOR * decoderObj->fSIDGain;
      }

      if(isVarZero(decoderObj->fCurrGain)) {
         ippsZero_32f(Excitation,FRM_LEN);
         PhaseDispersionUpdate_G729D(0.f,decoderObj->fCurrGain,decoderObj->phdMem);
         PhaseDispersionUpdate_G729D(0.f,decoderObj->fCurrGain,decoderObj->phdMem);
      } else {
         ComfortNoiseExcitation_G729(decoderObj->fCurrGain, Excitation, &decoderObj->sCNGSeed, DECODER,NULL,decoderObj->phdMem,(Ipp8s *)TmpAlignVec);
      }

      /* Interpolate the Lsp vectors */
      ippsInterpolateC_G729_32f(decoderObj->OldLSP, 0.5, decoderObj->SIDLSP, 0.5, tmpLSP, LPC_ORDER);

      ippsLSPToLPC_G729_32f(tmpLSP, forwardLPC);
      ippsLSPToLPC_G729_32f(decoderObj->SIDLSP, &forwardLPC[LPC_ORDER+1]);
      ippsCopy_32f(decoderObj->SIDLSP, decoderObj->OldLSP, LPC_ORDER);

      pLPC = forwardLPC;
      if(decoderObj->objPrm.codecType!=G729A_CODEC) {
         for (NSbfr = 0; NSbfr < FRM_LEN; NSbfr += SUBFR_LEN) {
            ippsSynthesisFilter_G729_32f(pLPC, LPC_ORDER, &Excitation[NSbfr], &pSynth[NSbfr], SUBFR_LEN, &decoderObj->SynFltMemory[BWD_LPC_ORDER-LPC_ORDER]);
            ippsMove_32f(&pSynth[NSbfr+SUBFR_LEN-BWD_LPC_ORDER], decoderObj->SynFltMemory, BWD_LPC_ORDER);

            T2[0] = decoderObj->prevPitchDelay;
            pLPC += LPC_ORDERP1;
         }
      } else {
         for (NSbfr = 0,i=0; NSbfr < FRM_LEN; NSbfr += SUBFR_LEN,i++) {
            ippsSynthesisFilter_G729_32f(pLPC, LPC_ORDER, &Excitation[NSbfr], &flDst[NSbfr], SUBFR_LEN, decoderObj->SynFltMemory);
            ippsCopy_32f(&flDst[NSbfr+SUBFR_LEN-LPC_ORDER], decoderObj->SynFltMemory, LPC_ORDER);

            T2[i] = decoderObj->prevPitchDelay;
            pLPC += LPC_ORDERP1;
         }
      }
      decoderObj->fBetaPreFilter = PITCH_SHARPMIN;
      decoderObj->fInterpolationCoeff = 1.1f;
      /* Reset for gain decoding in case of frame erasure */
      decoderObj->sBWDStatInd = 0;
      highStatIndicator = 0;
      /* Reset for pitch tracking  in case of frame erasure */
      decoderObj->lStatPitchPT = 0;
      /* Update the previous filter for the next frame */
      ippsCopy_32f(&forwardLPC[LPC_ORDERP1], decoderObj->PrevFlt, LPC_ORDERP1);
      for(i=LPC_ORDERP1; i<BWD_LPC_ORDERP1; i++) decoderObj->PrevFlt[i] = 0.;
   } else {
      /* Active frame */
      decoderObj->sCNGSeed = INIT_SEED_VAL;
      /* Forward mode */
      if (LPCMode == 0) {

         /* Decode the LSFs to CurrLSP */
         if(bfi){
            ippsCopy_32f(decoderObj->prevLSF, CurrLSP, LPC_ORDER );
            ippsLSFDecodeErased_G729_32f(decoderObj->prevMA, (Ipp32f*)decoderObj->PrevFreq, decoderObj->prevLSF);
         }else{
            Ipp32s indexes[4];
            indexes[0] = (pDecPrm[0] >> N_BITS_1ST_STAGE) & 1;
            indexes[1] = pDecPrm[0] & (Ipp16s)(N_ELEM_1ST_STAGE - 1);
            indexes[2] = (pDecPrm[1] >> N_BITS_2ND_STAGE) & (Ipp16s)(N_ELEM_2ND_STAGE - 1);
            indexes[3] = pDecPrm[1] & (Ipp16s)(N_ELEM_2ND_STAGE - 1);

            decoderObj->prevMA = indexes[0];
            ippsLSFDecode_G729_32f(indexes, (Ipp32f*)decoderObj->PrevFreq, CurrLSP);

            ippsCopy_32f(CurrLSP, decoderObj->prevLSF, LPC_ORDER );

         }

         /* Convert to LSPs */
         ownCOS_G729_32f(CurrLSP, CurrLSP, LPC_ORDER);

         pDecPrm += 2;
         if( decoderObj->prevLPCMode == 0) { /* Interpolation of LPC for the 2 subframes */
            ippsInterpolateC_G729_32f(decoderObj->OldLSP, 0.5, CurrLSP, 0.5, tmpLSP, LPC_ORDER);

            ippsLSPToLPC_G729_32f(tmpLSP, forwardLPC);
            ippsLSPToLPC_G729_32f(CurrLSP, &forwardLPC[LPC_ORDER+1]);
         } else {
            /* no interpolation */
            ippsLSPToLPC_G729_32f(CurrLSP, forwardLPC); /* Subframe 1*/
            ippsCopy_32f(forwardLPC, &forwardLPC[LPC_ORDERP1], LPC_ORDERP1); /* Subframe 2 */
         }

         /* update the LSFs for the next frame */
         ippsCopy_32f(CurrLSP, decoderObj->OldLSP, LPC_ORDER);

         decoderObj->fInterpolationCoeff = 1.1f;
         pLPC = forwardLPC;
         aqLen = LPC_ORDER;
         /* update the previous filter for the next frame */
         ippsCopy_32f(&forwardLPC[LPC_ORDERP1], decoderObj->PrevFlt, LPC_ORDERP1);
         for(i=LPC_ORDERP1; i<BWD_LPC_ORDERP1; i++) decoderObj->PrevFlt[i] = 0.;
      } else {
         InterpolatedBackwardFilter_G729(backwardLPC, decoderObj->PrevFlt, &decoderObj->fInterpolationCoeff);
         pLPC = backwardLPC;
         aqLen = BWD_LPC_ORDER;
         /* update the previous filter for the next frame */
         ippsCopy_32f(&backwardLPC[BWD_LPC_ORDERP1], decoderObj->PrevFlt, BWD_LPC_ORDERP1);
      }

      for (NSbfr = 0,nsubfr=0; NSbfr < FRM_LEN; NSbfr += SUBFR_LEN,nsubfr++) {
         index = *pDecPrm++;            /* pitch index */

         if(NSbfr == 0) {
            if (rate == G729D_MODE) i = 0;      /* no pitch parity at 6.4 kb/s */
            else i = *pDecPrm++;           /* get parity check result */
            badPitch = bfi + i;