owng723.c

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

         for(lOffset=0; lOffset<8; lOffset+=2) {
            lPos = (sCdbkPos & 0x7) ;
            lPos = (lPos<<3) + sCdbkShift + lOffset;
            if (lPos < G723_SBFR_LEN)
               pDst[lPos] = (Ipp16s)(sCdbkGain * ((sCdbkSign & 2 ) - 1));
            sCdbkPos >>= 3;
            sCdbkSign >>= 1;
         }

         *pLag = PitchContrb[lGain<<1];
         *pGain = PitchContrb[(lGain<<1) + 1];
         break;
      }
   }
}

void ResidualInterpolation_G723_16s_I
        (Ipp16s *pSrcDst, Ipp16s *pDst,  Ipp32s lag, Ipp16s gain, Ipp16s *pSeed)
{
   Ipp32s   i, lenBlk, len_res;
   if ( lag ) {/* Voiced case*/
      for ( i = 0 ; i < lag ; i ++ ){ /* attenuation  */
         pSrcDst[G723_MAX_PITCH+i-lag] = (Ipp16s)((pSrcDst[G723_MAX_PITCH+i-lag] * 0x6000)>>15);
      }
      lenBlk = G723_FRM_LEN/lag;
      len_res = G723_FRM_LEN - (lenBlk*lag);
      for(i=0; i<lenBlk; i++) {
        ippsCopy_16s(&pSrcDst[G723_MAX_PITCH-lag], &pSrcDst[G723_MAX_PITCH+i*lag], lag);
      }
      if(len_res > 0) ippsCopy_16s(&pSrcDst[G723_MAX_PITCH-lag], &pSrcDst[G723_MAX_PITCH+i*lag], len_res);
      ippsCopy_16s(&pSrcDst[G723_MAX_PITCH],pDst,G723_FRM_LEN);
   } else {/* Unvoiced case*/
      for ( i = 0 ; i < G723_FRM_LEN ; i ++ )
         pDst[i] = (Ipp16s)((gain * Rand2_16s(pSeed))>>15);
      /* reset memory */
      ippsZero_16s(pSrcDst,G723_FRM_LEN+G723_MAX_PITCH);
   }
   return;
}

static __ALIGN32 CONST Ipp16s BetaTbl[2*170] = {
  1024,  1024,   1308,  1591,   1906, 1678,   2291, 1891,   2511,  2120,
  2736,  2399,   3298,  2966,   3489, 3049,   3531, 3185,   3844,  3317,
  4360,  3433,   4541,  3523,   4684, 3729,   4813, 3779,   5069,  3789,
  5528,  4262,   5577,  4450,   5713, 4469,   5923, 4713,   5958,  4944,
  5958,  4950,   6064,  4980,   6132, 5010,   6331, 5032,   6370,  5299,
  6527,  5389,   6533,  5389,   6575, 5389,   6633, 5646,   6671,  5701,
  6832,  5733,   6832,  5765,   6972, 5997,   6996, 5997,   7199,  6150,
  7205,  6211,   7414,  6336,   7529, 6360,   7543, 6415,   7543,  6415,
  7692,  6430,   7758,  6440,   7839, 6461,   7839, 6461,   7869,  6512,
  7992,  6601,   8000,  6787,   8016, 6872,   8055, 6931,   8079,  6972,
  8119,  6984,   8208,  7056,   8250, 7056,   8266, 7105,   8291,  7117,
  8300,  7123,   8325,  7136,   8402, 7161,   8445, 7167,   8605,  7180,
  8623,  7180,   8687,  7262,   8752, 7308,   8837, 7334,   8847,  7334,
  8973,  7387,   9002,  7407,   9012, 7434,   9184, 7441,   9593,  7441,
  9672,  7481,   9752,  7536,   9846, 7564,   9978, 7592,  10139,  7685,
 10202,  7714,  10317,  7758,  10476, 7772,  10598, 7794,  10598,  7802,
 10695,  7817,  11425,  7839,  11670, 7869,  14629, 7885,  15255,  7907,
     0,  7946,      0,  7992,      0, 8039,      0, 8063,      0,  8087,
     0,  8087,      0,  8167,      0, 8184,      0, 8200,      0,  8200,
     0,  8241,      0,  8266,      0, 8283,      0, 8308,      0,  8308,
     0,  8334,      0,  8376,      0, 8402,      0, 8463,      0,  8516,
     0,  8524,      0,  8533,      0, 8641,      0, 8669,      0,  8696,
     0,  8752,      0,  8761,      0, 8799,      0, 8828,      0,  8943,
     0,  9112,      0,  9122,      0, 9133,      0, 9153,      0,  9288,
     0,  9299,      0,  9373,      0, 9384,      0, 9384,      0,  9405,
     0,  9416,      0,  9471,      0, 9503,      0, 9559,      0,  9581,
     0,  9660,      0,  9660,      0, 9718,      0, 9799,      0,  9823,
     0,  9846,      0,  9846,      0, 9930,      0,10039,      0, 10164,
     0, 10227,      0, 10291,      0,10436,      0,10503,      0, 10516,
     0, 10530,      0, 10598,      0,10611,      0,10625,      0, 11040,
     0, 11070,      0, 11100,      0,11115,      0,11315,      0, 11331,
     0, 11804,      0, 12100,      0,12263,      0,12263,      0, 12300,
     0, 12337,      0, 12431,      0,12800,      0,12962,      0, 13065,
     0, 13496,      0, 13815,      0,14100,      0,14198,      0, 18409
};

