ae46ef52a88fa8729fe87135031931ba9b87a10b
[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       st->mem_alloc_size = st->filt_len-1;
226       /*speex_warning("init filter");*/
227    } else if (!st->started)
228    {
229       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
230       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
231          st->mem[i] = 0;
232       st->mem_alloc_size = st->filt_len-1;
233       /*speex_warning("reinit filter");*/
234    } else if (st->filt_len > old_length)
235    {
236       /* Increase the filter length */
237       /*speex_warning("increase filter size");*/
238       if (st->filt_len-1 > st->mem_alloc_size)
239       {
240          st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
241          st->mem_alloc_size = st->filt_len-1;
242       }
243       for (i=0;i<st->nb_channels;i++)
244       {
245          int j;
246          /* Copy data going backward */
247          for (j=0;j<old_length-1;j++)
248             st->mem[i*(st->filt_len-1)+(st->filt_len-2-j)] = st->mem[i*(old_length-1)+(old_length-2-j)];
249          /* Then put zeros for lack of anything better */
250          for (;j<st->filt_len-1;j++)
251             st->mem[i*(st->filt_len-1)+(st->filt_len-2-j)] = 0;
252          /* Adjust last_sample */
253          st->last_sample += (st->filt_len - old_length)/2;
254       }
255    } else if (st->filt_len < old_length)
256    {
257       /* Reduce filter length, this a bit tricky */
258       /*speex_warning("decrease filter size (unimplemented)");*/
259       /* Adjust last_sample (which will likely end up negative) */
260       /*st->last_sample += (st->filt_len - old_length)/2;*/
261    }
262
263 }
264
265
266 SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
267 {
268    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
269    st->initialised = 0;
270    st->started = 0;
271    st->in_rate = 0;
272    st->out_rate = 0;
273    st->num_rate = 0;
274    st->den_rate = 0;
275    st->quality = -1;
276    st->sinc_table_length = 0;
277    st->mem_alloc_size = 0;
278    st->filt_len = 0;
279    st->mem = 0;
280    
281    st->cutoff = 1.f;
282    st->nb_channels = nb_channels;
283    st->last_sample = 0;
284    
285    st->in_stride = 1;
286    st->out_stride = 1;
287
288    speex_resampler_set_quality(st, quality);
289    speex_resampler_set_rate(st, ratio_num, ratio_den, in_rate, out_rate);
290
291    
292    update_filter(st);
293    
294    st->initialised = 1;
295    return st;
296 }
297
298 void speex_resampler_destroy(SpeexResamplerState *st)
299 {
300    speex_free(st->mem);
301    speex_free(st->sinc_table);
302    speex_free(st);
303 }
304
305 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)
306 {
307    int j=0;
308    int N = st->filt_len;
309    int out_sample = 0;
310    spx_word16_t *mem;
311    mem = st->mem + channel_index * (N-1);
312    st->started = 1;
313    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
314    {
315       int j;
316       spx_word32_t sum=0;
317       
318       if (st->type == SPEEX_RESAMPLER_DIRECT)
319       {
320          /* We already have all the filter coefficients pre-computed in the table */
321          const spx_word16_t *ptr;
322          /* Do the memory part */
323          for (j=0;st->last_sample-N+1+j < 0;j++)
324          {
325             sum += MULT16_16(mem[st->last_sample+j],st->sinc_table[st->samp_frac_num*st->filt_len+j]);
326          }
327          
328          /* Do the new part */
329          ptr = in+st->last_sample-N+1+j;
330          for (;j<N;j++)
331          {
332             sum += MULT16_16(*ptr,st->sinc_table[st->samp_frac_num*st->filt_len+j]);
333             ptr += st->in_stride;
334          }
335       } else {
336          /* We need to interpolate the sinc filter */
337          spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
338          float interp[4];
339          const spx_word16_t *ptr;
340          float alpha = ((float)st->samp_frac_num)/st->den_rate;
341          int offset = st->samp_frac_num*st->oversample/st->den_rate;
342          float frac = alpha*st->oversample - offset;
343          /* This code is written like this to make it easy to optimise with SIMD.
344             For most DSPs, it would be best to split the loops in two because most DSPs 
345             have only two accumulators */
346          for (j=0;st->last_sample-N+1+j < 0;j++)
347          {
348             spx_word16_t curr_mem = mem[st->last_sample+j];
349             accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
350             accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
351             accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
352             accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
353          }
354          ptr = in+st->last_sample-N+1+j;
355          /* Do the new part */
356          for (;j<N;j++)
357          {
358             spx_word16_t curr_in = *ptr;
359             ptr += st->in_stride;
360             accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
361             accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
362             accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
363             accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
364          }
365          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
366             but I know it's MMSE-optimal on a sinc */
367          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
368          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
369          /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
370          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
371          /* Just to make sure we don't have rounding problems */
372          interp[2] = 1.f-interp[0]-interp[1]-interp[3];
373          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
374          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
375       }
376       *out = PSHR32(sum,15);
377       out += st->out_stride;
378       out_sample++;
379       st->last_sample += st->int_advance;
380       st->samp_frac_num += st->frac_advance;
381       if (st->samp_frac_num >= st->den_rate)
382       {
383          st->samp_frac_num -= st->den_rate;
384          st->last_sample++;
385       }
386    }
387    if (st->last_sample < *in_len)
388       *in_len = st->last_sample;
389    *out_len = out_sample;
390    st->last_sample -= *in_len;
391    
392    for (j=0;j<N-1-*in_len;j++)
393       mem[j] = mem[j+*in_len];
394    for (;j<N-1;j++)
395       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
396    
397 }
398
399 #ifdef FIXED_POINT
400 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
401 {
402    int i;
403    spx_word16_t x[*in_len];
404    spx_word16_t y[*out_len];
405    for (i=0;i<*in_len;i++)
406       x[i] = WORD2INT(in[i]);
407    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
408    for (i=0;i<*out_len;i++)
409       out[i] = y[i];
410
411 }
412 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)
413 {
414    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
415 }
416 #else
417 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
418 {
419    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
420 }
421 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)
422 {
423    int i;
424    spx_word16_t x[*in_len];
425    spx_word16_t y[*out_len];
426    for (i=0;i<*out_len;i++)
427       x[i] = in[i];
428    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
429    for (i=0;i<*in_len;i++)
430       out[i] = WORD2INT(y[i]);
431 }
432 #endif
433
434 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
435 {
436    int i;
437    int istride_save, ostride_save;
438    istride_save = st->in_stride;
439    ostride_save = st->out_stride;
440    st->in_stride = st->out_stride = st->nb_channels;
441    for (i=0;i<st->nb_channels;i++)
442    {
443       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
444    }
445    st->in_stride = istride_save;
446    st->out_stride = ostride_save;
447 }
448
449
450 void speex_resampler_set_rate(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
451 {
452    int fact;
453    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
454       return;
455    
456    st->in_rate = in_rate;
457    st->out_rate = out_rate;
458    st->num_rate = ratio_num;
459    st->den_rate = ratio_den;
460    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
461    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
462    {
463       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
464       {
465          st->num_rate /= fact;
466          st->den_rate /= fact;
467       }
468    }
469       
470    if (st->initialised)
471       update_filter(st);
472 }
473
474 void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
475 {
476    if (quality < 0)
477       quality = 0;
478    if (quality > 10)
479       quality = 10;
480    if (st->quality == quality)
481       return;
482    st->quality = quality;
483    if (st->initialised)
484       update_filter(st);
485 }
486
487 void speex_resampler_set_input_stride(SpeexResamplerState *st, int stride)
488 {
489    st->in_stride = stride;
490 }
491
492 void speex_resampler_set_output_stride(SpeexResamplerState *st, int stride)
493 {
494    st->out_stride = stride;
495 }
496
497 void speex_resampler_skip_zeros(SpeexResamplerState *st)
498 {
499    st->last_sample = st->filt_len/2;
500 }
501
502 void speex_resampler_reset_mem(SpeexResamplerState *st)
503 {
504    int i;
505    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
506       st->mem[i] = 0;
507 }
508