Tables for Kaiser orders 4, 6, 8.
[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 struct QualityMapping {
86    int base_length;
87    int oversample;
88    float downsample_bandwidth;
89    float upsample_bandwidth;
90 };
91
92 /* This table maps conversion quality to internal parameters. There are two
93    reasons that explain why the up-sampling bandwidth is larger than the 
94    down-sampling bandwidth:
95    1) When up-sampling, we can assume that the spectrum is already attenuated
96       close to the Nyquist rate (from an A/D or a previous resampling filter)
97    2) Any aliasing that occurs very close to the Nyquist rate will be masked
98       by the sinusoids/noise just below the Nyquist rate (guaranteed only for
99       up-sampling).
100 */
101 const struct QualityMapping quality_map[11] = {
102    {  8,  4, 0.830f, 0.860f}, /* Q0 */  /* 63.5% cutoff ( ~20 dB stop) 4  */
103    { 16,  4, 0.850f, 0.880f}, /* Q1 */  /* 75.3% cutoff ( ~40 dB stop) 4  */ 
104    { 32,  4, 0.882f, 0.910f}, /* Q2 */  /* 82.3% cutoff ( ~60 dB stop) 6  */
105    { 48,  8, 0.895f, 0.917f}, /* Q3 */  /* 84.9% cutoff ( ~80 dB stop) 8  */
106    { 64,  8, 0.921f, 0.940f}, /* Q4 */  /* 88.7% cutoff ( ~80 dB stop) 8  */
107    { 80,  8, 0.922f, 0.940f}, /* Q5 */  /* 89.1% cutoff (~100 dB stop) 10 */
108    { 96,  8, 0.940f, 0.945f}, /* Q6 */  /* 91.5% cutoff (~100 dB stop) 10 */
109    {128, 16, 0.950f, 0.950f}, /* Q7 */  /* 93.1% cutoff (~100 dB stop) 10 */
110    {160, 16, 0.960f, 0.960f}, /* Q8 */  /* 94.5% cutoff (~100 dB stop) 10 */
111    {192, 16, 0.968f, 0.968f}, /* Q9 */  /* 95.5% cutoff (~100 dB stop) 10 */
112    {256, 16, 0.975f, 0.975f}, /* Q10 */ /* 96.6% cutoff (~100 dB stop) 10 */
113 };
114
115 typedef enum {SPEEX_RESAMPLER_DIRECT_SINGLE=0, SPEEX_RESAMPLER_INTERPOLATE_SINGLE=1} SpeexSincType;
116
117 typedef int (*resampler_basic_func)(SpeexResamplerState *, int , const spx_word16_t *, int *, spx_word16_t *, int *);
118
119 struct SpeexResamplerState_ {
120    int    in_rate;
121    int    out_rate;
122    int    num_rate;
123    int    den_rate;
124    
125    int    quality;
126    int    nb_channels;
127    int    filt_len;
128    int    mem_alloc_size;
129    int    int_advance;
130    int    frac_advance;
131    float  cutoff;
132    int    oversample;
133    int    initialised;
134    int    started;
135    
136    /* These are per-channel */
137    int    *last_sample;
138    int    *samp_frac_num;
139    int    *magic_samples;
140    
141    spx_word16_t *mem;
142    spx_word16_t *sinc_table;
143    int    sinc_table_length;
144    resampler_basic_func resampler_ptr;
145          
146    int    in_stride;
147    int    out_stride;
148    SpeexSincType type;
149 } ;
150
151 static double kaiser10_table[36] = {
152    0.99537781, 1.00000000, 0.99537781, 0.98162644, 0.95908712, 0.92831446,
153    0.89005583, 0.84522401, 0.79486424, 0.74011713, 0.68217934, 0.62226347,
154    0.56155915, 0.50119680, 0.44221549, 0.38553619, 0.33194107, 0.28205962,
155    0.23636152, 0.19515633, 0.15859932, 0.12670280, 0.09935205, 0.07632451,
156    0.05731132, 0.04193980, 0.02979584, 0.02044510, 0.01345224, 0.00839739,
157    0.00488951, 0.00257636, 0.00115101, 0.00035515, 0.00000000, 0.00000000};
158
159 static double kaiser8_table[36] = {
160    0.99635258, 1.00000000, 0.99635258, 0.98548012, 0.96759014, 0.94302200,
161    0.91223751, 0.87580811, 0.83439927, 0.78875245, 0.73966538, 0.68797126,
162    0.63451750, 0.58014482, 0.52566725, 0.47185369, 0.41941150, 0.36897272,
163    0.32108304, 0.27619388, 0.23465776, 0.19672670, 0.16255380, 0.13219758,
164    0.10562887, 0.08273982, 0.06335451, 0.04724088, 0.03412321, 0.02369490,
165    0.01563093, 0.00959968, 0.00527363, 0.00233883, 0.00050000, 0.00000000};
166    
167 static double kaiser6_table[36] = {
168    0.99733006, 1.00000000, 0.99733006, 0.98935595, 0.97618418, 0.95799003,
169    0.93501423, 0.90755855, 0.87598009, 0.84068475, 0.80211977, 0.76076565,
170    0.71712752, 0.67172623, 0.62508937, 0.57774224, 0.53019925, 0.48295561,
171    0.43647969, 0.39120616, 0.34752997, 0.30580127, 0.26632152, 0.22934058,
172    0.19505503, 0.16360756, 0.13508755, 0.10953262, 0.08693120, 0.06722600,
173    0.05031820, 0.03607231, 0.02432151, 0.01487334, 0.00752000, 0.00000000};
174
175 static double kaiser4_table[36] = {
176    0.99831452, 1.00000000, 0.99831452, 0.99327105, 0.98490841, 0.97329088,
177    0.95850750, 0.94067123, 0.91991776, 0.89640418, 0.87030740, 0.84182235,
178    0.81116010, 0.77854570, 0.74421601, 0.70841739, 0.67140328, 0.63343178,
179    0.59476318, 0.55565754, 0.51637222, 0.47715958, 0.43826468, 0.39992313,
180    0.36235905, 0.32578323, 0.29039137, 0.25636266, 0.22385840, 0.19302093,
181    0.16397278, 0.13681602, 0.11163187, 0.08848053, 0.06740000, 0.00000000};
182
183 struct FuncDef {
184    double *table;
185    int oversample;
186 };
187       
188 static struct FuncDef _KAISER10 = {kaiser10_table, 32};
189 #define KAISER10 (&_KAISER10)
190 static struct FuncDef _KAISER8 = {kaiser8_table, 32};
191 #define KAISER8 (&_KAISER8)
192 static struct FuncDef _KAISER6 = {kaiser6_table, 32};
193 #define KAISER6 (&_KAISER6)
194 static struct FuncDef _KAISER4 = {kaiser4_table, 32};
195 #define KAISER4 (&_KAISER4)
196
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)
233 {
234    /*fprintf (stderr, "%f ", x);*/
235    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*x)/(M_PI*x) * compute_func(fabs(2.*x/N), KAISER10));
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)
246 {
247    /*fprintf (stderr, "%f ", x);*/
248    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*x)/(M_PI*x) * compute_func(fabs(2.*x/N), KAISER10);
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);
411          }
412       }
413       st->type = SPEEX_RESAMPLER_DIRECT_SINGLE;
414       st->resampler_ptr = resampler_basic_direct_single;
415       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
416    } else {
417       if (!st->sinc_table)
418          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
419       else if (st->sinc_table_length < st->filt_len*st->oversample+8)
420       {
421          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
422          st->sinc_table_length = st->filt_len*st->oversample+8;
423       }
424       for (i=-4;i<st->oversample*st->filt_len+4;i++)
425          st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len);
426       st->type = SPEEX_RESAMPLER_INTERPOLATE_SINGLE;
427       st->resampler_ptr = resampler_basic_interpolate_single;
428       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
429    }
430    st->int_advance = st->num_rate/st->den_rate;
431    st->frac_advance = st->num_rate%st->den_rate;
432
433    if (!st->mem)
434    {
435       st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
436       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
437          st->mem[i] = 0;
438       st->mem_alloc_size = st->filt_len-1;
439       /*speex_warning("init filter");*/
440    } else if (!st->started)
441    {
442       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
443       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
444          st->mem[i] = 0;
445       st->mem_alloc_size = st->filt_len-1;
446       /*speex_warning("reinit filter");*/
447    } else if (st->filt_len > old_length)
448    {
449       /* Increase the filter length */
450       /*speex_warning("increase filter size");*/
451       int old_alloc_size = st->mem_alloc_size;
452       if (st->filt_len-1 > st->mem_alloc_size)
453       {
454          st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
455          st->mem_alloc_size = st->filt_len-1;
456       }
457       for (i=0;i<st->nb_channels;i++)
458       {
459          int j;
460          /* Copy data going backward */
461          for (j=0;j<old_length-1;j++)
462             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*old_alloc_size+(old_length-2-j)];
463          /* Then put zeros for lack of anything better */
464          for (;j<st->filt_len-1;j++)
465             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
466          /* Adjust last_sample */
467          st->last_sample[i] += (st->filt_len - old_length)/2;
468       }
469    } else if (st->filt_len < old_length)
470    {
471       /* Reduce filter length, this a bit tricky */
472       /*speex_warning("decrease filter size (unimplemented)");*/
473       /* Adjust last_sample (which will likely end up negative) */
474       /*st->last_sample += (st->filt_len - old_length)/2;*/
475       for (i=0;i<st->nb_channels;i++)
476       {
477          int j;
478          st->magic_samples[i] = (old_length - st->filt_len)/2;
479          /* Copy data going backward */
480          for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
481             st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
482       }
483    }
484
485 }
486
487
488 SpeexResamplerState *speex_resampler_init(int nb_channels, int ratio_num, int ratio_den, int in_rate, int out_rate, int quality)
489 {
490    int i;
491    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
492    st->initialised = 0;
493    st->started = 0;
494    st->in_rate = 0;
495    st->out_rate = 0;
496    st->num_rate = 0;
497    st->den_rate = 0;
498    st->quality = -1;
499    st->sinc_table_length = 0;
500    st->mem_alloc_size = 0;
501    st->filt_len = 0;
502    st->mem = 0;
503    st->resampler_ptr = 0;
504          
505    st->cutoff = 1.f;
506    st->nb_channels = nb_channels;
507    st->in_stride = 1;
508    st->out_stride = 1;
509    
510    /* Per channel data */
511    st->last_sample = (int*)speex_alloc(nb_channels*sizeof(int));
512    st->magic_samples = (int*)speex_alloc(nb_channels*sizeof(int));
513    st->samp_frac_num = (int*)speex_alloc(nb_channels*sizeof(int));
514    for (i=0;i<nb_channels;i++)
515    {
516       st->last_sample[i] = 0;
517       st->magic_samples[i] = 0;
518       st->samp_frac_num[i] = 0;
519    }
520
521    speex_resampler_set_quality(st, quality);
522    speex_resampler_set_rate(st, ratio_num, ratio_den, in_rate, out_rate);
523
524    
525    update_filter(st);
526    
527    st->initialised = 1;
528    return st;
529 }
530
531 void speex_resampler_destroy(SpeexResamplerState *st)
532 {
533    speex_free(st->mem);
534    speex_free(st->sinc_table);
535    speex_free(st->last_sample);
536    speex_free(st->magic_samples);
537    speex_free(st->samp_frac_num);
538    speex_free(st);
539 }
540
541
542
543 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)
544 {
545    int j=0;
546    int N = st->filt_len;
547    int out_sample = 0;
548    spx_word16_t *mem;
549    int tmp_out_len = 0;
550    mem = st->mem + channel_index * st->mem_alloc_size;
551    st->started = 1;
552    
553    /* Handle the case where we have samples left from a reduction in filter length */
554    if (st->magic_samples)
555    {
556       int tmp_in_len;
557       tmp_in_len = st->magic_samples[channel_index];
558       tmp_out_len = *out_len;
559       /* FIXME: Need to handle the case where the out array is too small */
560       /* magic_samples needs to be set to zero to avoid infinite recursion */
561       st->magic_samples = 0;
562       speex_resampler_process_native(st, channel_index, mem+N-1, &tmp_in_len, out, &tmp_out_len);
563       /*speex_warning_int("extra samples:", tmp_out_len);*/
564       out += tmp_out_len;
565    }
566    
567    /* Call the right resampler through the function ptr */
568    out_sample = st->resampler_ptr(st, channel_index, in, in_len, out, out_len);
569    
570    if (st->last_sample[channel_index] < *in_len)
571       *in_len = st->last_sample[channel_index];
572    *out_len = out_sample+tmp_out_len;
573    st->last_sample[channel_index] -= *in_len;
574    
575    for (j=0;j<N-1-*in_len;j++)
576       mem[j] = mem[j+*in_len];
577    for (;j<N-1;j++)
578       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
579    
580 }
581
582 #ifdef FIXED_POINT
583 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
584 {
585    int i;
586    int istride_save, ostride_save;
587    spx_word16_t x[*in_len];
588    spx_word16_t y[*out_len];
589    istride_save = st->in_stride;
590    ostride_save = st->out_stride;
591    for (i=0;i<*in_len;i++)
592       x[i] = WORD2INT(in[i*st->in_stride]);
593    st->in_stride = st->out_stride = 1;
594    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
595    st->in_stride = istride_save;
596    st->out_stride = ostride_save;
597    for (i=0;i<*out_len;i++)
598       out[i*st->out_stride] = y[i];
599 }
600 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)
601 {
602    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
603 }
604 #else
605 void speex_resampler_process_float(SpeexResamplerState *st, int channel_index, const float *in, int *in_len, float *out, int *out_len)
606 {
607    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
608 }
609 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)
610 {
611    int i;
612    int istride_save, ostride_save;
613    spx_word16_t x[*in_len];
614    spx_word16_t y[*out_len];
615    istride_save = st->in_stride;
616    ostride_save = st->out_stride;
617    for (i=0;i<*in_len;i++)
618       x[i] = in[i+st->in_stride];
619    st->in_stride = st->out_stride = 1;
620    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
621    st->in_stride = istride_save;
622    st->out_stride = ostride_save;
623    for (i=0;i<*out_len;i++)
624       out[i+st->out_stride] = WORD2INT(y[i]);
625 }
626 #endif
627
628 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, int *in_len, float *out, int *out_len)
629 {
630    int i;
631    int istride_save, ostride_save;
632    istride_save = st->in_stride;
633    ostride_save = st->out_stride;
634    st->in_stride = st->out_stride = st->nb_channels;
635    for (i=0;i<st->nb_channels;i++)
636    {
637       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
638    }
639    st->in_stride = istride_save;
640    st->out_stride = ostride_save;
641 }
642
643
644 void speex_resampler_set_rate(SpeexResamplerState *st, int ratio_num, int ratio_den, int in_rate, int out_rate)
645 {
646    int fact;
647    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
648       return;
649    
650    st->in_rate = in_rate;
651    st->out_rate = out_rate;
652    st->num_rate = ratio_num;
653    st->den_rate = ratio_den;
654    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
655    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
656    {
657       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
658       {
659          st->num_rate /= fact;
660          st->den_rate /= fact;
661       }
662    }
663       
664    if (st->initialised)
665       update_filter(st);
666 }
667
668 void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
669 {
670    if (quality < 0)
671       quality = 0;
672    if (quality > 10)
673       quality = 10;
674    if (st->quality == quality)
675       return;
676    st->quality = quality;
677    if (st->initialised)
678       update_filter(st);
679 }
680
681 void speex_resampler_set_input_stride(SpeexResamplerState *st, int stride)
682 {
683    st->in_stride = stride;
684 }
685
686 void speex_resampler_set_output_stride(SpeexResamplerState *st, int stride)
687 {
688    st->out_stride = stride;
689 }
690
691 void speex_resampler_skip_zeros(SpeexResamplerState *st)
692 {
693    int i;
694    for (i=0;i<st->nb_channels;i++)
695       st->last_sample[i] = st->filt_len/2;
696 }
697
698 void speex_resampler_reset_mem(SpeexResamplerState *st)
699 {
700    int i;
701    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
702       st->mem[i] = 0;
703 }
704