void ErrorUpdate_G723(Ipp32s *pError, Ipp16s openLoopLag, Ipp16s AdCbbkLag, Ipp16s AdCbbkGain, G723_Rate currRate)
{
    Ipp32s ilag, lag, e0, e1, li0=0,li1=0;
    Ipp16s sBeta;

    lag = openLoopLag - 1 + AdCbbkLag;

    AdCbbkGain<<=1;
    if ( currRate == G723_Rate63) {
        if ( openLoopLag >= (G723_SBFR_LEN-2) ) AdCbbkGain++;
    } else {
        AdCbbkGain++;
    }
    sBeta = BetaTbl[AdCbbkGain];

    if(lag > 30) {
        ilag = (lag * 273)>>13;   /*  x 1/30 */
        if(30*(ilag+1) != lag) {
            if(ilag == 1) {
               if(pError[0] <= pError[1]){
                  li0 = 1;
                  li1 = 1;
               }
            }else {
               li0 = ilag-2;
               li1 = ilag-1;
               if(pError[li1] > pError[li0]){
                  li0 = li1;
               }
               if(pError[li1] <= pError[ilag]){
                  li1 = ilag;
               }
            }
        }
        else {  /* lag = 60, 90, 120 */
           li0 = ilag-1;
           li1 = ilag;
        }
    }
    e0 = Add_32s(ShiftL_32s(MulC_32s(sBeta,pError[li0]),2),4);
    e1 = Add_32s(ShiftL_32s(MulC_32s(sBeta,pError[li1]),2),4);
    pError[4] = pError[2];
    pError[3] = pError[1];
    pError[2] = pError[0];
    pError[1] = e1;
    pError[0] = e0;
    return;
}

