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