exc_lbc.c

语音编解码算法G.723.1的C语言算法原代码

          ptr_h1 = ptr_hd;
          ptr_h2 =  ptr_hf;
          for(i=k+(Word16)1; i<NB_POS; i++ ) {
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p3 = extract_h(cor);

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p2 = extract_h(cor);

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p1 = extract_h(cor);

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p0 = extract_h(cor);

                  p3 -= ldec;
                  p2 -= ldec;
                  p1 -= ldec;
                  p0 -= ldec;
          }
          cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
          cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
          *p3 = extract_h(cor);

          cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
          cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
          *p2 = extract_h(cor);


          l_fin_sup -= NB_POS;
          l_fin_inf--;
          ptr_hf += STEP;
  }

 /*
  * Compute elements of: rri0i1[], rri0i3[], rri1i2[] and rri2i3[]
  */

  ptr_hd = h;
  ptr_hf = ptr_hd + 6;
  l_fin_sup = MSIZE-1;
  l_fin_inf = l_fin_sup-(Word16)1;
  for(k=0; k<NB_POS; k++) {

          p3 = rri0i3 + l_fin_sup;
          p2 = rri2i3 + l_fin_inf;
          p1 = rri1i2 + l_fin_inf;
          p0 = rri0i1 + l_fin_inf;

          ptr_h1 = ptr_hd;
          ptr_h2 =  ptr_hf;
          cor = 0;
          for(i=k+(Word16)1; i<NB_POS; i++ ) {

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p3 = extract_h(cor);

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p2 = extract_h(cor);

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p1 = extract_h(cor);

                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
                  *p0 = extract_h(cor);

                  p3 -= ldec;
                  p2 -= ldec;
                  p1 -= ldec;
                  p0 -= ldec;
          }
          cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
          cor = L_mac(cor, *ptr_h1, *ptr_h2); ptr_h1++; ptr_h2++;
          *p3 = extract_h(cor);

          l_fin_sup -= NB_POS;
          l_fin_inf--;
          ptr_hf += STEP;
  }
  return;
}

/*
**
**  Function:     Corr_h_X()
**
**  Description:    Compute  correlations of input response h[] with
**                  the target vector X[].
**
**  Links to the text: Section 2.16
**
** Arguments:
**
**      Word16 h[]              Impulse response.
**      Word16 X[]              Target vector.
**      Word16 D[]              Correlations.
**
**  Outputs:
**
**      Word16 D[]              Correlations.
**
**  Return value:           None
*/
void Cor_h_X(Word16 h[], Word16 X[], Word16 D[])
{
   Word16 i, j;
   Word32 s, max;
   Word32 y32[SubFrLen];

   /* first keep the result on 32 bits and find absolute maximum */

   max = 0;

   for (i = 0; i < SubFrLen; i++)
   {
     s = 0;
     for (j = i; j <  SubFrLen; j++)
       s = L_mac(s, X[j], h[j-i]);

     y32[i] = s;

     s = L_abs(s);
     if(s > max) max = s;
   }

   /*
   * Find the number of right shifts to do on y32[]
   * so that maximum is on 13 bits
   */

   j = norm_l(max);
   if( sub(j,16) > 0) j = 16;

   j = sub(18, j);

   for(i=0; i<SubFrLen; i++)
     D[i] = extract_l( L_shr(y32[i], j) );

   return;

}
/*
** Function:            Reset_max_time()
**
**  Description:        This function should be called at the beginning
**                      of each frame.
**
**  Links to the text:  Section 2.16
**
**  Arguments:          None
**
**  Inputs:             None
**
**  Outputs:
**
**      Word16          extra
**
**  Return value:           None
**
*/
static Word16 extra;
void reset_max_time(void)
{
  extra = 120;
  return;
}

