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