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