nb_celp.c

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

         /* Long-term prediction */
         if (SUBMODE(lbr_pitch) != -1)
         {
            /* Low bit-rate pitch handling */
            int margin;
            margin = SUBMODE(lbr_pitch);
            if (margin)
            {
               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;
            } else {
               pit_min=pit_max=ol_pitch;
            }
         } else {
            pit_min = st->min_pitch;
            pit_max = st->max_pitch;
         }
         
         /* Force pitch to use only the current frame if needed */
         if (st->bounded_pitch && pit_max>offset)
            pit_max=offset;

         /* Perform pitch search */
         pitch = SUBMODE(ltp_quant)(target, sw, interp_qlpc, bw_lpc1, bw_lpc2,
                                    exc32, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
                                    st->lpcSize, st->subframeSize, bits, stack, 
                                    exc, syn_resp, st->complexity, 0, st->plc_tuning, &st->cumul_gain);

         st->pitch[sub]=pitch;
      }
      /* Quantization of innovation */
      SPEEX_MEMSET(innov, 0, st->subframeSize);
      
      /* FIXME: Make sure this is save from overflows (so far so good) */
      for (i=0;i<st->subframeSize;i++)
         real_exc[i] = EXTRACT16(SUB32(EXTEND32(real_exc[i]), PSHR32(exc32[i],SIG_SHIFT-1)));
      
      ener = SHL32(EXTEND32(compute_rms16(real_exc, st->subframeSize)),SIG_SHIFT);
      
      /*FIXME: Should use DIV32_16 and make sure result fits in 16 bits */
#ifdef FIXED_POINT
      {
         spx_word32_t f = PDIV32(ener,PSHR32(ol_gain,SIG_SHIFT));
         if (f<=32767)
            fine_gain = f;
         else
            fine_gain = 32767;
      }
#else
      fine_gain = PDIV32_16(ener,PSHR32(ol_gain,SIG_SHIFT));
#endif
      /* Calculate gain correction for the sub-frame (if any) */
      if (SUBMODE(have_subframe_gain)) 
      {
         int qe;
         if (SUBMODE(have_subframe_gain)==3)
         {
            qe = scal_quant(fine_gain, exc_gain_quant_scal3_bound, 8);
            speex_bits_pack(bits, qe, 3);
            ener=MULT16_32_Q14(exc_gain_quant_scal3[qe],ol_gain);
         } else {
            qe = scal_quant(fine_gain, exc_gain_quant_scal1_bound, 2);
            speex_bits_pack(bits, qe, 1);
            ener=MULT16_32_Q14(exc_gain_quant_scal1[qe],ol_gain);               
         }
      } else {
         ener=ol_gain;
      }
      
      /*printf ("%f %f\n", ener, ol_gain);*/
      
      /* Normalize innovation */
      signal_div(target, target, ener, st->subframeSize);
      
      /* Quantize innovation */
      speex_assert (SUBMODE(innovation_quant));
      {
         /* Codebook search */
         SUBMODE(innovation_quant)(target, interp_qlpc, bw_lpc1, bw_lpc2, 
                  SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
                  innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
         
         /* De-normalize innovation and update excitation */
         signal_mul(innov, innov, ener, st->subframeSize);
         
         for (i=0;i<st->subframeSize;i++)
            exc[i] = EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));

         /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
         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);
            for (i=0;i<st->subframeSize;i++)
               target[i]=MULT16_16_P13(QCONST16(2.2f,13), target[i]);
            SUBMODE(innovation_quant)(target, interp_qlpc, bw_lpc1, bw_lpc2, 
                                      SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
                                      innov2, syn_resp, bits, stack, st->complexity, 0);
            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));
         if (st->innov_rms_save)
         {
            st->innov_rms_save[sub] = compute_rms(innov, st->subframeSize);
         }
      }

      /* Final signal synthesis from excitation */
      iir_mem16(exc, interp_qlpc, sw, st->subframeSize, st->lpcSize, st->mem_sp, stack);

      /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
      if (st->complexity!=0)
         filter_mem16(sw, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw, stack);
      
   }

   /* Store the LSPs for interpolation in the next frame */
   if (st->submodeID>=1)
   {
      for (i=0;i<st->lpcSize;i++)
         st->old_lsp[i] = lsp[i];
      for (i=0;i<st->lpcSize;i++)
         st->old_qlsp[i] = qlsp[i];
   }

