ltp.c

一个开源SIP协议栈

      for (j=0;j<p;j++)
         mm[j] = 0;
      filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack);
      for (j=0;j<nsf;j++)
         x[2][j] = e[j];
   }
   for (i=1;i>=0;i--)
   {
      spx_word16_t e0=exc2[-pitch-1+i];
#ifdef FIXED_POINT
      /* Scale excitation down if needed (avoiding overflow) */
      if (scaledown)
         e0 = SHR16(e0,1);
#endif
      x[i][0]=MULT16_16_Q14(r[0], e0);
      for (j=0;j<nsf-1;j++)
         x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0));
   }

   for (i=0;i<3;i++)
      corr[i]=inner_prod(x[i],new_target,nsf);
   for (i=0;i<3;i++)
      for (j=0;j<=i;j++)
         A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);

   {
      spx_word32_t C[9];
#ifdef FIXED_POINT
      spx_word16_t C16[9];
#else
      spx_word16_t *C16=C;
#endif      
      C[0]=corr[2];
      C[1]=corr[1];
      C[2]=corr[0];
      C[3]=A[1][2];
      C[4]=A[0][1];
      C[5]=A[0][2];      
      C[6]=A[2][2];
      C[7]=A[1][1];
      C[8]=A[0][0];
      
      /*plc_tuning *= 2;*/
      if (plc_tuning<2)
         plc_tuning=2;
      if (plc_tuning>30)
         plc_tuning=30;
#ifdef FIXED_POINT
      C[0] = SHL32(C[0],1);
      C[1] = SHL32(C[1],1);
      C[2] = SHL32(C[2],1);
      C[3] = SHL32(C[3],1);
      C[4] = SHL32(C[4],1);
      C[5] = SHL32(C[5],1);
      C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]);
      C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]);
      C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]);
      normalize16(C, C16, 32767, 9);
#else
      C[6]*=.5*(1+.02*plc_tuning);
      C[7]*=.5*(1+.02*plc_tuning);
      C[8]*=.5*(1+.02*plc_tuning);
#endif

      best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);

#ifdef FIXED_POINT
      gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]);
      gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]);
      gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]);
      /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
#else
      gain[0] = 0.015625*gain_cdbk[best_cdbk*4]  + .5;
      gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5;
      gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5;
#endif
      *cdbk_index=best_cdbk;
   }

   for (i=0;i<nsf;i++)
      exc[i]=0;
   for (i=0;i<3;i++)
   {
      int j;
      int tmp1, tmp3;
      int pp=pitch+1-i;
      tmp1=nsf;
      if (tmp1>pp)
         tmp1=pp;
      for (j=0;j<tmp1;j++)
         exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]);
      tmp3=nsf;
      if (tmp3>pp+pitch)
         tmp3=pp+pitch;
      for (j=tmp1;j<tmp3;j++)
         exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]);
   }
   for (i=0;i<nsf;i++)
   {
      spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])),
                            MULT16_16(gain[2],x[0][i]));
      new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp,6)));
   }
   err = inner_prod(new_target, new_target, nsf);

   return err;
}

/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
int pitch_search_3tap(
spx_word16_t target[],                 /* Target vector */
spx_word16_t *sw,
spx_coef_t ak[],                     /* LPCs for this subframe */
spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[],                    /* Excitation */
const void *par,
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
int   p,                        /* Number of LPC coeffs */
int   nsf,                      /* Number of samples in subframe */
SpeexBits *bits,
char *stack,
spx_word16_t *exc2,
spx_word16_t *r,
int complexity,
int cdbk_offset,
int plc_tuning,
spx_word32_t *cumul_gain
)
{
   int i,j;
   int cdbk_index, pitch=0, best_gain_index=0;
   VARDECL(spx_sig_t *best_exc);
   VARDECL(spx_word16_t *new_target);
   VARDECL(spx_word16_t *best_target);
   int best_pitch=0;
   spx_word32_t err, best_err=-1;
   int N;
   const ltp_params *params;
   const signed char *gain_cdbk;
   int   gain_cdbk_size;
   int scaledown=0;
         
   VARDECL(int *nbest);
   
   params = (const ltp_params*) par;
   gain_cdbk_size = 1<<params->gain_bits;
   gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
   
   N=complexity;
   if (N>10)
      N=10;
   if (N<1)
      N=1;

   ALLOC(nbest, N, int);
   params = (const ltp_params*) par;

   if (end<start)
   {
      speex_bits_pack(bits, 0, params->pitch_bits);
      speex_bits_pack(bits, 0, params->gain_bits);
      for (i=0;i<nsf;i++)
         exc[i]=0;
      return start;
   }
   
#ifdef FIXED_POINT
   /* Check if we need to scale everything down in the pitch search to avoid overflows */
   for (i=0;i<nsf;i++)
   {
      if (ABS16(target[i])>16383)
      {
         scaledown=1;
         break;
      }
   }
   for (i=-end;i<nsf;i++)
   {
      if (ABS16(exc2[i])>16383)
      {
         scaledown=1;
         break;
      }
   }
#endif
   if (N>end-start+1)
      N=end-start+1;
   if (end != start)
      open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack);
   else
      nbest[0] = start;
   
   ALLOC(best_exc, nsf, spx_sig_t);
   ALLOC(new_target, nsf, spx_word16_t);
   ALLOC(best_target, nsf, spx_word16_t);
   
   for (i=0;i<N;i++)
   {
      pitch=nbest[i];
      for (j=0;j<nsf;j++)
         exc[j]=0;
      err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf,
                                 bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain, scaledown);
      if (err<best_err || best_err<0)
      {
         for (j=0;j<nsf;j++)
            best_exc[j]=exc[j];
         for (j=0;j<nsf;j++)
            best_target[j]=new_target[j];
         best_err=err;
         best_pitch=pitch;
         best_gain_index=cdbk_index;
      }
   }
   /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
   speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
   speex_bits_pack(bits, best_gain_index, params->gain_bits);
