owng723.c

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

   Ipp64f dFac,dTmp;
   Ipp32s i;
   dFac = dInit;
   for(i=0;i<10;i++) {
      dTmp = dFac*32768.f;
      SATURATE_fs(dTmp,Tbl[i]);
      dFac = dFac*dInit;
   }
}*/
static __ALIGN32 CONST Ipp16s  PostFiltTable[2*G723_LPC_ORDER] = {
   21299 ,  13844,8999,5849,3802,2471,1606,1044, 679, 441 , /* Zero part */
   24576,18432,13824,10368,7776,5832,4374,3281,2460,1845 , /* Pole part */
};
void  PostFilter(G723Decoder_Obj* decoderObj, Ipp16s *pSrcDstSignal, Ipp16s *pSrcLPC )
{
   Ipp32s   i, lTmp, lSignalEnergy, lSfs;
   Ipp16s sTmp;

   LOCAL_ARRAY(Ipp16s, FltCoef,2*G723_LPC_ORDER, decoderObj);
   LOCAL_ARRAY(Ipp32s, pFltSignal,G723_LPC_ORDER+G723_SBFR_LEN, decoderObj);
   LOCAL_ARRAY(Ipp32s, pAutoCorr,2, decoderObj);
   LOCAL_ARRAY(Ipp16s, pTmpVec,G723_SBFR_LEN, decoderObj);

   /* Normalize the input speech vector.  */
   lSfs=3;
   ippsAutoScale_16s(pSrcDstSignal,  pTmpVec, (Ipp16s) G723_SBFR_LEN, &lSfs ) ;

   /* Compute the first two autocorrelation coefficients*/
   ippsDotProd_16s32s_Sfs(pTmpVec,pTmpVec,G723_SBFR_LEN,pAutoCorr,0);
   ippsDotProd_16s32s_Sfs(pTmpVec,pTmpVec+1,G723_SBFR_LEN-1,pAutoCorr+1,0);
   /* Compute new reflection coefficient.*/
   sTmp = (Ipp16s)(pAutoCorr[0]>>15);
   if ( sTmp ) {
      sTmp = (Ipp16s)((pAutoCorr[1]>>1)/(sTmp));
   }

   /* Compute interpolated reflection coefficient use the new and previouse one.*/
   lTmp =  ((decoderObj->ReflectionCoeff << 2) - decoderObj->ReflectionCoeff + sTmp);
   decoderObj->ReflectionCoeff = (Ipp16s)((lTmp+0x2)>>2);

   sTmp  = (Ipp16s)(((Ipp32s)decoderObj->ReflectionCoeff * (Ipp32s)SmoothCoeff)>>15);
   sTmp &= 0xfffc ;

   /* Compute FIR and IIR coefficients. Note the table can be generated using the GenPostFiltTable function */
   ippsMul_NR_16s_Sfs(pSrcLPC,PostFiltTable,FltCoef,G723_LPC_ORDER,15);
   ippsMul_NR_16s_Sfs(pSrcLPC,PostFiltTable+G723_LPC_ORDER,FltCoef+G723_LPC_ORDER,G723_LPC_ORDER,15);

   /* 32s output needs for compensate filter */
   for(i=0; i<G723_LPC_ORDER; i++){
      pFltSignal[i] = decoderObj->PostFilterMem[G723_LPC_ORDER+i]<<16;
   }
   ippsIIR16s_G723_16s32s(FltCoef,pSrcDstSignal,&pFltSignal[G723_LPC_ORDER],decoderObj->PostFilterMem);

   /* perform the tilt fitering. */
   ippsTiltCompensation_G723_32s16s(sTmp, &pFltSignal[G723_LPC_ORDER-1], pSrcDstSignal);

   /* Gain scaling.  Section 3.9 */
   /* Compute normalized signal energy.*/
   sTmp = (Ipp16s)(2*lSfs + 3);
   if (sTmp < 0) {
      lSignalEnergy = ShiftL_32s(pAutoCorr[0],(Ipp16u)(-sTmp));
   } else {
      lSignalEnergy = pAutoCorr[0]>>sTmp;
   }
   ippsGainControl_G723_16s_I(lSignalEnergy,pSrcDstSignal,&decoderObj->PstFltGain);

   LOCAL_ARRAY_FREE(Ipp16s, pTmpVec,G723_SBFR_LEN, decoderObj);
   LOCAL_ARRAY_FREE(Ipp32s, pAutoCorr,2, decoderObj);
   LOCAL_ARRAY_FREE(Ipp32s, pFltSignal,G723_LPC_ORDER+G723_SBFR_LEN, decoderObj);
   LOCAL_ARRAY_FREE(Ipp16s, FltCoef,2*G723_LPC_ORDER, decoderObj);

   return;
}