/*
**
**  Function:       D4i64_LBC
**
**  Description:       Algebraic codebook for LBC.
**                     -> 17 bits; 4 pulses in a frame of 60 samples
**
**                     The code length is 60, containing 4 nonzero pulses
**                     i0, i1, i2, i3. Each pulse can have 8 possible
**                     positions (positive or negative):
**
**                     i0 (+-1) : 0, 8,  16, 24, 32, 40, 48, 56
**                     i1 (+-1) : 2, 10, 18, 26, 34, 42, 50, 58
**                     i2 (+-1) : 4, 12, 20, 28, 36, 44, 52, (60)
**                     i3 (+-1) : 6, 14, 22, 30, 38, 46, 54, (62)
**
**                     All the pulse can be shifted by one.
**                     The last position of the last 2 pulses falls outside the
**                     frame and signifies that the pulse is not present.
**                     The threshold controls if a section of the innovative
**                     codebook should be searched or not.
**
**  Links to the text: Section 2.16
**
**  Input arguments:
**
**      Word16 Dn[]       Correlation between target vector and impulse response h[]
**      Word16 rr[]       Correlations of impulse response h[]
**      Word16 h[]        Impulse response of filters
**
**  Output arguments:
**
**      Word16 cod[]      Selected algebraic codeword
**      Word16 y[]        Filtered codeword
**      Word16 code_shift Shift of the codeword
**      Word16 sign       Signs of the 4 pulses.
**
**  Return value:
**
**      Word16   Index of selected codevector
**
*/
Word16 D4i64_LBC(Word16 Dn[], Word16 rr[], Word16 h[], Word16 cod[],
                 Word16 y[], Word16 *code_shift, Word16 *sign)
{
   Word16  i0, i1, i2, i3, ip0, ip1, ip2, ip3;
   Word16  i, j, time;
   Word16  shif, shift;
   Word16  ps0, ps1, ps2, ps3, alp, alp0;
   Word32  alp1, alp2, alp3, L32;
   Word16  ps0a, ps1a, ps2a;
   Word16  ps3c, psc, alpha;
   Word16  means, max0, max1, max2, thres;

   Word16  *rri0i0, *rri1i1, *rri2i2, *rri3i3;
   Word16  *rri0i1, *rri0i2, *rri0i3;
   Word16  *rri1i2, *rri1i3, *rri2i3;

   Word16  *ptr_ri0i0, *ptr_ri1i1, *ptr_ri2i2, *ptr_ri3i3;
   Word16  *ptr_ri0i1, *ptr_ri0i2, *ptr_ri0i3;
   Word16  *ptr_ri1i2, *ptr_ri1i3, *ptr_ri2i3;

   Word16  *ptr1_ri0i1, *ptr1_ri0i2, *ptr1_ri0i3;
   Word16  *ptr1_ri1i2, *ptr1_ri1i3, *ptr1_ri2i3;

   Word16  p_sign[SubFrLen2/2];

  /* Init pointers */

  rri0i0 = rr;
  rri1i1 = rri0i0 + NB_POS;
  rri2i2 = rri1i1 + NB_POS;
  rri3i3 = rri2i2 + NB_POS;

  rri0i1 = rri3i3 + NB_POS;
  rri0i2 = rri0i1 + MSIZE;
  rri0i3 = rri0i2 + MSIZE;
  rri1i2 = rri0i3 + MSIZE;
  rri1i3 = rri1i2 + MSIZE;
  rri2i3 = rri1i3 + MSIZE;

 /*
  * Extend the backward filtered target vector by zeros
  */

   for (i = SubFrLen; i < SubFrLen2; i++) Dn[i] = 0;

 /*
  * Chose the sign of the impulse.
  */

   for (i=0; i<SubFrLen; i+=2)
   {
     if( add(Dn[i],Dn[i+1]) >= 0)
     {
       p_sign[i/2] = 1;
     }
     else
     {
       p_sign[i/2] = -1;
       Dn[i] = -Dn[i];
       Dn[i+1] = -Dn[i+1];
     }
   }
   p_sign[30] = p_sign[31] = 1;

 /*
  *   Compute the search threshold after three pulses
  */

   /* odd positions */
   /* Find maximum of Dn[i0]+Dn[i1]+Dn[i2] */

   max0 = Dn[0];
   max1 = Dn[2];
   max2 = Dn[4];
   for (i = 8; i < SubFrLen; i+=STEP)
   {
     if (Dn[i]   > max0) max0 = Dn[i];
     if (Dn[i+2] > max1) max1 = Dn[i+2];
     if (Dn[i+4] > max2) max2 = Dn[i+4];
   }
   max0 = add(max0, max1);
   max0 = add(max0, max2);

   /* Find means of Dn[i0]+Dn[i1]+Dn[i2] */

   L32 = 0;
   for (i = 0; i < SubFrLen; i+=STEP)
   {
     L32 = L_mac(L32, Dn[i], 1);
     L32 = L_mac(L32, Dn[i+2], 1);
     L32 = L_mac(L32, Dn[i+4], 1);
   }
   means =extract_l( L_shr(L32, 4));

   /* thres = means + (max0-means)*threshold; */

   thres = sub(max0, means);
   thres = mult(thres, threshold);
   thres = add(thres, means);

   /* even positions */
   /* Find maximum of Dn[i0]+Dn[i1]+Dn[i2] */

   max0 = Dn[1];
   max1 = Dn[3];
   max2 = Dn[5];
   for (i = 9; i < SubFrLen; i+=STEP)
   {
     if (Dn[i]   > max0) max0 = Dn[i];
     if (Dn[i+2] > max1) max1 = Dn[i+2];
     if (Dn[i+4] > max2) max2 = Dn[i+4];
   }
   max0 = add(max0, max1);
   max0 = add(max0, max2);

   /* Find means of Dn[i0]+Dn[i1]+Dn[i2] */

   L32 = 0;
   for (i = 1; i < SubFrLen; i+=STEP)
   {
     L32 = L_mac(L32, Dn[i], 1);
     L32 = L_mac(L32, Dn[i+2], 1);
     L32 = L_mac(L32, Dn[i+4], 1);
   }
   means =extract_l( L_shr(L32, 4));


   /* max1 = means + (max0-means)*threshold */

   max1 = sub(max0, means);
   max1 = mult(max1, threshold);
   max1 = add(max1, means);

   /* Keep maximum threshold between odd and even position */

   if(max1 > thres) thres = max1;

 /*
  * Modification of rrixiy[] to take signs into account.
  */

  ptr_ri0i1 = rri0i1;
  ptr_ri0i2 = rri0i2;
  ptr_ri0i3 = rri0i3;
  ptr1_ri0i1 = rri0i1;
  ptr1_ri0i2 = rri0i2;
  ptr1_ri0i3 = rri0i3;

  for(i0=0; i0<SubFrLen/2; i0+=STEP/2)
  {
     for(i1=2/2; i1<SubFrLen/2; i1+=STEP/2)
     {
       *ptr_ri0i1++ = i_mult(*ptr1_ri0i1++, i_mult(p_sign[i0], p_sign[i1]));
       *ptr_ri0i2++ = i_mult(*ptr1_ri0i2++, i_mult(p_sign[i0], p_sign[i1+1]));
       *ptr_ri0i3++ = i_mult(*ptr1_ri0i3++, i_mult(p_sign[i0], p_sign[i1+2]));
     }
  }

  ptr_ri1i2 = rri1i2;
  ptr_ri1i3 = rri1i3;
  ptr1_ri1i2 = rri1i2;
  ptr1_ri1i3 = rri1i3;
  for(i1=2/2; i1<SubFrLen/2; i1+=STEP/2)
  {
     for(i2=4/2; i2<SubFrLen2/2; i2+=STEP/2)
     {
       *ptr_ri1i2++ = i_mult(*ptr1_ri1i2++, i_mult(p_sign[i1], p_sign[i2]));
       *ptr_ri1i3++ = i_mult(*ptr1_ri1i3++, i_mult(p_sign[i1], p_sign[i2+1]));

     }
  }

  ptr_ri2i3 = rri2i3;

  ptr1_ri2i3 = rri2i3;
  for(i2=4/2; i2<SubFrLen2/2; i2+=STEP/2)
  {
     for(i3=6/2; i3<SubFrLen2/2; i3+=STEP/2)
       *ptr_ri2i3++ = i_mult(*ptr1_ri2i3++, i_mult(p_sign[i2], p_sign[i3]));
  }

 /*
  * Search the optimum positions of the four  pulses which maximize
  *     square(correlation) / energy
  * The search is performed in four  nested loops. At each loop, one
  * pulse contribution is added to the correlation and energy.
  *
  * The fourth loop is entered only if the correlation due to the
  *  contribution of the first three pulses exceeds the preset
  *  threshold.
  */

 /* Default values */

 ip0    = 0;
 ip1    = 2;
 ip2    = 4;
 ip3    = 6;
 shif   = 0;
 psc    = 0;
 alpha  = 32767;
 time   = add(max_time, extra);



 /* Four loops to search innovation code. */

 ptr_ri0i0 = rri0i0;    /* Init. pointers that depend on first loop */
 ptr_ri0i1 = rri0i1;
 ptr_ri0i2 = rri0i2;
 ptr_ri0i3 = rri0i3;

 for (i0 = 0; i0 < SubFrLen; i0 += STEP)        /* first pulse loop  */
 {
   ps0  = Dn[i0];
   ps0a = Dn[i0+1];
   alp0 = *ptr_ri0i0++;

   ptr_ri1i1 = rri1i1;    /* Init. pointers that depend on second loop */
   ptr_ri1i2 = rri1i2;
   ptr_ri1i3 = rri1i3;

   for (i1 = 2; i1 < SubFrLen; i1 += STEP)      /* second pulse loop */
   {
     ps1  = add(ps0, Dn[i1]);
     ps1a = add(ps0a, Dn[i1+1]);

     /* alp1 = alp0 + *ptr_ri1i1++ + 2.0 * ( *ptr_ri0i1++); */

     alp1 = L_mult(alp0, 1);
     alp1 = L_mac(alp1, *ptr_ri1i1++, 1);
     alp1 = L_mac(alp1, *ptr_ri0i1++, 2);

     ptr_ri2i2 = rri2i2;     /* Init. pointers that depend on third loop */
     ptr_ri2i3 = rri2i3;

     for (i2 = 4; i2 < SubFrLen2; i2 += STEP)    /* third pulse loop */
     {
       ps2  = add(ps1, Dn[i2]);
       ps2a = add(ps1a, Dn[i2+1]);

       /* alp2 = alp1 + *ptr_ri2i2++ + 2.0 * (*ptr_ri0i2++ + *ptr_ri1i2++); */

       alp2 = L_mac(alp1, *ptr_ri2i2++, 1);
       alp2 = L_mac(alp2, *ptr_ri0i2++, 2);
       alp2 = L_mac(alp2, *ptr_ri1i2++, 2);

       /* Decide the shift */

       shift = 0;
       if(ps2a > ps2)
       {
         shift = 1;
         ps2   = ps2a;
       }

       /* Test threshold */

       if ( ps2 > thres)
       {

         ptr_ri3i3 = rri3i3;    /* Init. pointers that depend on 4th loop */

         for (i3 = 6; i3 < SubFrLen2; i3 += STEP)      /* 4th pulse loop */
         {
           ps3 = add(ps2, Dn[i3+shift]);

           /* alp3 = alp2 + *ptr_ri3i3++                                */
           /*       + 2.0*( *ptr_ri0i3++ + *ptr_ri1i3++ + *ptr_ri2i3++); */

           alp3 = L_mac(alp2, *ptr_ri3i3++, 1);
           alp3 = L_mac(alp3, *ptr_ri0i3++, 2);
           alp3 = L_mac(alp3, *ptr_ri1i3++, 2);
           alp3 = L_mac(alp3, *ptr_ri2i3++, 2);
           alp  = extract_l(L_shr(alp3, 5));

           ps3c = mult(ps3, ps3);
           if( L_mult(ps3c, alpha) > L_mult(psc, alp) )
           {