nb_celp.c

来自「基于sip协议的网络电话源码」· C语言 代码 · 共 2,058 行 · 第 1/5 页

   stack=st->stack;

   /* Check if we're in DTX mode*/
   if (!bits && st->dtx_enabled)
   {
      st->submodeID=0;
   } else 
   {
      /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
      if (!bits)
      {
         nb_decode_lost(st, out, stack);
         return 0;
      }

      if (st->encode_submode)
      {
#ifdef EPIC_48K
         if (!st->lbr_48k) {
#endif

      /* Search for next narrowband block (handle requests, skip wideband blocks) */
      do {
         if (speex_bits_remaining(bits)<5)
            return -1;
         wideband = speex_bits_unpack_unsigned(bits, 1);
         if (wideband) /* Skip wideband block (for compatibility) */
         {
            int submode;
            int advance;
            advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
            speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
            if (advance < 0)
            {
               speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
               return -2;
            } 
            advance -= (SB_SUBMODE_BITS+1);
            speex_bits_advance(bits, advance);
            
            if (speex_bits_remaining(bits)<5)
               return -1;
            wideband = speex_bits_unpack_unsigned(bits, 1);
            if (wideband)
            {
               advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
               speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
               if (advance < 0)
               {
                  speex_warning ("Invalid wideband mode encountered: corrupted stream?");
                  return -2;
               } 
               advance -= (SB_SUBMODE_BITS+1);
               speex_bits_advance(bits, advance);
               wideband = speex_bits_unpack_unsigned(bits, 1);
               if (wideband)
               {
                  speex_warning ("More than two wideband layers found: corrupted stream?");
                  return -2;
               }

            }
         }
         if (speex_bits_remaining(bits)<4)
            return -1;
         /* FIXME: Check for overflow */
         m = speex_bits_unpack_unsigned(bits, 4);
         if (m==15) /* We found a terminator */
         {
            return -1;
         } else if (m==14) /* Speex in-band request */
         {
            int ret = speex_inband_handler(bits, st->speex_callbacks, state);
            if (ret)
               return ret;
         } else if (m==13) /* User in-band request */
         {
            int ret = st->user_callback.func(bits, state, st->user_callback.data);
            if (ret)
               return ret;
         } else if (m>8) /* Invalid mode */
         {
            speex_warning("Invalid mode encountered: corrupted stream?");
            return -2;
         }
      
      } while (m>8);

      /* Get the sub-mode that was used */
      st->submodeID = m;
#ifdef EPIC_48K
         }
#endif
      }

   }

   /* Shift all buffers by one frame */
   speex_move(st->excBuf, st->excBuf+st->frameSize, (2*st->max_pitch + st->subframeSize + 12)*sizeof(spx_word16_t));

   /* If null mode (no transmission), just set a couple things to zero*/
   if (st->submodes[st->submodeID] == NULL)
   {
      VARDECL(spx_coef_t *lpc);
      ALLOC(lpc, st->lpcSize, spx_coef_t);
      bw_lpc(GAMMA_SCALING*.93, st->interp_qlpc, lpc, st->lpcSize);
      {
         float innov_gain=0;
         float pgain=GAIN_SCALING_1*st->last_pitch_gain;
         if (pgain>.6)
            pgain=.6;
         /* FIXME: This was innov, not exc */
         innov_gain = compute_rms16(st->exc, st->frameSize);
         for (i=0;i<st->frameSize;i++)
            st->exc[i]=speex_rand(innov_gain, &st->seed);
      }


      st->first=1;

      for (i=0;i<st->frameSize;i++)
         out[i] = st->exc[i];
      /* Final signal synthesis from excitation */
      iir_mem16(out, lpc, out, st->frameSize, st->lpcSize, st->mem_sp, stack);

      st->count_lost=0;
      return 0;
   }

   ALLOC(qlsp, st->lpcSize, spx_lsp_t);

   /* Unquantize LSPs */
   SUBMODE(lsp_unquant)(qlsp, st->lpcSize, bits);

   /*Damp memory if a frame was lost and the LSP changed too much*/
   if (st->count_lost)
   {
      spx_word16_t fact;
      spx_word32_t lsp_dist=0;
      for (i=0;i<st->lpcSize;i++)
         lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - qlsp[i])));
#ifdef FIXED_POINT
      fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));      
#else
      fact = .6*exp(-.2*lsp_dist);