static void CodewordImpConv_G723(Ipp16s *pSrc, Ipp16s *pDst, Ipp16s *pSrcSign, Ipp16s *pPos, Ipp16s* pDstSign, Ipp32s* ComposedIdx)
{
   Ipp32s i;
   Ipp16s sImpPos0, sImpPos1, sImpPos2, sImpPos3;
   Ipp16s sImpSign0, sImpSign1, sImpSign2, sImpSign3;

   sImpPos0 = pPos[0];
   sImpPos1 = pPos[1];
   sImpPos2 = pPos[2];
   sImpPos3 = pPos[3];

   sImpSign0 = pSrcSign[0];
   sImpSign1 = pSrcSign[1];
   sImpSign2 = pSrcSign[2];
   sImpSign3 = pSrcSign[3];

   /* find codebook index;  17-bit address */

   *pDstSign = 0;
   if(sImpSign0 > 0) *pDstSign=1;
   if(sImpSign1 > 0) *pDstSign += 2;
   if(sImpSign2 > 0) *pDstSign += 4;
   if(sImpSign3 > 0) *pDstSign += 8;

   i = sImpPos0 >> 3;
   i += sImpPos1 & 0xfff8;
   i += (sImpPos2 & 0xfff8) << 3;
   i += (sImpPos3 & 0xfff8) << 6;

   *ComposedIdx = i;

   /* Compute the filtered codeword */

   ippsZero_16s(pDst,G723_SBFR_LEN);

   if (sImpPos0>sImpPos1) {i=(Ipp32s)sImpPos0; sImpPos0=sImpPos1; sImpPos1=(Ipp16s)i; i=(Ipp32s)sImpSign0; sImpSign0=sImpSign1; sImpSign1=(Ipp16s)i;}
   if (sImpPos2>sImpPos3) {i=(Ipp32s)sImpPos2; sImpPos2=sImpPos3; sImpPos3=(Ipp16s)i; i=(Ipp32s)sImpSign2; sImpSign2=sImpSign3; sImpSign3=(Ipp16s)i;}
   if (sImpPos0>sImpPos2) {i=(Ipp32s)sImpPos0; sImpPos0=sImpPos2; sImpPos2=(Ipp16s)i; i=(Ipp32s)sImpSign0; sImpSign0=sImpSign2; sImpSign2=(Ipp16s)i;}
   if (sImpPos1>sImpPos3) {i=(Ipp32s)sImpPos1; sImpPos1=sImpPos3; sImpPos3=(Ipp16s)i; i=(Ipp32s)sImpSign1; sImpSign1=sImpSign3; sImpSign3=(Ipp16s)i;}
   if (sImpPos1>sImpPos2) {i=(Ipp32s)sImpPos1; sImpPos1=sImpPos2; sImpPos2=(Ipp16s)i; i=(Ipp32s)sImpSign1; sImpSign1=sImpSign2; sImpSign2=(Ipp16s)i;}

   for (i=0; i<sImpPos0; i++)
      pDst[i]=0;
   for (; i<sImpPos1; i++)
      pDst[i]=(Ipp16s)(sImpSign0*pSrc[i-sImpPos0]);
   for (; i<sImpPos2; i++)
      pDst[i]=(Ipp16s)(sImpSign0*pSrc[i-sImpPos0]+sImpSign1*pSrc[i-sImpPos1]);
   for (; i<sImpPos3; i++)
      pDst[i]=(Ipp16s)(sImpSign0*pSrc[i-sImpPos0]+sImpSign1*pSrc[i-sImpPos1]+sImpSign2*pSrc[i-sImpPos2]);
   for (; i<G723_SBFR_LEN; i++)
      pDst[i]=(Ipp16s)(sImpSign0*pSrc[i-sImpPos0]+sImpSign1*pSrc[i-sImpPos1]+sImpSign2*pSrc[i-sImpPos2]+sImpSign3*pSrc[i-sImpPos3]);

   return;
}

