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