nb_celp.c

ppciaxclient softphone

   st->frame = st->inBuf;
   st->excBuf = speex_alloc((st->frameSize + st->max_pitch + 1)*sizeof(spx_sig_t));
   st->exc = st->excBuf + st->max_pitch + 1;
   for (i=0;i<st->frameSize;i++)
      st->inBuf[i]=0;
   for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
      st->excBuf[i]=0;
   st->innov = speex_alloc((st->frameSize)*sizeof(spx_sig_t));

   st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(spx_coef_t));
   st->qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->old_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->interp_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->mem_sp = speex_alloc((5*st->lpcSize)*sizeof(spx_mem_t));
   st->comb_mem = speex_alloc(sizeof(CombFilterMem));
   comb_filter_mem_init (st->comb_mem);

   st->pi_gain = 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->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;
#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;
   
   speex_free(state);
}

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

static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
{
   int i, sub;
   VARDECL(spx_coef_t *awk1);
   VARDECL(spx_coef_t *awk2);
   VARDECL(spx_coef_t *awk3);
   float pitch_gain, fact;
   spx_word16_t gain_med;

   fact = exp(-.04*st->count_lost*st->count_lost);
   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;
   
   pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
   if (pitch_gain>.95)
      pitch_gain=.95;

   pitch_gain = fact*pitch_gain + VERY_SMALL;

   /* Shift all buffers by one frame */
   /*speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(spx_sig_t));*/
   speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch + 1)*sizeof(spx_sig_t));

   ALLOC(awk1, (st->lpcSize+1), spx_coef_t);
   ALLOC(awk2, (st->lpcSize+1), spx_coef_t);
   ALLOC(awk3, (st->lpcSize+1), spx_coef_t);

   for (sub=0;sub<st->nbSubframes;sub++)
   {
      int offset;
      spx_sig_t *sp, *exc;
      /* Offset relative to start of frame */
      offset = st->subframeSize*sub;
      /* Original signal */
      sp=st->frame+offset;
      /* Excitation */
      exc=st->exc+offset;
      /* Excitation after post-filter*/

      /* Calculate perceptually enhanced LPC filter */
      if (st->lpc_enh_enabled)
      {
         spx_word16_t k1,k2,k3;
         if (st->submodes[st->submodeID] != NULL)
         {
            k1=SUBMODE(lpc_enh_k1);
            k2=SUBMODE(lpc_enh_k2);
            k3=SUBMODE(lpc_enh_k3);
         } else {
            k1=k2=.7*GAMMA_SCALING;
            k3=.0;
         }
         bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
         bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
         bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
      }
        
      /* Make up a plausible excitation */
      /* FIXME: THIS CAN BE IMPROVED */
      /*if (pitch_gain>.95)
        pitch_gain=.95;*/
      {
      float innov_gain=0;
      innov_gain = compute_rms(st->innov, st->frameSize);
      for (i=0;i<st->subframeSize;i++)
      {
#if 0
         exc[i] = pitch_gain * exc[i - st->last_pitch] + fact*sqrt(1-pitch_gain)*st->innov[i+offset];
         /*Just so it give the same lost packets as with if 0*/
         /*rand();*/
#else
         /*exc[i]=pitch_gain*exc[i-st->last_pitch] +  fact*st->innov[i+offset];*/
         exc[i]=pitch_gain*(exc[i-st->last_pitch]+VERY_SMALL) + 
         fact*sqrt(1-pitch_gain)*speex_rand(innov_gain);
#endif
      }
      }
      for (i=0;i<st->subframeSize;i++)
         sp[i]=exc[i];
      
      /* Signal synthesis */
      if (st->lpc_enh_enabled)
      {
         filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
                     st->mem_sp+st->lpcSize);
         filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
                     st->mem_sp);
      } else {
         for (i=0;i<st->lpcSize;i++)
            st->mem_sp[st->lpcSize+i] = 0;
         iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
                     st->mem_sp);
      }      
   }

   for (i=0;i<st->frameSize;i++)
   {
      spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
      if (sig>32767)
         sig = 32767;
      if (sig<-32767)
         sig = -32767;
     out[i]=sig;
   }
   
   st->first = 0;
   st->count_lost++;
   st->pitch_gain_buf[st->pitch_gain_buf_idx++] = GAIN_SCALING*pitch_gain;
   if (st->pitch_gain_buf_idx > 2) /* rollover */
      st->pitch_gain_buf_idx = 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_coef_t *awk1);
   VARDECL(spx_coef_t *awk2);
   VARDECL(spx_coef_t *awk3);
   spx_word16_t pitch_average=0;
