resample.c

一个开源的sip源代码

#else
/* This is the same as the previous function, except with a double-precision accumulator */
static int resampler_basic_direct_double(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 N = st->filt_len;
   int out_sample = 0;
   spx_word16_t *mem;
   int last_sample = st->last_sample[channel_index];
   spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
   mem = st->mem + channel_index * st->mem_alloc_size;
   while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
   {
      int j;
      double sum=0;
      
      /* We already have all the filter coefficients pre-computed in the table */
      const spx_word16_t *ptr;
      /* Do the memory part */
      for (j=0;last_sample-N+1+j < 0;j++)
      {
         sum += MULT16_16(mem[last_sample+j],(double)st->sinc_table[samp_frac_num*st->filt_len+j]);
      }
      
      /* Do the new part */
      ptr = in+st->in_stride*(last_sample-N+1+j);
      for (;j<N;j++)
      {
         sum += MULT16_16(*ptr,(double)st->sinc_table[samp_frac_num*st->filt_len+j]);
         ptr += st->in_stride;
      }
   
      *out = sum;
      out += st->out_stride;
      out_sample++;
      last_sample += st->int_advance;
      samp_frac_num += st->frac_advance;
      if (samp_frac_num >= st->den_rate)
      {
         samp_frac_num -= st->den_rate;
         last_sample++;
      }
   }
   st->last_sample[channel_index] = last_sample;
   st->samp_frac_num[channel_index] = samp_frac_num;
   return out_sample;
}
#endif

static int resampler_basic_interpolate_single(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 N = st->filt_len;
   int out_sample = 0;
   spx_word16_t *mem;
   int last_sample = st->last_sample[channel_index];
   spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
   mem = st->mem + channel_index * st->mem_alloc_size;
   while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
   {
      int j;
      spx_word32_t sum=0;
      
      /* We need to interpolate the sinc filter */
      spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
      spx_word16_t interp[4];
      const spx_word16_t *ptr;
      int offset;
      spx_word16_t frac;
      offset = samp_frac_num*st->oversample/st->den_rate;
#ifdef FIXED_POINT
      frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
#else
      frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
#endif
         /* This code is written like this to make it easy to optimise with SIMD.
      For most DSPs, it would be best to split the loops in two because most DSPs 
      have only two accumulators */
      for (j=0;last_sample-N+1+j < 0;j++)
      {
         spx_word16_t curr_mem = mem[last_sample+j];
         accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
         accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
         accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
         accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
      }
      ptr = in+st->in_stride*(last_sample-N+1+j);
      /* Do the new part */
      for (;j<N;j++)
      {
         spx_word16_t curr_in = *ptr;
         ptr += st->in_stride;
         accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
         accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
         accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
         accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
      }
      cubic_coef(frac, interp);
      sum = MULT16_32_Q15(interp[0],accum[0]) + MULT16_32_Q15(interp[1],accum[1]) + MULT16_32_Q15(interp[2],accum[2]) + MULT16_32_Q15(interp[3],accum[3]);
   
      *out = PSHR32(sum,15);
      out += st->out_stride;
      out_sample++;
      last_sample += st->int_advance;
      samp_frac_num += st->frac_advance;
      if (samp_frac_num >= st->den_rate)
      {
         samp_frac_num -= st->den_rate;
         last_sample++;
      }
   }
   st->last_sample[channel_index] = last_sample;
   st->samp_frac_num[channel_index] = samp_frac_num;
   return out_sample;
}

#ifdef FIXED_POINT
#else
/* This is the same as the previous function, except with a double-precision accumulator */
static int resampler_basic_interpolate_double(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 N = st->filt_len;
   int out_sample = 0;
   spx_word16_t *mem;
   int last_sample = st->last_sample[channel_index];
   spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
   mem = st->mem + channel_index * st->mem_alloc_size;
   while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
   {
      int j;
      spx_word32_t sum=0;
      
      /* We need to interpolate the sinc filter */
      double accum[4] = {0.f,0.f, 0.f, 0.f};
      float interp[4];
      const spx_word16_t *ptr;
      float alpha = ((float)samp_frac_num)/st->den_rate;
      int offset = samp_frac_num*st->oversample/st->den_rate;
      float frac = alpha*st->oversample - offset;
         /* This code is written like this to make it easy to optimise with SIMD.
      For most DSPs, it would be best to split the loops in two because most DSPs 
      have only two accumulators */
      for (j=0;last_sample-N+1+j < 0;j++)
      {
         double curr_mem = mem[last_sample+j];
         accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
         accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
         accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
         accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
      }
      ptr = in+st->in_stride*(last_sample-N+1+j);
      /* Do the new part */
      for (;j<N;j++)
      {
         double curr_in = *ptr;
         ptr += st->in_stride;
         accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
         accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
         accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
         accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
      }
      cubic_coef(frac, interp);
      sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
   
      *out = PSHR32(sum,15);
      out += st->out_stride;
      out_sample++;
      last_sample += st->int_advance;
      samp_frac_num += st->frac_advance;
      if (samp_frac_num >= st->den_rate)
      {
         samp_frac_num -= st->den_rate;
         last_sample++;
      }
   }
   st->last_sample[channel_index] = last_sample;
   st->samp_frac_num[channel_index] = samp_frac_num;
   return out_sample;
}
#endif

