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