void  FixedCodebookSearch_G723_16s(G723Encoder_Obj *encoderObj, ParamStream_G723 *Params, Ipp16s *pSrcDst, Ipp16s *ImpResp, Ipp16s sNSbfr)
{
    Ipp16s sPitchPeriod, sGain;

    switch(Params->currRate)  {

    case G723_Rate63:
       {
          ippsMPMLQFixedCodebookSearch_G723(Params->PitchLag[sNSbfr>>1], ImpResp, pSrcDst, pSrcDst,
             &Params->sGrid[sNSbfr], &Params->sTrainDirac[sNSbfr], &Params->sAmpIndex[sNSbfr], &Params->sAmplitude[sNSbfr], &Params->sPosition[sNSbfr], sNSbfr);
          break;
        }

    case G723_Rate53:
       {
          Ipp16s sCurrGrid, sSign, sGainCoeff;
          Ipp32s lCdbkIdx, lGainIdx;

          LOCAL_ALIGN_ARRAY(16, Ipp16s, pTargetImpRespCorr, G723_SBFR_LEN+4, encoderObj) ;
          LOCAL_ALIGN_ARRAY(16, Ipp16s, pFixedVector,       G723_SBFR_LEN+4, encoderObj) ;
          LOCAL_ALIGN_ARRAY(16, Ipp16s, pFltFixedVector,    G723_SBFR_LEN+4, encoderObj) ;
          LOCAL_ALIGN_ARRAY(16, Ipp32s,   pToeplizMatrix,     G723_TOEPLIZ_MATRIX_SIZE, encoderObj) ;
          LOCAL_ALIGN_ARRAY(16, Ipp16s, pAlignBuff,         G723_SBFR_LEN, encoderObj) ;
          LOCAL_ARRAY(Ipp16s, pDstFixedSign,    4, encoderObj) ;
          LOCAL_ARRAY(Ipp16s, pDstFixedPosition,4, encoderObj) ;

          sPitchPeriod = (Ipp16s)(Params->PitchLag[sNSbfr>>1]-1+Params->AdCdbkLag[sNSbfr] + PitchContrb[Params->AdCdbkGain[sNSbfr]<<1]);
          sGain = PitchContrb[(Params->AdCdbkGain[sNSbfr]<<1)+1];

          /* Find correlations of h[] needed for the codebook search. */
          ippsRShiftC_16s_I(1,ImpResp,G723_SBFR_LEN); /* Q13 -->  Q12*/
          if (sPitchPeriod < G723_SBFR_LEN-2) {
              ippsHarmonicFilter_16s_I(sGain,sPitchPeriod,&ImpResp[sPitchPeriod],G723_SBFR_LEN-sPitchPeriod);
          }
          /* Compute correlation of target vector with impulse response. */

          ippsCrossCorr_NormM_16s(ImpResp, pSrcDst, G723_SBFR_LEN, pTargetImpRespCorr);
          /* Compute the covariance matrix of the impulse response. */
          ippsToeplizMatrix_G723_16s32s(ImpResp, pToeplizMatrix);

          /* Find innovative codebook (filtered codeword) */
          ippsACELPFixedCodebookSearch_G723_32s16s(pTargetImpRespCorr, pToeplizMatrix, pDstFixedSign, pDstFixedPosition, &sCurrGrid, pFixedVector, &encoderObj->sSearchTime);

          CodewordImpConv_G723(ImpResp, pFltFixedVector, pDstFixedSign, pDstFixedPosition, &sSign, &lCdbkIdx);
          /* Compute innovation vector gain */
          FixedCodebookGain_G723_16s(pSrcDst,pFltFixedVector,&sGainCoeff,&lGainIdx,pAlignBuff);

          ippsMulC_16s_Sfs(pFixedVector,sGainCoeff,pSrcDst,G723_SBFR_LEN,0);
          if(sPitchPeriod < G723_SBFR_LEN-2)
              ippsHarmonicFilter_16s_I(sGain,sPitchPeriod,&pSrcDst[sPitchPeriod],G723_SBFR_LEN-sPitchPeriod);

          Params->sTrainDirac[sNSbfr] = 0;
          Params->sAmpIndex[sNSbfr] = (Ipp16s)lGainIdx;
          Params->sGrid[sNSbfr] = sCurrGrid;
          Params->sAmplitude[sNSbfr] = sSign;
          Params->sPosition[sNSbfr] = lCdbkIdx;

          LOCAL_ARRAY_FREE(Ipp16s, pDstFixedPosition,4, encoderObj) ;
          LOCAL_ARRAY_FREE(Ipp16s, pDstFixedSign,    4, encoderObj) ;
          LOCAL_ALIGN_ARRAY_FREE(16, Ipp16s, pAlignBuff,         G723_SBFR_LEN, encoderObj) ;
          LOCAL_ALIGN_ARRAY_FREE(16, Ipp32s,   pToeplizMatrix,     G723_TOEPLIZ_MATRIX_SIZE, encoderObj) ;
          LOCAL_ALIGN_ARRAY_FREE(16, Ipp16s, pFltFixedVector,    G723_SBFR_LEN+4, encoderObj) ;
          LOCAL_ALIGN_ARRAY_FREE(16, Ipp16s, pFixedVector,       G723_SBFR_LEN+4, encoderObj) ;
          LOCAL_ALIGN_ARRAY_FREE(16, Ipp16s, pTargetImpRespCorr, G723_SBFR_LEN+4, encoderObj) ;

          break;
       }
    }
}