#ifdef FIXED_POINT
   *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain));
#else
   *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3];
#endif
   /*printf ("%f\n", cumul_gain);*/
   /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
   for (i=0;i<nsf;i++)
      exc[i]=best_exc[i];
   for (i=0;i<nsf;i++)
      target[i]=best_target[i];
#ifdef FIXED_POINT
   /* Scale target back up if needed */
   if (scaledown)
   {
      for (i=0;i<nsf;i++)
         target[i]=SHL16(target[i],1);
   }
#endif
   return pitch;
}

void pitch_unquant_3tap(
spx_word16_t exc[],             /* Input excitation */
spx_word32_t exc_out[],         /* Output excitation */
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */
const void *par,
int   nsf,                      /* Number of samples in subframe */
int *pitch_val,
spx_word16_t *gain_val,
SpeexBits *bits,
char *stack,
int count_lost,
int subframe_offset,
spx_word16_t last_pitch_gain,
int cdbk_offset
)
{
   int i;
   int pitch;
   int gain_index;
   spx_word16_t gain[3];
   const signed char *gain_cdbk;
   int gain_cdbk_size;
   const ltp_params *params;

   params = (const ltp_params*) par;
   gain_cdbk_size = 1<<params->gain_bits;
   gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;

   pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
   pitch += start;
   gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
   /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
#ifdef FIXED_POINT
   gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]);
   gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]);
   gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]);
#else
   gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5;
   gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5;
   gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5;
#endif

   if (count_lost && pitch > subframe_offset)
   {
      spx_word16_t gain_sum;
      if (1) {
#ifdef FIXED_POINT
         spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1);
         if (tmp>62)
            tmp=62;
#else
         spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain;
         if (tmp>.95)
            tmp=.95;
#endif
         gain_sum = gain_3tap_to_1tap(gain);

         if (gain_sum > tmp)
         {
            spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum);
            for (i=0;i<3;i++)
               gain[i]=MULT16_16_Q14(fact,gain[i]);
         }

      }

   }

   *pitch_val = pitch;
   gain_val[0]=gain[0];
   gain_val[1]=gain[1];
   gain_val[2]=gain[2];
   gain[0] = SHL16(gain[0],7);
   gain[1] = SHL16(gain[1],7);
   gain[2] = SHL16(gain[2],7);
   for (i=0;i<nsf;i++)
      exc_out[i]=0;
   for (i=0;i<3;i++)
   {
      int j;
      int tmp1, tmp3;
      int pp=pitch+1-i;
      tmp1=nsf;
      if (tmp1>pp)
         tmp1=pp;
      for (j=0;j<tmp1;j++)
         exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]);
      tmp3=nsf;
      if (tmp3>pp+pitch)
         tmp3=pp+pitch;
      for (j=tmp1;j<tmp3;j++)
         exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]);
   }
   /*for (i=0;i<nsf;i++)
   exc[i]=PSHR32(exc32[i],13);*/
}


/** Forced pitch delay and gain */
int forced_pitch_quant(
spx_word16_t target[],                 /* Target vector */
spx_word16_t *sw,
spx_coef_t ak[],                     /* LPCs for this subframe */
spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
spx_sig_t exc[],                    /* Excitation */
const void *par,
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
int   p,                        /* Number of LPC coeffs */
int   nsf,                      /* Number of samples in subframe */
SpeexBits *bits,
char *stack,
spx_word16_t *exc2,
spx_word16_t *r,
int complexity,
int cdbk_offset,
int plc_tuning,
spx_word32_t *cumul_gain
)
{
   int i;
   VARDECL(spx_sig_t *res);
   ALLOC(res, nsf, spx_sig_t);
#ifdef FIXED_POINT
   if (pitch_coef>63)
      pitch_coef=63;
#else
   if (pitch_coef>.99)
      pitch_coef=.99;
#endif
   for (i=0;i<nsf&&i<start;i++)
   {
      exc[i]=MULT16_16(SHL16(pitch_coef, 7),exc2[i-start]);
   }
   for (;i<nsf;i++)
   {
      exc[i]=MULT16_32_Q15(SHL16(pitch_coef, 9),exc[i-start]);
   }
   syn_percep_zero(exc, ak, awk1, awk2, res, nsf, p, stack);
   for (i=0;i<nsf;i++)
      target[i]=EXTRACT16(SATURATE(SUB32(EXTEND32(target[i]),PSHR32(res[i],SIG_SHIFT-1)),32700));
   return start;
}

/** Unquantize forced pitch delay and gain */
void forced_pitch_unquant(
spx_word16_t exc[],             /* Input excitation */
spx_word32_t exc_out[],         /* Output excitation */
int   start,                    /* Smallest pitch value allowed */
int   end,                      /* Largest pitch value allowed */
spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */
const void *par,
int   nsf,                      /* Number of samples in subframe */
int *pitch_val,
spx_word16_t *gain_val,
SpeexBits *bits,
char *stack,
int count_lost,
int subframe_offset,
spx_word16_t last_pitch_gain,
int cdbk_offset
)
{
   int i;
#ifdef FIXED_POINT
   if (pitch_coef>63)
      pitch_coef=63;
#else
   if (pitch_coef>.99)
      pitch_coef=.99;
#endif
   for (i=0;i<nsf;i++)
   {
      exc_out[i]=MULT16_16(exc[i-start],SHL16(pitch_coef,7));
      exc[i] = PSHR(exc_out[i],13);
   }
   *pitch_val = start;
   gain_val[0]=gain_val[2]=0;
   gain_val[1] = pitch_coef;
}