void  LSPInterpolation(const Ipp16s *pSrcLSP, const Ipp16s *pSrcPrevLSP, Ipp16s *pDstLPC)
{

    pDstLPC[0] = 4096;
    ippsInterpolateC_NR_G729_16s_Sfs(pSrcLSP,4096,pSrcPrevLSP,12288,&pDstLPC[1],G723_LPC_ORDER,14);
    ippsLSFToLPC_G723_16s(&pDstLPC[1], &pDstLPC[1]);

    pDstLPC[G723_LPC_ORDERP1] = 4096;
    ippsInterpolateC_NR_G729_16s_Sfs(pSrcLSP,8192,pSrcPrevLSP,8192,&pDstLPC[1+G723_LPC_ORDERP1],G723_LPC_ORDER,14);
    ippsLSFToLPC_G723_16s(&pDstLPC[1+G723_LPC_ORDERP1], &pDstLPC[1+G723_LPC_ORDERP1]);

    pDstLPC[2*G723_LPC_ORDERP1] = 4096;
    ippsInterpolateC_NR_G729_16s_Sfs(pSrcLSP,12288,pSrcPrevLSP,4096,&pDstLPC[1+2*G723_LPC_ORDERP1],G723_LPC_ORDER,14);
    ippsLSFToLPC_G723_16s(&pDstLPC[1+2*G723_LPC_ORDERP1], &pDstLPC[1+2*G723_LPC_ORDERP1]);

    pDstLPC[3*G723_LPC_ORDERP1] = 4096;
    ippsCopy_16s(pSrcLSP,&pDstLPC[1+3*G723_LPC_ORDERP1],G723_LPC_ORDER);
    ippsLSFToLPC_G723_16s(&pDstLPC[1+3*G723_LPC_ORDERP1], &pDstLPC[1+3*G723_LPC_ORDERP1]);

    return;
}

/*static void GetAverScaleTable(Ipp16s *pTbl)
{
   Ipp32s i;
   Ipp64f dTmp, dAlphaW=2.70375;
   dTmp = 32768.f/((Ipp64f)G723_HALFFRM_LEN);
   SATURATE_fs(dTmp,pTbl[0]);
   for(i=1;i<4;i++) {
      dTmp = 32768.f*dAlphaW*dAlphaW/((Ipp64f)i*(Ipp64f)G723_FRM_LEN);
      SATURATE_fs(dTmp,pTbl[i]);
   }
   return;
}*/

static __ALIGN32 CONST Ipp16s AverScaleTbl_G723[4] = {273, 998,
                              499, 333}; /*This table can be generateg using GetAverScaleTable function.*/
static __ALIGN32 CONST Ipp32s LogEnerLevel[3] = {2048, 18432, 231233};
static __ALIGN32 CONST Ipp16s FirstCode[3] = {0, 32, 96};

