resample.c

一个开源的sip源代码

   st->last_sample = (spx_int32_t*)speex_alloc(nb_channels*sizeof(int));
   st->magic_samples = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(int));
   st->samp_frac_num = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(int));
   for (i=0;i<nb_channels;i++)
   {
      st->last_sample[i] = 0;
      st->magic_samples[i] = 0;
      st->samp_frac_num[i] = 0;
   }

   speex_resampler_set_quality(st, quality);
   speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);

   
   update_filter(st);
   
   st->initialised = 1;
   if (err)
      *err = RESAMPLER_ERR_SUCCESS;

   return st;
}

void speex_resampler_destroy(SpeexResamplerState *st)
{
   speex_free(st->mem);
   speex_free(st->sinc_table);
   speex_free(st->last_sample);
   speex_free(st->magic_samples);
   speex_free(st->samp_frac_num);
   speex_free(st);
}



static int speex_resampler_process_native(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
{
   int j=0;
   int N = st->filt_len;
   int out_sample = 0;
   spx_word16_t *mem;
   spx_uint32_t tmp_out_len = 0;
   mem = st->mem + channel_index * st->mem_alloc_size;
   st->started = 1;
   
   /* Handle the case where we have samples left from a reduction in filter length */
   if (st->magic_samples[channel_index])
   {
      spx_uint32_t tmp_in_len;
      spx_uint32_t tmp_magic;
      tmp_in_len = st->magic_samples[channel_index];
      tmp_out_len = *out_len;
      /* FIXME: Need to handle the case where the out array is too small */
      /* magic_samples needs to be set to zero to avoid infinite recursion */
      tmp_magic = st->magic_samples[channel_index];
      st->magic_samples[channel_index] = 0;
      speex_resampler_process_native(st, channel_index, mem+N-1, &tmp_in_len, out, &tmp_out_len);
      /*speex_warning_int("extra samples:", tmp_out_len);*/
      /* If we couldn't process all "magic" input samples, save the rest for next time */
      if (tmp_in_len < tmp_magic)
      {
         spx_uint32_t i;
         st->magic_samples[channel_index] = tmp_magic-tmp_in_len;
         for (i=0;i<st->magic_samples[channel_index];i++)
            mem[N-1+i]=mem[N-1+i+tmp_in_len];
      }
      out += tmp_out_len;
   }
   
   /* Call the right resampler through the function ptr */
   out_sample = st->resampler_ptr(st, channel_index, in, in_len, out, out_len);
   
   if (st->last_sample[channel_index] < (spx_int32_t)*in_len)
      *in_len = st->last_sample[channel_index];
   *out_len = out_sample+tmp_out_len;
   st->last_sample[channel_index] -= *in_len;
   
   for (j=0;j<N-1-(spx_int32_t)*in_len;j++)
      mem[j] = mem[j+*in_len];
   for (;j<N-1;j++)
      mem[j] = in[st->in_stride*(j+*in_len-N+1)];
   
   return RESAMPLER_ERR_SUCCESS;
}

#define FIXED_STACK_ALLOC 1024

#ifdef FIXED_POINT
int speex_resampler_process_float(SpeexResamplerState *st, spx_uint32_t channel_index, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
{
   spx_uint32_t i;
   int istride_save, ostride_save;
#ifdef VAR_ARRAYS
   spx_word16_t x[*in_len];
   spx_word16_t y[*out_len];
   /*VARDECL(spx_word16_t *x);
   VARDECL(spx_word16_t *y);
   ALLOC(x, *in_len, spx_word16_t);
   ALLOC(y, *out_len, spx_word16_t);*/
   istride_save = st->in_stride;
   ostride_save = st->out_stride;
   for (i=0;i<*in_len;i++)
      x[i] = WORD2INT(in[i*st->in_stride]);
   st->in_stride = st->out_stride = 1;
   speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
   st->in_stride = istride_save;
   st->out_stride = ostride_save;
   for (i=0;i<*out_len;i++)
      out[i*st->out_stride] = y[i];
#else
   spx_word16_t x[FIXED_STACK_ALLOC];
   spx_word16_t y[FIXED_STACK_ALLOC];
   spx_uint32_t ilen=*in_len, olen=*out_len;
   istride_save = st->in_stride;
   ostride_save = st->out_stride;
   while (ilen && olen)
   {
      spx_uint32_t ichunk, ochunk;
      ichunk = ilen;
      ochunk = olen;
      if (ichunk>FIXED_STACK_ALLOC)
         ichunk=FIXED_STACK_ALLOC;
      if (ochunk>FIXED_STACK_ALLOC)
         ochunk=FIXED_STACK_ALLOC;
      for (i=0;i<ichunk;i++)
         x[i] = WORD2INT(in[i*st->in_stride]);
      st->in_stride = st->out_stride = 1;
      speex_resampler_process_native(st, channel_index, x, &ichunk, y, &ochunk);
      st->in_stride = istride_save;
      st->out_stride = ostride_save;
      for (i=0;i<ochunk;i++)
         out[i*st->out_stride] = y[i];
      out += ochunk;
      in += ichunk;
      ilen -= ichunk;
      olen -= ochunk;
   }
   *in_len -= ilen;
   *out_len -= olen;   
#endif
   return RESAMPLER_ERR_SUCCESS;
}
int speex_resampler_process_int(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
{
   return speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
}
#else
int speex_resampler_process_float(SpeexResamplerState *st, spx_uint32_t channel_index, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
{
   return speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
}
int speex_resampler_process_int(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
{
   spx_uint32_t i;
   int istride_save, ostride_save;
#ifdef VAR_ARRAYS
   spx_word16_t x[*in_len];
   spx_word16_t y[*out_len];
   /*VARDECL(spx_word16_t *x);
   VARDECL(spx_word16_t *y);
   ALLOC(x, *in_len, spx_word16_t);
   ALLOC(y, *out_len, spx_word16_t);*/
   istride_save = st->in_stride;
   ostride_save = st->out_stride;
   for (i=0;i<*in_len;i++)
      x[i] = in[i*st->in_stride];
   st->in_stride = st->out_stride = 1;
   speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
   st->in_stride = istride_save;
   st->out_stride = ostride_save;
   for (i=0;i<*out_len;i++)
      out[i*st->out_stride] = WORD2INT(y[i]);
#else
   spx_word16_t x[FIXED_STACK_ALLOC];
   spx_word16_t y[FIXED_STACK_ALLOC];
   spx_uint32_t ilen=*in_len, olen=*out_len;
   istride_save = st->in_stride;
   ostride_save = st->out_stride;
   while (ilen && olen)
   {
      spx_uint32_t ichunk, ochunk;
      ichunk = ilen;
      ochunk = olen;
      if (ichunk>FIXED_STACK_ALLOC)
         ichunk=FIXED_STACK_ALLOC;
      if (ochunk>FIXED_STACK_ALLOC)
         ochunk=FIXED_STACK_ALLOC;
      for (i=0;i<ichunk;i++)
         x[i] = in[i*st->in_stride];
      st->in_stride = st->out_stride = 1;
      speex_resampler_process_native(st, channel_index, x, &ichunk, y, &ochunk);
      st->in_stride = istride_save;
      st->out_stride = ostride_save;
      for (i=0;i<ochunk;i++)
         out[i*st->out_stride] = WORD2INT(y[i]);
      out += ochunk;
      in += ichunk;
      ilen -= ichunk;
      olen -= ochunk;
   }
   *in_len -= ilen;
   *out_len -= olen;   
#endif
   return RESAMPLER_ERR_SUCCESS;
}
#endif

int speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
{
   spx_uint32_t i;
   int istride_save, ostride_save;
   istride_save = st->in_stride;
   ostride_save = st->out_stride;
   st->in_stride = st->out_stride = st->nb_channels;
   for (i=0;i<st->nb_channels;i++)
   {
      speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
   }
   st->in_stride = istride_save;
   st->out_stride = ostride_save;
   return RESAMPLER_ERR_SUCCESS;
}

int speex_resampler_process_interleaved_int(SpeexResamplerState *st, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
{
   spx_uint32_t i;
   int istride_save, ostride_save;
   istride_save = st->in_stride;
   ostride_save = st->out_stride;
   st->in_stride = st->out_stride = st->nb_channels;
   for (i=0;i<st->nb_channels;i++)
   {
      speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
   }
   st->in_stride = istride_save;
   st->out_stride = ostride_save;
   return RESAMPLER_ERR_SUCCESS;
}

int speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)
{
   return speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
}

void speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)
{
   *in_rate = st->in_rate;
   *out_rate = st->out_rate;
}

int speex_resampler_set_rate_frac(SpeexResamplerState *st, spx_uint32_t ratio_num, spx_uint32_t ratio_den, spx_uint32_t in_rate, spx_uint32_t out_rate)
{
   int fact;
   if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
      return RESAMPLER_ERR_SUCCESS;
   
   st->in_rate = in_rate;
   st->out_rate = out_rate;
   st->num_rate = ratio_num;
   st->den_rate = ratio_den;
   /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
   for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
   {
      while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
      {
         st->num_rate /= fact;
         st->den_rate /= fact;
      }
   }
      
   if (st->initialised)
      update_filter(st);
   return RESAMPLER_ERR_SUCCESS;
}

void speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)
{
   *ratio_num = st->num_rate;
   *ratio_den = st->den_rate;
}

int speex_resampler_set_quality(SpeexResamplerState *st, int quality)
{
   if (quality > 10 || quality < 0)
      return RESAMPLER_ERR_INVALID_ARG;
   if (st->quality == quality)
      return RESAMPLER_ERR_SUCCESS;
   st->quality = quality;
   if (st->initialised)
      update_filter(st);
   return RESAMPLER_ERR_SUCCESS;
}

void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
{
   *quality = st->quality;
}

void speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)
{
   st->in_stride = stride;
}

void speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)
{
   *stride = st->in_stride;
}

void speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)
{
   st->out_stride = stride;
}

void speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)
{
   *stride = st->out_stride;
}

int speex_resampler_skip_zeros(SpeexResamplerState *st)
{
   spx_uint32_t i;
   for (i=0;i<st->nb_channels;i++)
      st->last_sample[i] = st->filt_len/2;
   return RESAMPLER_ERR_SUCCESS;
}

int speex_resampler_reset_mem(SpeexResamplerState *st)
{
   spx_uint32_t i;
   for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
      st->mem[i] = 0;
   return RESAMPLER_ERR_SUCCESS;
}

const char *speex_resampler_strerror(int err)
{
   switch (err)
   {
      case RESAMPLER_ERR_SUCCESS:
         return "Success.";
      case RESAMPLER_ERR_ALLOC_FAILED:
         return "Memory allocation failed.";
      case RESAMPLER_ERR_BAD_STATE:
         return "Bad resampler state.";
      case RESAMPLER_ERR_INVALID_ARG:
         return "Invalid argument.";
      case RESAMPLER_ERR_PTR_OVERLAP:
         return "Input and output buffers overlap.";
      default:
         return "Unknown error. Bad error code or strange version mismatch.";
   }
}