void InterpolationIndex_G723_16s( Ipp16s *pDecodedExc, Ipp16s sPitchLag, Ipp16s *pGain, Ipp16s *pGainSFS, Ipp16s *pDstIdx)
{
    Ipp32s   lSfs, lIdx, lTmp;
    Ipp16s sTargetVecEnergy, sMaxCorr, sBestEnergy;
    Ipp16s *pExc;

    /* Normalize the excitation */
    lSfs=3;
    ippsAutoScale_16s_I( pDecodedExc, G723_MAX_PITCH+G723_FRM_LEN, &lSfs );
    *pGainSFS = (Ipp16s)lSfs;

    if ( sPitchLag > (Ipp16s) (G723_MAX_PITCH-3) )
        sPitchLag = (Ipp16s) (G723_MAX_PITCH-3);

    lIdx = sPitchLag;
    pExc = &pDecodedExc[G723_MAX_PITCH+G723_FRM_LEN-2*G723_SBFR_LEN];
    ippsAutoCorrLagMax_Inv_16s(pExc,2*G723_SBFR_LEN,sPitchLag-3,sPitchLag+3,&lTmp,&lIdx);

    if(lTmp > 0 ) {
      sMaxCorr = Cnvrt_NR_32s16s( lTmp );
      /* Compute target energy */
      ippsDotProd_16s32s_Sfs(pExc,pExc,2*G723_SBFR_LEN,&lTmp,0);
       lTmp <<=1;
      sTargetVecEnergy = Cnvrt_NR_32s16s( lTmp );
      *pGain = sTargetVecEnergy;

      /* Calculate the best energy */
      ippsDotProd_16s32s_Sfs(pExc-lIdx,pExc-lIdx,2*G723_SBFR_LEN,&lTmp,0);
       lTmp <<=1;

      sBestEnergy = Cnvrt_NR_32s16s( lTmp );

      lTmp = sBestEnergy * sTargetVecEnergy;
      lTmp >>= 3;

      if ( lTmp < sMaxCorr * sMaxCorr ) *pDstIdx = (Ipp16s)lIdx;
      else *pDstIdx = 0;
    } else *pDstIdx = 0;

    return;
}


static __ALIGN32 CONST Ipp16s StratingPositionTbl[G723_SBFR_LEN/GRIDSIZE] = {
   0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58};

#define MAX_CACHE_NUM 32
static __ALIGN32 CONST Ipp16s SeedCacheTbl[MAX_CACHE_NUM] = {
    17547, -15555,  11855,  26561, -20077, -21051, 24151,   9545,
   -15205,  24141,  29791,  -5935,   -605,  21717, -3993,  12889,
    -9045,   4445,  21103, -32287,  -7757,   5093,  6775,  22377,
   -29509,  -9107, -14209,  13041,  24003,  -5387, -9081, -27527};

