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