initial support for changing the filter length on the fly...
[speexdsp.git] / libspeex / resample.c
index 06f889c..29ddb59 100644 (file)
@@ -65,7 +65,6 @@ void speex_free (void *ptr) {free(ptr);}
 #include "misc.h"
 #endif
 
-
 #include <math.h>
 #include "speex/speex_resampler.h"
 
@@ -81,6 +80,26 @@ void speex_free (void *ptr) {free(ptr);}
 
 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
 
+struct QualityMapping {
+   int base_length;
+   int oversample;
+};
+
+
+struct QualityMapping quality_map[11] = {
+   {  8,  4}, /* 0 */
+   { 16,  4}, /* 1 */
+   { 32,  4}, /* 2 */
+   { 48,  8}, /* 3 */
+   { 64,  8}, /* 4 */
+   { 80,  8}, /* 5 */
+   { 96,  8}, /* 6 */
+   {128, 16}, /* 7 */
+   {160, 16}, /* 8 */
+   {192, 16}, /* 9 */
+   {256, 16}, /* 10 */
+};
+
 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
 
 struct SpeexResamplerState_ {
@@ -89,13 +108,20 @@ struct SpeexResamplerState_ {
    int    num_rate;
    int    den_rate;
    
+   int    quality;
    int    nb_channels;
-   int    last_sample;
-   int    samp_frac_num;
    int    filt_len;
+   int    mem_alloc_size;
    int    int_advance;
    int    frac_advance;
    float  cutoff;
+   int    oversample;
+   int    initialised;
+   int    started;
+   
+   /* FIXME: Need those per-channel */
+   int    last_sample;
+   int    samp_frac_num;
    
    spx_word16_t *mem;
    spx_word16_t *sinc_table;
@@ -133,30 +159,131 @@ static spx_word16_t sinc(float cutoff, float x, int N)
 }
 #endif
 
-/*SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)*/
-
-SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
+static void update_filter(SpeexResamplerState *st)
 {
    int i;
+   int 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 = .92f * 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);
+   } else {
+      /* up-sampling */
+      st->cutoff = .97f;
+   }
+
+   /* Choose the resampling type that requires the least amount of memory */
+   if (st->den_rate <= st->oversample)
+   {
+      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++)
+      {
+         int 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);
+         }
+      }
+      st->type = SPEEX_RESAMPLER_DIRECT;
+      /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
+   } else {
+      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<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);
+      st->type = SPEEX_RESAMPLER_INTERPOLATE;
+      /*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)
+   {
+      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;
+      speex_warning("init filter");
+   } else if (!st->started)
+   {
+      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;
+      speex_warning("reinit filter");
+   } else if (st->filt_len > old_length)
+   {
+      /* Increase the filter length */
+      speex_warning("increase filter size");
+      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;i++)
+      {
+         int j;
+         /* Copy data going backward */
+         for (j=0;j<old_length-1;j++)
+            st->mem[i*(st->filt_len-1)+(st->filt_len-2-j)] = st->mem[i*(old_length-1)+(old_length-2-j)];
+         /* Then put zeros for lack of anything better */
+         for (;j<st->filt_len-1;j++)
+            st->mem[i*(st->filt_len-1)+(st->filt_len-2-j)] = 0;
+         /* Adjust last_sample */
+         st->last_sample += (st->filt_len - old_length)/2;
+      }
+   } else if (st->filt_len < old_length)
+   {
+      /* Reduce filter length */
+      speex_warning("decrease filter size (unimplemented)");
+   }
+
+}
+
+
+SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
+{
    SpeexResamplerState *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->cutoff = 1.f;
    st->nb_channels = nb_channels;
    st->last_sample = 0;
-   st->filt_len = FILTER_SIZE;
-   st->mem = (spx_word16_t*)speex_alloc(nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
-   for (i=0;i<nb_channels*(st->filt_len-1);i++)
-      st->mem[i] = 0;
-   st->sinc_table_length = 0;
    
-   speex_resample_set_rate(st, in_rate, out_rate, in_rate_den, out_rate_den);
-
    st->in_stride = 1;
    st->out_stride = 1;
+
+   speex_resampler_set_quality(st, quality);
+   speex_resampler_set_rate(st, ratio_num, ratio_den, in_rate, out_rate);
+
+   
+   update_filter(st);
+   
+   st->initialised = 1;
    return st;
 }
 
@@ -174,6 +301,7 @@ static void speex_resampler_process_native(SpeexResamplerState *st, int channel_
    int out_sample = 0;
    spx_word16_t *mem;
    mem = st->mem + channel_index * (N-1);
+   st->started = 1;
    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
    {
       int j;
@@ -202,18 +330,18 @@ static void speex_resampler_process_native(SpeexResamplerState *st, int channel_
          float interp[4];
          const spx_word16_t *ptr;
          float alpha = ((float)st->samp_frac_num)/st->den_rate;
-         int offset = st->samp_frac_num*OVERSAMPLE/st->den_rate;
-         float frac = alpha*OVERSAMPLE - offset;
+         int offset = st->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;st->last_sample-N+1+j < 0;j++)
          {
             spx_word16_t curr_mem = mem[st->last_sample+j];
-            accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2]);
-            accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1]);
-            accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset]);
-            accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1]);
+            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->last_sample-N+1+j;
          /* Do the new part */
