sb_celp.c

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

   return 1;
}





void *sb_decoder_init(const SpeexMode *m)
{
   spx_int32_t tmp;
   SBDecState *st;
   const SpeexSBMode *mode;
   st = (SBDecState*)speex_alloc(sizeof(SBDecState));
   if (!st)
      return NULL;
   st->mode = m;
   mode=(const SpeexSBMode*)m->mode;
   st->encode_submode = 1;

   st->st_low = speex_decoder_init(mode->nb_mode);
#if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
   st->stack = NULL;
#else
   /*st->stack = (char*)speex_alloc_scratch(SB_DEC_STACK);*/
   speex_decoder_ctl(st->st_low, SPEEX_GET_STACK, &st->stack);
#endif

   st->full_frame_size = 2*mode->frameSize;
   st->frame_size = mode->frameSize;
   st->subframeSize = mode->subframeSize;
   st->nbSubframes = mode->frameSize/mode->subframeSize;
   st->lpcSize=mode->lpcSize;
   speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
   st->sampling_rate*=2;
   tmp=1;
   speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);

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

   st->first=1;

   st->g0_mem = (spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
   st->g1_mem = (spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));

   st->excBuf = (spx_word16_t*)speex_alloc((st->subframeSize)*sizeof(spx_word16_t));

   st->old_qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));

   st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
   st->exc_rms = (spx_word16_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word16_t));
   st->mem_sp = (spx_mem_t*)speex_alloc((2*st->lpcSize)*sizeof(spx_mem_t));
   
   st->innov_save = NULL;


   st->lpc_enh_enabled=0;
   st->seed = 1000;

#ifdef ENABLE_VALGRIND
   VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
#endif
   return st;
}

void sb_decoder_destroy(void *state)
{
   SBDecState *st;
   st = (SBDecState*)state;
   speex_decoder_destroy(st->st_low);
#if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
   /*speex_free_scratch(st->stack);*/
#endif

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

   speex_free(state);
}

static void sb_decode_lost(SBDecState *st, spx_word16_t *out, int dtx, char *stack)
{
   int i;
   int saved_modeid=0;

   if (dtx)
   {
      saved_modeid=st->submodeID;
      st->submodeID=1;
   } else {
      bw_lpc(QCONST16(0.99f,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
   }

   st->first=1;
   
   
   /* Final signal synthesis from excitation */
   if (!dtx)
   {
      st->last_ener =  MULT16_16_Q15(QCONST16(.9f,15),st->last_ener);
   }
   for (i=0;i<st->frame_size;i++)
      out[i+st->frame_size] = speex_rand(st->last_ener, &st->seed);

   iir_mem16(out+st->frame_size, st->interp_qlpc, out+st->frame_size, st->frame_size, st->lpcSize, 
            st->mem_sp, stack);
   
   
   /* Reconstruct the original */
   qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
   if (dtx)
   {
      st->submodeID=saved_modeid;
   }

   return;
}

int sb_decode(void *state, SpeexBits *bits, void *vout)
{
   int i, sub;
   SBDecState *st;
   int wideband;
   int ret;
   char *stack;
   VARDECL(spx_word32_t *low_pi_gain);
   VARDECL(spx_word16_t *low_exc_rms);
   VARDECL(spx_coef_t *ak);
   VARDECL(spx_lsp_t *qlsp);
   VARDECL(spx_lsp_t *interp_qlsp);
   spx_int32_t dtx;
   const SpeexSBMode *mode;
   spx_word16_t *out = (spx_word16_t*)vout;
   spx_word16_t *low_innov_alias;
   spx_word32_t exc_ener_sum = 0;
   
   st = (SBDecState*)state;
   stack=st->stack;
   mode = (const SpeexSBMode*)(st->mode->mode);

   low_innov_alias = out+st->frame_size;
   speex_decoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_alias);
   /* Decode the low-band */
   ret = speex_decode_native(st->st_low, bits, out);

   speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);

   /* If error decoding the narrowband part, propagate error */
   if (ret!=0)
   {
      return ret;
   }

   if (!bits)
   {
      sb_decode_lost(st, out, dtx, stack);
      return 0;
   }

   if (st->encode_submode)
   {

      /*Check "wideband bit"*/
      if (speex_bits_remaining(bits)>0)
         wideband = speex_bits_peek(bits);
      else
         wideband = 0;
      if (wideband)
      {
         /*Regular wideband frame, read the submode*/
         wideband = speex_bits_unpack_unsigned(bits, 1);
         st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
      } else
      {
         /*Was a narrowband frame, set "null submode"*/
         st->submodeID = 0;
      }
      if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
      {
         speex_notify("Invalid mode encountered. The stream is corrupted.");
         return -2;
      }
   }

   /* If null mode (no transmission), just set a couple things to zero*/
   if (st->submodes[st->submodeID] == NULL)
   {
      if (dtx)
      {
         sb_decode_lost(st, out, 1, stack);
         return 0;
      }

      for (i=0;i<st->frame_size;i++)
         out[st->frame_size+i]=VERY_SMALL;

      st->first=1;

      /* Final signal synthesis from excitation */
      iir_mem16(out+st->frame_size, st->interp_qlpc, out+st->frame_size, st->frame_size, st->lpcSize, st->mem_sp, stack);

      qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);

      return 0;

   }

   ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
   ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
   speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
   speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);

   ALLOC(qlsp, st->lpcSize, spx_lsp_t);
   ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
   SUBMODE(lsp_unquant)(qlsp, st->lpcSize, bits);
   
   if (st->first)
   {
      for (i=0;i<st->lpcSize;i++)
         st->old_qlsp[i] = qlsp[i];
   }
   
   ALLOC(ak, st->lpcSize, spx_coef_t);

   for (sub=0;sub<st->nbSubframes;sub++)
   {
      VARDECL(spx_word32_t *exc);
      spx_word16_t *innov_save=NULL;
      spx_word16_t *sp;
      spx_word16_t filter_ratio;
      spx_word16_t el=0;
      int offset;
      spx_word32_t rl=0,rh=0;
      
      offset = st->subframeSize*sub;
      sp=out+st->frame_size+offset;
      ALLOC(exc, st->subframeSize, spx_word32_t);
      /* Pointer for saving innovation */
      if (st->innov_save)
      {
         innov_save = st->innov_save+2*offset;
         SPEEX_MEMSET(innov_save, 0, 2*st->subframeSize);
      }
      
      /* LSP interpolation */
      lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);

      lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);

      /* LSP to LPC */
      lsp_to_lpc(interp_qlsp, ak, st->lpcSize, stack);

      /* Calculate reponse ratio between the low and high filter in the middle
         of the band (4000 Hz) */
      
         st->pi_gain[sub]=LPC_SCALING;
         rh = LPC_SCALING;
         for (i=0;i<st->lpcSize;i+=2)
         {
            rh += ak[i+1] - ak[i];
            st->pi_gain[sub] += ak[i] + ak[i+1];
         }

         rl = low_pi_gain[sub];