void QuantSIDGain_G723_16s(const Ipp16s *pSrc, const Ipp16s *pSrcSfs, Ipp32s len, Ipp32s *pIndx)
{
   Ipp16s sTmp, sNseg, sNsegP1;
   Ipp16s j, m, k, expBase;
   Ipp32s lTmp1, lTmp2, lTmp3;
   Ipp16s sfs;
   Ipp32s i;

   if(len == 0) {
      /* Quantize energy  */
      sTmp = (Ipp16s)((*pSrcSfs)<<1);
      sTmp = (Ipp16s)(16 - sTmp);
      lTmp1 = MulC_32s(AverScaleTbl_G723[0], (*pSrc)<<sTmp);
   } else {
      /* Compute weighted average sum*/
      sfs = pSrcSfs[0];
      for(i=1; i<len; i++) {
         if(pSrcSfs[i] < sfs) sfs = pSrcSfs[i];
      }
      for(i=0, lTmp1=0; i<len; i++) {
         sTmp = (Ipp16s)(pSrcSfs[i] - sfs);
         sTmp = (Ipp16s)(pSrc[i]>>sTmp);
         sTmp = (Ipp16s)(((AverScaleTbl_G723[len]*sTmp)+0x4000)>>15);
         lTmp1 += sTmp;
      }
      sTmp = (Ipp16s)(15 - sfs);
      if(sTmp < 0) lTmp1 >>= (-sTmp);
      else         lTmp1 <<= sTmp;
   }

   *pIndx = 63;
   if(lTmp1 < LogEnerLevel[2]) {

      /* Compute segment number */
      if(lTmp1 >= LogEnerLevel[1]) {
         expBase = 4; sNseg = 2;
      } else {
         expBase  = 3;
         sNseg=(Ipp16s)((lTmp1 >= LogEnerLevel[0])?1:0);
      }

      sNsegP1 = (Ipp16s)(sNseg + 1);
      j = (Ipp16s)(1<<expBase);
      k = (Ipp16s)(j>>1);

      /* Binary search */
      for(i=0; i<expBase; i++) {
         sTmp = (Ipp16s)(FirstCode[sNseg] + (j<<sNsegP1));
         lTmp2 = 2*sTmp * sTmp;
         if(lTmp1 >= lTmp2) j = (Ipp16s)(j + k);
         else j = (Ipp16s)(j - k);
         k >>= 1;
      }

      sTmp = (Ipp16s)(FirstCode[sNseg] + (j<<sNsegP1));
      lTmp2 = 2* sTmp * sTmp - lTmp1;
      if(lTmp2 <= 0) {
         m    = (Ipp16s)(j + 1);
         sTmp  = (Ipp16s)(FirstCode[sNseg] + (m<<sNsegP1));
         lTmp3 = lTmp1 - 2 * sTmp * sTmp;
         if(lTmp2 > lTmp3) sTmp = (Ipp16s)((sNseg<<4) + j);
         else sTmp = (Ipp16s)((sNseg<<4)+m);
      } else {
         m    = (Ipp16s)(j - 1);
         sTmp  = (Ipp16s)(FirstCode[sNseg] + (m<<sNsegP1));
         lTmp3 = lTmp1 - 2 * sTmp * sTmp;
         if(lTmp2 < lTmp3) sTmp = (Ipp16s)((sNseg<<4) + j);
         else sTmp = (Ipp16s)((sNseg<<4) + m);
      }
      *pIndx = sTmp;
   }
   return;
}

void DecodeSIDGain_G723_16s (Ipp32s pIndx, Ipp16s *pGain)
{
   Ipp16s i, sNseg;
   Ipp16s sTmp;

   sNseg = (Ipp16s)(pIndx>>4);
   if(sNseg == 3) sNseg = 2;
   i = (Ipp16s)(pIndx - (sNseg<<4));
   sTmp = (Ipp16s)(sNseg + 1);
   sTmp = (Ipp16s)(i<<sTmp);
   sTmp = (Ipp16s)(sTmp + FirstCode[sNseg]);  /* SidGain */
   sTmp <<= 5; /* << 5 */
   *pGain = sTmp;

   return;
}

void FixedCodebookGain_G723_16s(const Ipp16s *pSrc1, const Ipp16s *pSrc2,
                                Ipp16s *pGainCoeff, Ipp32s *pGainIdx, Ipp16s *pAlignTmp)
{
   Ipp16s i;
   Ipp16s sNormCorr, sNormEnergy, sCorrSFS, sEnergySFS, sBestQntGain, sCurrGain;
   Ipp32s lCorr, lEnergy;
   Ipp16s sCurrDist, sMinDist;

   ippsRShiftC_16s (pSrc2, 3, pAlignTmp, G723_SBFR_LEN); /* to avoid overflow */

   ippsDotProd_16s32s_Sfs(pSrc1, pAlignTmp, G723_SBFR_LEN,&lCorr,0);

   sBestQntGain = 0;

   if(lCorr > 0) {
      sCorrSFS = Norm_32s_I(&lCorr);
      sNormCorr = (Ipp16s)(lCorr>>17); /* Be sure sNormCorr < sNormEnergy */

      ippsDotProd_16s32s_Sfs(pAlignTmp,pAlignTmp,G723_SBFR_LEN, &lEnergy,0);

      sEnergySFS = Norm_32s_I(&lEnergy);
      sNormEnergy     = (Ipp16s)(lEnergy>>16);

      /* compute sCurrGain = sNormCorr/sNormEnergy */
      sCurrGain =(Ipp16s)( (sNormEnergy > 0)? ((sNormCorr<<15)/sNormEnergy) : IPP_MAX_16S);

      i = (Ipp16s)(sCorrSFS - sEnergySFS + 5);          /* Denormalization of division */

      if(i < 0) sCurrGain = ShiftL_16s(sCurrGain, (Ipp16u)(-i));
      else
         sCurrGain >>= i;

      sBestQntGain = 0;
      sMinDist = (Ipp16s)Abs_16s(sCurrGain + GainDBLvls[0]);
      for ( i =  1; i <N_GAINS ; i ++ ) {
         sCurrDist = (Ipp16s)Abs_16s(sCurrGain + GainDBLvls[i]);
         if ( sCurrDist< sMinDist) { sMinDist = sCurrDist; sBestQntGain = i; }
      }
   }
   *pGainCoeff = (Ipp16s)(-GainDBLvls[sBestQntGain]);
   *pGainIdx = sBestQntGain;
}