#endif
      for (i=0;i<st->lpcSize;i++)
         st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
   }


   /* Handle first frame and lost-packet case */
   if (st->first || st->count_lost)
   {
      for (i=0;i<st->lpcSize;i++)
         st->old_qlsp[i] = qlsp[i];
   }

#ifdef EPIC_48K
   if (st->lbr_48k) {
      pitch_half[0] = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
      pitch_half[1] = pitch_half[0]+speex_bits_unpack_unsigned(bits, 2)-1;

      ol_pitch_id = speex_bits_unpack_unsigned(bits, 3);
      ol_pitch_coef=GAIN_SCALING*0.13514*ol_pitch_id;

      {
         int qe;
         qe = speex_bits_unpack_unsigned(bits, 4);
         ol_gain = SIG_SCALING*exp((qe+2)/2.1),SIG_SHIFT;
      }

   } else {
#endif

   /* Get open-loop pitch estimation for low bit-rate pitch coding */
   if (SUBMODE(lbr_pitch)!=-1)
   {
      ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
   } 
   
   if (SUBMODE(forced_pitch_gain))
   {
      int quant;
      quant = speex_bits_unpack_unsigned(bits, 4);
      ol_pitch_coef=GAIN_SCALING*0.066667*quant;
   }
   
   /* Get global excitation gain */
   {
      int qe;
      qe = speex_bits_unpack_unsigned(bits, 5);
#ifdef FIXED_POINT
      /* FIXME: Perhaps we could slightly lower the gain here when the output is going to saturate? */
      ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
#else
      ol_gain = SIG_SCALING*exp(qe/3.5);
#endif
   }
#ifdef EPIC_48K
   }
