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