#ifdef VORBIS_PSYCHO
   if (st->submodeID>=1)
      SPEEX_COPY(st->old_curve, st->curve, 128);
#endif

   if (st->submodeID==1)
   {
#ifndef DISABLE_VBR
      if (st->dtx_count)
         speex_bits_pack(bits, 15, 4);
      else
#endif
         speex_bits_pack(bits, 0, 4);
   }

   /* The next frame will not be the first (Duh!) */
   st->first = 0;
   SPEEX_COPY(st->winBuf, in+2*st->frameSize-st->windowSize, st->windowSize-st->frameSize);

   if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
      st->bounded_pitch = 1;
   else
      st->bounded_pitch = 0;

   return 1;
}

void *nb_decoder_init(const SpeexMode *m)
{
   DecState *st;
   const SpeexNBMode *mode;
   int i;

   mode=(const SpeexNBMode*)m->mode;
   st = (DecState *)speex_alloc(sizeof(DecState));
   if (!st)
      return NULL;
#if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
   st->stack = NULL;
#else
   st->stack = (char*)speex_alloc_scratch(NB_DEC_STACK);
#endif

   st->mode=m;


   st->encode_submode = 1;

   st->first=1;
   /* Codec parameters, should eventually have several "modes"*/
   st->frameSize = mode->frameSize;
   st->nbSubframes=mode->frameSize/mode->subframeSize;
   st->subframeSize=mode->subframeSize;
   st->lpcSize = mode->lpcSize;
   st->min_pitch=mode->pitchStart;
   st->max_pitch=mode->pitchEnd;

   st->submodes=mode->submodes;
   st->submodeID=mode->defaultSubmode;

   st->lpc_enh_enabled=1;

   st->excBuf = (spx_word16_t*)speex_alloc((st->frameSize + 2*st->max_pitch + st->subframeSize + 12)*sizeof(spx_word16_t));
   st->exc = st->excBuf + 2*st->max_pitch + st->subframeSize + 6;
   SPEEX_MEMSET(st->excBuf, 0, st->frameSize + st->max_pitch);

   st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
   st->old_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
   st->mem_sp = (spx_mem_t*)speex_alloc(st->lpcSize*sizeof(spx_mem_t));
   st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
   st->last_pitch = 40;
   st->count_lost=0;
   st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
   st->pitch_gain_buf_idx = 0;
   st->seed = 1000;
   
   st->sampling_rate=8000;
   st->last_ol_gain = 0;

   st->user_callback.func = &speex_default_user_handler;
   st->user_callback.data = NULL;
   for (i=0;i<16;i++)
      st->speex_callbacks[i].func = NULL;

   st->voc_m1=st->voc_m2=st->voc_mean=0;
   st->voc_offset=0;
   st->dtx_enabled=0;
   st->isWideband = 0;
   st->highpass_enabled = 1;

#ifdef ENABLE_VALGRIND
   VALGRIND_MAKE_READABLE(st, NB_DEC_STACK);
#endif
   return st;
}

void nb_decoder_destroy(void *state)
{
   DecState *st;
   st=(DecState*)state;
   
#if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
   speex_free_scratch(st->stack);
#endif

   speex_free (st->excBuf);
   speex_free (st->interp_qlpc);
   speex_free (st->old_qlsp);
   speex_free (st->mem_sp);
   speex_free (st->pi_gain);

   speex_free(state);
}

#define median3(a, b, c)	((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))