#endif

   ALLOC(ak, st->lpcSize, spx_coef_t);
   ALLOC(innov, st->subframeSize, spx_sig_t);
   ALLOC(exc32, st->subframeSize, spx_word32_t);

   if (st->submodeID==1)
   {
      int extra;
      extra = speex_bits_unpack_unsigned(bits, 4);

      if (extra==15)
         st->dtx_enabled=1;
      else
         st->dtx_enabled=0;
   }
   if (st->submodeID>1)
      st->dtx_enabled=0;

   /*Loop on subframes */
   for (sub=0;sub<st->nbSubframes;sub++)
   {
      int offset;
      spx_word16_t *exc;
      spx_word16_t *sp;
      spx_sig_t *innov_save = NULL;
      spx_word16_t tmp;

#ifdef EPIC_48K
      if (st->lbr_48k)
      {
         if (sub*2 < st->nbSubframes)
            ol_pitch = pitch_half[0];
         else
            ol_pitch = pitch_half[1];
      }
#endif

      /* Offset relative to start of frame */
      offset = st->subframeSize*sub;
      /* Excitation */
      exc=st->exc+offset;
      /* Original signal */
      sp=out+offset;
      if (st->innov_save)
         innov_save = st->innov_save+offset;


      /* Reset excitation */
      for (i=0;i<st->subframeSize;i++)
         exc[i]=0;

      /*Adaptive codebook contribution*/
      if (SUBMODE(ltp_unquant))
      {
         int pit_min, pit_max;
         /* Handle pitch constraints if any */
         if (SUBMODE(lbr_pitch) != -1)
         {
            int margin;
            margin = SUBMODE(lbr_pitch);
            if (margin)
            {
/* GT - need optimization?
               if (ol_pitch < st->min_pitch+margin-1)
                  ol_pitch=st->min_pitch+margin-1;
               if (ol_pitch > st->max_pitch-margin)
                  ol_pitch=st->max_pitch-margin;
               pit_min = ol_pitch-margin+1;
               pit_max = ol_pitch+margin;
*/
               pit_min = ol_pitch-margin+1;
               if (pit_min < st->min_pitch)
		  pit_min = st->min_pitch;
               pit_max = ol_pitch+margin;
               if (pit_max > st->max_pitch)
		  pit_max = st->max_pitch;
            } else {
               pit_min = pit_max = ol_pitch;
            }
         } else {
            pit_min = st->min_pitch;
            pit_max = st->max_pitch;
         }


#ifdef EPIC_48K
         if (st->lbr_48k)
         {
             SUBMODE(ltp_unquant)(exc, exc32, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
                                  st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
                                  st->count_lost, offset, st->last_pitch_gain, ol_pitch_id);
         } else {
#endif

             SUBMODE(ltp_unquant)(exc, exc32, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
                                  st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
                                  st->count_lost, offset, st->last_pitch_gain, 0);

#ifdef EPIC_48K
         }
#endif

         tmp = gain_3tap_to_1tap(pitch_gain);

         pitch_average += tmp;
         if ((tmp>best_pitch_gain&&ABS(2*best_pitch-pitch)>=3&&ABS(3*best_pitch-pitch)>=4&&ABS(4*best_pitch-pitch)>=5) 
              || (tmp>MULT16_16_Q15(QCONST16(.6,15),best_pitch_gain)&&(ABS(best_pitch-2*pitch)<3||ABS(best_pitch-3*pitch)<4||ABS(best_pitch-4*pitch)<5)) 
              || (MULT16_16_Q15(QCONST16(.67,15),tmp)>best_pitch_gain&&(ABS(2*best_pitch-pitch)<3||ABS(3*best_pitch-pitch)<4||ABS(4*best_pitch-pitch)<5)) )
         {
            best_pitch = pitch;
            if (tmp > best_pitch_gain)
               best_pitch_gain = tmp;
         }
      } else {
         speex_error("No pitch prediction, what's wrong");
      }
      
      /* Unquantize the innovation */
      {
         int q_energy;
         spx_word32_t ener;
         
         for (i=0;i<st->subframeSize;i++)
            innov[i]=0;

         /* Decode sub-frame gain correction */
         if (SUBMODE(have_subframe_gain)==3)
         {
            q_energy = speex_bits_unpack_unsigned(bits, 3);
            ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
         } else if (SUBMODE(have_subframe_gain)==1)
         {
            q_energy = speex_bits_unpack_unsigned(bits, 1);
            ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
         } else {
            ener = ol_gain;
         }
                  
         if (SUBMODE(innovation_unquant))
         {
            /*Fixed codebook contribution*/
            SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack, &st->seed);
         } else {
            speex_error("No fixed codebook");
         }

         /* De-normalize innovation and update excitation */
#ifdef FIXED_POINT
         signal_mul(innov, innov, ener, st->subframeSize);
#else
         signal_mul(innov, innov, ener, st->subframeSize);
#endif
         /*Vocoder mode*/
         if (st->submodeID==1) 
         {
            float g=ol_pitch_coef*GAIN_SCALING_1;

            
            for (i=0;i<st->subframeSize;i++)
               exc[i]=0;
            while (st->voc_offset<st->subframeSize)
            {
               if (st->voc_offset>=0)
                  exc[st->voc_offset]=sqrt(1.0*ol_pitch);
               st->voc_offset+=ol_pitch;
            }
            st->voc_offset -= st->subframeSize;

            g=.5+2*(g-.6);
            if (g<0)
               g=0;
            if (g>1)
               g=1;
            for (i=0;i<st->subframeSize;i++)
            {
               spx_word16_t exci=exc[i];
               /* FIXME: cleanup the innov[i]/SIG_SCALING */
               exc[i]=.8*g*exc[i]*PSHR32(ol_gain,SIG_SHIFT) + .6*g*st->voc_m1*PSHR32(ol_gain,SIG_SHIFT) + (1-.5*g)*PSHR32(innov[i],SIG_SHIFT) - .5*g*PSHR32(st->voc_m2,SIG_SHIFT);
               st->voc_m1 = exci;
               st->voc_m2=innov[i];
               st->voc_mean = .95*st->voc_mean + .05*exc[i];
               exc[i]-=st->voc_mean;
            }
         } else {
            for (i=0;i<st->subframeSize;i++)
               exc[i]=EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));
            /*print_vec(exc, 40, "innov");*/
         }
         if (innov_save)
         {
            for (i=0;i<st->subframeSize;i++)
               innov_save[i] = innov[i];
         }
         /* Decode second codebook (only for some modes) */
         if (SUBMODE(double_codebook))
         {
            char *tmp_stack=stack;
            VARDECL(spx_sig_t *innov2);
            ALLOC(innov2, st->subframeSize, spx_sig_t);
            for (i=0;i<st->subframeSize;i++)
               innov2[i]=0;
            SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack, &st->seed);
            signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545,15),ener), st->subframeSize);
            for (i=0;i<st->subframeSize;i++)
               exc[i] = EXTRACT16(SATURATE32(ADD32(EXTEND32(exc[i]),PSHR32(innov2[i],SIG_SHIFT)),32767));
            if (innov_save)
            {
               for (i=0;i<st->subframeSize;i++)
                  innov_save[i] = ADD32(innov_save[i],innov2[i]);
            }