void ExcitationResidual_G723_16s(const Ipp16s *pSrc1, const Ipp16s *pSrc2,  Ipp16s *pSrcDst,G723Encoder_Obj *encoderObj)
{
   Ipp32s i;

   LOCAL_ARRAY(Ipp32s, lConv,G723_SBFR_LEN,encoderObj) ;
   LOCAL_ARRAY(Ipp16s, sNormConv,G723_SBFR_LEN,encoderObj) ;

   ippsConvPartial_16s32s(pSrc1,pSrc2,lConv,G723_SBFR_LEN);
   for ( i = 0 ; i < G723_SBFR_LEN ; i ++ ) {
       sNormConv[i] = (Ipp16s)((-lConv[i] + 0x2000) >> 14);
   }
   ippsAdd_16s_I(sNormConv,pSrcDst,G723_SBFR_LEN);

   LOCAL_ARRAY_FREE(Ipp16s, sNormConv,G723_SBFR_LEN,encoderObj) ;
   LOCAL_ARRAY_FREE(Ipp32s, lConv,G723_SBFR_LEN,encoderObj) ;

}

#define COMBTBL_LINE_LEN (G723_SBFR_LEN/GRIDSIZE)
static __ALIGN32 CONST Ipp32s CombTbl[N_PULSES*COMBTBL_LINE_LEN] =
{
   118755,  98280,  80730,  65780,  53130, 42504,  33649,  26334,  20349,  15504,
    11628,   8568,   6188,   4368,   3003,  2002,   1287,    792,    462,    252,
      126,     56,     21,      6,      1,     0,      0,      0,      0,      0,
    23751,  20475,  17550,  14950,  12650, 10626,   8855,   7315,   5985,   4845,
     3876,   3060,   2380,   1820,   1365,  1001,    715,    495,    330,    210,
      126,     70,     35,     15,      5,     1,      0,      0,      0,      0,
     3654,   3276,   2925,   2600,   2300,  2024,   1771,   1540,   1330,   1140,
      969,    816,    680,    560,    455,   364,    286,    220,    165,    120,
       84,     56,     35,     20,     10,     4,      1,      0,      0,      0,
      406,    378,    351,    325,    300,   276,    253,    231,    210,    190,
      171,    153,    136,    120,    105,    91,     78,     66,     55,     45,
       36,     28,     21,     15,     10,     6,      3,      1,      0,      0,
       29,     28,     27,     26,     25,    24,     23,     22,     21,     20,
       19,     18,     17,     16,     15,    14,     13,     12,     11,     10,
        9,      8,      7,      6,      5,     4,      3,      2,      1,      0,
        1,      1,      1,      1,      1,     1,      1,      1,      1,      1,
        1,      1,      1,      1,      1,     1,      1,      1,      1,      1,
        1,      1,      1,      1,      1,     1,      1,      1,      1,      1
};

static __ALIGN32 CONST Ipp32s MaxPosition[4] = {593775, 142506, 593775, 142506};

