1 /* Copyright (C) 2007-2008 Jean-Marc Valin
2    Copyright (C) 2008      Thorvald Natvig
3 
4    File: resample.c
5    Arbitrary resampling code
6 
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions are
9    met:
10 
11    1. Redistributions of source code must retain the above copyright notice,
12    this list of conditions and the following disclaimer.
13 
14    2. Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17 
18    3. The name of the author may not be used to endorse or promote products
19    derived from this software without specific prior written permission.
20 
21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31    POSSIBILITY OF SUCH DAMAGE.
32 */
33 
34 /*
35    The design goals of this code are:
36       - Very fast algorithm
37       - SIMD-friendly algorithm
38       - Low memory requirement
39       - Good *perceptual* quality (and not best SNR)
40 
41    Warning: This resampler is relatively new. Although I think I got rid of
42    all the major bugs and I don't expect the API to change anymore, there
43    may be something I've missed. So use with caution.
44 
45    This algorithm is based on this original resampling algorithm:
46    Smith, Julius O. Digital Audio Resampling Home Page
47    Center for Computer Research in Music and Acoustics (CCRMA),
48    Stanford University, 2007.
49    Web published at https://ccrma.stanford.edu/~jos/resample/.
50 
51    There is one main difference, though. This resampler uses cubic
52    interpolation instead of linear interpolation in the above paper. This
53    makes the table much smaller and makes it possible to compute that table
54    on a per-stream basis. In turn, being able to tweak the table for each
55    stream makes it possible to both reduce complexity on simple ratios
56    (e.g. 2/3), and get rid of the rounding operations in the inner loop.
57    The latter both reduces CPU time and makes the algorithm more SIMD-friendly.
58 */
59 
60 #ifdef HAVE_CONFIG_H
61 #include "config.h"
62 #endif
63 
64 #ifdef OUTSIDE_SPEEX
65 #include <stdlib.h>
speex_alloc(int size)66 static void *speex_alloc(int size) {return calloc(size,1);}
speex_realloc(void *ptr, int size)67 static void *speex_realloc(void *ptr, int size) {return realloc(ptr, size);}
speex_free(void *ptr)68 static void speex_free(void *ptr) {free(ptr);}
69 #ifndef EXPORT
70 #define EXPORT
71 #endif
72 #include "speex_resampler.h"
73 #include "arch.h"
74 #else /* OUTSIDE_SPEEX */
75 
76 #include "speex/speex_resampler.h"
77 #include "arch.h"
78 #include "os_support.h"
79 #endif /* OUTSIDE_SPEEX */
80 
81 #include <math.h>
82 #include <limits.h>
83 
84 #ifndef M_PI
85 #define M_PI 3.14159265358979323846
86 #endif
87 
88 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
89 #define IMIN(a,b) ((a) < (b) ? (a) : (b))
90 
91 #ifndef NULL
92 #define NULL 0
93 #endif
94 
95 #ifndef UINT32_MAX
96 #define UINT32_MAX 4294967295U
97 #endif
98 
99 #ifdef USE_SSE
100 #include "resample_sse.h"
101 #endif
102 
103 #ifdef USE_NEON
104 #include "resample_neon.h"
105 #endif
106 
107 /* Numer of elements to allocate on the stack */
108 #ifdef VAR_ARRAYS
109 #define FIXED_STACK_ALLOC 8192
110 #else
111 #define FIXED_STACK_ALLOC 1024
112 #endif
113 
114 #define EXPORT __attribute__((visibility("default")))
115 
116 typedef int (*resampler_basic_func)(SpeexResamplerState *, spx_uint32_t , const spx_word16_t *, spx_uint32_t *, spx_word16_t *, spx_uint32_t *);
117 
118 struct SpeexResamplerState_ {
119    spx_uint32_t in_rate;
120    spx_uint32_t out_rate;
121    spx_uint32_t num_rate;
122    spx_uint32_t den_rate;
123 
124    int    quality;
125    spx_uint32_t nb_channels;
126    spx_uint32_t filt_len;
127    spx_uint32_t mem_alloc_size;
128    spx_uint32_t buffer_size;
129    int          int_advance;
130    int          frac_advance;
131    float  cutoff;
132    spx_uint32_t oversample;
133    int          initialised;
134    int          started;
135 
136    /* These are per-channel */
137    spx_int32_t  *last_sample;
138    spx_uint32_t *samp_frac_num;
139    spx_uint32_t *magic_samples;
140 
141    spx_word16_t *mem;
142    spx_word16_t *sinc_table;
143    spx_uint32_t sinc_table_length;
144    resampler_basic_func resampler_ptr;
145 
146    int    in_stride;
147    int    out_stride;
148 } ;
149 
150 static const double kaiser12_table[68] = {
151    0.99859849, 1.00000000, 0.99859849, 0.99440475, 0.98745105, 0.97779076,
152    0.96549770, 0.95066529, 0.93340547, 0.91384741, 0.89213598, 0.86843014,
153    0.84290116, 0.81573067, 0.78710866, 0.75723148, 0.72629970, 0.69451601,
154    0.66208321, 0.62920216, 0.59606986, 0.56287762, 0.52980938, 0.49704014,
155    0.46473455, 0.43304576, 0.40211431, 0.37206735, 0.34301800, 0.31506490,
156    0.28829195, 0.26276832, 0.23854851, 0.21567274, 0.19416736, 0.17404546,
157    0.15530766, 0.13794294, 0.12192957, 0.10723616, 0.09382272, 0.08164178,
158    0.07063950, 0.06075685, 0.05193064, 0.04409466, 0.03718069, 0.03111947,
159    0.02584161, 0.02127838, 0.01736250, 0.01402878, 0.01121463, 0.00886058,
160    0.00691064, 0.00531256, 0.00401805, 0.00298291, 0.00216702, 0.00153438,
161    0.00105297, 0.00069463, 0.00043489, 0.00025272, 0.00013031, 0.0000527734,
162    0.00001000, 0.00000000};
163 /*
164 static const double kaiser12_table[36] = {
165    0.99440475, 1.00000000, 0.99440475, 0.97779076, 0.95066529, 0.91384741,
166    0.86843014, 0.81573067, 0.75723148, 0.69451601, 0.62920216, 0.56287762,
167    0.49704014, 0.43304576, 0.37206735, 0.31506490, 0.26276832, 0.21567274,
168    0.17404546, 0.13794294, 0.10723616, 0.08164178, 0.06075685, 0.04409466,
169    0.03111947, 0.02127838, 0.01402878, 0.00886058, 0.00531256, 0.00298291,
170    0.00153438, 0.00069463, 0.00025272, 0.0000527734, 0.00000500, 0.00000000};
171 */
172 static const double kaiser10_table[36] = {
173    0.99537781, 1.00000000, 0.99537781, 0.98162644, 0.95908712, 0.92831446,
174    0.89005583, 0.84522401, 0.79486424, 0.74011713, 0.68217934, 0.62226347,
175    0.56155915, 0.50119680, 0.44221549, 0.38553619, 0.33194107, 0.28205962,
176    0.23636152, 0.19515633, 0.15859932, 0.12670280, 0.09935205, 0.07632451,
177    0.05731132, 0.04193980, 0.02979584, 0.02044510, 0.01345224, 0.00839739,
178    0.00488951, 0.00257636, 0.00115101, 0.00035515, 0.00000000, 0.00000000};
179 
180 static const double kaiser8_table[36] = {
181    0.99635258, 1.00000000, 0.99635258, 0.98548012, 0.96759014, 0.94302200,
182    0.91223751, 0.87580811, 0.83439927, 0.78875245, 0.73966538, 0.68797126,
183    0.63451750, 0.58014482, 0.52566725, 0.47185369, 0.41941150, 0.36897272,
184    0.32108304, 0.27619388, 0.23465776, 0.19672670, 0.16255380, 0.13219758,
185    0.10562887, 0.08273982, 0.06335451, 0.04724088, 0.03412321, 0.02369490,
186    0.01563093, 0.00959968, 0.00527363, 0.00233883, 0.00050000, 0.00000000};
187 
188 static const double kaiser6_table[36] = {
189    0.99733006, 1.00000000, 0.99733006, 0.98935595, 0.97618418, 0.95799003,
190    0.93501423, 0.90755855, 0.87598009, 0.84068475, 0.80211977, 0.76076565,
191    0.71712752, 0.67172623, 0.62508937, 0.57774224, 0.53019925, 0.48295561,
192    0.43647969, 0.39120616, 0.34752997, 0.30580127, 0.26632152, 0.22934058,
193    0.19505503, 0.16360756, 0.13508755, 0.10953262, 0.08693120, 0.06722600,
194    0.05031820, 0.03607231, 0.02432151, 0.01487334, 0.00752000, 0.00000000};
195 
196 struct FuncDef {
197    const double *table;
198    int oversample;
199 };
200 
201 static const struct FuncDef kaiser12_funcdef = {kaiser12_table, 64};
202 #define KAISER12 (&kaiser12_funcdef)
203 static const struct FuncDef kaiser10_funcdef = {kaiser10_table, 32};
204 #define KAISER10 (&kaiser10_funcdef)
205 static const struct FuncDef kaiser8_funcdef = {kaiser8_table, 32};
206 #define KAISER8 (&kaiser8_funcdef)
207 static const struct FuncDef kaiser6_funcdef = {kaiser6_table, 32};
208 #define KAISER6 (&kaiser6_funcdef)
209 
210 struct QualityMapping {
211    int base_length;
212    int oversample;
213    float downsample_bandwidth;
214    float upsample_bandwidth;
215    const struct FuncDef *window_func;
216 };
217 
218 
219 /* This table maps conversion quality to internal parameters. There are two
220    reasons that explain why the up-sampling bandwidth is larger than the
221    down-sampling bandwidth:
222    1) When up-sampling, we can assume that the spectrum is already attenuated
223       close to the Nyquist rate (from an A/D or a previous resampling filter)
224    2) Any aliasing that occurs very close to the Nyquist rate will be masked
225       by the sinusoids/noise just below the Nyquist rate (guaranteed only for
226       up-sampling).
227 */
228 static const struct QualityMapping quality_map[11] = {
229    {  8,  4, 0.830f, 0.860f, KAISER6 }, /* Q0 */
230    { 16,  4, 0.850f, 0.880f, KAISER6 }, /* Q1 */
231    { 32,  4, 0.882f, 0.910f, KAISER6 }, /* Q2 */  /* 82.3% cutoff ( ~60 dB stop) 6  */
232    { 48,  8, 0.895f, 0.917f, KAISER8 }, /* Q3 */  /* 84.9% cutoff ( ~80 dB stop) 8  */
233    { 64,  8, 0.921f, 0.940f, KAISER8 }, /* Q4 */  /* 88.7% cutoff ( ~80 dB stop) 8  */
234    { 80, 16, 0.922f, 0.940f, KAISER10}, /* Q5 */  /* 89.1% cutoff (~100 dB stop) 10 */
235    { 96, 16, 0.940f, 0.945f, KAISER10}, /* Q6 */  /* 91.5% cutoff (~100 dB stop) 10 */
236    {128, 16, 0.950f, 0.950f, KAISER10}, /* Q7 */  /* 93.1% cutoff (~100 dB stop) 10 */
237    {160, 16, 0.960f, 0.960f, KAISER10}, /* Q8 */  /* 94.5% cutoff (~100 dB stop) 10 */
238    {192, 32, 0.968f, 0.968f, KAISER12}, /* Q9 */  /* 95.5% cutoff (~100 dB stop) 10 */
239    {256, 32, 0.975f, 0.975f, KAISER12}, /* Q10 */ /* 96.6% cutoff (~100 dB stop) 10 */
240 };
241 /*8,24,40,56,80,104,128,160,200,256,320*/
compute_func(float x, const struct FuncDef *func)242 static double compute_func(float x, const struct FuncDef *func)
243 {
244    float y, frac;
245    double interp[4];
246    int ind;
247    y = x*func->oversample;
248    ind = (int)floor(y);
249    frac = (y-ind);
250    /* CSE with handle the repeated powers */
251    interp[3] =  -0.1666666667*frac + 0.1666666667*(frac*frac*frac);
252    interp[2] = frac + 0.5*(frac*frac) - 0.5*(frac*frac*frac);
253    /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
254    interp[0] = -0.3333333333*frac + 0.5*(frac*frac) - 0.1666666667*(frac*frac*frac);
255    /* Just to make sure we don't have rounding problems */
256    interp[1] = 1.f-interp[3]-interp[2]-interp[0];
257 
258    /*sum = frac*accum[1] + (1-frac)*accum[2];*/
259    return interp[0]*func->table[ind] + interp[1]*func->table[ind+1] + interp[2]*func->table[ind+2] + interp[3]*func->table[ind+3];
260 }
261 
262 #if 0
263 #include <stdio.h>
264 int main(int argc, char **argv)
265 {
266    int i;
267    for (i=0;i<256;i++)
268    {
269       printf ("%f\n", compute_func(i/256., KAISER12));
270    }
271    return 0;
272 }
273 #endif
274 
275 #ifdef FIXED_POINT
276 /* The slow way of computing a sinc for the table. Should improve that some day */
sinc(float cutoff, float x, int N, const struct FuncDef *window_func)277 static spx_word16_t sinc(float cutoff, float x, int N, const struct FuncDef *window_func)
278 {
279    /*fprintf (stderr, "%f ", x);*/
280    float xx = x * cutoff;
281    if (fabs(x)<1e-6f)
282       return WORD2INT(32768.*cutoff);
283    else if (fabs(x) > .5f*N)
284       return 0;
285    /*FIXME: Can it really be any slower than this? */
286    return WORD2INT(32768.*cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func));
287 }
288 #else
289 /* The slow way of computing a sinc for the table. Should improve that some day */
sinc(float cutoff, float x, int N, const struct FuncDef *window_func)290 static spx_word16_t sinc(float cutoff, float x, int N, const struct FuncDef *window_func)
291 {
292    /*fprintf (stderr, "%f ", x);*/
293    float xx = x * cutoff;
294    if (fabs(x)<1e-6)
295       return cutoff;
296    else if (fabs(x) > .5*N)
297       return 0;
298    /*FIXME: Can it really be any slower than this? */
299    return cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func);
300 }
301 #endif
302 
303 #ifdef FIXED_POINT
cubic_coef(spx_word16_t x, spx_word16_t interp[4])304 static void cubic_coef(spx_word16_t x, spx_word16_t interp[4])
305 {
306    /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
307    but I know it's MMSE-optimal on a sinc */
308    spx_word16_t x2, x3;
309    x2 = MULT16_16_P15(x, x);
310    x3 = MULT16_16_P15(x, x2);
311    interp[0] = PSHR32(MULT16_16(QCONST16(-0.16667f, 15),x) + MULT16_16(QCONST16(0.16667f, 15),x3),15);
312    interp[1] = EXTRACT16(EXTEND32(x) + SHR32(SUB32(EXTEND32(x2),EXTEND32(x3)),1));
313    interp[3] = PSHR32(MULT16_16(QCONST16(-0.33333f, 15),x) + MULT16_16(QCONST16(.5f,15),x2) - MULT16_16(QCONST16(0.16667f, 15),x3),15);
314    /* Just to make sure we don't have rounding problems */
315    interp[2] = Q15_ONE-interp[0]-interp[1]-interp[3];
316    if (interp[2]<32767)
317       interp[2]+=1;
318 }
319 #else
cubic_coef(spx_word16_t frac, spx_word16_t interp[4])320 static void cubic_coef(spx_word16_t frac, spx_word16_t interp[4])
321 {
322    /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
323    but I know it's MMSE-optimal on a sinc */
324    interp[0] =  -0.16667f*frac + 0.16667f*frac*frac*frac;
325    interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
326    /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
327    interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
328    /* Just to make sure we don't have rounding problems */
329    interp[2] = 1.-interp[0]-interp[1]-interp[3];
330 }
331 #endif
332 
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)333 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)
334 {
335    const int N = st->filt_len;
336    int out_sample = 0;
337    int last_sample = st->last_sample[channel_index];
338    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
339    const spx_word16_t *sinc_table = st->sinc_table;
340    const int out_stride = st->out_stride;
341    const int int_advance = st->int_advance;
342    const int frac_advance = st->frac_advance;
343    const spx_uint32_t den_rate = st->den_rate;
344    spx_word32_t sum;
345 
346    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
347    {
348       const spx_word16_t *sinct = & sinc_table[samp_frac_num*N];
349       const spx_word16_t *iptr = & in[last_sample];
350 
351 #ifndef OVERRIDE_INNER_PRODUCT_SINGLE
352       int j;
353       sum = 0;
354       for(j=0;j<N;j++) sum += MULT16_16(sinct[j], iptr[j]);
355 
356 /*    This code is slower on most DSPs which have only 2 accumulators.
357       Plus this this forces truncation to 32 bits and you lose the HW guard bits.
358       I think we can trust the compiler and let it vectorize and/or unroll itself.
359       spx_word32_t accum[4] = {0,0,0,0};
360       for(j=0;j<N;j+=4) {
361         accum[0] += MULT16_16(sinct[j], iptr[j]);
362         accum[1] += MULT16_16(sinct[j+1], iptr[j+1]);
363         accum[2] += MULT16_16(sinct[j+2], iptr[j+2]);
364         accum[3] += MULT16_16(sinct[j+3], iptr[j+3]);
365       }
366       sum = accum[0] + accum[1] + accum[2] + accum[3];
367 */
368       sum = SATURATE32PSHR(sum, 15, 32767);
369 #else
370       sum = inner_product_single(sinct, iptr, N);
371 #endif
372 
373       out[out_stride * out_sample++] = sum;
374       last_sample += int_advance;
375       samp_frac_num += frac_advance;
376       if (samp_frac_num >= den_rate)
377       {
378          samp_frac_num -= den_rate;
379          last_sample++;
380       }
381    }
382 
383    st->last_sample[channel_index] = last_sample;
384    st->samp_frac_num[channel_index] = samp_frac_num;
385    return out_sample;
386 }
387 
388 #ifdef FIXED_POINT
389 #else
390 /* This is the same as the previous function, except with a double-precision accumulator */
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)391 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)
392 {
393    const int N = st->filt_len;
394    int out_sample = 0;
395    int last_sample = st->last_sample[channel_index];
396    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
397    const spx_word16_t *sinc_table = st->sinc_table;
398    const int out_stride = st->out_stride;
399    const int int_advance = st->int_advance;
400    const int frac_advance = st->frac_advance;
401    const spx_uint32_t den_rate = st->den_rate;
402    double sum;
403 
404    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
405    {
406       const spx_word16_t *sinct = & sinc_table[samp_frac_num*N];
407       const spx_word16_t *iptr = & in[last_sample];
408 
409 #ifndef OVERRIDE_INNER_PRODUCT_DOUBLE
410       int j;
411       double accum[4] = {0,0,0,0};
412 
413       for(j=0;j<N;j+=4) {
414         accum[0] += sinct[j]*iptr[j];
415         accum[1] += sinct[j+1]*iptr[j+1];
416         accum[2] += sinct[j+2]*iptr[j+2];
417         accum[3] += sinct[j+3]*iptr[j+3];
418       }
419       sum = accum[0] + accum[1] + accum[2] + accum[3];
420 #else
421       sum = inner_product_double(sinct, iptr, N);
422 #endif
423 
424       out[out_stride * out_sample++] = PSHR32(sum, 15);
425       last_sample += int_advance;
426       samp_frac_num += frac_advance;
427       if (samp_frac_num >= den_rate)
428       {
429          samp_frac_num -= den_rate;
430          last_sample++;
431       }
432    }
433 
434    st->last_sample[channel_index] = last_sample;
435    st->samp_frac_num[channel_index] = samp_frac_num;
436    return out_sample;
437 }
438 #endif
439 
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)440 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)
441 {
442    const int N = st->filt_len;
443    int out_sample = 0;
444    int last_sample = st->last_sample[channel_index];
445    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
446    const int out_stride = st->out_stride;
447    const int int_advance = st->int_advance;
448    const int frac_advance = st->frac_advance;
449    const spx_uint32_t den_rate = st->den_rate;
450    spx_word32_t sum;
451 
452    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
453    {
454       const spx_word16_t *iptr = & in[last_sample];
455 
456       const int offset = samp_frac_num*st->oversample/st->den_rate;
457 #ifdef FIXED_POINT
458       const spx_word16_t frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
459 #else
460       const spx_word16_t frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
461 #endif
462       spx_word16_t interp[4];
463 
464 
465 #ifndef OVERRIDE_INTERPOLATE_PRODUCT_SINGLE
466       int j;
467       spx_word32_t accum[4] = {0,0,0,0};
468 
469       for(j=0;j<N;j++) {
470         const spx_word16_t curr_in=iptr[j];
471         accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
472         accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
473         accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
474         accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
475       }
476 
477       cubic_coef(frac, interp);
478       sum = MULT16_32_Q15(interp[0],SHR32(accum[0], 1)) + MULT16_32_Q15(interp[1],SHR32(accum[1], 1)) + MULT16_32_Q15(interp[2],SHR32(accum[2], 1)) + MULT16_32_Q15(interp[3],SHR32(accum[3], 1));
479       sum = SATURATE32PSHR(sum, 15, 32767);
480 #else
481       cubic_coef(frac, interp);
482       sum = interpolate_product_single(iptr, st->sinc_table + st->oversample + 4 - offset - 2, N, st->oversample, interp);
483 #endif
484 
485       out[out_stride * out_sample++] = sum;
486       last_sample += int_advance;
487       samp_frac_num += frac_advance;
488       if (samp_frac_num >= den_rate)
489       {
490          samp_frac_num -= den_rate;
491          last_sample++;
492       }
493    }
494 
495    st->last_sample[channel_index] = last_sample;
496    st->samp_frac_num[channel_index] = samp_frac_num;
497    return out_sample;
498 }
499 
500 #ifdef FIXED_POINT
501 #else
502 /* This is the same as the previous function, except with a double-precision accumulator */
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)503 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)
504 {
505    const int N = st->filt_len;
506    int out_sample = 0;
507    int last_sample = st->last_sample[channel_index];
508    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
509    const int out_stride = st->out_stride;
510    const int int_advance = st->int_advance;
511    const int frac_advance = st->frac_advance;
512    const spx_uint32_t den_rate = st->den_rate;
513    spx_word32_t sum;
514 
515    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
516    {
517       const spx_word16_t *iptr = & in[last_sample];
518 
519       const int offset = samp_frac_num*st->oversample/st->den_rate;
520 #ifdef FIXED_POINT
521       const spx_word16_t frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
522 #else
523       const spx_word16_t frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
524 #endif
525       spx_word16_t interp[4];
526 
527 
528 #ifndef OVERRIDE_INTERPOLATE_PRODUCT_DOUBLE
529       int j;
530       double accum[4] = {0,0,0,0};
531 
532       for(j=0;j<N;j++) {
533         const double curr_in=iptr[j];
534         accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
535         accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
536         accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
537         accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
538       }
539 
540       cubic_coef(frac, interp);
541       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]);
542 #else
543       cubic_coef(frac, interp);
544       sum = interpolate_product_double(iptr, st->sinc_table + st->oversample + 4 - offset - 2, N, st->oversample, interp);
545 #endif
546 
547       out[out_stride * out_sample++] = PSHR32(sum,15);
548       last_sample += int_advance;
549       samp_frac_num += frac_advance;
550       if (samp_frac_num >= den_rate)
551       {
552          samp_frac_num -= den_rate;
553          last_sample++;
554       }
555    }
556 
557    st->last_sample[channel_index] = last_sample;
558    st->samp_frac_num[channel_index] = samp_frac_num;
559    return out_sample;
560 }
561 #endif
562 
563 /* This resampler is used to produce zero output in situations where memory
564    for the filter could not be allocated.  The expected numbers of input and
565    output samples are still processed so that callers failing to check error
566    codes are not surprised, possibly getting into infinite loops. */
resampler_basic_zero(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)567 static int resampler_basic_zero(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)
568 {
569    int out_sample = 0;
570    int last_sample = st->last_sample[channel_index];
571    spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
572    const int out_stride = st->out_stride;
573    const int int_advance = st->int_advance;
574    const int frac_advance = st->frac_advance;
575    const spx_uint32_t den_rate = st->den_rate;
576 
577    (void)in;
578    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
579    {
580       out[out_stride * out_sample++] = 0;
581       last_sample += int_advance;
582       samp_frac_num += frac_advance;
583       if (samp_frac_num >= den_rate)
584       {
585          samp_frac_num -= den_rate;
586          last_sample++;
587       }
588    }
589 
590    st->last_sample[channel_index] = last_sample;
591    st->samp_frac_num[channel_index] = samp_frac_num;
592    return out_sample;
593 }
594 
multiply_frac(spx_uint32_t *result, spx_uint32_t value, spx_uint32_t num, spx_uint32_t den)595 static int multiply_frac(spx_uint32_t *result, spx_uint32_t value, spx_uint32_t num, spx_uint32_t den)
596 {
597    spx_uint32_t major = value / den;
598    spx_uint32_t remain = value % den;
599    /* TODO: Could use 64 bits operation to check for overflow. But only guaranteed in C99+ */
600    if (remain > UINT32_MAX / num || major > UINT32_MAX / num
601        || major * num > UINT32_MAX - remain * num / den)
602       return RESAMPLER_ERR_OVERFLOW;
603    *result = remain * num / den + major * num;
604    return RESAMPLER_ERR_SUCCESS;
605 }
606 
update_filter(SpeexResamplerState *st)607 static int update_filter(SpeexResamplerState *st)
608 {
609    spx_uint32_t old_length = st->filt_len;
610    spx_uint32_t old_alloc_size = st->mem_alloc_size;
611    int use_direct;
612    spx_uint32_t min_sinc_table_length;
613    spx_uint32_t min_alloc_size;
614 
615    st->int_advance = st->num_rate/st->den_rate;
616    st->frac_advance = st->num_rate%st->den_rate;
617    st->oversample = quality_map[st->quality].oversample;
618    st->filt_len = quality_map[st->quality].base_length;
619 
620    if (st->num_rate > st->den_rate)
621    {
622       /* down-sampling */
623       st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
624       if (multiply_frac(&st->filt_len,st->filt_len,st->num_rate,st->den_rate) != RESAMPLER_ERR_SUCCESS)
625          goto fail;
626       /* Round up to make sure we have a multiple of 8 for SSE */
627       st->filt_len = ((st->filt_len-1)&(~0x7))+8;
628       if (2*st->den_rate < st->num_rate)
629          st->oversample >>= 1;
630       if (4*st->den_rate < st->num_rate)
631          st->oversample >>= 1;
632       if (8*st->den_rate < st->num_rate)
633          st->oversample >>= 1;
634       if (16*st->den_rate < st->num_rate)
635          st->oversample >>= 1;
636       if (st->oversample < 1)
637          st->oversample = 1;
638    } else {
639       /* up-sampling */
640       st->cutoff = quality_map[st->quality].upsample_bandwidth;
641    }
642 
643 #ifdef RESAMPLE_FULL_SINC_TABLE
644    use_direct = 1;
645    if (INT_MAX/sizeof(spx_word16_t)/st->den_rate < st->filt_len)
646       goto fail;
647 #else
648    /* Choose the resampling type that requires the least amount of memory */
649    use_direct = st->filt_len*st->den_rate <= st->filt_len*st->oversample+8
650                 && INT_MAX/sizeof(spx_word16_t)/st->den_rate >= st->filt_len;
651 #endif
652    if (use_direct)
653    {
654       min_sinc_table_length = st->filt_len*st->den_rate;
655    } else {
656       if ((INT_MAX/sizeof(spx_word16_t)-8)/st->oversample < st->filt_len)
657          goto fail;
658 
659       min_sinc_table_length = st->filt_len*st->oversample+8;
660    }
661    if (st->sinc_table_length < min_sinc_table_length)
662    {
663       spx_word16_t *sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,min_sinc_table_length*sizeof(spx_word16_t));
664       if (!sinc_table)
665          goto fail;
666 
667       st->sinc_table = sinc_table;
668       st->sinc_table_length = min_sinc_table_length;
669    }
670    if (use_direct)
671    {
672       spx_uint32_t i;
673       for (i=0;i<st->den_rate;i++)
674       {
675          spx_int32_t j;
676          for (j=0;j<st->filt_len;j++)
677          {
678             st->sinc_table[i*st->filt_len+j] = sinc(st->cutoff,((j-(spx_int32_t)st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len, quality_map[st->quality].window_func);
679          }
680       }
681 #ifdef FIXED_POINT
682       st->resampler_ptr = resampler_basic_direct_single;
683 #else
684       if (st->quality>8)
685          st->resampler_ptr = resampler_basic_direct_double;
686       else
687          st->resampler_ptr = resampler_basic_direct_single;
688 #endif
689       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
690    } else {
691       spx_int32_t i;
692       for (i=-4;i<(spx_int32_t)(st->oversample*st->filt_len+4);i++)
693          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);
694 #ifdef FIXED_POINT
695       st->resampler_ptr = resampler_basic_interpolate_single;
696 #else
697       if (st->quality>8)
698          st->resampler_ptr = resampler_basic_interpolate_double;
699       else
700          st->resampler_ptr = resampler_basic_interpolate_single;
701 #endif
702       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
703    }
704 
705    /* Here's the place where we update the filter memory to take into account
706       the change in filter length. It's probably the messiest part of the code
707       due to handling of lots of corner cases. */
708 
709    /* Adding buffer_size to filt_len won't overflow here because filt_len
710       could be multiplied by sizeof(spx_word16_t) above. */
711    min_alloc_size = st->filt_len-1 + st->buffer_size;
712    if (min_alloc_size > st->mem_alloc_size)
713    {
714       spx_word16_t *mem;
715       if (INT_MAX/sizeof(spx_word16_t)/st->nb_channels < min_alloc_size)
716           goto fail;
717       else if (!(mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*min_alloc_size * sizeof(*mem))))
718           goto fail;
719 
720       st->mem = mem;
721       st->mem_alloc_size = min_alloc_size;
722    }
723    if (!st->started)
724    {
725       spx_uint32_t i;
726       for (i=0;i<st->nb_channels*st->mem_alloc_size;i++)
727          st->mem[i] = 0;
728       /*speex_warning("reinit filter");*/
729    } else if (st->filt_len > old_length)
730    {
731       spx_uint32_t i;
732       /* Increase the filter length */
733       /*speex_warning("increase filter size");*/
734       for (i=st->nb_channels;i--;)
735       {
736          spx_uint32_t j;
737          spx_uint32_t olen = old_length;
738          /*if (st->magic_samples[i])*/
739          {
740             /* Try and remove the magic samples as if nothing had happened */
741 
742             /* FIXME: This is wrong but for now we need it to avoid going over the array bounds */
743             olen = old_length + 2*st->magic_samples[i];
744             for (j=old_length-1+st->magic_samples[i];j--;)
745                st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]] = st->mem[i*old_alloc_size+j];
746             for (j=0;j<st->magic_samples[i];j++)
747                st->mem[i*st->mem_alloc_size+j] = 0;
748             st->magic_samples[i] = 0;
749          }
750          if (st->filt_len > olen)
751          {
752             /* If the new filter length is still bigger than the "augmented" length */
753             /* Copy data going backward */
754             for (j=0;j<olen-1;j++)
755                st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*st->mem_alloc_size+(olen-2-j)];
756             /* Then put zeros for lack of anything better */
757             for (;j<st->filt_len-1;j++)
758                st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
759             /* Adjust last_sample */
760             st->last_sample[i] += (st->filt_len - olen)/2;
761          } else {
762             /* Put back some of the magic! */
763             st->magic_samples[i] = (olen - st->filt_len)/2;
764             for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
765                st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
766          }
767       }
768    } else if (st->filt_len < old_length)
769    {
770       spx_uint32_t i;
771       /* Reduce filter length, this a bit tricky. We need to store some of the memory as "magic"
772          samples so they can be used directly as input the next time(s) */
773       for (i=0;i<st->nb_channels;i++)
774       {
775          spx_uint32_t j;
776          spx_uint32_t old_magic = st->magic_samples[i];
777          st->magic_samples[i] = (old_length - st->filt_len)/2;
778          /* We must copy some of the memory that's no longer used */
779          /* Copy data going backward */
780          for (j=0;j<st->filt_len-1+st->magic_samples[i]+old_magic;j++)
781             st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
782          st->magic_samples[i] += old_magic;
783       }
784    }
785    return RESAMPLER_ERR_SUCCESS;
786 
787 fail:
788    st->resampler_ptr = resampler_basic_zero;
789    /* st->mem may still contain consumed input samples for the filter.
790       Restore filt_len so that filt_len - 1 still points to the position after
791       the last of these samples. */
792    st->filt_len = old_length;
793    return RESAMPLER_ERR_ALLOC_FAILED;
794 }
795 
speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)796 EXPORT SpeexResamplerState *speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
797 {
798    return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality, err);
799 }
800 
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)801 EXPORT 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)
802 {
803    SpeexResamplerState *st;
804    int filter_err;
805 
806    if (nb_channels == 0 || ratio_num == 0 || ratio_den == 0 || quality > 10 || quality < 0)
807    {
808       if (err)
809          *err = RESAMPLER_ERR_INVALID_ARG;
810       return NULL;
811    }
812    st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
813    if (!st)
814    {
815       if (err)
816          *err = RESAMPLER_ERR_ALLOC_FAILED;
817       return NULL;
818    }
819    st->initialised = 0;
820    st->started = 0;
821    st->in_rate = 0;
822    st->out_rate = 0;
823    st->num_rate = 0;
824    st->den_rate = 0;
825    st->quality = -1;
826    st->sinc_table_length = 0;
827    st->mem_alloc_size = 0;
828    st->filt_len = 0;
829    st->mem = 0;
830    st->resampler_ptr = 0;
831 
832    st->cutoff = 1.f;
833    st->nb_channels = nb_channels;
834    st->in_stride = 1;
835    st->out_stride = 1;
836 
837    st->buffer_size = 160;
838 
839    /* Per channel data */
840    if (!(st->last_sample = (spx_int32_t*)speex_alloc(nb_channels*sizeof(spx_int32_t))))
841       goto fail;
842    if (!(st->magic_samples = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(spx_uint32_t))))
843       goto fail;
844    if (!(st->samp_frac_num = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(spx_uint32_t))))
845       goto fail;
846 
847    speex_resampler_set_quality(st, quality);
848    speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);
849 
850    filter_err = update_filter(st);
851    if (filter_err == RESAMPLER_ERR_SUCCESS)
852    {
853       st->initialised = 1;
854    } else {
855       speex_resampler_destroy(st);
856       st = NULL;
857    }
858    if (err)
859       *err = filter_err;
860 
861    return st;
862 
863 fail:
864    if (err)
865       *err = RESAMPLER_ERR_ALLOC_FAILED;
866    speex_resampler_destroy(st);
867    return NULL;
868 }
869 
speex_resampler_destroy(SpeexResamplerState *st)870 EXPORT void speex_resampler_destroy(SpeexResamplerState *st)
871 {
872    speex_free(st->mem);
873    speex_free(st->sinc_table);
874    speex_free(st->last_sample);
875    speex_free(st->magic_samples);
876    speex_free(st->samp_frac_num);
877    speex_free(st);
878 }
879 
speex_resampler_process_native(SpeexResamplerState *st, spx_uint32_t channel_index, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)880 static int speex_resampler_process_native(SpeexResamplerState *st, spx_uint32_t channel_index, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
881 {
882    int j=0;
883    const int N = st->filt_len;
884    int out_sample = 0;
885    spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
886    spx_uint32_t ilen;
887 
888    st->started = 1;
889 
890    /* Call the right resampler through the function ptr */
891    out_sample = st->resampler_ptr(st, channel_index, mem, in_len, out, out_len);
892 
893    if (st->last_sample[channel_index] < (spx_int32_t)*in_len)
894       *in_len = st->last_sample[channel_index];
895    *out_len = out_sample;
896    st->last_sample[channel_index] -= *in_len;
897 
898    ilen = *in_len;
899 
900    for(j=0;j<N-1;++j)
901      mem[j] = mem[j+ilen];
902 
903    return RESAMPLER_ERR_SUCCESS;
904 }
905 
speex_resampler_magic(SpeexResamplerState *st, spx_uint32_t channel_index, spx_word16_t **out, spx_uint32_t out_len)906 static int speex_resampler_magic(SpeexResamplerState *st, spx_uint32_t channel_index, spx_word16_t **out, spx_uint32_t out_len) {
907    spx_uint32_t tmp_in_len = st->magic_samples[channel_index];
908    spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
909    const int N = st->filt_len;
910 
911    speex_resampler_process_native(st, channel_index, &tmp_in_len, *out, &out_len);
912 
913    st->magic_samples[channel_index] -= tmp_in_len;
914 
915    /* If we couldn't process all "magic" input samples, save the rest for next time */
916    if (st->magic_samples[channel_index])
917    {
918       spx_uint32_t i;
919       for (i=0;i<st->magic_samples[channel_index];i++)
920          mem[N-1+i]=mem[N-1+i+tmp_in_len];
921    }
922    *out += out_len*st->out_stride;
923    return out_len;
924 }
925 
926 #ifdef FIXED_POINT
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)927 EXPORT 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)
928 #else
929 EXPORT 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)
930 #endif
931 {
932    int j;
933    spx_uint32_t ilen = *in_len;
934    spx_uint32_t olen = *out_len;
935    spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
936    const int filt_offs = st->filt_len - 1;
937    const spx_uint32_t xlen = st->mem_alloc_size - filt_offs;
938    const int istride = st->in_stride;
939 
940    if (st->magic_samples[channel_index])
941       olen -= speex_resampler_magic(st, channel_index, &out, olen);
942    if (! st->magic_samples[channel_index]) {
943       while (ilen && olen) {
944         spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
945         spx_uint32_t ochunk = olen;
946 
947         if (in) {
948            for(j=0;j<ichunk;++j)
949               x[j+filt_offs]=in[j*istride];
950         } else {
951           for(j=0;j<ichunk;++j)
952             x[j+filt_offs]=0;
953         }
954         speex_resampler_process_native(st, channel_index, &ichunk, out, &ochunk);
955         ilen -= ichunk;
956         olen -= ochunk;
957         out += ochunk * st->out_stride;
958         if (in)
959            in += ichunk * istride;
960       }
961    }
962    *in_len -= ilen;
963    *out_len -= olen;
964    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
965 }
966 
967 #ifdef FIXED_POINT
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)968 EXPORT 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)
969 #else
970 EXPORT 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)
971 #endif
972 {
973    int j;
974    const int istride_save = st->in_stride;
975    const int ostride_save = st->out_stride;
976    spx_uint32_t ilen = *in_len;
977    spx_uint32_t olen = *out_len;
978    spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
979    const spx_uint32_t xlen = st->mem_alloc_size - (st->filt_len - 1);
980 #ifdef VAR_ARRAYS
981    const unsigned int ylen = (olen < FIXED_STACK_ALLOC) ? olen : FIXED_STACK_ALLOC;
982    spx_word16_t ystack[ylen];
983 #else
984    const unsigned int ylen = FIXED_STACK_ALLOC;
985    spx_word16_t ystack[FIXED_STACK_ALLOC];
986 #endif
987 
988    st->out_stride = 1;
989 
990    while (ilen && olen) {
991      spx_word16_t *y = ystack;
992      spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
993      spx_uint32_t ochunk = (olen > ylen) ? ylen : olen;
994      spx_uint32_t omagic = 0;
995 
996      if (st->magic_samples[channel_index]) {
997        omagic = speex_resampler_magic(st, channel_index, &y, ochunk);
998        ochunk -= omagic;
999        olen -= omagic;
1000      }
1001      if (! st->magic_samples[channel_index]) {
1002        if (in) {
1003          for(j=0;j<ichunk;++j)
1004 #ifdef FIXED_POINT
1005            x[j+st->filt_len-1]=WORD2INT(in[j*istride_save]);
1006 #else
1007            x[j+st->filt_len-1]=in[j*istride_save];
1008 #endif
1009        } else {
1010          for(j=0;j<ichunk;++j)
1011            x[j+st->filt_len-1]=0;
1012        }
1013 
1014        speex_resampler_process_native(st, channel_index, &ichunk, y, &ochunk);
1015      } else {
1016        ichunk = 0;
1017        ochunk = 0;
1018      }
1019 
1020      for (j=0;j<ochunk+omagic;++j)
1021 #ifdef FIXED_POINT
1022         out[j*ostride_save] = ystack[j];
1023 #else
1024         out[j*ostride_save] = WORD2INT(ystack[j]);
1025 #endif
1026 
1027      ilen -= ichunk;
1028      olen -= ochunk;
1029      out += (ochunk+omagic) * ostride_save;
1030      if (in)
1031        in += ichunk * istride_save;
1032    }
1033    st->out_stride = ostride_save;
1034    *in_len -= ilen;
1035    *out_len -= olen;
1036 
1037    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1038 }
1039 
speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)1040 EXPORT int speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
1041 {
1042    spx_uint32_t i;
1043    int istride_save, ostride_save;
1044    spx_uint32_t bak_out_len = *out_len;
1045    spx_uint32_t bak_in_len = *in_len;
1046    istride_save = st->in_stride;
1047    ostride_save = st->out_stride;
1048    st->in_stride = st->out_stride = st->nb_channels;
1049    for (i=0;i<st->nb_channels;i++)
1050    {
1051       *out_len = bak_out_len;
1052       *in_len = bak_in_len;
1053       if (in != NULL)
1054          speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
1055       else
1056          speex_resampler_process_float(st, i, NULL, in_len, out+i, out_len);
1057    }
1058    st->in_stride = istride_save;
1059    st->out_stride = ostride_save;
1060    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1061 }
1062 
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)1063 EXPORT 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)
1064 {
1065    spx_uint32_t i;
1066    int istride_save, ostride_save;
1067    spx_uint32_t bak_out_len = *out_len;
1068    spx_uint32_t bak_in_len = *in_len;
1069    istride_save = st->in_stride;
1070    ostride_save = st->out_stride;
1071    st->in_stride = st->out_stride = st->nb_channels;
1072    for (i=0;i<st->nb_channels;i++)
1073    {
1074       *out_len = bak_out_len;
1075       *in_len = bak_in_len;
1076       if (in != NULL)
1077          speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
1078       else
1079          speex_resampler_process_int(st, i, NULL, in_len, out+i, out_len);
1080    }
1081    st->in_stride = istride_save;
1082    st->out_stride = ostride_save;
1083    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1084 }
1085 
speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)1086 EXPORT int speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)
1087 {
1088    return speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
1089 }
1090 
speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)1091 EXPORT void speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)
1092 {
1093    *in_rate = st->in_rate;
1094    *out_rate = st->out_rate;
1095 }
1096 
compute_gcd(spx_uint32_t a, spx_uint32_t b)1097 static inline spx_uint32_t compute_gcd(spx_uint32_t a, spx_uint32_t b)
1098 {
1099    while (b != 0)
1100    {
1101       spx_uint32_t temp = a;
1102 
1103       a = b;
1104       b = temp % b;
1105    }
1106    return a;
1107 }
1108 
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)1109 EXPORT 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)
1110 {
1111    spx_uint32_t fact;
1112    spx_uint32_t old_den;
1113    spx_uint32_t i;
1114 
1115    if (ratio_num == 0 || ratio_den == 0)
1116       return RESAMPLER_ERR_INVALID_ARG;
1117 
1118    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
1119       return RESAMPLER_ERR_SUCCESS;
1120 
1121    old_den = st->den_rate;
1122    st->in_rate = in_rate;
1123    st->out_rate = out_rate;
1124    st->num_rate = ratio_num;
1125    st->den_rate = ratio_den;
1126 
1127    fact = compute_gcd(st->num_rate, st->den_rate);
1128 
1129    st->num_rate /= fact;
1130    st->den_rate /= fact;
1131 
1132    if (old_den > 0)
1133    {
1134       for (i=0;i<st->nb_channels;i++)
1135       {
1136          if (multiply_frac(&st->samp_frac_num[i],st->samp_frac_num[i],st->den_rate,old_den) != RESAMPLER_ERR_SUCCESS)
1137             return RESAMPLER_ERR_OVERFLOW;
1138          /* Safety net */
1139          if (st->samp_frac_num[i] >= st->den_rate)
1140             st->samp_frac_num[i] = st->den_rate-1;
1141       }
1142    }
1143 
1144    if (st->initialised)
1145       return update_filter(st);
1146    return RESAMPLER_ERR_SUCCESS;
1147 }
1148 
speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)1149 EXPORT void speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)
1150 {
1151    *ratio_num = st->num_rate;
1152    *ratio_den = st->den_rate;
1153 }
1154 
speex_resampler_set_quality(SpeexResamplerState *st, int quality)1155 EXPORT int speex_resampler_set_quality(SpeexResamplerState *st, int quality)
1156 {
1157    if (quality > 10 || quality < 0)
1158       return RESAMPLER_ERR_INVALID_ARG;
1159    if (st->quality == quality)
1160       return RESAMPLER_ERR_SUCCESS;
1161    st->quality = quality;
1162    if (st->initialised)
1163       return update_filter(st);
1164    return RESAMPLER_ERR_SUCCESS;
1165 }
1166 
speex_resampler_get_quality(SpeexResamplerState *st, int *quality)1167 EXPORT void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
1168 {
1169    *quality = st->quality;
1170 }
1171 
speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)1172 EXPORT void speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)
1173 {
1174    st->in_stride = stride;
1175 }
1176 
speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)1177 EXPORT void speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)
1178 {
1179    *stride = st->in_stride;
1180 }
1181 
speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)1182 EXPORT void speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)
1183 {
1184    st->out_stride = stride;
1185 }
1186 
speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)1187 EXPORT void speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)
1188 {
1189    *stride = st->out_stride;
1190 }
1191 
speex_resampler_get_input_latency(SpeexResamplerState *st)1192 EXPORT int speex_resampler_get_input_latency(SpeexResamplerState *st)
1193 {
1194   return st->filt_len / 2;
1195 }
1196 
speex_resampler_get_output_latency(SpeexResamplerState *st)1197 EXPORT int speex_resampler_get_output_latency(SpeexResamplerState *st)
1198 {
1199   return ((st->filt_len / 2) * st->den_rate + (st->num_rate >> 1)) / st->num_rate;
1200 }
1201 
speex_resampler_skip_zeros(SpeexResamplerState *st)1202 EXPORT int speex_resampler_skip_zeros(SpeexResamplerState *st)
1203 {
1204    spx_uint32_t i;
1205    for (i=0;i<st->nb_channels;i++)
1206       st->last_sample[i] = st->filt_len/2;
1207    return RESAMPLER_ERR_SUCCESS;
1208 }
1209 
speex_resampler_reset_mem(SpeexResamplerState *st)1210 EXPORT int speex_resampler_reset_mem(SpeexResamplerState *st)
1211 {
1212    spx_uint32_t i;
1213    for (i=0;i<st->nb_channels;i++)
1214    {
1215       st->last_sample[i] = 0;
1216       st->magic_samples[i] = 0;
1217       st->samp_frac_num[i] = 0;
1218    }
1219    for (i=0;i<st->nb_channels*st->mem_alloc_size;i++)
1220       st->mem[i] = 0;
1221    return RESAMPLER_ERR_SUCCESS;
1222 }
1223 
speex_resampler_strerror(int err)1224 EXPORT const char *speex_resampler_strerror(int err)
1225 {
1226    switch (err)
1227    {
1228       case RESAMPLER_ERR_SUCCESS:
1229          return "Success.";
1230       case RESAMPLER_ERR_ALLOC_FAILED:
1231          return "Memory allocation failed.";
1232       case RESAMPLER_ERR_BAD_STATE:
1233          return "Bad resampler state.";
1234       case RESAMPLER_ERR_INVALID_ARG:
1235          return "Invalid argument.";
1236       case RESAMPLER_ERR_PTR_OVERLAP:
1237          return "Input and output buffers overlap.";
1238       default:
1239          return "Unknown error. Bad error code or strange version mismatch.";
1240    }
1241 }
1242