Initial support for changing the filter length without screwing up the
[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       - Fix multi-channel (need one sample pos per channel)
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    int    magic_samples;
126    
127    spx_word16_t *mem;
128    spx_word16_t *sinc_table;
129    int    sinc_table_length;
130    int    in_stride;
131    int    out_stride;
132    SpeexSincType type;
133 } ;
134
135 #ifdef FIXED_POINT
136 /* The slow way of computing a sinc for the table. Should improve that some day */
137 static spx_word16_t sinc(float cutoff, float x, int N)
138 {
139    /*fprintf (stderr, "%f ", x);*/
140    x *= cutoff;
141    if (fabs(x)<1e-6f)
142       return WORD2INT(32768.*cutoff);
143    else if (fabs(x) > .5f*N)
144       return 0;
145    /*FIXME: Can it really be any slower than this? */
146    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)));
147 }
148 #else
149 /* The slow way of computing a sinc for the table. Should improve that some day */
150 static spx_word16_t sinc(float cutoff, float x, int N)
151 {
152    /*fprintf (stderr, "%f ", x);*/
153    x *= cutoff;
154    if (fabs(x)<1e-6)
155       return cutoff;
156    else if (fabs(x) > .5*N)
157       return 0;
158    /*FIXME: Can it really be any slower than this? */
159    return cutoff*sin(M_PI*x)/(M_PI*x) * (.42+.5*cos(2*x*M_PI/N)+.08*cos(4*x*M_PI/N));
160 }
161 #endif
162
163 static void update_filter(SpeexResamplerState *st)
164 {
165    int i;
166    int old_length;
167    
168    old_length = st->filt_len;
169    st->oversample = quality_map[st->quality].oversample;
170    st->filt_len = quality_map[st->quality].base_length;
171    
172    if (st->num_rate > st->den_rate)
173    {
174       /* down-sampling */
175       st->cutoff = .92f * st->den_rate / st->num_rate;
176       /* FIXME: divide the numerator and denominator by a certain amount if they're too large */
177       st->filt_len = st->filt_len*st->num_rate / st->den_rate;
178       /* Round down to make sure we have a multiple of 4 */
179       st->filt_len &= (~0x3);
180    } else {
181       /* up-sampling */
182       st->cutoff = .97f;
183    }
184
185    /* Choose the resampling type that requires the least amount of memory */
186    if (st->den_rate <= st->oversample)
187    {
188       if (!st->sinc_table)
189          st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
190       else if (st->sinc_table_length < st->filt_len*st->den_rate)
191       {
192          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
193          st->sinc_table_length = st->filt_len*st->den_rate;
194       }
195       for (i=0;i<st->den_rate;i++)
196       {
197          int j;
198          for (j=0;j<st->filt_len;j++)
199          {
200             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);
201          }
202       }
203       st->type = SPEEX_RESAMPLER_DIRECT;
204       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
205    } else {
206       if (!st->sinc_table)
207          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
208       else if (st->sinc_table_length < st->filt_len*st->oversample+8)
209       {
210          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
211          st->sinc_table_length = st->filt_len*st->oversample+8;
212       }
213       for (i=-4;i<st->oversample*st->filt_len+4;i++)
214          st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len);
215       st->type = SPEEX_RESAMPLER_INTERPOLATE;
216       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
217    }
218    st->int_advance = st->num_rate/st->den_rate;
219    st->frac_advance = st->num_rate%st->den_rate;
220
221    if (!st->mem)
222    {
223       st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
224       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
225          st->mem[i] = 0;
226       st->mem_alloc_size = st->filt_len-1;
227       /*speex_warning("init filter");*/
228    } else if (!st->started)
229    {
230       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
231       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
232          st->mem[i] = 0;
233       st->mem_alloc_size = st->filt_len-1;
234       /*speex_warning("reinit filter");*/
235    } else if (st->filt_len > old_length)
236    {
237       /* Increase the filter length */
238       /*speex_warning("increase filter size");*/
239       int old_alloc_size = st->mem_alloc_size;
240       if (st->filt_len-1 > st->mem_alloc_size)
241       {
242          st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
243          st->mem_alloc_size = st->filt_len-1;
244       }
245       for (i=0;i<st->nb_channels;i++)
246       {
247          int j;
248          /* Copy data going backward */
249          for (j=0;j<old_length-1;j++)
250             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*old_alloc_size+(old_length-2-j)];
251          /* Then put zeros for lack of anything better */
252          for (;j<st->filt_len-1;j++)
253             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
254          /* Adjust last_sample */
255          st->last_sample += (st->filt_len - old_length)/2;
256       }
257    } else if (st->filt_len < old_length)
258    {
259       /* Reduce filter length, this a bit tricky */
260       /*speex_warning("decrease filter size (unimplemented)");*/
261       /* Adjust last_sample (which will likely end up negative) */
262       /*st->last_sample += (st->filt_len - old_length)/2;*/
263       st->magic_samples = (old_length - st->filt_len)/2;
264       for (i=0;i<st->nb_channels;i++)
265       {
266          int j;
267          /* Copy data going backward */
268          for (j=0;j<old_length-1;j++)
269             st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples];
270       }
271    }
272
273 }
274
275
276 SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
277 {
278    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
279    st->initialised = 0;
280    st->started = 0;
281    st->in_rate = 0;
282    st->out_rate = 0;
283    st->num_rate = 0;
284    st->den_rate = 0;
285    st->quality = -1;
286    st->sinc_table_length = 0;
287    st->mem_alloc_size = 0;
288    st->filt_len = 0;
289    st->mem = 0;
290    
291    st->cutoff = 1.f;
292    st->nb_channels = nb_channels;
293    st->in_stride = 1;
294    st->out_stride = 1;
295    
296    /* FIXME: Make that per channel */
297    st->last_sample = 0;
298    st->magic_samples = 0;
299    st->samp_frac_num = 0;
300    
301
302    speex_resampler_set_quality(st, quality);
303    speex_resampler_set_rate(st, ratio_num, ratio_den, in_rate, out_rate);
304
305    
306    update_filter(st);
307    
308    st->initialised = 1;
309    return st;
310 }
311
312 void speex_resampler_destroy(SpeexResamplerState *st)
313 {
314    speex_free(st->mem);
315    speex_free(st->sinc_table);
316    speex_free(st);
317 }
318
319 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)
320 {
321    int j=0;
322    int N = st->filt_len;
323    int out_sample = 0;
324    spx_word16_t *mem;
325    int tmp_out_len = 0;
326    mem = st->mem + channel_index * st->mem_alloc_size;
327    st->started = 1;
328    if (st->magic_samples)
329    {
330       int tmp_in_len;
331       tmp_in_len = st->magic_samples;
332       tmp_out_len = *out_len;
333       /* FIXME: Need to handle the case where the out array is too small */
334       /* magic_samples needs to be set to zero to avoid infinite recursion */
335       st->magic_samples = 0;
336       speex_resampler_process_native(st, channel_index, mem+N-1, &tmp_in_len, out, &tmp_out_len);
337       speex_warning_int("extra samples:", tmp_out_len);
338       out += tmp_out_len;
339    }
340    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
341    {
342       int j;
343       spx_word32_t sum=0;
344       
345       if (st->type == SPEEX_RESAMPLER_DIRECT)
346       {
347          /* We already have all the filter coefficients pre-computed in the table */
348          const spx_word16_t *ptr;
349          /* Do the memory part */
350          for (j=0;st->last_sample-N+1+j < 0;j++)
351          {
352             sum += MULT16_16(mem[st->last_sample+j],st->sinc_table[st->samp_frac_num*st->filt_len+j]);
353          }
354          
355          /* Do the new part */
356          ptr = in+st->last_sample-N+1+j;
357          for (;j<N;j++)
358          {
359             sum += MULT16_16(*ptr,st->sinc_table[st->samp_frac_num*st->filt_len+j]);
360             ptr += st->in_stride;
361          }
362       } else {
363          /* We need to interpolate the sinc filter */
364          spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
365          float interp[4];
366          const spx_word16_t *ptr;
367          float alpha = ((float)st->samp_frac_num)/st->den_rate;
368          int offset = st->samp_frac_num*st->oversample/st->den_rate;
369          float frac = alpha*st->oversample - offset;
370          /* This code is written like this to make it easy to optimise with SIMD.
371             For most DSPs, it would be best to split the loops in two because most DSPs 
372             have only two accumulators */
373          for (j=0;st->last_sample-N+1+j < 0;j++)
374          {
375             spx_word16_t curr_mem = mem[st->last_sample+j];
376             accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
377             accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
378             accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
379             accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
380          }
381          ptr = in+st->last_sample-N+1+j;
382          /* Do the new part */
383          for (;j<N;j++)
384          {
385             spx_word16_t curr_in = *ptr;
386             ptr += st->in_stride;
387             accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
388             accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
389             accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
390             accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
391          }
392          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
393             but I know it's MMSE-optimal on a sinc */
394          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
395          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
396          /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
397          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
398          /* Just to make sure we don't have rounding problems */
399          interp[2] = 1.f-interp[0]-interp[1]-interp[3];
400          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
401          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
402       }
403       *out = PSHR32(sum,15);
404       out += st->out_stride;
405       out_sample++;
406       st->last_sample += st->int_advance;
407       st->samp_frac_num += st->frac_advance;
408       if (st->samp_frac_num >= st->den_rate)
409       {
410          st->samp_frac_num -= st->den_rate;
411          st->last_sample++;
412       }
413    }
414    if (st->last_sample < *in_len)
415       *in_len = st->last_sample;
416    *out_len = out_sample+tmp_out_len;
417    st->last_sample -= *in_len;
418    
419    for (j=0;j<N-1-*in_len;j++)
420       mem[j] = mem[j+*in_len];
421    for (;j<N-1;j++)
422       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
423    
424 }
425
426 #ifdef FIXED_POINT
427 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
428 {
429    int i;
430    spx_word16_t x[*in_len];
431    spx_word16_t y[*out_len];
432    for (i=0;i<*in_len;i++)
433       x[i] = WORD2INT(in[i]);
434    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
435    for (i=0;i<*out_len;i++)
436       out[i] = y[i];
437
438 }
439 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)
440 {
441    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
442 }
443 #else
444 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
445 {
446    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
447 }
448 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)
449 {
450    int i;
451    spx_word16_t x[*in_len];
452    spx_word16_t y[*out_len];
453    for (i=0;i<*out_len;i++)
454       x[i] = in[i];
455    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
456    for (i=0;i<*in_len;i++)
457       out[i] = WORD2INT(y[i]);
458 }
459 #endif
460
461 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
462 {
463    int i;
464    int istride_save, ostride_save;
465    istride_save = st->in_stride;
466    ostride_save = st->out_stride;
467    st->in_stride = st->out_stride = st->nb_channels;
468    for (i=0;i<st->nb_channels;i++)
469    {
470       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
471    }
472    st->in_stride = istride_save;
473    st->out_stride = ostride_save;
474 }
475
476
477 void speex_resampler_set_rate(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
478 {
479    int fact;
480    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
481       return;
482    
483    st->in_rate = in_rate;
484    st->out_rate = out_rate;
485    st->num_rate = ratio_num;
486    st->den_rate = ratio_den;
487    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
488    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
489    {
490       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
491       {
492          st->num_rate /= fact;
493          st->den_rate /= fact;
494       }
495    }
496       
497    if (st->initialised)
498       update_filter(st);
499 }
500
501 void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
502 {
503    if (quality < 0)
504       quality = 0;
505    if (quality > 10)
506       quality = 10;
507    if (st->quality == quality)
508       return;
509    st->quality = quality;
510    if (st->initialised)
511       update_filter(st);
512 }
513
514 void speex_resampler_set_input_stride(SpeexResamplerState *st, int stride)
515 {
516    st->in_stride = stride;
517 }
518
519 void speex_resampler_set_output_stride(SpeexResamplerState *st, int stride)
520 {
521    st->out_stride = stride;
522 }
523
524 void speex_resampler_skip_zeros(SpeexResamplerState *st)
525 {
526    st->last_sample = st->filt_len/2;
527 }
528
529 void speex_resampler_reset_mem(SpeexResamplerState *st)
530 {
531    int i;
532    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
533       st->mem[i] = 0;
534 }
535