static void update_filter(SpeexResamplerState *st)
{
   spx_uint32_t old_length;
   
   old_length = st->filt_len;
   st->oversample = quality_map[st->quality].oversample;
   st->filt_len = quality_map[st->quality].base_length;
   
   if (st->num_rate > st->den_rate)
   {
      /* down-sampling */
      st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
      /* FIXME: divide the numerator and denominator by a certain amount if they're too large */
      st->filt_len = st->filt_len*st->num_rate / st->den_rate;
      /* Round down to make sure we have a multiple of 4 */
      st->filt_len &= (~0x3);
      if (2*st->den_rate < st->num_rate)
         st->oversample >>= 1;
      if (4*st->den_rate < st->num_rate)
         st->oversample >>= 1;
      if (8*st->den_rate < st->num_rate)
         st->oversample >>= 1;
      if (16*st->den_rate < st->num_rate)
         st->oversample >>= 1;
      if (st->oversample < 1)
         st->oversample = 1;
   } else {
      /* up-sampling */
      st->cutoff = quality_map[st->quality].upsample_bandwidth;
   }

   /* Choose the resampling type that requires the least amount of memory */
   if (st->den_rate <= st->oversample)
   {
      spx_uint32_t i;
      if (!st->sinc_table)
         st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
      else if (st->sinc_table_length < st->filt_len*st->den_rate)
      {
         st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
         st->sinc_table_length = st->filt_len*st->den_rate;
      }
      for (i=0;i<st->den_rate;i++)
      {
         spx_uint32_t j;
         for (j=0;j<st->filt_len;j++)
         {
            st->sinc_table[i*st->filt_len+j] = sinc(st->cutoff,((j-st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len, quality_map[st->quality].window_func);
         }
      }
#ifdef FIXED_POINT
      st->resampler_ptr = resampler_basic_direct_single;
#else
      if (st->quality>8)
         st->resampler_ptr = resampler_basic_direct_double;
      else
         st->resampler_ptr = resampler_basic_direct_single;
#endif
      /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
   } else {
      spx_int32_t i;
      if (!st->sinc_table)
         st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
      else if (st->sinc_table_length < st->filt_len*st->oversample+8)
      {
         st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
         st->sinc_table_length = st->filt_len*st->oversample+8;
      }
      for (i=-4;i<(spx_int32_t)(st->oversample*st->filt_len+4);i++)
         st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len, quality_map[st->quality].window_func);
#ifdef FIXED_POINT
      st->resampler_ptr = resampler_basic_interpolate_single;
#else
      if (st->quality>8)
         st->resampler_ptr = resampler_basic_interpolate_double;
      else
         st->resampler_ptr = resampler_basic_interpolate_single;
#endif
      /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
   }
   st->int_advance = st->num_rate/st->den_rate;
   st->frac_advance = st->num_rate%st->den_rate;

   if (!st->mem)
   {
      spx_uint32_t i;
      st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
      for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
         st->mem[i] = 0;
      st->mem_alloc_size = st->filt_len-1;
      /*speex_warning("init filter");*/
   } else if (!st->started)
   {
      spx_uint32_t i;
      st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
      for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
         st->mem[i] = 0;
      st->mem_alloc_size = st->filt_len-1;
      /*speex_warning("reinit filter");*/
   } else if (st->filt_len > old_length)
   {
      spx_uint32_t i;
      /* Increase the filter length */
      /*speex_warning("increase filter size");*/
      int old_alloc_size = st->mem_alloc_size;
      if (st->filt_len-1 > st->mem_alloc_size)
      {
         st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
         st->mem_alloc_size = st->filt_len-1;
      }
      for (i=0;i<st->nb_channels;i++)
      {
         spx_uint32_t j;
         /* Copy data going backward */
         for (j=0;j<old_length-1;j++)
            st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*old_alloc_size+(old_length-2-j)];
         /* Then put zeros for lack of anything better */
         for (;j<st->filt_len-1;j++)
            st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
         /* Adjust last_sample */
         st->last_sample[i] += (st->filt_len - old_length)/2;
      }
   } else if (st->filt_len < old_length)
   {
      spx_uint32_t i;
      /* Reduce filter length, this a bit tricky */
      /*speex_warning("decrease filter size (unimplemented)");*/
      /* Adjust last_sample (which will likely end up negative) */
      /*st->last_sample += (st->filt_len - old_length)/2;*/
      for (i=0;i<st->nb_channels;i++)
      {
         spx_uint32_t j;
         st->magic_samples[i] = (old_length - st->filt_len)/2;
         /* Copy data going backward */
         for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
            st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
      }
   }

}

SpeexResamplerState *speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
{
   return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality, err);
}

SpeexResamplerState *speex_resampler_init_frac(spx_uint32_t nb_channels, spx_uint32_t ratio_num, spx_uint32_t ratio_den, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
{
   spx_uint32_t i;
   SpeexResamplerState *st;
   if (quality > 10 || quality < 0)
   {
      if (err)
         *err = RESAMPLER_ERR_INVALID_ARG;
      return NULL;
   }
   st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
   st->initialised = 0;
   st->started = 0;
   st->in_rate = 0;
   st->out_rate = 0;
   st->num_rate = 0;
   st->den_rate = 0;
   st->quality = -1;
   st->sinc_table_length = 0;
   st->mem_alloc_size = 0;
   st->filt_len = 0;
   st->mem = 0;
   st->resampler_ptr = 0;
         
   st->cutoff = 1.f;
   st->nb_channels = nb_channels;
   st->in_stride = 1;
   st->out_stride = 1;
   
   /* Per channel data */