nb_celp.c

一个开源SIP协议栈

      fir_mem16(real_exc, interp_qlpc, real_exc, st->subframeSize, st->lpcSize, st->mem_exc2, stack);
      
      if (st->complexity==0)
         response_bound >>= 1;
      compute_impulse_response(interp_qlpc, bw_lpc1, bw_lpc2, syn_resp, response_bound, st->lpcSize, stack);
      for (i=response_bound;i<st->subframeSize;i++)
         syn_resp[i]=VERY_SMALL;
      
      /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
      for (i=0;i<st->lpcSize;i++)
         mem[i]=SHL32(st->mem_sp[i],1);
      for (i=0;i<st->subframeSize;i++)
         ringing[i] = VERY_SMALL;
#ifdef SHORTCUTS2
      iir_mem16(ringing, interp_qlpc, ringing, response_bound, st->lpcSize, mem, stack);
      for (i=0;i<st->lpcSize;i++)
         mem[i]=SHL32(st->mem_sw[i],1);
      filter_mem16(ringing, st->bw_lpc1, st->bw_lpc2, ringing, response_bound, st->lpcSize, mem, stack);
      for (i=response_bound;i<st->subframeSize;i++)
         ringing[i]=0;
#else
      iir_mem16(ringing, interp_qlpc, ringing, st->subframeSize, st->lpcSize, mem, stack);
      for (i=0;i<st->lpcSize;i++)
         mem[i]=SHL32(st->mem_sw[i],1);
      filter_mem16(ringing, bw_lpc1, bw_lpc2, ringing, st->subframeSize, st->lpcSize, mem, stack);
#endif
      
      /* Compute weighted signal */
      for (i=0;i<st->lpcSize;i++)
         mem[i]=st->mem_sw[i];
      filter_mem16(sw, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, mem, stack);
      
      if (st->complexity==0)
         for (i=0;i<st->lpcSize;i++)
            st->mem_sw[i]=mem[i];
      
      /* Compute target signal (saturation prevents overflows on clipped input speech) */
      for (i=0;i<st->subframeSize;i++)
         target[i]=EXTRACT16(SATURATE(SUB32(sw[i],PSHR32(ringing[i],1)),32767));

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

      /* If we have a long-term predictor (otherwise, something's wrong) */
      if (SUBMODE(ltp_quant))
      {
         int pit_min, pit_max;
         /* 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;

#ifdef EPIC_48K
         if (st->lbr_48k)
         {
            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, ol_pitch_id, st->plc_tuning, &st->cumul_gain);
         } else {
#endif

         /* 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);
#ifdef EPIC_48K
         }
#endif

         st->pitch[sub]=pitch;
      } else {
         speex_error ("No pitch prediction, what's wrong");
      }

      /* Quantization of innovation */
      {
         spx_word32_t ener=0;
         spx_word16_t fine_gain;

         for (i=0;i<st->subframeSize;i++)
            innov[i]=0;
         
         /* FIXME: Make sure this is save from overflows (so far so good) */
         for (i=0;i<st->subframeSize;i++)
            real_exc[i] = SUB16(real_exc[i], EXTRACT16(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 */
         if (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));
         } else {
            speex_error("No fixed codebook");
         }

         if (innov_save)
         {
            for (i=0;i<st->subframeSize;i++)
               innov_save[i] = innov[i];
         }
         /* 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);
            for (i=0;i<st->subframeSize;i++)
               innov2[i]=0;
            for (i=0;i<st->subframeSize;i++)
               target[i]=MULT16_16_P13(QCONST16(2.2,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.454545,15),ener), st->subframeSize);
            for (i=0;i<st->subframeSize;i++)
               exc[i] = ADD32(exc[i],PSHR32(innov2[i],SIG_SHIFT));
            if (innov_save)
            {
               for (i=0;i<st->subframeSize;i++)
                  innov_save[i] = ADD32(innov_save[i],innov2[i]);
            }
            stack = tmp_stack;
         }

      }

      for (i=0;i<st->subframeSize;i++)
         sw[i] = exc[i];
      /* Final signal synthesis from excitation */
      iir_mem16(sw, 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)
   {
      for (i=0;i<128;i++)
         st->old_curve[i] = st->curve[i];
   }
#endif

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

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

   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;
#ifdef EPIC_48K
   st->lbr_48k=mode->lbr48k;
#endif

   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;
   for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
      st->excBuf[i]=0;

   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, (st->stack-(char*)st));
#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 = SHL(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)*sizeof(spx_word16_t));
   

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

   for (i=0;i<st->frameSize;i++)
      out[i]=st->exc[i-st->subframeSize];
   bw_lpc(QCONST16(.98,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
   iir_mem16(out, 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++] = PSHR(pitch_gain,9);
   if (st->pitch_gain_buf_idx > 2) /* rollover */
      st->pitch_gain_buf_idx = 0;