5ea31670a330bcec919a3f0daca418317fd2818b
[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 calculation resolution depending on quality setting
47          - Single vs double in float mode
48          - 16-bit vs 32-bit (sinc only) in fixed-point mode
49       - Make sure the filter update works even when changing params 
50              after only a few samples procesed
51 */
52
53 #ifdef HAVE_CONFIG_H
54 #include "config.h"
55 #endif
56
57 #ifdef OUTSIDE_SPEEX
58 #include <stdlib.h>
59 void *speex_alloc (int size) {return calloc(size,1);}
60 void *speex_realloc (void *ptr, int size) {return realloc(ptr, size);}
61 void speex_free (void *ptr) {free(ptr);}
62 #else
63 #include "misc.h"
64 #endif
65
66 #include <math.h>
67 #include "speex/speex_resampler.h"
68
69 #ifndef M_PI
70 #define M_PI 3.14159263
71 #endif
72
73 #ifdef FIXED_POINT
74 #define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))  
75 #else
76 #define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))  
77 #endif
78                
79 /*#define float double*/
80 #define FILTER_SIZE 64
81 #define OVERSAMPLE 8
82
83 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
84
85
86 typedef int (*resampler_basic_func)(SpeexResamplerState *, int , const spx_word16_t *, int *, spx_word16_t *, int *);
87
88 struct SpeexResamplerState_ {
89    int    in_rate;
90    int    out_rate;
91    int    num_rate;
92    int    den_rate;
93    
94    int    quality;
95    int    nb_channels;
96    int    filt_len;
97    int    mem_alloc_size;
98    int    int_advance;
99    int    frac_advance;
100    float  cutoff;
101    int    oversample;
102    int    initialised;
103    int    started;
104    
105    /* These are per-channel */
106    int    *last_sample;
107    int    *samp_frac_num;
108    int    *magic_samples;
109    
110    spx_word16_t *mem;
111    spx_word16_t *sinc_table;
112    int    sinc_table_length;
113    resampler_basic_func resampler_ptr;
114          
115    int    in_stride;
116    int    out_stride;
117 } ;
118
119 static double kaiser10_table[36] = {
120    0.99537781, 1.00000000, 0.99537781, 0.98162644, 0.95908712, 0.92831446,
121    0.89005583, 0.84522401, 0.79486424, 0.74011713, 0.68217934, 0.62226347,
122    0.56155915, 0.50119680, 0.44221549, 0.38553619, 0.33194107, 0.28205962,
123    0.23636152, 0.19515633, 0.15859932, 0.12670280, 0.09935205, 0.07632451,
124    0.05731132, 0.04193980, 0.02979584, 0.02044510, 0.01345224, 0.00839739,
125    0.00488951, 0.00257636, 0.00115101, 0.00035515, 0.00000000, 0.00000000};
126
127 static double kaiser8_table[36] = {
128    0.99635258, 1.00000000, 0.99635258, 0.98548012, 0.96759014, 0.94302200,
129    0.91223751, 0.87580811, 0.83439927, 0.78875245, 0.73966538, 0.68797126,
130    0.63451750, 0.58014482, 0.52566725, 0.47185369, 0.41941150, 0.36897272,
131    0.32108304, 0.27619388, 0.23465776, 0.19672670, 0.16255380, 0.13219758,
132    0.10562887, 0.08273982, 0.06335451, 0.04724088, 0.03412321, 0.02369490,
133    0.01563093, 0.00959968, 0.00527363, 0.00233883, 0.00050000, 0.00000000};
134    
135 static double kaiser6_table[36] = {
136    0.99733006, 1.00000000, 0.99733006, 0.98935595, 0.97618418, 0.95799003,
137    0.93501423, 0.90755855, 0.87598009, 0.84068475, 0.80211977, 0.76076565,
138    0.71712752, 0.67172623, 0.62508937, 0.57774224, 0.53019925, 0.48295561,
139    0.43647969, 0.39120616, 0.34752997, 0.30580127, 0.26632152, 0.22934058,
140    0.19505503, 0.16360756, 0.13508755, 0.10953262, 0.08693120, 0.06722600,
141    0.05031820, 0.03607231, 0.02432151, 0.01487334, 0.00752000, 0.00000000};
142
143 static double kaiser4_table[36] = {
144    0.99831452, 1.00000000, 0.99831452, 0.99327105, 0.98490841, 0.97329088,
145    0.95850750, 0.94067123, 0.91991776, 0.89640418, 0.87030740, 0.84182235,
146    0.81116010, 0.77854570, 0.74421601, 0.70841739, 0.67140328, 0.63343178,
147    0.59476318, 0.55565754, 0.51637222, 0.47715958, 0.43826468, 0.39992313,
148    0.36235905, 0.32578323, 0.29039137, 0.25636266, 0.22385840, 0.19302093,
149    0.16397278, 0.13681602, 0.11163187, 0.08848053, 0.06740000, 0.00000000};
150
151 struct FuncDef {
152    double *table;
153    int oversample;
154 };
155       
156 static struct FuncDef _KAISER10 = {kaiser10_table, 32};
157 #define KAISER10 (&_KAISER10)
158 static struct FuncDef _KAISER8 = {kaiser8_table, 32};
159 #define KAISER8 (&_KAISER8)
160 static struct FuncDef _KAISER6 = {kaiser6_table, 32};
161 #define KAISER6 (&_KAISER6)
162 static struct FuncDef _KAISER4 = {kaiser4_table, 32};
163 #define KAISER4 (&_KAISER4)
164
165 struct QualityMapping {
166    int base_length;
167    int oversample;
168    float downsample_bandwidth;
169    float upsample_bandwidth;
170    struct FuncDef *window_func;
171 };
172
173
174 /* This table maps conversion quality to internal parameters. There are two
175    reasons that explain why the up-sampling bandwidth is larger than the 
176    down-sampling bandwidth:
177    1) When up-sampling, we can assume that the spectrum is already attenuated
178       close to the Nyquist rate (from an A/D or a previous resampling filter)
179    2) Any aliasing that occurs very close to the Nyquist rate will be masked
180       by the sinusoids/noise just below the Nyquist rate (guaranteed only for
181       up-sampling).
182 */
183 const struct QualityMapping quality_map[11] = {
184    {  8,  4, 0.830f, 0.860f, KAISER4 }, /* Q0 */  /* 63.5% cutoff ( ~20 dB stop) 4  */
185    { 16,  4, 0.850f, 0.880f, KAISER4 }, /* Q1 */  /* 75.3% cutoff ( ~40 dB stop) 4  */ 
186    { 32,  4, 0.882f, 0.910f, KAISER6 }, /* Q2 */  /* 82.3% cutoff ( ~60 dB stop) 6  */
187    { 48,  8, 0.895f, 0.917f, KAISER8 }, /* Q3 */  /* 84.9% cutoff ( ~80 dB stop) 8  */
188    { 64,  8, 0.921f, 0.940f, KAISER8 }, /* Q4 */  /* 88.7% cutoff ( ~80 dB stop) 8  */
189    { 80,  8, 0.922f, 0.940f, KAISER10}, /* Q5 */  /* 89.1% cutoff (~100 dB stop) 10 */
190    { 96,  8, 0.940f, 0.945f, KAISER10}, /* Q6 */  /* 91.5% cutoff (~100 dB stop) 10 */
191    {128, 16, 0.950f, 0.950f, KAISER10}, /* Q7 */  /* 93.1% cutoff (~100 dB stop) 10 */
192    {160, 16, 0.960f, 0.960f, KAISER10}, /* Q8 */  /* 94.5% cutoff (~100 dB stop) 10 */
193    {192, 16, 0.968f, 0.968f, KAISER10}, /* Q9 */  /* 95.5% cutoff (~100 dB stop) 10 */
194    {256, 16, 0.975f, 0.975f, KAISER10}, /* Q10 */ /* 96.6% cutoff (~100 dB stop) 10 */
195 };
196 /*8,24,40,56,80,104,128,160,200,256,320*/
197 static double compute_func(float x, struct FuncDef *func)
198 {
199    float y, frac;
200    double interp[4];
201    int ind; 
202    y = x*func->oversample;
203    ind = (int)floor(y);
204    frac = (y-ind);
205    /* CSE with handle the repeated powers */
206    interp[3] =  -0.1666666667*frac + 0.1666666667*(frac*frac*frac);
207    interp[2] = frac + 0.5*(frac*frac) - 0.5*(frac*frac*frac);
208    /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
209    interp[0] = -0.3333333333*frac + 0.5*(frac*frac) - 0.1666666667*(frac*frac*frac);
210    /* Just to make sure we don't have rounding problems */
211    interp[1] = 1.f-interp[3]-interp[2]-interp[0];
212    
213    /*sum = frac*accum[1] + (1-frac)*accum[2];*/
214    return interp[0]*func->table[ind] + interp[1]*func->table[ind+1] + interp[2]*func->table[ind+2] + interp[3]*func->table[ind+3];
215 }
216
217 #if 0
218 #include <stdio.h>
219 int main(int argc, char **argv)
220 {
221    int i;
222    for (i=0;i<256;i++)
223    {
224       printf ("%f\n", compute_func(i/256., KAISER4));
225    }
226    return 0;
227 }
228 #endif
229
230 #ifdef FIXED_POINT
231 /* The slow way of computing a sinc for the table. Should improve that some day */
232 static spx_word16_t sinc(float cutoff, float x, int N, struct FuncDef *window_func)
233 {
234    /*fprintf (stderr, "%f ", x);*/
235    float xx = x * cutoff;
236    if (fabs(x)<1e-6f)
237       return WORD2INT(32768.*cutoff);
238    else if (fabs(x) > .5f*N)
239       return 0;
240    /*FIXME: Can it really be any slower than this? */
241    return WORD2INT(32768.*cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func));
242 }
243 #else
244 /* The slow way of computing a sinc for the table. Should improve that some day */
245 static spx_word16_t sinc(float cutoff, float x, int N, struct FuncDef *window_func)
246 {
247    /*fprintf (stderr, "%f ", x);*/
248    float xx = x * cutoff;
249    if (fabs(x)<1e-6)
250       return cutoff;
251    else if (fabs(x) > .5*N)
252       return 0;
253    /*FIXME: Can it really be any slower than this? */
254    return cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func);
255 }
256 #endif
257
258 static int resampler_basic_direct_single(SpeexResamplerState *st, int channel_index, const spx_word16_t *in, int *in_len, spx_word16_t *out, int *out_len)
259 {
260    int N = st->filt_len;
261    int out_sample = 0;
262    spx_word16_t *mem;
263    int last_sample = st->last_sample[channel_index];
264    int samp_frac_num = st->samp_frac_num[channel_index];
265    mem = st->mem + channel_index * st->mem_alloc_size;
266    while (!(last_sample >= *in_len || out_sample >= *out_len))
267    {
268       int j;
269       spx_word32_t sum=0;
270       
271       /* We already have all the filter coefficients pre-computed in the table */
272       const spx_word16_t *ptr;
273       /* Do the memory part */
274       for (j=0;last_sample-N+1+j < 0;j++)
275       {
276          sum += MULT16_16(mem[last_sample+j],st->sinc_table[samp_frac_num*st->filt_len+j]);
277       }
278       
279       /* Do the new part */
280       ptr = in+st->in_stride*(last_sample-N+1+j);
281       for (;j<N;j++)
282       {
283          sum += MULT16_16(*ptr,st->sinc_table[samp_frac_num*st->filt_len+j]);
284          ptr += st->in_stride;
285       }
286    
287       *out = PSHR32(sum,15);
288       out += st->out_stride;
289       out_sample++;
290       last_sample += st->int_advance;
291       samp_frac_num += st->frac_advance;
292       if (samp_frac_num >= st->den_rate)
293       {
294          samp_frac_num -= st->den_rate;
295          last_sample++;
296       }
297    }
298    st->last_sample[channel_index] = last_sample;
299    st->samp_frac_num[channel_index] = samp_frac_num;
300    return out_sample;
301 }
302
303 static int resampler_basic_interpolate_single(SpeexResamplerState *st, int channel_index, const spx_word16_t *in, int *in_len, spx_word16_t *out, int *out_len)
304 {
305    int N = st->filt_len;
306    int out_sample = 0;
307    spx_word16_t *mem;
308    int last_sample = st->last_sample[channel_index];
309    int samp_frac_num = st->samp_frac_num[channel_index];
310    mem = st->mem + channel_index * st->mem_alloc_size;
311    while (!(last_sample >= *in_len || out_sample >= *out_len))
312    {
313       int j;
314       spx_word32_t sum=0;
315       
316       /* We need to interpolate the sinc filter */
317       spx_word32_t accum[4] = {0.f,0.f, 0.f, 0.f};
318       float interp[4];
319       const spx_word16_t *ptr;
320       float alpha = ((float)samp_frac_num)/st->den_rate;
321       int offset = samp_frac_num*st->oversample/st->den_rate;
322       float frac = alpha*st->oversample - offset;
323          /* This code is written like this to make it easy to optimise with SIMD.
324       For most DSPs, it would be best to split the loops in two because most DSPs 
325       have only two accumulators */
326       for (j=0;last_sample-N+1+j < 0;j++)
327       {
328          spx_word16_t curr_mem = mem[last_sample+j];
329          accum[0] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
330          accum[1] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
331          accum[2] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset]);
332          accum[3] += MULT16_16(curr_mem,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
333       }
334       ptr = in+st->in_stride*(last_sample-N+1+j);
335       /* Do the new part */
336       for (;j<N;j++)
337       {
338          spx_word16_t curr_in = *ptr;
339          ptr += st->in_stride;
340          accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
341          accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
342          accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
343          accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
344       }
345          /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
346       but I know it's MMSE-optimal on a sinc */
347       interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
348       interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
349       /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
350       interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
351       /* Just to make sure we don't have rounding problems */
352       interp[2] = 1.f-interp[0]-interp[1]-interp[3];
353       /*sum = frac*accum[1] + (1-frac)*accum[2];*/
354       sum = interp[0]*accum[0] + interp[1]*accum[1] + interp[2]*accum[2] + interp[3]*accum[3];
355    
356       *out = PSHR32(sum,15);
357       out += st->out_stride;
358       out_sample++;
359       last_sample += st->int_advance;
360       samp_frac_num += st->frac_advance;
361       if (samp_frac_num >= st->den_rate)
362       {
363          samp_frac_num -= st->den_rate;
364          last_sample++;
365       }
366    }
367    st->last_sample[channel_index] = last_sample;
368    st->samp_frac_num[channel_index] = samp_frac_num;
369    return out_sample;
370 }
371
372
373 static void update_filter(SpeexResamplerState *st)
374 {
375    int i;
376    int old_length;
377    
378    old_length = st->filt_len;
379    st->oversample = quality_map[st->quality].oversample;
380    st->filt_len = quality_map[st->quality].base_length;
381    
382    if (st->num_rate > st->den_rate)
383    {
384       /* down-sampling */
385       st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
386       /* FIXME: divide the numerator and denominator by a certain amount if they're too large */
387       st->filt_len = st->filt_len*st->num_rate / st->den_rate;
388       /* Round down to make sure we have a multiple of 4 */
389       st->filt_len &= (~0x3);
390    } else {
391       /* up-sampling */
392       st->cutoff = quality_map[st->quality].upsample_bandwidth;
393    }
394
395    /* Choose the resampling type that requires the least amount of memory */
396    if (st->den_rate <= st->oversample)
397    {
398       if (!st->sinc_table)
399          st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
400       else if (st->sinc_table_length < st->filt_len*st->den_rate)
401       {
402          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
403          st->sinc_table_length = st->filt_len*st->den_rate;
404       }
405       for (i=0;i<st->den_rate;i++)
406       {
407          int j;
408          for (j=0;j<st->filt_len;j++)
409          {
410             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, quality_map[st->quality].window_func);
411          }
412       }
413       st->resampler_ptr = resampler_basic_direct_single;
414       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
415    } else {
416       if (!st->sinc_table)
417          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
418       else if (st->sinc_table_length < st->filt_len*st->oversample+8)
419       {
420          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
421          st->sinc_table_length = st->filt_len*st->oversample+8;
422       }
423       for (i=-4;i<st->oversample*st->filt_len+4;i++)
424          st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len, quality_map[st->quality].window_func);
425       st->resampler_ptr = resampler_basic_interpolate_single;
426       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
427    }
428    st->int_advance = st->num_rate/st->den_rate;
429    st->frac_advance = st->num_rate%st->den_rate;
430
431    if (!st->mem)
432    {
433       st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
434       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
435          st->mem[i] = 0;
436       st->mem_alloc_size = st->filt_len-1;
437       /*speex_warning("init filter");*/
438    } else if (!st->started)
439    {
440       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
441       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
442          st->mem[i] = 0;
443       st->mem_alloc_size = st->filt_len-1;
444       /*speex_warning("reinit filter");*/
445    } else if (st->filt_len > old_length)
446    {
447       /* Increase the filter length */
448       /*speex_warning("increase filter size");*/
449       int old_alloc_size = st->mem_alloc_size;
450       if (st->filt_len-1 > st->mem_alloc_size)
451       {
452          st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
453          st->mem_alloc_size = st->filt_len-1;
454       }
455       for (i=0;i<st->nb_channels;i++)
456       {
457          int j;
458          /* Copy data going backward */
459          for (j=0;j<old_length-1;j++)
460             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*old_alloc_size+(old_length-2-j)];
461          /* Then put zeros for lack of anything better */
462          for (;j<st->filt_len-1;j++)
463             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
464          /* Adjust last_sample */
465          st->last_sample[i] += (st->filt_len - old_length)/2;
466       }
467    } else if (st->filt_len < old_length)
468    {
469       /* Reduce filter length, this a bit tricky */
470       /*speex_warning("decrease filter size (unimplemented)");*/
471       /* Adjust last_sample (which will likely end up negative) */
472       /*st->last_sample += (st->filt_len - old_length)/2;*/
473       for (i=0;i<st->nb_channels;i++)
474       {
475          int j;
476          st->magic_samples[i] = (old_length - st->filt_len)/2;
477          /* Copy data going backward */
478          for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
479             st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
480       }
481    }
482
483 }
484
485 SpeexResamplerState *speex_resampler_init(int nb_channels, int in_rate, int out_rate, int quality)
486 {
487    return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality);
488 }
489
490 SpeexResamplerState *speex_resampler_init_frac(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
491 {
492    int i;
493    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
494    st->initialised = 0;
495    st->started = 0;
496    st->in_rate = 0;
497    st->out_rate = 0;
498    st->num_rate = 0;
499    st->den_rate = 0;
500    st->quality = -1;
501    st->sinc_table_length = 0;
502    st->mem_alloc_size = 0;
503    st->filt_len = 0;
504    st->mem = 0;
505    st->resampler_ptr = 0;
506          
507    st->cutoff = 1.f;
508    st->nb_channels = nb_channels;
509    st->in_stride = 1;
510    st->out_stride = 1;
511    
512    /* Per channel data */
513    st->last_sample = (int*)speex_alloc(nb_channels*sizeof(int));
514    st->magic_samples = (int*)speex_alloc(nb_channels*sizeof(int));
515    st->samp_frac_num = (int*)speex_alloc(nb_channels*sizeof(int));
516    for (i=0;i<nb_channels;i++)
517    {
518       st->last_sample[i] = 0;
519       st->magic_samples[i] = 0;
520       st->samp_frac_num[i] = 0;
521    }
522
523    speex_resampler_set_quality(st, quality);
524    speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);
525
526    
527    update_filter(st);
528    
529    st->initialised = 1;
530    return st;
531 }
532
533 void speex_resampler_destroy(SpeexResamplerState *st)
534 {
535    speex_free(st->mem);
536    speex_free(st->sinc_table);
537    speex_free(st->last_sample);
538    speex_free(st->magic_samples);
539    speex_free(st->samp_frac_num);
540    speex_free(st);
541 }
542
543
544
545 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)
546 {
547    int j=0;
548    int N = st->filt_len;
549    int out_sample = 0;
550    spx_word16_t *mem;
551    int tmp_out_len = 0;
552    mem = st->mem + channel_index * st->mem_alloc_size;
553    st->started = 1;
554    
555    /* Handle the case where we have samples left from a reduction in filter length */
556    if (st->magic_samples[channel_index])
557    {
558       int tmp_in_len;
559       int tmp_magic;
560       tmp_in_len = st->magic_samples[channel_index];
561       tmp_out_len = *out_len;
562       /* FIXME: Need to handle the case where the out array is too small */
563       /* magic_samples needs to be set to zero to avoid infinite recursion */
564       tmp_magic = st->magic_samples[channel_index];
565       st->magic_samples[channel_index] = 0;
566       speex_resampler_process_native(st, channel_index, mem+N-1, &tmp_in_len, out, &tmp_out_len);
567       /*speex_warning_int("extra samples:", tmp_out_len);*/
568       /* If we couldn't process all "magic" input samples, save the rest for next time */
569       if (tmp_in_len < tmp_magic)
570       {
571          int i;
572          st->magic_samples[channel_index] = tmp_magic-tmp_in_len;
573          for (i=0;i<st->magic_samples[channel_index];i++)
574             mem[N-1+i]=mem[N-1+i+tmp_in_len];
575       }
576       out += tmp_out_len;
577    }
578    
579    /* Call the right resampler through the function ptr */
580    out_sample = st->resampler_ptr(st, channel_index, in, in_len, out, out_len);
581    
582    if (st->last_sample[channel_index] < *in_len)
583       *in_len = st->last_sample[channel_index];
584    *out_len = out_sample+tmp_out_len;
585    st->last_sample[channel_index] -= *in_len;
586    
587    for (j=0;j<N-1-*in_len;j++)
588       mem[j] = mem[j+*in_len];
589    for (;j<N-1;j++)
590       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
591    
592 }
593
594 #ifdef FIXED_POINT
595 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
596 {
597    int i;
598    int istride_save, ostride_save;
599    spx_word16_t x[*in_len];
600    spx_word16_t y[*out_len];
601    istride_save = st->in_stride;
602    ostride_save = st->out_stride;
603    for (i=0;i<*in_len;i++)
604       x[i] = WORD2INT(in[i*st->in_stride]);
605    st->in_stride = st->out_stride = 1;
606    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
607    st->in_stride = istride_save;
608    st->out_stride = ostride_save;
609    for (i=0;i<*out_len;i++)
610       out[i*st->out_stride] = y[i];
611 }
612 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)
613 {
614    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
615 }
616 #else
617 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
618 {
619    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
620 }
621 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)
622 {
623    int i;
624    int istride_save, ostride_save;
625    spx_word16_t x[*in_len];
626    spx_word16_t y[*out_len];
627    istride_save = st->in_stride;
628    ostride_save = st->out_stride;
629    for (i=0;i<*in_len;i++)
630       x[i] = in[i+st->in_stride];
631    st->in_stride = st->out_stride = 1;
632    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
633    st->in_stride = istride_save;
634    st->out_stride = ostride_save;
635    for (i=0;i<*out_len;i++)
636       out[i+st->out_stride] = WORD2INT(y[i]);
637 }
638 #endif
639
640 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
641 {
642    int i;
643    int istride_save, ostride_save;
644    istride_save = st->in_stride;
645    ostride_save = st->out_stride;
646    st->in_stride = st->out_stride = st->nb_channels;
647    for (i=0;i<st->nb_channels;i++)
648    {
649       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
650    }
651    st->in_stride = istride_save;
652    st->out_stride = ostride_save;
653 }
654
655 void speex_resampler_process_interleaved_int(SpeexResamplerState *st, const spx_int16_t *in, int *in_len, spx_int16_t *out, int *out_len)
656 {
657    int i;
658    int istride_save, ostride_save;
659    istride_save = st->in_stride;
660    ostride_save = st->out_stride;
661    st->in_stride = st->out_stride = st->nb_channels;
662    for (i=0;i<st->nb_channels;i++)
663    {
664       speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
665    }
666    st->in_stride = istride_save;
667    st->out_stride = ostride_save;
668 }
669
670 void speex_resampler_set_rate(SpeexResamplerState *st, int in_rate, int out_rate)
671 {
672    speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
673 }
674
675 void speex_resampler_get_rate(SpeexResamplerState *st, int *in_rate, int *out_rate)
676 {
677    *in_rate = st->in_rate;
678    *out_rate = st->out_rate;
679 }
680
681 void speex_resampler_set_rate_frac(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
682 {
683    int fact;
684    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
685       return;
686    
687    st->in_rate = in_rate;
688    st->out_rate = out_rate;
689    st->num_rate = ratio_num;
690    st->den_rate = ratio_den;
691    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
692    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
693    {
694       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
695       {
696          st->num_rate /= fact;
697          st->den_rate /= fact;
698       }
699    }
700       
701    if (st->initialised)
702       update_filter(st);
703 }
704
705 void speex_resampler_get_ratio(SpeexResamplerState *st, int *ratio_num, int *ratio_den)
706 {
707    *ratio_num = st->num_rate;
708    *ratio_den = st->den_rate;
709 }
710
711 void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
712 {
713    if (quality < 0)
714       quality = 0;
715    if (quality > 10)
716       quality = 10;
717    if (st->quality == quality)
718       return;
719    st->quality = quality;
720    if (st->initialised)
721       update_filter(st);
722 }
723
724 void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
725 {
726    *quality = st->quality;
727 }
728
729 void speex_resampler_set_input_stride(SpeexResamplerState *st, int stride)
730 {
731    st->in_stride = stride;
732 }
733
734 void speex_resampler_get_input_stride(SpeexResamplerState *st, int *stride)
735 {
736    *stride = st->in_stride;
737 }
738
739 void speex_resampler_set_output_stride(SpeexResamplerState *st, int stride)
740 {
741    st->out_stride = stride;
742 }
743
744 void speex_resampler_get_output_stride(SpeexResamplerState *st, int *stride)
745 {
746    *stride = st->out_stride;
747 }
748
749 void speex_resampler_skip_zeros(SpeexResamplerState *st)
750 {
751    int i;
752    for (i=0;i<st->nb_channels;i++)
753       st->last_sample[i] = st->filt_len/2;
754 }
755
756 void speex_resampler_reset_mem(SpeexResamplerState *st)
757 {
758    int i;
759    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
760       st->mem[i] = 0;
761 }
762