#ifdef FIXED_POINT
const spx_word16_t attenuation[10] = {32767, 31483, 27923, 22861, 17278, 12055, 7764, 4616, 2533, 1283};
#else
const spx_word16_t attenuation[10] = {1., 0.961, 0.852, 0.698, 0.527, 0.368, 0.237, 0.141, 0.077, 0.039};

#endif

static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
{
   int i;
   int pitch_val;
   spx_word16_t pitch_gain;
   spx_word16_t fact;
   spx_word16_t gain_med;
   spx_word16_t innov_gain;
   spx_word16_t noise_gain;
   
   if (st->count_lost<10)
      fact = attenuation[st->count_lost];
   else
      fact = 0;

   gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
   if (gain_med < st->last_pitch_gain)
      st->last_pitch_gain = gain_med;
   
#ifdef FIXED_POINT
   pitch_gain = st->last_pitch_gain;
   if (pitch_gain>54)
      pitch_gain = 54;
   pitch_gain = SHL16(pitch_gain, 9);
#else   
   pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
   if (pitch_gain>.85)
      pitch_gain=.85;
#endif
   pitch_gain = MULT16_16_Q15(fact,pitch_gain) + VERY_SMALL;
   /* FIXME: This was rms of innovation (not exc) */
   innov_gain = compute_rms16(st->exc, st->frameSize);
   noise_gain = MULT16_16_Q15(innov_gain, MULT16_16_Q15(fact, SUB16(Q15ONE,MULT16_16_Q15(pitch_gain,pitch_gain))));
   /* Shift all buffers by one frame */
   SPEEX_MOVE(st->excBuf, st->excBuf+st->frameSize, 2*st->max_pitch + st->subframeSize + 12);
   

   pitch_val = st->last_pitch + SHR32((spx_int32_t)speex_rand(1+st->count_lost, &st->seed),SIG_SHIFT);
   if (pitch_val > st->max_pitch)
      pitch_val = st->max_pitch;
   if (pitch_val < st->min_pitch)
      pitch_val = st->min_pitch;
   for (i=0;i<st->frameSize;i++)
   {
      st->exc[i]= MULT16_16_Q15(pitch_gain, (st->exc[i-pitch_val]+VERY_SMALL)) + 
            speex_rand(noise_gain, &st->seed);
   }

   bw_lpc(QCONST16(.98,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
   iir_mem16(&st->exc[-st->subframeSize], st->interp_qlpc, out, st->frameSize,
             st->lpcSize, st->mem_sp, stack);
   highpass(out, out, st->frameSize, HIGHPASS_NARROWBAND|HIGHPASS_OUTPUT, st->mem_hp);
   
   st->first = 0;
   st->count_lost++;
   st->pitch_gain_buf[st->pitch_gain_buf_idx++] = PSHR16(pitch_gain,9);
   if (st->pitch_gain_buf_idx > 2) /* rollover */
      st->pitch_gain_buf_idx = 0;
}

/* Just so we don't need to carry the complete wideband mode information */
static const int wb_skip_table[8] = {0, 36, 112, 192, 352, 0, 0, 0};
   
int nb_decode(void *state, SpeexBits *bits, void *vout)
{
   DecState *st;
   int i, sub;
   int pitch;
   spx_word16_t pitch_gain[3];
   spx_word32_t ol_gain=0;
   int ol_pitch=0;
   spx_word16_t ol_pitch_coef=0;
   int best_pitch=40;
   spx_word16_t best_pitch_gain=0;
   int wideband;
   int m;
   char *stack;
   VARDECL(spx_sig_t *innov);
   VARDECL(spx_word32_t *exc32);
   VARDECL(spx_coef_t *ak);
   VARDECL(spx_lsp_t *qlsp);
   spx_word16_t pitch_average=0;
   
   spx_word16_t *out = (spx_word16_t*)vout;
   VARDECL(spx_lsp_t *interp_qlsp);

   st=(DecState*)state;
   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)
      {

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