@@ -221,10 +349,10 @@ static void speex_resampler_process_native(SpeexResamplerState *st, int channel_
          {
             spx_word16_t curr_in = *ptr;
             ptr += st->in_stride;
-            accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-2]);
-            accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset-1]);
-            accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset]);
-            accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*OVERSAMPLE-offset+1]);
+            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]);
          }
          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
             but I know it's MMSE-optimal on a sinc */
@@ -310,57 +438,17 @@ void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const fl
    st->out_stride = ostride_save;
 }
 
-static void update_filter(SpeexResamplerState *st)
-{
-   int i;
-   /* Choose the resampling type that requires the least amount of memory */
-   if (st->den_rate <= OVERSAMPLE)
-   {
-      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++)
-      {
-         int 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);
-         }
-      }
-      st->type = SPEEX_RESAMPLER_DIRECT;
-      /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
-   } else {
-      if (!st->sinc_table)
-         st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*OVERSAMPLE+8)*sizeof(spx_word16_t));
-      else if (st->sinc_table_length < st->filt_len*OVERSAMPLE+8)
-      {
-         st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*OVERSAMPLE+8)*sizeof(spx_word16_t));
-         st->sinc_table_length = st->filt_len*OVERSAMPLE+8;
-      }
-      for (i=-4;i<OVERSAMPLE*st->filt_len+4;i++)
-         st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)OVERSAMPLE - st->filt_len/2), st->filt_len);
-      st->type = SPEEX_RESAMPLER_INTERPOLATE;
-      /*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;
 
-}
-
-void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate, int in_rate_den, int out_rate_den)
+void speex_resampler_set_rate(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
 {
    int fact;
-   if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == in_rate && st->den_rate == out_rate)
+   if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
       return;
    
    st->in_rate = in_rate;
    st->out_rate = out_rate;
-   st->num_rate = in_rate;
-   st->den_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++)
    {
@@ -370,36 +458,40 @@ void speex_resample_set_rate(SpeexResamplerState *st, int in_rate, int out_rate,
          st->den_rate /= fact;
       }
    }
-   
-   /* FIXME: Is there a danger of overflow? */
-   if (in_rate*out_rate_den > out_rate*in_rate_den)
-   {
-      /* down-sampling */
-      st->cutoff = .92f * out_rate*in_rate_den / (in_rate*out_rate_den);
-   } else {
-      /* up-sampling */
-      st->cutoff = .97f;
-   }
-   update_filter(st);
+      
+   if (st->initialised)
+      update_filter(st);
 }
 
+void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
+{
+   if (quality < 0)
+      quality = 0;
+   if (quality > 10)
+      quality = 10;
+   if (st->quality == quality)
+      return;
+   st->quality = quality;
+   if (st->initialised)
+      update_filter(st);
+}
 
-void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
+void speex_resampler_set_input_stride(SpeexResamplerState *st, int stride)
 {
    st->in_stride = stride;
 }
 
-void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
+void speex_resampler_set_output_stride(SpeexResamplerState *st, int stride)
 {
    st->out_stride = stride;
 }
 
-void speex_resample_skip_zeros(SpeexResamplerState *st)
+void speex_resampler_skip_zeros(SpeexResamplerState *st)
 {
    st->last_sample = st->filt_len/2;
 }
 
-void speex_resample_reset_mem(SpeexResamplerState *st)
+void speex_resampler_reset_mem(SpeexResamplerState *st)
 {
    int i;
    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)