static __ALIGN32 CONST Ipp16s OlpCache[MAX_CACHE_NUM][2] = {
   {129, 137}, {143, 135}, {137, 140}, {133, 136},
   {129, 124}, {139, 140}, {129, 133}, {123, 129},
   {139, 128}, {128, 125}, {140, 132}, {137, 133},
   {136, 127}, {131, 132}, {129, 138}, {133, 127},
   {142, 123}, {127, 141}, {137, 128}, {134, 133},
   {136, 135}, {137, 129}, {143, 125}, {138, 130},
   {139, 143}, {140, 139}, {127, 128}, {124, 138},
   {130, 126}, {136, 129}, {130, 138}, {136, 136}};

static __ALIGN32 CONST Ipp16s GainCache[MAX_CACHE_NUM][4] = {
   {34, 31, 18, 35}, {38, 16, 47, 22}, { 9, 29, 48, 36}, {16, 44,  4, 15},
   {16, 27, 34, 34}, { 5, 26, 45,  8}, {14, 32, 31, 36}, {12, 18, 25,  8},
   { 7, 50, 46, 49}, {43, 26,  1, 21}, { 3, 37, 35, 27}, { 5,  7, 29,  4},
   { 6, 50,  4, 28}, { 3, 16, 16, 12}, {25, 44, 10,  8}, {43, 10, 17,  2},
   {14, 27,  8, 22}, {33, 46, 40, 30}, {31,  4,  6, 28}, {28, 29, 39,  3},
   {31, 32,  9, 31}, {34, 15, 22, 26}, {20, 17, 23, 38}, { 9, 12, 44,  6},
   { 6, 14,  5,  5}, { 7, 25, 13, 49}, {44, 31, 11, 38}, {36, 10, 33, 11},
   {40, 24, 48, 44}, { 1, 24, 12, 49}, {50, 48, 21, 28}, { 9, 23,  5, 19}};

static __ALIGN32 CONST Ipp16s SignCache[MAX_CACHE_NUM][24] = {
   { 16384,  16384, -16384, -16384,  16384,  16384,  16384, -16384,
     16384, -16384,  16384,  16384,  16384, -16384, -16384, -16384,
     16384,  16384,  16384, -16384, -16384, -16384,      0,      0},
   {-16384, -16384, -16384, -16384, -16384, -16384,  16384, -16384,
     16384, -16384, -16384, -16384, -16384, -16384, -16384,  16384,
    -16384,  16384, -16384,  16384,  16384, -16384,      0,      0},
   {-16384,  16384, -16384, -16384, -16384, -16384, -16384,  16384,
     16384,  16384,  16384,  16384,  16384, -16384, -16384, -16384,
     16384,  16384,  16384,  16384,  16384,  16384,      0,      0},
   {-16384,  16384, -16384,  16384,  16384, -16384,  16384, -16384,
     16384,  16384,  16384, -16384, -16384,  16384,  16384,  16384,
     16384, -16384,  16384, -16384,  16384,  16384,      0,      0},
   {-16384, -16384, -16384,  16384, -16384,  16384,  16384,  16384,
     16384, -16384,  16384,  16384,  16384, -16384,  16384,  16384,
     16384,  16384,  16384,  16384, -16384,  16384,      0,      0},
   {-16384, -16384,  16384,  16384, -16384, -16384,  16384, -16384,
    -16384,  16384,  16384, -16384, -16384, -16384, -16384, -16384,
    -16384,  16384,  16384,  16384, -16384, -16384,      0,      0},
   {-16384,  16384,  16384, -16384, -16384,  16384, -16384,  16384,
     16384, -16384, -16384,  16384,  16384, -16384,  16384, -16384,
     16384, -16384, -16384, -16384, -16384,  16384,      0,      0},
   {-16384,  16384,  16384, -16384,  16384, -16384, -16384, -16384,
     16384,  16384,  16384, -16384, -16384,  16384,  16384,  16384,
    -16384,  16384, -16384,  16384, -16384,  16384,      0,      0},
   {-16384, -16384,  16384,  16384,  16384,  16384, -16384,  16384,
     16384,  16384, -16384, -16384, -16384,  16384, -16384,  16384,