• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 http://www-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 #include "speex_resampler.h"
70 #include "arch.h"
71 #else /* OUTSIDE_SPEEX */
72 
73 #include "speex/speex_resampler.h"
74 #include "arch.h"
75 #include "os_support.h"
76 #endif /* OUTSIDE_SPEEX */
77 
78 #include "stack_alloc.h"
79 #include <math.h>
80 #include <limits.h>
81 
82 #ifndef M_PI
83 #define M_PI 3.14159265358979323846
84 #endif
85 
86 #ifdef FIXED_POINT
87 #define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))
88 #else
89 #define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))
90 #endif
91 
92 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
93 #define IMIN(a,b) ((a) < (b) ? (a) : (b))
94 
95 #ifndef NULL
96 #define NULL 0
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 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 = {kaiser12_table, 64};
200 #define KAISER12 (&_KAISER12)
201 /*static struct FuncDef _KAISER12 = {kaiser12_table, 32};
202 #define KAISER12 (&_KAISER12)*/
203 static const struct FuncDef _KAISER10 = {kaiser10_table, 32};
204 #define KAISER10 (&_KAISER10)
205 static const struct FuncDef _KAISER8 = {kaiser8_table, 32};
206 #define KAISER8 (&_KAISER8)
207 static const struct FuncDef _KAISER6 = {kaiser6_table, 32};
208 #define KAISER6 (&_KAISER6)
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    while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
578    {
579       out[out_stride * out_sample++] = 0;
580       last_sample += int_advance;
581       samp_frac_num += frac_advance;
582       if (samp_frac_num >= den_rate)
583       {
584          samp_frac_num -= den_rate;
585          last_sample++;
586       }
587    }
588 
589    st->last_sample[channel_index] = last_sample;
590    st->samp_frac_num[channel_index] = samp_frac_num;
591    return out_sample;
592 }
593 
update_filter(SpeexResamplerState * st)594 static int update_filter(SpeexResamplerState *st)
595 {
596    spx_uint32_t old_length = st->filt_len;
597    spx_uint32_t old_alloc_size = st->mem_alloc_size;
598    int use_direct;
599    spx_uint32_t min_sinc_table_length;
600    spx_uint32_t min_alloc_size;
601 
602    st->int_advance = st->num_rate/st->den_rate;
603    st->frac_advance = st->num_rate%st->den_rate;
604    st->oversample = quality_map[st->quality].oversample;
605    st->filt_len = quality_map[st->quality].base_length;
606 
607    if (st->num_rate > st->den_rate)
608    {
609       /* down-sampling */
610       st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
611       /* FIXME: divide the numerator and denominator by a certain amount if they're too large */
612       st->filt_len = st->filt_len*st->num_rate / st->den_rate;
613       /* Round up to make sure we have a multiple of 8 for SSE */
614       st->filt_len = ((st->filt_len-1)&(~0x7))+8;
615       if (2*st->den_rate < st->num_rate)
616          st->oversample >>= 1;
617       if (4*st->den_rate < st->num_rate)
618          st->oversample >>= 1;
619       if (8*st->den_rate < st->num_rate)
620          st->oversample >>= 1;
621       if (16*st->den_rate < st->num_rate)
622          st->oversample >>= 1;
623       if (st->oversample < 1)
624          st->oversample = 1;
625    } else {
626       /* up-sampling */
627       st->cutoff = quality_map[st->quality].upsample_bandwidth;
628    }
629 
630    /* Choose the resampling type that requires the least amount of memory */
631 #ifdef RESAMPLE_FULL_SINC_TABLE
632    use_direct = 1;
633    if (INT_MAX/sizeof(spx_word16_t)/st->den_rate < st->filt_len)
634       goto fail;
635 #else
636    use_direct = st->filt_len*st->den_rate <= st->filt_len*st->oversample+8
637                 && INT_MAX/sizeof(spx_word16_t)/st->den_rate >= st->filt_len;
638 #endif
639    if (use_direct)
640    {
641       min_sinc_table_length = st->filt_len*st->den_rate;
642    } else {
643       if ((INT_MAX/sizeof(spx_word16_t)-8)/st->oversample < st->filt_len)
644          goto fail;
645 
646       min_sinc_table_length = st->filt_len*st->oversample+8;
647    }
648    if (st->sinc_table_length < min_sinc_table_length)
649    {
650       spx_word16_t *sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,min_sinc_table_length*sizeof(spx_word16_t));
651       if (!sinc_table)
652          goto fail;
653 
654       st->sinc_table = sinc_table;
655       st->sinc_table_length = min_sinc_table_length;
656    }
657    if (use_direct)
658    {
659       spx_uint32_t i;
660       for (i=0;i<st->den_rate;i++)
661       {
662          spx_int32_t j;
663          for (j=0;j<st->filt_len;j++)
664          {
665             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);
666          }
667       }
668 #ifdef FIXED_POINT
669       st->resampler_ptr = resampler_basic_direct_single;
670 #else
671       if (st->quality>8)
672          st->resampler_ptr = resampler_basic_direct_double;
673       else
674          st->resampler_ptr = resampler_basic_direct_single;
675 #endif
676       /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
677    } else {
678       spx_int32_t i;
679       for (i=-4;i<(spx_int32_t)(st->oversample*st->filt_len+4);i++)
680          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);
681 #ifdef FIXED_POINT
682       st->resampler_ptr = resampler_basic_interpolate_single;
683 #else
684       if (st->quality>8)
685          st->resampler_ptr = resampler_basic_interpolate_double;
686       else
687          st->resampler_ptr = resampler_basic_interpolate_single;
688 #endif
689       /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
690    }
691 
692    /* Here's the place where we update the filter memory to take into account
693       the change in filter length. It's probably the messiest part of the code
694       due to handling of lots of corner cases. */
695 
696    /* Adding buffer_size to filt_len won't overflow here because filt_len
697       could be multiplied by sizeof(spx_word16_t) above. */
698    min_alloc_size = st->filt_len-1 + st->buffer_size;
699    if (min_alloc_size > st->mem_alloc_size)
700    {
701       spx_word16_t *mem;
702       if (INT_MAX/sizeof(spx_word16_t)/st->nb_channels < min_alloc_size)
703           goto fail;
704       else if (!(mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*min_alloc_size * sizeof(*mem))))
705           goto fail;
706 
707       st->mem = mem;
708       st->mem_alloc_size = min_alloc_size;
709    }
710    if (!st->started)
711    {
712       spx_uint32_t i;
713       for (i=0;i<st->nb_channels*st->mem_alloc_size;i++)
714          st->mem[i] = 0;
715       /*speex_warning("reinit filter");*/
716    } else if (st->filt_len > old_length)
717    {
718       spx_uint32_t i;
719       /* Increase the filter length */
720       /*speex_warning("increase filter size");*/
721       for (i=st->nb_channels;i--;)
722       {
723          spx_uint32_t j;
724          spx_uint32_t olen = old_length;
725          /*if (st->magic_samples[i])*/
726          {
727             /* Try and remove the magic samples as if nothing had happened */
728 
729             /* FIXME: This is wrong but for now we need it to avoid going over the array bounds */
730             olen = old_length + 2*st->magic_samples[i];
731             for (j=old_length-1+st->magic_samples[i];j--;)
732                st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]] = st->mem[i*old_alloc_size+j];
733             for (j=0;j<st->magic_samples[i];j++)
734                st->mem[i*st->mem_alloc_size+j] = 0;
735             st->magic_samples[i] = 0;
736          }
737          if (st->filt_len > olen)
738          {
739             /* If the new filter length is still bigger than the "augmented" length */
740             /* Copy data going backward */
741             for (j=0;j<olen-1;j++)
742                st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*st->mem_alloc_size+(olen-2-j)];
743             /* Then put zeros for lack of anything better */
744             for (;j<st->filt_len-1;j++)
745                st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
746             /* Adjust last_sample */
747             st->last_sample[i] += (st->filt_len - olen)/2;
748          } else {
749             /* Put back some of the magic! */
750             st->magic_samples[i] = (olen - st->filt_len)/2;
751             for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
752                st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
753          }
754       }
755    } else if (st->filt_len < old_length)
756    {
757       spx_uint32_t i;
758       /* Reduce filter length, this a bit tricky. We need to store some of the memory as "magic"
759          samples so they can be used directly as input the next time(s) */
760       for (i=0;i<st->nb_channels;i++)
761       {
762          spx_uint32_t j;
763          spx_uint32_t old_magic = st->magic_samples[i];
764          st->magic_samples[i] = (old_length - st->filt_len)/2;
765          /* We must copy some of the memory that's no longer used */
766          /* Copy data going backward */
767          for (j=0;j<st->filt_len-1+st->magic_samples[i]+old_magic;j++)
768             st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
769          st->magic_samples[i] += old_magic;
770       }
771    }
772    return RESAMPLER_ERR_SUCCESS;
773 
774 fail:
775    st->resampler_ptr = resampler_basic_zero;
776    /* st->mem may still contain consumed input samples for the filter.
777       Restore filt_len so that filt_len - 1 still points to the position after
778       the last of these samples. */
779    st->filt_len = old_length;
780    return RESAMPLER_ERR_ALLOC_FAILED;
781 }
782 
speex_resampler_init(spx_uint32_t nb_channels,spx_uint32_t in_rate,spx_uint32_t out_rate,int quality,int * err)783 EXPORT SpeexResamplerState *speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
784 {
785    return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality, err);
786 }
787 
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)788 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)
789 {
790    spx_uint32_t i;
791    SpeexResamplerState *st;
792    int filter_err;
793 
794    if (quality > 10 || quality < 0)
795    {
796       if (err)
797          *err = RESAMPLER_ERR_INVALID_ARG;
798       return NULL;
799    }
800    st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
801    st->initialised = 0;
802    st->started = 0;
803    st->in_rate = 0;
804    st->out_rate = 0;
805    st->num_rate = 0;
806    st->den_rate = 0;
807    st->quality = -1;
808    st->sinc_table_length = 0;
809    st->mem_alloc_size = 0;
810    st->filt_len = 0;
811    st->mem = 0;
812    st->resampler_ptr = 0;
813 
814    st->cutoff = 1.f;
815    st->nb_channels = nb_channels;
816    st->in_stride = 1;
817    st->out_stride = 1;
818 
819    st->buffer_size = 160;
820 
821    /* Per channel data */
822    st->last_sample = (spx_int32_t*)speex_alloc(nb_channels*sizeof(spx_int32_t));
823    st->magic_samples = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(spx_uint32_t));
824    st->samp_frac_num = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(spx_uint32_t));
825    for (i=0;i<nb_channels;i++)
826    {
827       st->last_sample[i] = 0;
828       st->magic_samples[i] = 0;
829       st->samp_frac_num[i] = 0;
830    }
831 
832    speex_resampler_set_quality(st, quality);
833    speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);
834 
835    filter_err = update_filter(st);
836    if (filter_err == RESAMPLER_ERR_SUCCESS)
837    {
838       st->initialised = 1;
839    } else {
840       speex_resampler_destroy(st);
841       st = NULL;
842    }
843    if (err)
844       *err = filter_err;
845 
846    return st;
847 }
848 
speex_resampler_destroy(SpeexResamplerState * st)849 EXPORT void speex_resampler_destroy(SpeexResamplerState *st)
850 {
851    speex_free(st->mem);
852    speex_free(st->sinc_table);
853    speex_free(st->last_sample);
854    speex_free(st->magic_samples);
855    speex_free(st->samp_frac_num);
856    speex_free(st);
857 }
858 
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)859 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)
860 {
861    int j=0;
862    const int N = st->filt_len;
863    int out_sample = 0;
864    spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
865    spx_uint32_t ilen;
866 
867    st->started = 1;
868 
869    /* Call the right resampler through the function ptr */
870    out_sample = st->resampler_ptr(st, channel_index, mem, in_len, out, out_len);
871 
872    if (st->last_sample[channel_index] < (spx_int32_t)*in_len)
873       *in_len = st->last_sample[channel_index];
874    *out_len = out_sample;
875    st->last_sample[channel_index] -= *in_len;
876 
877    ilen = *in_len;
878 
879    for(j=0;j<N-1;++j)
880      mem[j] = mem[j+ilen];
881 
882    return RESAMPLER_ERR_SUCCESS;
883 }
884 
speex_resampler_magic(SpeexResamplerState * st,spx_uint32_t channel_index,spx_word16_t ** out,spx_uint32_t out_len)885 static int speex_resampler_magic(SpeexResamplerState *st, spx_uint32_t channel_index, spx_word16_t **out, spx_uint32_t out_len) {
886    spx_uint32_t tmp_in_len = st->magic_samples[channel_index];
887    spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
888    const int N = st->filt_len;
889 
890    speex_resampler_process_native(st, channel_index, &tmp_in_len, *out, &out_len);
891 
892    st->magic_samples[channel_index] -= tmp_in_len;
893 
894    /* If we couldn't process all "magic" input samples, save the rest for next time */
895    if (st->magic_samples[channel_index])
896    {
897       spx_uint32_t i;
898       for (i=0;i<st->magic_samples[channel_index];i++)
899          mem[N-1+i]=mem[N-1+i+tmp_in_len];
900    }
901    *out += out_len*st->out_stride;
902    return out_len;
903 }
904 
905 #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)906 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)
907 #else
908 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)
909 #endif
910 {
911    int j;
912    spx_uint32_t ilen = *in_len;
913    spx_uint32_t olen = *out_len;
914    spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
915    const int filt_offs = st->filt_len - 1;
916    const spx_uint32_t xlen = st->mem_alloc_size - filt_offs;
917    const int istride = st->in_stride;
918 
919    if (st->magic_samples[channel_index])
920       olen -= speex_resampler_magic(st, channel_index, &out, olen);
921    if (! st->magic_samples[channel_index]) {
922       while (ilen && olen) {
923         spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
924         spx_uint32_t ochunk = olen;
925 
926         if (in) {
927            for(j=0;j<ichunk;++j)
928               x[j+filt_offs]=in[j*istride];
929         } else {
930           for(j=0;j<ichunk;++j)
931             x[j+filt_offs]=0;
932         }
933         speex_resampler_process_native(st, channel_index, &ichunk, out, &ochunk);
934         ilen -= ichunk;
935         olen -= ochunk;
936         out += ochunk * st->out_stride;
937         if (in)
938            in += ichunk * istride;
939       }
940    }
941    *in_len -= ilen;
942    *out_len -= olen;
943    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
944 }
945 
946 #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)947 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)
948 #else
949 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)
950 #endif
951 {
952    int j;
953    const int istride_save = st->in_stride;
954    const int ostride_save = st->out_stride;
955    spx_uint32_t ilen = *in_len;
956    spx_uint32_t olen = *out_len;
957    spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
958    const spx_uint32_t xlen = st->mem_alloc_size - (st->filt_len - 1);
959 #ifdef VAR_ARRAYS
960    const unsigned int ylen = (olen < FIXED_STACK_ALLOC) ? olen : FIXED_STACK_ALLOC;
961    VARDECL(spx_word16_t *ystack);
962    ALLOC(ystack, ylen, spx_word16_t);
963 #else
964    const unsigned int ylen = FIXED_STACK_ALLOC;
965    spx_word16_t ystack[FIXED_STACK_ALLOC];
966 #endif
967 
968    st->out_stride = 1;
969 
970    while (ilen && olen) {
971      spx_word16_t *y = ystack;
972      spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
973      spx_uint32_t ochunk = (olen > ylen) ? ylen : olen;
974      spx_uint32_t omagic = 0;
975 
976      if (st->magic_samples[channel_index]) {
977        omagic = speex_resampler_magic(st, channel_index, &y, ochunk);
978        ochunk -= omagic;
979        olen -= omagic;
980      }
981      if (! st->magic_samples[channel_index]) {
982        if (in) {
983          for(j=0;j<ichunk;++j)
984 #ifdef FIXED_POINT
985            x[j+st->filt_len-1]=WORD2INT(in[j*istride_save]);
986 #else
987            x[j+st->filt_len-1]=in[j*istride_save];
988 #endif
989        } else {
990          for(j=0;j<ichunk;++j)
991            x[j+st->filt_len-1]=0;
992        }
993 
994        speex_resampler_process_native(st, channel_index, &ichunk, y, &ochunk);
995      } else {
996        ichunk = 0;
997        ochunk = 0;
998      }
999 
1000      for (j=0;j<ochunk+omagic;++j)
1001 #ifdef FIXED_POINT
1002         out[j*ostride_save] = ystack[j];
1003 #else
1004         out[j*ostride_save] = WORD2INT(ystack[j]);
1005 #endif
1006 
1007      ilen -= ichunk;
1008      olen -= ochunk;
1009      out += (ochunk+omagic) * ostride_save;
1010      if (in)
1011        in += ichunk * istride_save;
1012    }
1013    st->out_stride = ostride_save;
1014    *in_len -= ilen;
1015    *out_len -= olen;
1016 
1017    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1018 }
1019 
speex_resampler_process_interleaved_float(SpeexResamplerState * st,const float * in,spx_uint32_t * in_len,float * out,spx_uint32_t * out_len)1020 EXPORT int speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
1021 {
1022    spx_uint32_t i;
1023    int istride_save, ostride_save;
1024    spx_uint32_t bak_out_len = *out_len;
1025    spx_uint32_t bak_in_len = *in_len;
1026    istride_save = st->in_stride;
1027    ostride_save = st->out_stride;
1028    st->in_stride = st->out_stride = st->nb_channels;
1029    for (i=0;i<st->nb_channels;i++)
1030    {
1031       *out_len = bak_out_len;
1032       *in_len = bak_in_len;
1033       if (in != NULL)
1034          speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
1035       else
1036          speex_resampler_process_float(st, i, NULL, in_len, out+i, out_len);
1037    }
1038    st->in_stride = istride_save;
1039    st->out_stride = ostride_save;
1040    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1041 }
1042 
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)1043 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)
1044 {
1045    spx_uint32_t i;
1046    int istride_save, ostride_save;
1047    spx_uint32_t bak_out_len = *out_len;
1048    spx_uint32_t bak_in_len = *in_len;
1049    istride_save = st->in_stride;
1050    ostride_save = st->out_stride;
1051    st->in_stride = st->out_stride = st->nb_channels;
1052    for (i=0;i<st->nb_channels;i++)
1053    {
1054       *out_len = bak_out_len;
1055       *in_len = bak_in_len;
1056       if (in != NULL)
1057          speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
1058       else
1059          speex_resampler_process_int(st, i, NULL, in_len, out+i, out_len);
1060    }
1061    st->in_stride = istride_save;
1062    st->out_stride = ostride_save;
1063    return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1064 }
1065 
speex_resampler_set_rate(SpeexResamplerState * st,spx_uint32_t in_rate,spx_uint32_t out_rate)1066 EXPORT int speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)
1067 {
1068    return speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
1069 }
1070 
speex_resampler_get_rate(SpeexResamplerState * st,spx_uint32_t * in_rate,spx_uint32_t * out_rate)1071 EXPORT void speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)
1072 {
1073    *in_rate = st->in_rate;
1074    *out_rate = st->out_rate;
1075 }
1076 
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)1077 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)
1078 {
1079    spx_uint32_t fact;
1080    spx_uint32_t old_den;
1081    spx_uint32_t i;
1082    if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
1083       return RESAMPLER_ERR_SUCCESS;
1084 
1085    old_den = st->den_rate;
1086    st->in_rate = in_rate;
1087    st->out_rate = out_rate;
1088    st->num_rate = ratio_num;
1089    st->den_rate = ratio_den;
1090    /* FIXME: This is terribly inefficient, but who cares (at least for now)? */
1091    for (fact=2;fact<=IMIN(st->num_rate, st->den_rate);fact++)
1092    {
1093       while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
1094       {
1095          st->num_rate /= fact;
1096          st->den_rate /= fact;
1097       }
1098    }
1099 
1100    if (old_den > 0)
1101    {
1102       for (i=0;i<st->nb_channels;i++)
1103       {
1104          st->samp_frac_num[i]=st->samp_frac_num[i]*st->den_rate/old_den;
1105          /* Safety net */
1106          if (st->samp_frac_num[i] >= st->den_rate)
1107             st->samp_frac_num[i] = st->den_rate-1;
1108       }
1109    }
1110 
1111    if (st->initialised)
1112       return update_filter(st);
1113    return RESAMPLER_ERR_SUCCESS;
1114 }
1115 
speex_resampler_get_ratio(SpeexResamplerState * st,spx_uint32_t * ratio_num,spx_uint32_t * ratio_den)1116 EXPORT void speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)
1117 {
1118    *ratio_num = st->num_rate;
1119    *ratio_den = st->den_rate;
1120 }
1121 
speex_resampler_set_quality(SpeexResamplerState * st,int quality)1122 EXPORT int speex_resampler_set_quality(SpeexResamplerState *st, int quality)
1123 {
1124    if (quality > 10 || quality < 0)
1125       return RESAMPLER_ERR_INVALID_ARG;
1126    if (st->quality == quality)
1127       return RESAMPLER_ERR_SUCCESS;
1128    st->quality = quality;
1129    if (st->initialised)
1130       return update_filter(st);
1131    return RESAMPLER_ERR_SUCCESS;
1132 }
1133 
speex_resampler_get_quality(SpeexResamplerState * st,int * quality)1134 EXPORT void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
1135 {
1136    *quality = st->quality;
1137 }
1138 
speex_resampler_set_input_stride(SpeexResamplerState * st,spx_uint32_t stride)1139 EXPORT void speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)
1140 {
1141    st->in_stride = stride;
1142 }
1143 
speex_resampler_get_input_stride(SpeexResamplerState * st,spx_uint32_t * stride)1144 EXPORT void speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)
1145 {
1146    *stride = st->in_stride;
1147 }
1148 
speex_resampler_set_output_stride(SpeexResamplerState * st,spx_uint32_t stride)1149 EXPORT void speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)
1150 {
1151    st->out_stride = stride;
1152 }
1153 
speex_resampler_get_output_stride(SpeexResamplerState * st,spx_uint32_t * stride)1154 EXPORT void speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)
1155 {
1156    *stride = st->out_stride;
1157 }
1158 
speex_resampler_get_input_latency(SpeexResamplerState * st)1159 EXPORT int speex_resampler_get_input_latency(SpeexResamplerState *st)
1160 {
1161   return st->filt_len / 2;
1162 }
1163 
speex_resampler_get_output_latency(SpeexResamplerState * st)1164 EXPORT int speex_resampler_get_output_latency(SpeexResamplerState *st)
1165 {
1166   return ((st->filt_len / 2) * st->den_rate + (st->num_rate >> 1)) / st->num_rate;
1167 }
1168 
speex_resampler_skip_zeros(SpeexResamplerState * st)1169 EXPORT int speex_resampler_skip_zeros(SpeexResamplerState *st)
1170 {
1171    spx_uint32_t i;
1172    for (i=0;i<st->nb_channels;i++)
1173       st->last_sample[i] = st->filt_len/2;
1174    return RESAMPLER_ERR_SUCCESS;
1175 }
1176 
speex_resampler_reset_mem(SpeexResamplerState * st)1177 EXPORT int speex_resampler_reset_mem(SpeexResamplerState *st)
1178 {
1179    spx_uint32_t i;
1180    for (i=0;i<st->nb_channels;i++)
1181    {
1182       st->last_sample[i] = 0;
1183       st->magic_samples[i] = 0;
1184       st->samp_frac_num[i] = 0;
1185    }
1186    for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
1187       st->mem[i] = 0;
1188    return RESAMPLER_ERR_SUCCESS;
1189 }
1190 
speex_resampler_strerror(int err)1191 EXPORT const char *speex_resampler_strerror(int err)
1192 {
1193    switch (err)
1194    {
1195       case RESAMPLER_ERR_SUCCESS:
1196          return "Success.";
1197       case RESAMPLER_ERR_ALLOC_FAILED:
1198          return "Memory allocation failed.";
1199       case RESAMPLER_ERR_BAD_STATE:
1200          return "Bad resampler state.";
1201       case RESAMPLER_ERR_INVALID_ARG:
1202          return "Invalid argument.";
1203       case RESAMPLER_ERR_PTR_OVERLAP:
1204          return "Input and output buffers overlap.";
1205       default:
1206          return "Unknown error. Bad error code or strange version mismatch.";
1207    }
1208 }
1209