initial support for changing the filter length on the fly...
[speexdsp.git] / libspeex / resample.c
1 /* Copyright (C) 2007 Jean-Marc Valin
2       
3    File: resample.c
4    Arbitrary resampling code
5
6    Redistribution and use in source and binary forms, with or without
7    modification, are permitted provided that the following conditions are
8    met:
9
10    1. Redistributions of source code must retain the above copyright notice,
11    this list of conditions and the following disclaimer.
12
13    2. Redistributions in binary form must reproduce the above copyright
14    notice, this list of conditions and the following disclaimer in the
15    documentation and/or other materials provided with the distribution.
16
17    3. The name of the author may not be used to endorse or promote products
18    derived from this software without specific prior written permission.
19
20    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
21    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
22    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
24    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
28    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30    POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 /*
34    The design goals of this code are:
35       - Very fast algorithm
36       - SIMD-friendly algorithm
37       - Low memory requirement
38       - Good *perceptual* quality (and not best SNR)
39
40    The code is working, but it's in a very early stage, so it may have
41    artifacts, noise or subliminal messages from satan. Also, the API 
42    isn't stable and I can actually promise that I *will* change the API
43    some time in the future.
44
45 TODO list:
46       - Variable length filter (depending on frequency/ratio and quality)
47       - Quality setting to control filter length (and sinc window?)
48       - Variable calculation resolution depending on quality setting
49          - Single vs double in float mode
50          - 16-bit vs 32-bit (sinc only) in fixed-point mode
51       - Make it possible to change the filter length without major artifacts
52
53 */
54
55 #ifdef HAVE_CONFIG_H
56 #include "config.h"
57 #endif
58
59 #ifdef OUTSIDE_SPEEX
60 #include <stdlib.h>
61 void *speex_alloc (int size) {return calloc(size,1);}
62 void *speex_realloc (void *ptr, int size) {return realloc(ptr, size);}
63 void speex_free (void *ptr) {free(ptr);}
64 #else
65 #include "misc.h"
66 #endif
67
68 #include <math.h>
69 #include "speex/speex_resampler.h"
70
71 #ifdef FIXED_POINT
72 #define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))  
73 #else
74 #define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))  
75 #endif
76                
77 /*#define float double*/
78 #define FILTER_SIZE 64
79 #define OVERSAMPLE 8
80
81 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
82
83 struct QualityMapping {
84    int base_length;
85    int oversample;
86 };
87
88
89 struct QualityMapping quality_map[11] = {
90    {  8,  4}, /* 0 */
91    { 16,  4}, /* 1 */
92    { 32,  4}, /* 2 */
93    { 48,  8}, /* 3 */
94    { 64,  8}, /* 4 */
95    { 80,  8}, /* 5 */
96    { 96,  8}, /* 6 */
97    {128, 16}, /* 7 */
98    {160, 16}, /* 8 */
99    {192, 16}, /* 9 */
100    {256, 16}, /* 10 */
101 };
102
103 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
104
105 struct SpeexResamplerState_ {
106    int    in_rate;
107    int    out_rate;
108    int    num_rate;
109    int    den_rate;
110    
111    int    quality;
112    int    nb_channels;
113    int    filt_len;
114    int    mem_alloc_size;
115    int    int_advance;
116    int    frac_advance;
117    float  cutoff;
118    int    oversample;
119    int    initialised;
120    int    started;
121    
122    /* FIXME: Need those per-channel */
123    int    last_sample;
124    int    samp_frac_num;
125    
126    spx_word16_t *mem;
127    spx_word16_t *sinc_table;
128    int    sinc_table_length;
129    int    in_stride;
130    int    out_stride;
131    SpeexSincType type;
132 } ;
133
134 #ifdef FIXED_POINT
135 /* The slow way of computing a sinc for the table. Should improve that some day */
136 static spx_word16_t sinc(float cutoff, float x, int N)
137 {
138    /*fprintf (stderr, "%f ", x);*/
139    x *= cutoff;
140    if (fabs(x)<1e-6f)
141       return WORD2INT(32768.*cutoff);
142    else if (fabs(x) > .5f*N)
143       return 0;
144    /*FIXME: Can it really be any slower than this? */
145    return WORD2INT(32768.*cutoff*sin(M_PI*x)/(M_PI*x) * (.42+.5*cos(2*x*M_PI/N)+.08*cos(4*x*M_PI/N)));
146 }
147 #else
148 /* The slow way of computing a sinc for the table. Should improve that some day */
149 static spx_word16_t sinc(float cutoff, float x, int N)
150 {
151    /*fprintf (stderr, "%f ", x);*/
152    x *= cutoff;
153    if (fabs(x)<1e-6)
154       return cutoff;
155    else if (fabs(x) > .5*N)
156       return 0;
157    /*FIXME: Can it really be any slower than this? */
158    return cutoff*sin(M_PI*x)/(M_PI*x) * (.42+.5*cos(2*x*M_PI/N)+.08*cos(4*x*M_PI/N));
159 }
160 #endif
161
162 static void update_filter(SpeexResamplerState *st)
163 {
164    int i;
165    int old_length;
166    
167    old_length = st->filt_len;
168    st->oversample = quality_map[st->quality].oversample;
169    st->filt_len = quality_map[st->quality].base_length;
170    
171    if (st->num_rate > st->den_rate)
172    {
173       /* down-sampling */
174       st->cutoff = .92f * st->den_rate / st->num_rate;
175       /* FIXME: divide the numerator and denominator by a certain amount if they're too large */
176       st->filt_len = st->filt_len*st->num_rate / st->den_rate;
177       /* Round down to make sure we have a multiple of 4 */
178       st->filt_len &= (~0x3);
179    } else {
180       /* up-sampling */
181       st->cutoff = .97f;
182    }
183
184    /* Choose the resampling type that requires the least amount of memory */
185    if (st->den_rate <= st->oversample)
186    {
187       if (!st->sinc_table)
188          st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
189       else if (st->sinc_table_length < st->filt_len*st->den_rate)
190       {
191          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
192          st->sinc_table_length = st->filt_len*st->den_rate;
193       }
194       for (i=0;i<st->den_rate;i++)
195       {
196          int j;
197          for (j=0;j<st->filt_len;j++)
198          {
199             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);
200          }
201       }
202       st->type = SPEEX_RESAMPLER_DIRECT;
203       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
204    } else {
205       if (!st->sinc_table)
206          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
207       else if (st->sinc_table_length < st->filt_len*st->oversample+8)
208       {
209          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
210          st->sinc_table_length = st->filt_len*st->oversample+8;
211       }
212       for (i=-4;i<st->oversample*st->filt_len+4;i++)
213          st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len);
214       st->type = SPEEX_RESAMPLER_INTERPOLATE;
215       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
216    }
217    st->int_advance = st->num_rate/st->den_rate;
218    st->frac_advance = st->num_rate%st->den_rate;
219
220    if (!st->mem)
221    {
222       st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
223       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
224          st->mem[i] = 0;
225       speex_warning("init filter");
226    } else if (!st->started)
227    {
228       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
229       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
230          st->mem[i] = 0;
231       speex_warning("reinit filter");
232    } else if (st->filt_len > old_length)
233    {
234       /* Increase the filter length */
235       speex_warning("increase filter size");
236       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
237       for (i=0;i<st->nb_channels;i++)
238       {
239          int j;
240          /* Copy data going backward */
241          for (j=0;j<old_length-1;j++)
242             st->mem[i*(st->filt_len-1)+(st->filt_len-2-j)] = st->mem[i*(old_length-1)+(old_length-2-j)];
243          /* Then put zeros for lack of anything better */
244          for (;j<st->filt_len-1;j++)
245             st->mem[i*(st->filt_len-1)+(st->filt_len-2-j)] = 0;
246          /* Adjust last_sample */
247          st->last_sample += (st->filt_len - old_length)/2;
248       }
249    } else if (st->filt_len < old_length)
250    {
251       /* Reduce filter length */
252       speex_warning("decrease filter size (unimplemented)");
253    }
254
255 }
256
257
258 SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
259 {
260    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
261    st->initialised = 0;
262    st->started = 0;
263    st->in_rate = 0;
264    st->out_rate = 0;
265    st->num_rate = 0;
266    st->den_rate = 0;
267    st->quality = -1;
268    st->sinc_table_length = 0;
269    st->mem_alloc_size = 0;
270    st->filt_len = 0;
271    st->mem = 0;
272    
273    st->cutoff = 1.f;
274    st->nb_channels = nb_channels;
275    st->last_sample = 0;
276    
277    st->in_stride = 1;
278    st->out_stride = 1;
279
280    speex_resampler_set_quality(st, quality);
281    speex_resampler_set_rate(st, ratio_num, ratio_den, in_rate, out_rate);
282
283    
284    update_filter(st);
285    
286    st->initialised = 1;
287    return st;
288 }
289
290 void speex_resampler_destroy(SpeexResamplerState *st)
291 {
292    speex_free(st->mem);
293    speex_free(st->sinc_table);
294    speex_free(st);
295 }
296
297 static void speex_resampler_process_native(SpeexResamplerState *st, int channel_index, const spx_word16_t *in, int *in_len, spx_word16_t *out, int *out_len)
298 {
299    int j=0;
300    int N = st->filt_len;
301    int out_sample = 0;
302    spx_word16_t *mem;
303    mem = st->mem + channel_index * (N-1);
304    st->started = 1;
305    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
306    {
307       int j;
308       spx_word32_t sum=0;
309       
310       if (st->type == SPEEX_RESAMPLER_DIRECT)
311       {
312          /* We already have all the filter coefficients pre-computed in the table */
313          const spx_word16_t *ptr;
314          /* Do the memory part */
315          for (j=0;st->last_sample-N+1+j < 0;j++)
316          {
317             sum += MULT16_16(mem[st->last_sample+j],st->sinc_table[st->samp_frac_num*st->filt_len+j]);
318          }
319          
320          /* Do the new part */
321          ptr = in+st->last_sample-N+1+j;
322          for (;j<N;j++)
323          {
324             sum += MULT16_16(*ptr,st->sinc_table[st->samp_frac_num*st->filt_len+j]);
325             ptr += st->in_stride;
326          }
327       } else {
328          /* We need to interpolate the sinc filter */
329          spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
330          float interp[4];
331          const spx_word16_t *ptr;
332          float alpha = ((float)st->samp_frac_num)/st->den_rate;
333          int offset = st->samp_frac_num*st->oversample/st->den_rate;
334          float frac = alpha*st->oversample - offset;
335          /* This code is written like this to make it easy to optimise with SIMD.
336             For most DSPs, it would be best to split the loops in two because most DSPs 
337             have only two accumulators */
338          for (j=0;st->last_sample-N+1+j < 0;j++)
339          {
340             spx_word16_t curr_mem = mem[st->last_sample+j];
341             accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
342             accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
343             accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
344             accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
345          }
346          ptr = in+st->last_sample-N+1+j;
347          /* Do the new part */
348          for (;j<N;j++)
349          {
350             spx_word16_t curr_in = *ptr;
351             ptr += st->in_stride;
352             accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
353             accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
354             accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
355             accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
356          }
357          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
358             but I know it's MMSE-optimal on a sinc */
359          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
360          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
361          /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
362          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
363          /* Just to make sure we don't have rounding problems */
364          interp[2] = 1.f-interp[0]-interp[1]-interp[3];
365          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
366          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
367       }
368       *out = PSHR32(sum,15);
369       out += st->out_stride;
370       out_sample++;
371       st->last_sample += st->int_advance;
372       st->samp_frac_num += st->frac_advance;
373       if (st->samp_frac_num >= st->den_rate)
374       {
375          st->samp_frac_num -= st->den_rate;
376          st->last_sample++;
377       }
378    }
379    if (st->last_sample < *in_len)
380       *in_len = st->last_sample;
381    *out_len = out_sample;
382    st->last_sample -= *in_len;
383    
384    for (j=0;j<N-1-*in_len;j++)
385       mem[j] = mem[j+*in_len];
386    for (;j<N-1;j++)
387       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
388    
389 }
390
391 #ifdef FIXED_POINT
392 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
393 {
394    int i;
395    spx_word16_t x[*in_len];
396    spx_word16_t y[*out_len];
397    for (i=0;i<*in_len;i++)
398       x[i] = WORD2INT(in[i]);
399    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
400    for (i=0;i<*out_len;i++)
401       out[i] = y[i];
402
403 }
404 void speex_resampler_process_int(SpeexResamplerState *st, int channel_index, const spx_int16_t *in, int *in_len, spx_int16_t *out, int *out_len)
405 {
406    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
407 }
408 #else
409 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
410 {
411    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
412 }
413 void speex_resampler_process_int(SpeexResamplerState *st, int channel_index, const spx_int16_t *in, int *in_len, spx_int16_t *out, int *out_len)
414 {
415    int i;
416    spx_word16_t x[*in_len];
417    spx_word16_t y[*out_len];
418    for (i=0;i<*out_len;i++)
419       x[i] = in[i];
420    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
421    for (i=0;i<*in_len;i++)
422       out[i] = WORD2INT(y[i]);
423 }
424 #endif
425
426 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
427 {
428    int i;
429    int istride_save, ostride_save;
430    istride_save = st->in_stride;
431    ostride_save = st->out_stride;
432    st->in_stride = st->out_stride = st->nb_channels;
433    for (i=0;i<st->nb_channels;i++)
434    {
435       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
436    }
437    st->in_stride = istride_save;
438    st->out_stride = ostride_save;
439 }
440
441
442 void speex_resampler_set_rate(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
443 {
444    int fact;
445    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
446       return;
447    
448    st->in_rate = in_rate;
449    st->out_rate = out_rate;
450    st->num_rate = ratio_num;
451    st->den_rate = ratio_den;
452    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
453    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
454    {
455       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
456       {
457          st->num_rate /= fact;
458          st->den_rate /= fact;
459       }
460    }
461       
462    if (st->initialised)
463       update_filter(st);
464 }
465
466 void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
467 {
468    if (quality < 0)
469       quality = 0;
470    if (quality > 10)
471       quality = 10;
472    if (st->quality == quality)
473       return;
474    st->quality = quality;
475    if (st->initialised)
476       update_filter(st);
477 }
478
479 void speex_resampler_set_input_stride(SpeexResamplerState *st, int stride)
480 {
481    st->in_stride = stride;
482 }
483
484 void speex_resampler_set_output_stride(SpeexResamplerState *st, int stride)
485 {
486    st->out_stride = stride;
487 }
488
489 void speex_resampler_skip_zeros(SpeexResamplerState *st)
490 {
491    st->last_sample = st->filt_len/2;
492 }
493
494 void speex_resampler_reset_mem(SpeexResamplerState *st)
495 {
496    int i;
497    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
498       st->mem[i] = 0;
499 }
500