Changed the resampler API again. Introducing a quality setting.
[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
69 #include <math.h>
70 #include "speex/speex_resampler.h"
71
72 #ifdef FIXED_POINT
73 #define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))  
74 #else
75 #define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))  
76 #endif
77                
78 /*#define float double*/
79 #define FILTER_SIZE 64
80 #define OVERSAMPLE 8
81
82 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
83
84 typedef enum {SPEEX_RESAMPLER_DIRECT=0, SPEEX_RESAMPLER_INTERPOLATE=1} SpeexSincType;
85
86 struct SpeexResamplerState_ {
87    int    in_rate;
88    int    out_rate;
89    int    num_rate;
90    int    den_rate;
91    
92    int    quality;
93    int    nb_channels;
94    int    last_sample;
95    int    samp_frac_num;
96    int    filt_len;
97    int    int_advance;
98    int    frac_advance;
99    float  cutoff;
100    int    oversample;
101    int    initialised;
102    
103    
104    spx_word16_t *mem;
105    spx_word16_t *sinc_table;
106    int    sinc_table_length;
107    int    in_stride;
108    int    out_stride;
109    SpeexSincType type;
110 } ;
111
112 #ifdef FIXED_POINT
113 /* The slow way of computing a sinc for the table. Should improve that some day */
114 static spx_word16_t sinc(float cutoff, float x, int N)
115 {
116    /*fprintf (stderr, "%f ", x);*/
117    x *= cutoff;
118    if (fabs(x)<1e-6f)
119       return WORD2INT(32768.*cutoff);
120    else if (fabs(x) > .5f*N)
121       return 0;
122    /*FIXME: Can it really be any slower than this? */
123    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)));
124 }
125 #else
126 /* The slow way of computing a sinc for the table. Should improve that some day */
127 static spx_word16_t sinc(float cutoff, float x, int N)
128 {
129    /*fprintf (stderr, "%f ", x);*/
130    x *= cutoff;
131    if (fabs(x)<1e-6)
132       return cutoff;
133    else if (fabs(x) > .5*N)
134       return 0;
135    /*FIXME: Can it really be any slower than this? */
136    return cutoff*sin(M_PI*x)/(M_PI*x) * (.42+.5*cos(2*x*M_PI/N)+.08*cos(4*x*M_PI/N));
137 }
138 #endif
139
140 static void update_filter(SpeexResamplerState *st)
141 {
142    int i;
143    
144    st->oversample = OVERSAMPLE;
145
146    st->filt_len = 8 + 12*st->quality;
147    
148    /* Choose the resampling type that requires the least amount of memory */
149    if (st->den_rate <= st->oversample)
150    {
151       if (!st->sinc_table)
152          st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
153       else if (st->sinc_table_length < st->filt_len*st->den_rate)
154       {
155          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
156          st->sinc_table_length = st->filt_len*st->den_rate;
157       }
158       for (i=0;i<st->den_rate;i++)
159       {
160          int j;
161          for (j=0;j<st->filt_len;j++)
162          {
163             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);
164          }
165       }
166       st->type = SPEEX_RESAMPLER_DIRECT;
167       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
168    } else {
169       if (!st->sinc_table)
170          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
171       else if (st->sinc_table_length < st->filt_len*st->oversample+8)
172       {
173          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
174          st->sinc_table_length = st->filt_len*st->oversample+8;
175       }
176       for (i=-4;i<st->oversample*st->filt_len+4;i++)
177          st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len);
178       st->type = SPEEX_RESAMPLER_INTERPOLATE;
179       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
180    }
181    st->int_advance = st->num_rate/st->den_rate;
182    st->frac_advance = st->num_rate%st->den_rate;
183
184    if (!st->mem)
185       st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
186    else
187       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
188    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
189       st->mem[i] = 0;
190
191 }
192
193
194 SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
195 {
196    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
197    st->initialised = 0;
198    st->in_rate = 0;
199    st->out_rate = 0;
200    st->num_rate = 0;
201    st->den_rate = 0;
202    st->quality = -1;
203    st->sinc_table_length = 0;
204    st->mem = 0;
205    
206    st->cutoff = 1.f;
207    st->nb_channels = nb_channels;
208    st->last_sample = 0;
209    
210    st->in_stride = 1;
211    st->out_stride = 1;
212
213    speex_resample_set_quality(st, quality);
214    speex_resample_set_rate(st, ratio_num, ratio_den, in_rate, out_rate);
215
216    
217    update_filter(st);
218    
219    st->initialised = 1;
220    return st;
221 }
222
223 void speex_resampler_destroy(SpeexResamplerState *st)
224 {
225    speex_free(st->mem);
226    speex_free(st->sinc_table);
227    speex_free(st);
228 }
229
230 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)
231 {
232    int j=0;
233    int N = st->filt_len;
234    int out_sample = 0;
235    spx_word16_t *mem;
236    mem = st->mem + channel_index * (N-1);
237    while (!(st->last_sample >= *in_len || out_sample >= *out_len))
238    {
239       int j;
240       spx_word32_t sum=0;
241       
242       if (st->type == SPEEX_RESAMPLER_DIRECT)
243       {
244          /* We already have all the filter coefficients pre-computed in the table */
245          const spx_word16_t *ptr;
246          /* Do the memory part */
247          for (j=0;st->last_sample-N+1+j < 0;j++)
248          {
249             sum += MULT16_16(mem[st->last_sample+j],st->sinc_table[st->samp_frac_num*st->filt_len+j]);
250          }
251          
252          /* Do the new part */
253          ptr = in+st->last_sample-N+1+j;
254          for (;j<N;j++)
255          {
256             sum += MULT16_16(*ptr,st->sinc_table[st->samp_frac_num*st->filt_len+j]);
257             ptr += st->in_stride;
258          }
259       } else {
260          /* We need to interpolate the sinc filter */
261          spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
262          float interp[4];
263          const spx_word16_t *ptr;
264          float alpha = ((float)st->samp_frac_num)/st->den_rate;
265          int offset = st->samp_frac_num*st->oversample/st->den_rate;
266          float frac = alpha*st->oversample - offset;
267          /* This code is written like this to make it easy to optimise with SIMD.
268             For most DSPs, it would be best to split the loops in two because most DSPs 
269             have only two accumulators */
270          for (j=0;st->last_sample-N+1+j < 0;j++)
271          {
272             spx_word16_t curr_mem = mem[st->last_sample+j];
273             accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
274             accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
275             accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
276             accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
277          }
278          ptr = in+st->last_sample-N+1+j;
279          /* Do the new part */
280          for (;j<N;j++)
281          {
282             spx_word16_t curr_in = *ptr;
283             ptr += st->in_stride;
284             accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
285             accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
286             accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
287             accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
288          }
289          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
290             but I know it's MMSE-optimal on a sinc */
291          interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
292          interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
293          /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
294          interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
295          /* Just to make sure we don't have rounding problems */
296          interp[2] = 1.f-interp[0]-interp[1]-interp[3];
297          /*sum = frac*accum[1] + (1-frac)*accum[2];*/
298          sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
299       }
300       *out = PSHR32(sum,15);
301       out += st->out_stride;
302       out_sample++;
303       st->last_sample += st->int_advance;
304       st->samp_frac_num += st->frac_advance;
305       if (st->samp_frac_num >= st->den_rate)
306       {
307          st->samp_frac_num -= st->den_rate;
308          st->last_sample++;
309       }
310    }
311    if (st->last_sample < *in_len)
312       *in_len = st->last_sample;
313    *out_len = out_sample;
314    st->last_sample -= *in_len;
315    
316    for (j=0;j<N-1-*in_len;j++)
317       mem[j] = mem[j+*in_len];
318    for (;j<N-1;j++)
319       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
320    
321 }
322
323 #ifdef FIXED_POINT
324 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
325 {
326    int i;
327    spx_word16_t x[*in_len];
328    spx_word16_t y[*out_len];
329    for (i=0;i<*in_len;i++)
330       x[i] = WORD2INT(in[i]);
331    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
332    for (i=0;i<*out_len;i++)
333       out[i] = y[i];
334
335 }
336 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)
337 {
338    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
339 }
340 #else
341 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
342 {
343    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
344 }
345 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)
346 {
347    int i;
348    spx_word16_t x[*in_len];
349    spx_word16_t y[*out_len];
350    for (i=0;i<*out_len;i++)
351       x[i] = in[i];
352    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
353    for (i=0;i<*in_len;i++)
354       out[i] = WORD2INT(y[i]);
355 }
356 #endif
357
358 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
359 {
360    int i;
361    int istride_save, ostride_save;
362    istride_save = st->in_stride;
363    ostride_save = st->out_stride;
364    st->in_stride = st->out_stride = st->nb_channels;
365    for (i=0;i<st->nb_channels;i++)
366    {
367       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
368    }
369    st->in_stride = istride_save;
370    st->out_stride = ostride_save;
371 }
372
373
374 void speex_resample_set_rate(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
375 {
376    int fact;
377    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
378       return;
379    
380    st->in_rate = in_rate;
381    st->out_rate = out_rate;
382    st->num_rate = ratio_num;
383    st->den_rate = ratio_den;
384    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
385    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
386    {
387       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
388       {
389          st->num_rate /= fact;
390          st->den_rate /= fact;
391       }
392    }
393    
394    if (ratio_num > ratio_den)
395    {
396       /* down-sampling */
397       st->cutoff = .92f * ratio_den / ratio_num;
398    } else {
399       /* up-sampling */
400       st->cutoff = .97f;
401    }
402    
403    if (st->initialised)
404       update_filter(st);
405 }
406
407 void speex_resample_set_quality(SpeexResamplerState *st, int quality)
408 {
409    if (st->quality == quality)
410       return;
411    st->quality = quality;
412    if (st->initialised)
413       update_filter(st);
414 }
415
416 void speex_resample_set_input_stride(SpeexResamplerState *st, int stride)
417 {
418    st->in_stride = stride;
419 }
420
421 void speex_resample_set_output_stride(SpeexResamplerState *st, int stride)
422 {
423    st->out_stride = stride;
424 }
425
426 void speex_resample_skip_zeros(SpeexResamplerState *st)
427 {
428    st->last_sample = st->filt_len/2;
429 }
430
431 void speex_resample_reset_mem(SpeexResamplerState *st)
432 {
433    int i;
434    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
435       st->mem[i] = 0;
436 }
437