nb_celp.c

mediastreamer2是开源的网络传输媒体流的库

            
            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);*/
               advance = wb_skip_table[submode];
               if (advance < 0)
               {
                  speex_notify("Invalid mode encountered. The stream is corrupted.");
                  return -2;
               } 
               advance -= (SB_SUBMODE_BITS+1);
               speex_bits_advance(bits, advance);
               wideband = speex_bits_unpack_unsigned(bits, 1);
               if (wideband)
               {
                  speex_notify("More than two wideband layers found. The stream is corrupted.");
                  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_notify("Invalid mode encountered. The stream is corrupted.");
            return -2;
         }
      
      } while (m>8);

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

   }

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

   /* 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(QCONST16(0.93f,15), st->interp_qlpc, lpc, st->lpcSize);
      {
         spx_word16_t innov_gain=0;
         /* 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;

      /* Final signal synthesis from excitation */
      iir_mem16(st->exc, 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];
   }

   /* 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=MULT16_16_P15(QCONST16(0.066667,15),SHL16(quant,GAIN_SHIFT));
   }
   
   /* 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
   }

   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_word16_t *innov_save = NULL;
      spx_word16_t tmp;

      /* 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 */
      SPEEX_MEMSET(exc, 0, st->subframeSize);

      /*Adaptive codebook contribution*/
      speex_assert (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;
         }



         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);

         /* Ensuring that things aren't blowing up as would happen if e.g. an encoder is 
         crafting packets to make us produce NaNs and slow down the decoder (vague DoS threat).
         We can probably be even more aggressive and limit to 15000 or so. */
         sanitize_values32(exc32, NEG32(QCONST32(32000,SIG_SHIFT-1)), QCONST32(32000,SIG_SHIFT-1), st->subframeSize);
         
         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;
         }
      }
      
      /* Unquantize the innovation */
      {
         int q_energy;
         spx_word32_t ener;
         
         SPEEX_MEMSET(innov, 0, st->subframeSize);

         /* 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;
         }
                  
         speex_assert (SUBMODE(innovation_unquant));
         {
            /*Fixed codebook contribution*/
            SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack, &st->seed);
            /* De-normalize innovation and update excitation */

            signal_mul(innov, innov, ener, st->subframeSize);

            /* 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);
               SPEEX_MEMSET(innov2, 0, st->subframeSize);
               SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack, &st->seed);
               signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545f,15),ener), st->subframeSize);
               for (i=0;i<st->subframeSize;i++)
                  innov[i] = ADD32(innov[i], innov2[i]);
               stack = tmp_stack;
            }
            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] = EXTRACT16(PSHR32(innov[i], SIG_SHIFT));
            }
         }

         /*Vocoder mode*/
         if (st->submodeID==1) 
         {
            spx_word16_t g=ol_pitch_coef;
            g=MULT16_16_P14(QCONST16(1.5f,14),(g-QCONST16(.2f,6)));
            if (g<0)
               g=0;
            if (g>GAIN_SCALING)
               g=GAIN_SCALING;
            
            SPEEX_MEMSET(exc, 0, st->subframeSize);
            while (st->voc_offset<st->subframeSize)
            {
               /* exc[st->voc_offset]= g*sqrt(2*ol_pitch)*ol_gain;
                  Not quite sure why we need the factor of two in the sqrt */
               if (st->voc_offset>=0)
                  exc[st->voc_offset]=MULT16_16(spx_sqrt(MULT16_16_16(2,ol_pitch)),EXTRACT16(PSHR32(MULT16_16(g,PSHR32(ol_gain,SIG_SHIFT)),6)));
               st->voc_offset+=ol_pitch;
            }
            st->voc_offset -= st->subframeSize;
            
            for (i=0;i<st->subframeSize;i++)
            {
               spx_word16_t exci=exc[i];
               exc[i]= ADD16(ADD16(MULT16_16_Q15(QCONST16(.7f,15),exc[i]) , MULT16_16_Q15(QCONST16(.3f,15),st->voc_m1)),
                             SUB16(MULT16_16_Q15(Q15_ONE-MULT16_16_16(QCONST16(.85f,9),g),EXTRACT16(PSHR32(innov[i],SIG_SHIFT))),
                                   MULT16_16_Q15(MULT16_16_16(QCONST16(.15f,9),g),EXTRACT16(PSHR32(st->voc_m2,SIG_SHIFT)))
                                  ));
               st->voc_m1 = exci;
               st->voc_m2=innov[i];
               st->voc_mean = EXTRACT16(PSHR32(ADD32(MULT16_16(QCONST16(.8f,15),st->voc_mean), MULT16_16(QCONST16(.2f,15),exc[i])), 15));
               exc[i]-=st->voc_mean;
            }
         }

      }
   }
   
   ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);

   if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0 && !st->count_lost)
   {
      multicomb(st->exc-st->subframeSize, out, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, best_pitch, 40, SUBMODE(comb_gain), stack);
      multicomb(st->exc+st->subframeSize, out+2*st->subframeSize, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, best_pitch, 40, SUBMODE(comb_gain), stack);
   } else {
      SPEEX_COPY(out, &st->exc[-st->subframeSize], st->frameSize);
   }
   
   /* If the last packet was lost, re-scale the excitation to obtain the same energy as encoded in ol_gain */
   if (st->count_lost) 
   {
      spx_word16_t exc_ener;
      spx_word32_t gain32;
      spx_word16_t gain;
      exc_ener = compute_rms16 (st->exc, st->frameSize);
      gain32 = PDIV32(ol_gain, ADD16(exc_ener,1));
#ifdef FIXED_POINT
      if (gain32 > 32767)
         gain32 = 32767;
      gain = EXTRACT16(gain32);
#else
      if (gain32 > 2)
         gain32=2;
      gain = gain32;
#endif
      for (i=0;i<st->frameSize;i++)
      {
         st->exc[i] = MULT16_16_Q14(gain, st->exc[i]);
         out[i]=st->exc[i-st->subframeSize];
      }
   }

   /*Loop on subframes */
   for (sub=0;sub<st->nbSubframes;sub++)
   {
      int offset;
      spx_word16_t *sp;
      spx_word16_t *exc;
      /* Offset relative to start of frame */
      offset = st->subframeSize*sub;
      /* Original signal */
      sp=out+offset;
      /* Excitation */
      exc=st->exc+offset;

      /* LSP interpolation (quantized and unquantized) */
      lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);

      /* Make sure the LSP's are stable */
      lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);

      /* Compute interpolated LPCs (unquantized) */
      lsp_to_lpc(interp_qlsp, ak, st->lpcSize, stack);

      /* Compute analysis filter at w=pi */
      {
         spx_word32_t pi_g=LPC_SCALING;