#ifdef EPIC_48K
   int pitch_half[2];
   int ol_pitch_id=0;
#endif
   spx_word16_t *out = vout;

   st=(DecState*)state;
   stack=st->stack;

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

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

      /* 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, (st->max_pitch + 1)*sizeof(spx_sig_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, 11, spx_coef_t);
      bw_lpc(GAMMA_SCALING*.93, st->interp_qlpc, lpc, 10);
      {
         float innov_gain=0;
         float pgain=GAIN_SCALING_1*st->last_pitch_gain;
         if (pgain>.6)
            pgain=.6;
	 innov_gain = compute_rms(st->innov, st->frameSize);
         for (i=0;i<st->frameSize;i++)
            st->exc[i]=VERY_SMALL;
         speex_rand_vec(innov_gain, st->exc, st->frameSize);
      }


      st->first=1;

      /* Final signal synthesis from excitation */
      iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);

      for (i=0;i<st->frameSize;i++)
      {
         spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
         if (sig>32767)
            sig = 32767;
         if (sig<-32767)
            sig = -32767;
         out[i]=sig;
      }

      st->count_lost=0;
      return 0;
   }

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

   /*Damp memory if a frame was lost and the LSP changed too much*/
   if (st->count_lost)
   {
      float lsp_dist=0, fact;
      for (i=0;i<st->lpcSize;i++)
         lsp_dist += fabs(st->old_qlsp[i] - st->qlsp[i]);
      lsp_dist /= LSP_SCALING;
      fact = .6*exp(-.2*lsp_dist);
      for (i=0;i<2*st->lpcSize;i++)
         st->mem_sp[i] = (spx_mem_t)(st->mem_sp[i]*fact);
   }


   /* Handle first frame and lost-packet case */
   if (st->first || st->count_lost)
   {
      for (i=0;i<st->lpcSize;i++)
         st->old_qlsp[i] = st->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
      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(awk1, st->lpcSize+1, spx_coef_t);
   ALLOC(awk2, st->lpcSize+1, spx_coef_t);
   ALLOC(awk3, st->lpcSize+1, spx_coef_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_sig_t *sp, *exc;
      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;
      /* Original signal */
      sp=st->frame+offset;
      /* Excitation */
      exc=st->exc+offset;
      /* Excitation after post-filter*/

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

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


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

      /* Compute enhanced synthesis filter */
      if (st->lpc_enh_enabled)
      {
         bw_lpc(SUBMODE(lpc_enh_k1), st->interp_qlpc, awk1, st->lpcSize);
         bw_lpc(SUBMODE(lpc_enh_k2), st->interp_qlpc, awk2, st->lpcSize);
         bw_lpc(SUBMODE(lpc_enh_k3), st->interp_qlpc, awk3, st->lpcSize);
      }

      /* Compute analysis filter at w=pi */
      {
         spx_word32_t pi_g=st->interp_qlpc[0];
         for (i=1;i<=st->lpcSize;i+=2)
         {
            /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
            pi_g = ADD32(pi_g, SUB32(st->interp_qlpc[i+1],st->interp_qlpc[i]));
         }
         st->pi_gain[sub] = pi_g;
      }

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