#ifdef FIXED_POINT
         filter_ratio=EXTRACT16(SATURATE(PDIV32(SHL32(ADD32(rl,82),7),ADD32(82,rh)),32767));
#else
         filter_ratio=(rl+.01)/(rh+.01);
#endif
      
      SPEEX_MEMSET(exc, 0, st->subframeSize);
      if (!SUBMODE(innovation_unquant))
      {
         spx_word32_t g;
         int quant;

         quant = speex_bits_unpack_unsigned(bits, 5);
         g= spx_exp(MULT16_16(QCONST16(.125f,11),(quant-10)));
         
         g = PDIV32(g, filter_ratio);
         
         for (i=0;i<st->subframeSize;i+=2)
         {
            exc[i]=SHL32(MULT16_32_P15(MULT16_16_Q15(mode->folding_gain,low_innov_alias[offset+i]),SHL32(g,6)),SIG_SHIFT);
            exc[i+1]=NEG32(SHL32(MULT16_32_P15(MULT16_16_Q15(mode->folding_gain,low_innov_alias[offset+i+1]),SHL32(g,6)),SIG_SHIFT));
         }
         
      } else {
         spx_word16_t gc;
         spx_word32_t scale;
         int qgc = speex_bits_unpack_unsigned(bits, 4);
         
         el = low_exc_rms[sub];
         gc = MULT16_16_Q15(QCONST16(0.87360,15),gc_quant_bound[qgc]);

         if (st->subframeSize==80)
            gc = MULT16_16_P14(QCONST16(1.4142f,14),gc);

         scale = SHL32(PDIV32(SHL32(MULT16_16(gc, el),3), filter_ratio),SIG_SHIFT-3);
         SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
                                     bits, stack, &st->seed);

         signal_mul(exc,exc,scale,st->subframeSize);

         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_P15(QCONST16(0.4f,15),scale), st->subframeSize);
            for (i=0;i<st->subframeSize;i++)
               exc[i] = ADD32(exc[i],innov2[i]);
            stack = tmp_stack;
         }

      }
      
      if (st->innov_save)
      {
         for (i=0;i<st->subframeSize;i++)
            innov_save[2*i]=EXTRACT16(PSHR32(exc[i],SIG_SHIFT));
      }
      
      iir_mem16(st->excBuf, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
               st->mem_sp, stack);
      for (i=0;i<st->subframeSize;i++)
         st->excBuf[i]=EXTRACT16(PSHR32(exc[i],SIG_SHIFT));
      for (i=0;i<st->lpcSize;i++)
         st->interp_qlpc[i] = ak[i];
      st->exc_rms[sub] = compute_rms16(st->excBuf, st->subframeSize);
      exc_ener_sum = ADD32(exc_ener_sum, DIV32(MULT16_16(st->exc_rms[sub],st->exc_rms[sub]), st->nbSubframes));
   }
   st->last_ener = spx_sqrt(exc_ener_sum);
   
   qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
   for (i=0;i<st->lpcSize;i++)
      st->old_qlsp[i] = qlsp[i];

   st->first=0;

   return 0;
}


int sb_encoder_ctl(void *state, int request, void *ptr)
{
   SBEncState *st;
   st=(SBEncState*)state;
   switch(request)
   {
   case SPEEX_GET_FRAME_SIZE:
      (*(spx_int32_t*)ptr) = st->full_frame_size;
      break;
   case SPEEX_SET_HIGH_MODE:
      st->submodeSelect = st->submodeID = (*(spx_int32_t*)ptr);
      break;
   case SPEEX_SET_LOW_MODE:
      speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
      break;
   case SPEEX_SET_DTX:
      speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
      break;
   case SPEEX_GET_DTX: