1 /* Copyright (C) 2007-2008 Jean-Marc Valin
2 Copyright (C) 2008 Thorvald Natvig
3
4 File: resample.c
5 Arbitrary resampling code
6
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are
9 met:
10
11 1. Redistributions of source code must retain the above copyright notice,
12 this list of conditions and the following disclaimer.
13
14 2. Redistributions in binary form must reproduce the above copyright
15 notice, this list of conditions and the following disclaimer in the
16 documentation and/or other materials provided with the distribution.
17
18 3. The name of the author may not be used to endorse or promote products
19 derived from this software without specific prior written permission.
20
21 THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29 STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 POSSIBILITY OF SUCH DAMAGE.
32 */
33
34 /*
35 The design goals of this code are:
36 - Very fast algorithm
37 - SIMD-friendly algorithm
38 - Low memory requirement
39 - Good *perceptual* quality (and not best SNR)
40
41 Warning: This resampler is relatively new. Although I think I got rid of
42 all the major bugs and I don't expect the API to change anymore, there
43 may be something I've missed. So use with caution.
44
45 This algorithm is based on this original resampling algorithm:
46 Smith, Julius O. Digital Audio Resampling Home Page
47 Center for Computer Research in Music and Acoustics (CCRMA),
48 Stanford University, 2007.
49 Web published at https://ccrma.stanford.edu/~jos/resample/.
50
51 There is one main difference, though. This resampler uses cubic
52 interpolation instead of linear interpolation in the above paper. This
53 makes the table much smaller and makes it possible to compute that table
54 on a per-stream basis. In turn, being able to tweak the table for each
55 stream makes it possible to both reduce complexity on simple ratios
56 (e.g. 2/3), and get rid of the rounding operations in the inner loop.
57 The latter both reduces CPU time and makes the algorithm more SIMD-friendly.
58 */
59
60 #ifdef HAVE_CONFIG_H
61 #include "config.h"
62 #endif
63
64 #ifdef OUTSIDE_SPEEX
65 #include <stdlib.h>
speex_alloc(int size)66 static void *speex_alloc(int size) {return calloc(size,1);}
speex_realloc(void * ptr,int size)67 static void *speex_realloc(void *ptr, int size) {return realloc(ptr, size);}
speex_free(void * ptr)68 static void speex_free(void *ptr) {free(ptr);}
69 #ifndef EXPORT
70 #define EXPORT
71 #endif
72 #include "speex_resampler.h"
73 #include "arch.h"
74 #else /* OUTSIDE_SPEEX */
75
76 #include "speex/speex_resampler.h"
77 #include "arch.h"
78 #include "os_support.h"
79 #endif /* OUTSIDE_SPEEX */
80
81 #include <math.h>
82 #include <limits.h>
83
84 #ifndef M_PI
85 #define M_PI 3.14159265358979323846
86 #endif
87
88 #define IMAX(a,b) ((a) > (b) ? (a) : (b))
89 #define IMIN(a,b) ((a) < (b) ? (a) : (b))
90
91 #ifndef NULL
92 #define NULL 0
93 #endif
94
95 #ifndef UINT32_MAX
96 #define UINT32_MAX 4294967295U
97 #endif
98
99 #ifdef USE_SSE
100 #include "resample_sse.h"
101 #endif
102
103 #ifdef USE_NEON
104 #include "resample_neon.h"
105 #endif
106
107 /* Numer of elements to allocate on the stack */
108 #ifdef VAR_ARRAYS
109 #define FIXED_STACK_ALLOC 8192
110 #else
111 #define FIXED_STACK_ALLOC 1024
112 #endif
113
114 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],accum[0]) + MULT16_32_Q15(interp[1],accum[1]) + MULT16_32_Q15(interp[2],accum[2]) + MULT16_32_Q15(interp[3],accum[3]);
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