Changed the sign of a bunch of parameters in the API. Tons of signed/unsigned
[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 *, spx_uint32_t , const spx_word16_t *, spx_uint32_t *, spx_word16_t *, spx_uint32_t *);
88
89 struct SpeexResamplerState_ {
90    spx_uint32_t in_rate;
91    spx_uint32_t out_rate;
92    spx_uint32_t num_rate;
93    spx_uint32_t den_rate;
94    
95    int    quality;
96    spx_uint32_t nb_channels;
97    spx_uint32_t filt_len;
98    spx_uint32_t mem_alloc_size;
99    int          int_advance;
100    int          frac_advance;
101    float  cutoff;
102    spx_uint32_t oversample;
103    int          initialised;
104    int          started;
105    
106    /* These are per-channel */
107    spx_int32_t  *last_sample;
108    spx_uint32_t *samp_frac_num;
109    spx_uint32_t *magic_samples;
110    
111    spx_word16_t *mem;
112    spx_word16_t *sinc_table;
113    spx_uint32_t 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 static 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, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *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    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
312    mem = st->mem + channel_index * st->mem_alloc_size;
313    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*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, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *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    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
360    mem = st->mem + channel_index * st->mem_alloc_size;
361    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*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, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *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    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
406    mem = st->mem + channel_index * st->mem_alloc_size;
407    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*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, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *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    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
475    mem = st->mem + channel_index * st->mem_alloc_size;
476    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*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    spx_uint32_t old_length;
533    
534    old_length = st->filt_len;
535    st->oversample = quality_map[st->quality].oversample;
536    st->filt_len = quality_map[st->quality].base_length;
537    
538    if (st->num_rate > st->den_rate)
539    {
540       /* down-sampling */
541       st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
542       /* FIXME: divide the numerator and denominator by a certain amount if they're too large */
543       st->filt_len = st->filt_len*st->num_rate / st->den_rate;
544       /* Round down to make sure we have a multiple of 4 */
545       st->filt_len &= (~0x3);
546    } else {
547       /* up-sampling */
548       st->cutoff = quality_map[st->quality].upsample_bandwidth;
549    }
550
551    /* Choose the resampling type that requires the least amount of memory */
552    if (st->den_rate <= st->oversample)
553    {
554       spx_uint32_t i;
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          spx_uint32_t 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       spx_int32_t i;
581       if (!st->sinc_table)
582          st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
583       else if (st->sinc_table_length < st->filt_len*st->oversample+8)
584       {
585          st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
586          st->sinc_table_length = st->filt_len*st->oversample+8;
587       }
588       for (i=-4;i<(spx_int32_t)(st->oversample*st->filt_len+4);i++)
589          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);
590 #ifdef FIXED_POINT
591       st->resampler_ptr = resampler_basic_interpolate_single;
592 #else
593       if (st->quality>8)
594          st->resampler_ptr = resampler_basic_interpolate_double;
595       else
596          st->resampler_ptr = resampler_basic_interpolate_single;
597 #endif
598       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
599    }
600    st->int_advance = st->num_rate/st->den_rate;
601    st->frac_advance = st->num_rate%st->den_rate;
602
603    if (!st->mem)
604    {
605       spx_uint32_t i;
606       st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
607       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
608          st->mem[i] = 0;
609       st->mem_alloc_size = st->filt_len-1;
610       /*speex_warning("init filter");*/
611    } else if (!st->started)
612    {
613       spx_uint32_t i;
614       st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
615       for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
616          st->mem[i] = 0;
617       st->mem_alloc_size = st->filt_len-1;
618       /*speex_warning("reinit filter");*/
619    } else if (st->filt_len > old_length)
620    {
621       spx_uint32_t i;
622       /* Increase the filter length */
623       /*speex_warning("increase filter size");*/
624       int old_alloc_size = st->mem_alloc_size;
625       if (st->filt_len-1 > st->mem_alloc_size)
626       {
627          st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*(st->filt_len-1) * sizeof(spx_word16_t));
628          st->mem_alloc_size = st->filt_len-1;
629       }
630       for (i=0;i<st->nb_channels;i++)
631       {
632          spx_uint32_t j;
633          /* Copy data going backward */
634          for (j=0;j<old_length-1;j++)
635             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*old_alloc_size+(old_length-2-j)];
636          /* Then put zeros for lack of anything better */
637          for (;j<st->filt_len-1;j++)
638             st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
639          /* Adjust last_sample */
640          st->last_sample[i] += (st->filt_len - old_length)/2;
641       }
642    } else if (st->filt_len < old_length)
643    {
644       spx_uint32_t i;
645       /* Reduce filter length, this a bit tricky */
646       /*speex_warning("decrease filter size (unimplemented)");*/
647       /* Adjust last_sample (which will likely end up negative) */
648       /*st->last_sample += (st->filt_len - old_length)/2;*/
649       for (i=0;i<st->nb_channels;i++)
650       {
651          spx_uint32_t j;
652          st->magic_samples[i] = (old_length - st->filt_len)/2;
653          /* Copy data going backward */
654          for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
655             st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
656       }
657    }
658
659 }
660
661 SpeexResamplerState *speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality)
662 {
663    return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality);
664 }
665
666 SpeexResamplerState *speex_resampler_init_frac(spx_uint32_t nb_channels, spx_uint32_t ratio_num, spx_uint32_t ratio_den, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality)
667 {
668    spx_uint32_t i;
669    SpeexResamplerState *st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
670    st->initialised = 0;
671    st->started = 0;
672    st->in_rate = 0;
673    st->out_rate = 0;
674    st->num_rate = 0;
675    st->den_rate = 0;
676    st->quality = -1;
677    st->sinc_table_length = 0;
678    st->mem_alloc_size = 0;
679    st->filt_len = 0;
680    st->mem = 0;
681    st->resampler_ptr = 0;
682          
683    st->cutoff = 1.f;
684    st->nb_channels = nb_channels;
685    st->in_stride = 1;
686    st->out_stride = 1;
687    
688    /* Per channel data */
689    st->last_sample = (spx_int32_t*)speex_alloc(nb_channels*sizeof(int));
690    st->magic_samples = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(int));
691    st->samp_frac_num = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(int));
692    for (i=0;i<nb_channels;i++)
693    {
694       st->last_sample[i] = 0;
695       st->magic_samples[i] = 0;
696       st->samp_frac_num[i] = 0;
697    }
698
699    speex_resampler_set_quality(st, quality);
700    speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);
701
702    
703    update_filter(st);
704    
705    st->initialised = 1;
706    return st;
707 }
708
709 void speex_resampler_destroy(SpeexResamplerState *st)
710 {
711    speex_free(st->mem);
712    speex_free(st->sinc_table);
713    speex_free(st->last_sample);
714    speex_free(st->magic_samples);
715    speex_free(st->samp_frac_num);
716    speex_free(st);
717 }
718
719
720
721 static void speex_resampler_process_native(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
722 {
723    int j=0;
724    int N = st->filt_len;
725    int out_sample = 0;
726    spx_word16_t *mem;
727    spx_uint32_t tmp_out_len = 0;
728    mem = st->mem + channel_index * st->mem_alloc_size;
729    st->started = 1;
730    
731    /* Handle the case where we have samples left from a reduction in filter length */
732    if (st->magic_samples[channel_index])
733    {
734       spx_uint32_t tmp_in_len;
735       spx_uint32_t tmp_magic;
736       tmp_in_len = st->magic_samples[channel_index];
737       tmp_out_len = *out_len;
738       /* FIXME: Need to handle the case where the out array is too small */
739       /* magic_samples needs to be set to zero to avoid infinite recursion */
740       tmp_magic = st->magic_samples[channel_index];
741       st->magic_samples[channel_index] = 0;
742       speex_resampler_process_native(st, channel_index, mem+N-1, &tmp_in_len, out, &tmp_out_len);
743       /*speex_warning_int("extra samples:", tmp_out_len);*/
744       /* If we couldn't process all "magic" input samples, save the rest for next time */
745       if (tmp_in_len < tmp_magic)
746       {
747          spx_uint32_t i;
748          st->magic_samples[channel_index] = tmp_magic-tmp_in_len;
749          for (i=0;i<st->magic_samples[channel_index];i++)
750             mem[N-1+i]=mem[N-1+i+tmp_in_len];
751       }
752       out += tmp_out_len;
753    }
754    
755    /* Call the right resampler through the function ptr */
756    out_sample = st->resampler_ptr(st, channel_index, in, in_len, out, out_len);
757    
758    if (st->last_sample[channel_index] < (spx_int32_t)*in_len)
759       *in_len = st->last_sample[channel_index];
760    *out_len = out_sample+tmp_out_len;
761    st->last_sample[channel_index] -= *in_len;
762    
763    for (j=0;j<N-1-(spx_int32_t)*in_len;j++)
764       mem[j] = mem[j+*in_len];
765    for (;j<N-1;j++)
766       mem[j] = in[st->in_stride*(j+*in_len-N+1)];
767    
768 }
769
770 #ifdef FIXED_POINT
771 void speex_resampler_process_float(SpeexResamplerState *st, spx_uint32_t channel_index, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
772 {
773    spx_uint32_t i;
774    int istride_save, ostride_save;
775    spx_word16_t x[*in_len];
776    spx_word16_t y[*out_len];
777    istride_save = st->in_stride;
778    ostride_save = st->out_stride;
779    for (i=0;i<*in_len;i++)
780       x[i] = WORD2INT(in[i*st->in_stride]);
781    st->in_stride = st->out_stride = 1;
782    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
783    st->in_stride = istride_save;
784    st->out_stride = ostride_save;
785    for (i=0;i<*out_len;i++)
786       out[i*st->out_stride] = y[i];
787 }
788 void speex_resampler_process_int(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
789 {
790    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
791 }
792 #else
793 void speex_resampler_process_float(SpeexResamplerState *st, spx_uint32_t channel_index, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
794 {
795    speex_resampler_process_native(st, channel_index, in, in_len, out, out_len);
796 }
797 void speex_resampler_process_int(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
798 {
799    spx_uint32_t i;
800    int istride_save, ostride_save;
801    spx_word16_t x[*in_len];
802    spx_word16_t y[*out_len];
803    istride_save = st->in_stride;
804    ostride_save = st->out_stride;
805    for (i=0;i<*in_len;i++)
806       x[i] = in[i*st->in_stride];
807    st->in_stride = st->out_stride = 1;
808    speex_resampler_process_native(st, channel_index, x, in_len, y, out_len);
809    st->in_stride = istride_save;
810    st->out_stride = ostride_save;
811    for (i=0;i<*out_len;i++)
812       out[i*st->out_stride] = WORD2INT(y[i]);
813 }
814 #endif
815
816 void speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
817 {
818    spx_uint32_t i;
819    int istride_save, ostride_save;
820    istride_save = st->in_stride;
821    ostride_save = st->out_stride;
822    st->in_stride = st->out_stride = st->nb_channels;
823    for (i=0;i<st->nb_channels;i++)
824    {
825       speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
826    }
827    st->in_stride = istride_save;
828    st->out_stride = ostride_save;
829 }
830
831 void speex_resampler_process_interleaved_int(SpeexResamplerState *st, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
832 {
833    spx_uint32_t i;
834    int istride_save, ostride_save;
835    istride_save = st->in_stride;
836    ostride_save = st->out_stride;
837    st->in_stride = st->out_stride = st->nb_channels;
838    for (i=0;i<st->nb_channels;i++)
839    {
840       speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
841    }
842    st->in_stride = istride_save;
843    st->out_stride = ostride_save;
844 }
845
846 void speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)
847 {
848    speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
849 }
850
851 void speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)
852 {
853    *in_rate = st->in_rate;
854    *out_rate = st->out_rate;
855 }
856
857 void speex_resampler_set_rate_frac(SpeexResamplerState *st, spx_uint32_t ratio_num, spx_uint32_t ratio_den, spx_uint32_t in_rate, spx_uint32_t out_rate)
858 {
859    int fact;
860    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
861       return;
862    
863    st->in_rate = in_rate;
864    st->out_rate = out_rate;
865    st->num_rate = ratio_num;
866    st->den_rate = ratio_den;
867    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
868    for (fact=2;fact<=sqrt(IMAX(in_rate, out_rate));fact++)
869    {
870       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
871       {
872          st->num_rate /= fact;
873          st->den_rate /= fact;
874       }
875    }
876       
877    if (st->initialised)
878       update_filter(st);
879 }
880
881 void speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)
882 {
883    *ratio_num = st->num_rate;
884    *ratio_den = st->den_rate;
885 }
886
887 void speex_resampler_set_quality(SpeexResamplerState *st, int quality)
888 {
889    if (quality < 0)
890       quality = 0;
891    if (quality > 10)
892       quality = 10;
893    if (st->quality == quality)
894       return;
895    st->quality = quality;
896    if (st->initialised)
897       update_filter(st);
898 }
899
900 void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
901 {
902    *quality = st->quality;
903 }
904
905 void speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)
906 {
907    st->in_stride = stride;
908 }
909
910 void speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)
911 {
912    *stride = st->in_stride;
913 }
914
915 void speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)
916 {
917    st->out_stride = stride;
918 }
919
920 void speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)
921 {
922    *stride = st->out_stride;
923 }
924
925 void speex_resampler_skip_zeros(SpeexResamplerState *st)
926 {
927    spx_uint32_t i;
928    for (i=0;i<st->nb_channels;i++)
929       st->last_sample[i] = st->filt_len/2;
930 }
931
932 void speex_resampler_reset_mem(SpeexResamplerState *st)
933 {
934    spx_uint32_t i;
935    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
936       st->mem[i] = 0;
937 }
938