static __ALIGN32 CONST Ipp16s PitchContrb[2*170] = {
    60,      0,     0,  2489,     60,     0,      0,  5217,      1,  6171,      0,   3953,     0,  10364,     1,  9357,     -1,   8843,     1,  9396,
     0,   5794,    -1, 10816,      2, 11606,     -2, 12072,      0,  8616,      1,  12170,     0,  14440,     0,  7787,     -1,  13721,     0, 18205,
     0,  14471,     0, 15807,      1, 15275,      0, 13480,     -1, 18375,     -1,      0,     1,  11194,    -1, 13010,      1,  18836,    -2, 20354,
     1,  16233,    -1,     0,     60,     0,      0, 12130,      0, 13385,      1,  17834,     1,  20875,     0, 21996,      1,      0,     1, 18277,
    -1,  21321,     1, 13738,     -1, 19094,     -1, 20387,     -1,     0,      0,  21008,    60,      0,    -2, 22807,      0,  15900,     1,     0,
     0,  17989,    -1, 22259,      1, 24395,      1, 23138,      0, 23948,      1,  22997,     2,  22604,    -1, 25942,      0,  26246,     1, 25321,
     0,  26423,     0, 24061,      0, 27247,     60,     0,     -1, 25572,      1,  23918,     1,  25930,     2, 26408,     -1,  19049,     1, 27357,
    -1,  24538,    60,     0,     -1, 25093,      0, 28549,      1,     0,      0,  22793,    -1,  25659,     0, 29377,      0,  30276,     0, 26198,
     1,  22521,    -1, 28919,      0, 27384,      1, 30162,     -1,     0,      0,  24237,    -1,  30062,     0, 21763,      1,  30917,    60,     0,
     0,  31284,     0, 29433,      1, 26821,      1, 28655,      0, 31327,      2,  30799,     1,  31389,     0, 32322,      1,  31760,    -2, 31830,
     0,  26936,    -1, 31180,      1, 30875,      0, 27873,     -1, 30429,      1,  31050,     0,      0,     0, 31912,      1,  31611,     0, 31565,
     0,  25557,     0, 31357,     60,     0,      1, 29536,      1, 28985,     -1,  26984,    -1,  31587,     2, 30836,     -2,  31133,     0, 30243,
    -1,  30742,    -1, 32090,     60,     0,      2, 30902,     60,     0,      0,  30027,     0,  29042,    60,     0,      0,  31756,     0, 24553,
     0,  25636,    -2, 30501,     60,     0,     -1, 29617,      0, 30649,     60,      0,     0,  29274,     2, 30415,      0,  27480,     0, 31213,
    -1,  28147,     0, 30600,      1, 31652,      2, 29068,     60,     0,      1,  28571,     1,  28730,     1, 31422,      0,  28257,     0, 24797,
    60,      0,     0,     0,     60,     0,      0, 22105,      0, 27852,     60,      0,    60,      0,    -1, 24214,      0,  24642,     0, 23305,
    60,      0,    60,     0,      1, 22883,      0, 21601,     60,     0,      2,  25650,    60,      0,    -2, 31253,     -2,  25144,     0, 17998
};

static __ALIGN32 CONST Ipp16s NPulse[4] =
{6,5,
 6,5};

void FixedCodebookVector_G723_16s( Ipp32s lDecPos, Ipp32s lAmplitude, Ipp32s mamp, Ipp32s lGrid,
       Ipp32s lGain, Ipp32s lNSbfr, G723_Rate currRate, Ipp16s *pDst, Ipp32s *pLag, Ipp16s *pGain )

{
   Ipp32s   i, j, lTmp, lOffset, lPos;
   Ipp16s sCdbkGain, sCdbkSign, sCdbkShift, sCdbkPos;

   ippsZero_16s(pDst,G723_SBFR_LEN);

   switch(currRate)  {
      case G723_Rate63: {
         if ( lDecPos < MaxPosition[lNSbfr] ){
            /* Decode the amplitudes and positions */

            j = N_PULSES - NPulse[lNSbfr] ;
            lTmp = lDecPos ;
            for ( i = 0 ; i < G723_SBFR_LEN/GRIDSIZE ; i ++ )  {
               lTmp -= CombTbl[j*COMBTBL_LINE_LEN + i];
               if ( lTmp < 0 ) {
                  lTmp += CombTbl[(j++)*COMBTBL_LINE_LEN+i];
                  if ( (lAmplitude & (1 << (N_PULSES-j) )) !=  0 )
                     pDst[lGrid + GRIDSIZE*i] = GainDBLvls[mamp] ;
                  else
                     pDst[lGrid + GRIDSIZE*i] = (Ipp16s)(-GainDBLvls[mamp]) ;

                  if ( j == N_PULSES ) break ;
               }
            }
         }
         break;
      }

      case G723_Rate53: {
         sCdbkGain = (Ipp16s)(-GainDBLvls[mamp]);
         sCdbkShift = (Ipp16s)lGrid;
         sCdbkSign = (Ipp16s)(lAmplitude<<1);
         sCdbkPos = (Ipp16s) lDecPos;