1
/* Copyright (C) 2007-2008 Jean-Marc Valin
2
Copyright (C) 2008 Thorvald Natvig
5
Arbitrary resampling code
7
Redistribution and use in source and binary forms, with or without
8
modification, are permitted provided that the following conditions are
11
1. Redistributions of source code must retain the above copyright notice,
12
this list of conditions and the following disclaimer.
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.
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.
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.
35
The design goals of this code are:
37
- SIMD-friendly algorithm
38
- Low memory requirement
39
- Good *perceptual* quality (and not best SNR)
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.
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/.
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.
66
static void *speex_alloc (int size) {return calloc(size,1);}
67
static void *speex_realloc (void *ptr, int size) {return realloc(ptr, size);}
68
static void speex_free (void *ptr) {free(ptr);}
69
#include "speex_resampler.h"
71
#else /* OUTSIDE_SPEEX */
73
#include "speex/speex_resampler.h"
75
#include "os_support.h"
76
#endif /* OUTSIDE_SPEEX */
78
#include "stack_alloc.h"
82
#define M_PI 3.14159263
86
#define WORD2INT(x) ((x) < -32767 ? -32768 : ((x) > 32766 ? 32767 : (x)))
88
#define WORD2INT(x) ((x) < -32767.5f ? -32768 : ((x) > 32766.5f ? 32767 : floor(.5+(x))))
91
#define IMAX(a,b) ((a) > (b) ? (a) : (b))
92
#define IMIN(a,b) ((a) < (b) ? (a) : (b))
99
#include "resample_sse.h"
102
/* Numer of elements to allocate on the stack */
104
#define FIXED_STACK_ALLOC 8192
106
#define FIXED_STACK_ALLOC 1024
109
typedef int (*resampler_basic_func)(SpeexResamplerState *, spx_uint32_t , const spx_word16_t *, spx_uint32_t *, spx_word16_t *, spx_uint32_t *);
111
struct SpeexResamplerState_ {
112
spx_uint32_t in_rate;
113
spx_uint32_t out_rate;
114
spx_uint32_t num_rate;
115
spx_uint32_t den_rate;
118
spx_uint32_t nb_channels;
119
spx_uint32_t filt_len;
120
spx_uint32_t mem_alloc_size;
121
spx_uint32_t buffer_size;
125
spx_uint32_t oversample;
129
/* These are per-channel */
130
spx_int32_t *last_sample;
131
spx_uint32_t *samp_frac_num;
132
spx_uint32_t *magic_samples;
135
spx_word16_t *sinc_table;
136
spx_uint32_t sinc_table_length;
137
resampler_basic_func resampler_ptr;
143
static double kaiser12_table[68] = {
144
0.99859849, 1.00000000, 0.99859849, 0.99440475, 0.98745105, 0.97779076,
145
0.96549770, 0.95066529, 0.93340547, 0.91384741, 0.89213598, 0.86843014,
146
0.84290116, 0.81573067, 0.78710866, 0.75723148, 0.72629970, 0.69451601,
147
0.66208321, 0.62920216, 0.59606986, 0.56287762, 0.52980938, 0.49704014,
148
0.46473455, 0.43304576, 0.40211431, 0.37206735, 0.34301800, 0.31506490,
149
0.28829195, 0.26276832, 0.23854851, 0.21567274, 0.19416736, 0.17404546,
150
0.15530766, 0.13794294, 0.12192957, 0.10723616, 0.09382272, 0.08164178,
151
0.07063950, 0.06075685, 0.05193064, 0.04409466, 0.03718069, 0.03111947,
152
0.02584161, 0.02127838, 0.01736250, 0.01402878, 0.01121463, 0.00886058,
153
0.00691064, 0.00531256, 0.00401805, 0.00298291, 0.00216702, 0.00153438,
154
0.00105297, 0.00069463, 0.00043489, 0.00025272, 0.00013031, 0.0000527734,
155
0.00001000, 0.00000000};
157
static double kaiser12_table[36] = {
158
0.99440475, 1.00000000, 0.99440475, 0.97779076, 0.95066529, 0.91384741,
159
0.86843014, 0.81573067, 0.75723148, 0.69451601, 0.62920216, 0.56287762,
160
0.49704014, 0.43304576, 0.37206735, 0.31506490, 0.26276832, 0.21567274,
161
0.17404546, 0.13794294, 0.10723616, 0.08164178, 0.06075685, 0.04409466,
162
0.03111947, 0.02127838, 0.01402878, 0.00886058, 0.00531256, 0.00298291,
163
0.00153438, 0.00069463, 0.00025272, 0.0000527734, 0.00000500, 0.00000000};
165
static double kaiser10_table[36] = {
166
0.99537781, 1.00000000, 0.99537781, 0.98162644, 0.95908712, 0.92831446,
167
0.89005583, 0.84522401, 0.79486424, 0.74011713, 0.68217934, 0.62226347,
168
0.56155915, 0.50119680, 0.44221549, 0.38553619, 0.33194107, 0.28205962,
169
0.23636152, 0.19515633, 0.15859932, 0.12670280, 0.09935205, 0.07632451,
170
0.05731132, 0.04193980, 0.02979584, 0.02044510, 0.01345224, 0.00839739,
171
0.00488951, 0.00257636, 0.00115101, 0.00035515, 0.00000000, 0.00000000};
173
static double kaiser8_table[36] = {
174
0.99635258, 1.00000000, 0.99635258, 0.98548012, 0.96759014, 0.94302200,
175
0.91223751, 0.87580811, 0.83439927, 0.78875245, 0.73966538, 0.68797126,
176
0.63451750, 0.58014482, 0.52566725, 0.47185369, 0.41941150, 0.36897272,
177
0.32108304, 0.27619388, 0.23465776, 0.19672670, 0.16255380, 0.13219758,
178
0.10562887, 0.08273982, 0.06335451, 0.04724088, 0.03412321, 0.02369490,
179
0.01563093, 0.00959968, 0.00527363, 0.00233883, 0.00050000, 0.00000000};
181
static double kaiser6_table[36] = {
182
0.99733006, 1.00000000, 0.99733006, 0.98935595, 0.97618418, 0.95799003,
183
0.93501423, 0.90755855, 0.87598009, 0.84068475, 0.80211977, 0.76076565,
184
0.71712752, 0.67172623, 0.62508937, 0.57774224, 0.53019925, 0.48295561,
185
0.43647969, 0.39120616, 0.34752997, 0.30580127, 0.26632152, 0.22934058,
186
0.19505503, 0.16360756, 0.13508755, 0.10953262, 0.08693120, 0.06722600,
187
0.05031820, 0.03607231, 0.02432151, 0.01487334, 0.00752000, 0.00000000};
194
static struct FuncDef _KAISER12 = {kaiser12_table, 64};
195
#define KAISER12 (&_KAISER12)
196
/*static struct FuncDef _KAISER12 = {kaiser12_table, 32};
197
#define KAISER12 (&_KAISER12)*/
198
static struct FuncDef _KAISER10 = {kaiser10_table, 32};
199
#define KAISER10 (&_KAISER10)
200
static struct FuncDef _KAISER8 = {kaiser8_table, 32};
201
#define KAISER8 (&_KAISER8)
202
static struct FuncDef _KAISER6 = {kaiser6_table, 32};
203
#define KAISER6 (&_KAISER6)
205
struct QualityMapping {
208
float downsample_bandwidth;
209
float upsample_bandwidth;
210
struct FuncDef *window_func;
214
/* This table maps conversion quality to internal parameters. There are two
215
reasons that explain why the up-sampling bandwidth is larger than the
216
down-sampling bandwidth:
217
1) When up-sampling, we can assume that the spectrum is already attenuated
218
close to the Nyquist rate (from an A/D or a previous resampling filter)
219
2) Any aliasing that occurs very close to the Nyquist rate will be masked
220
by the sinusoids/noise just below the Nyquist rate (guaranteed only for
223
static const struct QualityMapping quality_map[11] = {
224
{ 8, 4, 0.830f, 0.860f, KAISER6 }, /* Q0 */
225
{ 16, 4, 0.850f, 0.880f, KAISER6 }, /* Q1 */
226
{ 32, 4, 0.882f, 0.910f, KAISER6 }, /* Q2 */ /* 82.3% cutoff ( ~60 dB stop) 6 */
227
{ 48, 8, 0.895f, 0.917f, KAISER8 }, /* Q3 */ /* 84.9% cutoff ( ~80 dB stop) 8 */
228
{ 64, 8, 0.921f, 0.940f, KAISER8 }, /* Q4 */ /* 88.7% cutoff ( ~80 dB stop) 8 */
229
{ 80, 16, 0.922f, 0.940f, KAISER10}, /* Q5 */ /* 89.1% cutoff (~100 dB stop) 10 */
230
{ 96, 16, 0.940f, 0.945f, KAISER10}, /* Q6 */ /* 91.5% cutoff (~100 dB stop) 10 */
231
{128, 16, 0.950f, 0.950f, KAISER10}, /* Q7 */ /* 93.1% cutoff (~100 dB stop) 10 */
232
{160, 16, 0.960f, 0.960f, KAISER10}, /* Q8 */ /* 94.5% cutoff (~100 dB stop) 10 */
233
{192, 32, 0.968f, 0.968f, KAISER12}, /* Q9 */ /* 95.5% cutoff (~100 dB stop) 10 */
234
{256, 32, 0.975f, 0.975f, KAISER12}, /* Q10 */ /* 96.6% cutoff (~100 dB stop) 10 */
236
/*8,24,40,56,80,104,128,160,200,256,320*/
237
static double compute_func(float x, struct FuncDef *func)
242
y = x*func->oversample;
245
/* CSE with handle the repeated powers */
246
interp[3] = -0.1666666667*frac + 0.1666666667*(frac*frac*frac);
247
interp[2] = frac + 0.5*(frac*frac) - 0.5*(frac*frac*frac);
248
/*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
249
interp[0] = -0.3333333333*frac + 0.5*(frac*frac) - 0.1666666667*(frac*frac*frac);
250
/* Just to make sure we don't have rounding problems */
251
interp[1] = 1.f-interp[3]-interp[2]-interp[0];
253
/*sum = frac*accum[1] + (1-frac)*accum[2];*/
254
return interp[0]*func->table[ind] + interp[1]*func->table[ind+1] + interp[2]*func->table[ind+2] + interp[3]*func->table[ind+3];
259
int main(int argc, char **argv)
264
printf ("%f\n", compute_func(i/256., KAISER12));
271
/* The slow way of computing a sinc for the table. Should improve that some day */
272
static spx_word16_t sinc(float cutoff, float x, int N, struct FuncDef *window_func)
274
/*fprintf (stderr, "%f ", x);*/
275
float xx = x * cutoff;
277
return WORD2INT(32768.*cutoff);
278
else if (fabs(x) > .5f*N)
280
/*FIXME: Can it really be any slower than this? */
281
return WORD2INT(32768.*cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func));
284
/* The slow way of computing a sinc for the table. Should improve that some day */
285
static spx_word16_t sinc(float cutoff, float x, int N, struct FuncDef *window_func)
287
/*fprintf (stderr, "%f ", x);*/
288
float xx = x * cutoff;
291
else if (fabs(x) > .5*N)
293
/*FIXME: Can it really be any slower than this? */
294
return cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func);
299
static void cubic_coef(spx_word16_t x, spx_word16_t interp[4])
301
/* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
302
but I know it's MMSE-optimal on a sinc */
304
x2 = MULT16_16_P15(x, x);
305
x3 = MULT16_16_P15(x, x2);
306
interp[0] = PSHR32(MULT16_16(QCONST16(-0.16667f, 15),x) + MULT16_16(QCONST16(0.16667f, 15),x3),15);
307
interp[1] = EXTRACT16(EXTEND32(x) + SHR32(SUB32(EXTEND32(x2),EXTEND32(x3)),1));
308
interp[3] = PSHR32(MULT16_16(QCONST16(-0.33333f, 15),x) + MULT16_16(QCONST16(.5f,15),x2) - MULT16_16(QCONST16(0.16667f, 15),x3),15);
309
/* Just to make sure we don't have rounding problems */
310
interp[2] = Q15_ONE-interp[0]-interp[1]-interp[3];
315
static void cubic_coef(spx_word16_t frac, spx_word16_t interp[4])
317
/* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
318
but I know it's MMSE-optimal on a sinc */
319
interp[0] = -0.16667f*frac + 0.16667f*frac*frac*frac;
320
interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
321
/*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
322
interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
323
/* Just to make sure we don't have rounding problems */
324
interp[2] = 1.-interp[0]-interp[1]-interp[3];
328
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)
330
const int N = st->filt_len;
332
int last_sample = st->last_sample[channel_index];
333
spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
334
const spx_word16_t *sinc_table = st->sinc_table;
335
const int out_stride = st->out_stride;
336
const int int_advance = st->int_advance;
337
const int frac_advance = st->frac_advance;
338
const spx_uint32_t den_rate = st->den_rate;
342
while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
344
const spx_word16_t *sinc = & sinc_table[samp_frac_num*N];
345
const spx_word16_t *iptr = & in[last_sample];
347
#ifndef OVERRIDE_INNER_PRODUCT_SINGLE
348
float accum[4] = {0,0,0,0};
351
accum[0] += sinc[j]*iptr[j];
352
accum[1] += sinc[j+1]*iptr[j+1];
353
accum[2] += sinc[j+2]*iptr[j+2];
354
accum[3] += sinc[j+3]*iptr[j+3];
356
sum = accum[0] + accum[1] + accum[2] + accum[3];
358
sum = inner_product_single(sinc, iptr, N);
361
out[out_stride * out_sample++] = PSHR32(sum, 15);
362
last_sample += int_advance;
363
samp_frac_num += frac_advance;
364
if (samp_frac_num >= den_rate)
366
samp_frac_num -= den_rate;
371
st->last_sample[channel_index] = last_sample;
372
st->samp_frac_num[channel_index] = samp_frac_num;
378
/* This is the same as the previous function, except with a double-precision accumulator */
379
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)
381
const int N = st->filt_len;
383
int last_sample = st->last_sample[channel_index];
384
spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
385
const spx_word16_t *sinc_table = st->sinc_table;
386
const int out_stride = st->out_stride;
387
const int int_advance = st->int_advance;
388
const int frac_advance = st->frac_advance;
389
const spx_uint32_t den_rate = st->den_rate;
393
while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
395
const spx_word16_t *sinc = & sinc_table[samp_frac_num*N];
396
const spx_word16_t *iptr = & in[last_sample];
398
#ifndef OVERRIDE_INNER_PRODUCT_DOUBLE
399
double accum[4] = {0,0,0,0};
402
accum[0] += sinc[j]*iptr[j];
403
accum[1] += sinc[j+1]*iptr[j+1];
404
accum[2] += sinc[j+2]*iptr[j+2];
405
accum[3] += sinc[j+3]*iptr[j+3];
407
sum = accum[0] + accum[1] + accum[2] + accum[3];
409
sum = inner_product_double(sinc, iptr, N);
412
out[out_stride * out_sample++] = PSHR32(sum, 15);
413
last_sample += int_advance;
414
samp_frac_num += frac_advance;
415
if (samp_frac_num >= den_rate)
417
samp_frac_num -= den_rate;
422
st->last_sample[channel_index] = last_sample;
423
st->samp_frac_num[channel_index] = samp_frac_num;
428
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)
430
const int N = st->filt_len;
432
int last_sample = st->last_sample[channel_index];
433
spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
434
const int out_stride = st->out_stride;
435
const int int_advance = st->int_advance;
436
const int frac_advance = st->frac_advance;
437
const spx_uint32_t den_rate = st->den_rate;
441
while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
443
const spx_word16_t *iptr = & in[last_sample];
445
const int offset = samp_frac_num*st->oversample/st->den_rate;
447
const spx_word16_t frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
449
const spx_word16_t frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
451
spx_word16_t interp[4];
454
#ifndef OVERRIDE_INTERPOLATE_PRODUCT_SINGLE
455
spx_word32_t accum[4] = {0,0,0,0};
458
const spx_word16_t curr_in=iptr[j];
459
accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
460
accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
461
accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
462
accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
465
cubic_coef(frac, interp);
466
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]);
468
cubic_coef(frac, interp);
469
sum = interpolate_product_single(iptr, st->sinc_table + st->oversample + 4 - offset - 2, N, st->oversample, interp);
472
out[out_stride * out_sample++] = PSHR32(sum,15);
473
last_sample += int_advance;
474
samp_frac_num += frac_advance;
475
if (samp_frac_num >= den_rate)
477
samp_frac_num -= den_rate;
482
st->last_sample[channel_index] = last_sample;
483
st->samp_frac_num[channel_index] = samp_frac_num;
489
/* This is the same as the previous function, except with a double-precision accumulator */
490
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)
492
const int N = st->filt_len;
494
int last_sample = st->last_sample[channel_index];
495
spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
496
const int out_stride = st->out_stride;
497
const int int_advance = st->int_advance;
498
const int frac_advance = st->frac_advance;
499
const spx_uint32_t den_rate = st->den_rate;
503
while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
505
const spx_word16_t *iptr = & in[last_sample];
507
const int offset = samp_frac_num*st->oversample/st->den_rate;
509
const spx_word16_t frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
511
const spx_word16_t frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
513
spx_word16_t interp[4];
516
#ifndef OVERRIDE_INTERPOLATE_PRODUCT_DOUBLE
517
double accum[4] = {0,0,0,0};
520
const double curr_in=iptr[j];
521
accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
522
accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
523
accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
524
accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
527
cubic_coef(frac, interp);
528
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]);
530
cubic_coef(frac, interp);
531
sum = interpolate_product_double(iptr, st->sinc_table + st->oversample + 4 - offset - 2, N, st->oversample, interp);
534
out[out_stride * out_sample++] = PSHR32(sum,15);
535
last_sample += int_advance;
536
samp_frac_num += frac_advance;
537
if (samp_frac_num >= den_rate)
539
samp_frac_num -= den_rate;
544
st->last_sample[channel_index] = last_sample;
545
st->samp_frac_num[channel_index] = samp_frac_num;
550
static void update_filter(SpeexResamplerState *st)
552
spx_uint32_t old_length;
554
old_length = st->filt_len;
555
st->oversample = quality_map[st->quality].oversample;
556
st->filt_len = quality_map[st->quality].base_length;
558
if (st->num_rate > st->den_rate)
561
st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
562
/* FIXME: divide the numerator and denominator by a certain amount if they're too large */
563
st->filt_len = st->filt_len*st->num_rate / st->den_rate;
564
/* Round down to make sure we have a multiple of 4 */
565
st->filt_len &= (~0x3);
566
if (2*st->den_rate < st->num_rate)
567
st->oversample >>= 1;
568
if (4*st->den_rate < st->num_rate)
569
st->oversample >>= 1;
570
if (8*st->den_rate < st->num_rate)
571
st->oversample >>= 1;
572
if (16*st->den_rate < st->num_rate)
573
st->oversample >>= 1;
574
if (st->oversample < 1)
578
st->cutoff = quality_map[st->quality].upsample_bandwidth;
581
/* Choose the resampling type that requires the least amount of memory */
582
if (st->den_rate <= st->oversample)
586
st->sinc_table = (spx_word16_t *)speex_alloc(st->filt_len*st->den_rate*sizeof(spx_word16_t));
587
else if (st->sinc_table_length < st->filt_len*st->den_rate)
589
st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,st->filt_len*st->den_rate*sizeof(spx_word16_t));
590
st->sinc_table_length = st->filt_len*st->den_rate;
592
for (i=0;i<st->den_rate;i++)
595
for (j=0;j<st->filt_len;j++)
597
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);
601
st->resampler_ptr = resampler_basic_direct_single;
604
st->resampler_ptr = resampler_basic_direct_double;
606
st->resampler_ptr = resampler_basic_direct_single;
608
/*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
612
st->sinc_table = (spx_word16_t *)speex_alloc((st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
613
else if (st->sinc_table_length < st->filt_len*st->oversample+8)
615
st->sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,(st->filt_len*st->oversample+8)*sizeof(spx_word16_t));
616
st->sinc_table_length = st->filt_len*st->oversample+8;
618
for (i=-4;i<(spx_int32_t)(st->oversample*st->filt_len+4);i++)
619
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);
621
st->resampler_ptr = resampler_basic_interpolate_single;
624
st->resampler_ptr = resampler_basic_interpolate_double;
626
st->resampler_ptr = resampler_basic_interpolate_single;
628
/*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
630
st->int_advance = st->num_rate/st->den_rate;
631
st->frac_advance = st->num_rate%st->den_rate;
634
/* Here's the place where we update the filter memory to take into account
635
the change in filter length. It's probably the messiest part of the code
636
due to handling of lots of corner cases. */
640
st->mem_alloc_size = st->filt_len-1 + st->buffer_size;
641
st->mem = (spx_word16_t*)speex_alloc(st->nb_channels*st->mem_alloc_size * sizeof(spx_word16_t));
642
for (i=0;i<st->nb_channels*st->mem_alloc_size;i++)
644
/*speex_warning("init filter");*/
645
} else if (!st->started)
648
st->mem_alloc_size = st->filt_len-1 + st->buffer_size;
649
st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*st->mem_alloc_size * sizeof(spx_word16_t));
650
for (i=0;i<st->nb_channels*st->mem_alloc_size;i++)
652
/*speex_warning("reinit filter");*/
653
} else if (st->filt_len > old_length)
656
/* Increase the filter length */
657
/*speex_warning("increase filter size");*/
658
int old_alloc_size = st->mem_alloc_size;
659
if ((st->filt_len-1 + st->buffer_size) > st->mem_alloc_size)
661
st->mem_alloc_size = st->filt_len-1 + st->buffer_size;
662
st->mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*st->mem_alloc_size * sizeof(spx_word16_t));
664
for (i=st->nb_channels-1;i>=0;i--)
667
spx_uint32_t olen = old_length;
668
/*if (st->magic_samples[i])*/
670
/* Try and remove the magic samples as if nothing had happened */
672
/* FIXME: This is wrong but for now we need it to avoid going over the array bounds */
673
olen = old_length + 2*st->magic_samples[i];
674
for (j=old_length-2+st->magic_samples[i];j>=0;j--)
675
st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]] = st->mem[i*old_alloc_size+j];
676
for (j=0;j<st->magic_samples[i];j++)
677
st->mem[i*st->mem_alloc_size+j] = 0;
678
st->magic_samples[i] = 0;
680
if (st->filt_len > olen)
682
/* If the new filter length is still bigger than the "augmented" length */
683
/* Copy data going backward */
684
for (j=0;j<olen-1;j++)
685
st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*st->mem_alloc_size+(olen-2-j)];
686
/* Then put zeros for lack of anything better */
687
for (;j<st->filt_len-1;j++)
688
st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
689
/* Adjust last_sample */
690
st->last_sample[i] += (st->filt_len - olen)/2;
692
/* Put back some of the magic! */
693
st->magic_samples[i] = (olen - st->filt_len)/2;
694
for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
695
st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
698
} else if (st->filt_len < old_length)
701
/* Reduce filter length, this a bit tricky. We need to store some of the memory as "magic"
702
samples so they can be used directly as input the next time(s) */
703
for (i=0;i<st->nb_channels;i++)
706
spx_uint32_t old_magic = st->magic_samples[i];
707
st->magic_samples[i] = (old_length - st->filt_len)/2;
708
/* We must copy some of the memory that's no longer used */
709
/* Copy data going backward */
710
for (j=0;j<st->filt_len-1+st->magic_samples[i]+old_magic;j++)
711
st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
712
st->magic_samples[i] += old_magic;
718
EXPORT SpeexResamplerState *speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
720
return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality, err);
723
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)
726
SpeexResamplerState *st;
727
if (quality > 10 || quality < 0)
730
*err = RESAMPLER_ERR_INVALID_ARG;
733
st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
741
st->sinc_table_length = 0;
742
st->mem_alloc_size = 0;
745
st->resampler_ptr = 0;
748
st->nb_channels = nb_channels;
753
st->buffer_size = 160;
755
st->buffer_size = 160;
758
/* Per channel data */
759
st->last_sample = (spx_int32_t*)speex_alloc(nb_channels*sizeof(int));
760
st->magic_samples = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(int));
761
st->samp_frac_num = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(int));
762
for (i=0;i<nb_channels;i++)
764
st->last_sample[i] = 0;
765
st->magic_samples[i] = 0;
766
st->samp_frac_num[i] = 0;
769
speex_resampler_set_quality(st, quality);
770
speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);
777
*err = RESAMPLER_ERR_SUCCESS;
782
EXPORT void speex_resampler_destroy(SpeexResamplerState *st)
785
speex_free(st->sinc_table);
786
speex_free(st->last_sample);
787
speex_free(st->magic_samples);
788
speex_free(st->samp_frac_num);
792
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)
795
const int N = st->filt_len;
797
spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
802
/* Call the right resampler through the function ptr */
803
out_sample = st->resampler_ptr(st, channel_index, mem, in_len, out, out_len);
805
if (st->last_sample[channel_index] < (spx_int32_t)*in_len)
806
*in_len = st->last_sample[channel_index];
807
*out_len = out_sample;
808
st->last_sample[channel_index] -= *in_len;
813
mem[j] = mem[j+ilen];
815
return RESAMPLER_ERR_SUCCESS;
818
static int speex_resampler_magic(SpeexResamplerState *st, spx_uint32_t channel_index, spx_word16_t **out, spx_uint32_t out_len) {
819
spx_uint32_t tmp_in_len = st->magic_samples[channel_index];
820
spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
821
const int N = st->filt_len;
823
speex_resampler_process_native(st, channel_index, &tmp_in_len, *out, &out_len);
825
st->magic_samples[channel_index] -= tmp_in_len;
827
/* If we couldn't process all "magic" input samples, save the rest for next time */
828
if (st->magic_samples[channel_index])
831
for (i=0;i<st->magic_samples[channel_index];i++)
832
mem[N-1+i]=mem[N-1+i+tmp_in_len];
834
*out += out_len*st->out_stride;
839
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)
841
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)
845
spx_uint32_t ilen = *in_len;
846
spx_uint32_t olen = *out_len;
847
spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
848
const int filt_offs = st->filt_len - 1;
849
const spx_uint32_t xlen = st->mem_alloc_size - filt_offs;
850
const int istride = st->in_stride;
852
if (st->magic_samples[channel_index])
853
olen -= speex_resampler_magic(st, channel_index, &out, olen);
854
if (! st->magic_samples[channel_index]) {
855
while (ilen && olen) {
856
spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
857
spx_uint32_t ochunk = olen;
860
for(j=0;j<ichunk;++j)
861
x[j+filt_offs]=in[j*istride];
863
for(j=0;j<ichunk;++j)
866
speex_resampler_process_native(st, channel_index, &ichunk, out, &ochunk);
869
out += ochunk * st->out_stride;
871
in += ichunk * istride;
876
return RESAMPLER_ERR_SUCCESS;
880
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)
882
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)
886
const int istride_save = st->in_stride;
887
const int ostride_save = st->out_stride;
888
spx_uint32_t ilen = *in_len;
889
spx_uint32_t olen = *out_len;
890
spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
891
const spx_uint32_t xlen = st->mem_alloc_size - (st->filt_len - 1);
893
const unsigned int ylen = (olen < FIXED_STACK_ALLOC) ? olen : FIXED_STACK_ALLOC;
894
VARDECL(spx_word16_t *ystack);
895
ALLOC(ystack, ylen, spx_word16_t);
897
const unsigned int ylen = FIXED_STACK_ALLOC;
898
spx_word16_t ystack[FIXED_STACK_ALLOC];
903
while (ilen && olen) {
904
spx_word16_t *y = ystack;
905
spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
906
spx_uint32_t ochunk = (olen > ylen) ? ylen : olen;
907
spx_uint32_t omagic = 0;
909
if (st->magic_samples[channel_index]) {
910
omagic = speex_resampler_magic(st, channel_index, &y, ochunk);
914
if (! st->magic_samples[channel_index]) {
916
for(j=0;j<ichunk;++j)
918
x[j+st->filt_len-1]=WORD2INT(in[j*istride_save]);
920
x[j+st->filt_len-1]=in[j*istride_save];
923
for(j=0;j<ichunk;++j)
924
x[j+st->filt_len-1]=0;
927
speex_resampler_process_native(st, channel_index, &ichunk, y, &ochunk);
933
for (j=0;j<ochunk+omagic;++j)
935
out[j*ostride_save] = ystack[j];
937
out[j*ostride_save] = WORD2INT(ystack[j]);
942
out += (ochunk+omagic) * ostride_save;
944
in += ichunk * istride_save;
946
st->out_stride = ostride_save;
950
return RESAMPLER_ERR_SUCCESS;
953
EXPORT int speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
956
int istride_save, ostride_save;
957
spx_uint32_t bak_len = *out_len;
958
istride_save = st->in_stride;
959
ostride_save = st->out_stride;
960
st->in_stride = st->out_stride = st->nb_channels;
961
for (i=0;i<st->nb_channels;i++)
965
speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
967
speex_resampler_process_float(st, i, NULL, in_len, out+i, out_len);
969
st->in_stride = istride_save;
970
st->out_stride = ostride_save;
971
return RESAMPLER_ERR_SUCCESS;
974
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)
977
int istride_save, ostride_save;
978
spx_uint32_t bak_len = *out_len;
979
istride_save = st->in_stride;
980
ostride_save = st->out_stride;
981
st->in_stride = st->out_stride = st->nb_channels;
982
for (i=0;i<st->nb_channels;i++)
986
speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
988
speex_resampler_process_int(st, i, NULL, in_len, out+i, out_len);
990
st->in_stride = istride_save;
991
st->out_stride = ostride_save;
992
return RESAMPLER_ERR_SUCCESS;
995
EXPORT int speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)
997
return speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
1000
EXPORT void speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)
1002
*in_rate = st->in_rate;
1003
*out_rate = st->out_rate;
1006
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)
1009
spx_uint32_t old_den;
1011
if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
1012
return RESAMPLER_ERR_SUCCESS;
1014
old_den = st->den_rate;
1015
st->in_rate = in_rate;
1016
st->out_rate = out_rate;
1017
st->num_rate = ratio_num;
1018
st->den_rate = ratio_den;
1019
/* FIXME: This is terribly inefficient, but who cares (at least for now)? */
1020
for (fact=2;fact<=IMIN(st->num_rate, st->den_rate);fact++)
1022
while ((st->num_rate % fact == 0) && (st->den_rate % fact == 0))
1024
st->num_rate /= fact;
1025
st->den_rate /= fact;
1031
for (i=0;i<st->nb_channels;i++)
1033
st->samp_frac_num[i]=st->samp_frac_num[i]*st->den_rate/old_den;
1035
if (st->samp_frac_num[i] >= st->den_rate)
1036
st->samp_frac_num[i] = st->den_rate-1;
1040
if (st->initialised)
1042
return RESAMPLER_ERR_SUCCESS;
1045
EXPORT void speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)
1047
*ratio_num = st->num_rate;
1048
*ratio_den = st->den_rate;
1051
EXPORT int speex_resampler_set_quality(SpeexResamplerState *st, int quality)
1053
if (quality > 10 || quality < 0)
1054
return RESAMPLER_ERR_INVALID_ARG;
1055
if (st->quality == quality)
1056
return RESAMPLER_ERR_SUCCESS;
1057
st->quality = quality;
1058
if (st->initialised)
1060
return RESAMPLER_ERR_SUCCESS;
1063
EXPORT void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
1065
*quality = st->quality;
1068
EXPORT void speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)
1070
st->in_stride = stride;
1073
EXPORT void speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)
1075
*stride = st->in_stride;
1078
EXPORT void speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)
1080
st->out_stride = stride;
1083
EXPORT void speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)
1085
*stride = st->out_stride;
1088
EXPORT int speex_resampler_get_input_latency(SpeexResamplerState *st)
1090
return st->filt_len / 2;
1093
EXPORT int speex_resampler_get_output_latency(SpeexResamplerState *st)
1095
return ((st->filt_len / 2) * st->den_rate + (st->num_rate >> 1)) / st->num_rate;
1098
EXPORT int speex_resampler_skip_zeros(SpeexResamplerState *st)
1101
for (i=0;i<st->nb_channels;i++)
1102
st->last_sample[i] = st->filt_len/2;
1103
return RESAMPLER_ERR_SUCCESS;
1106
EXPORT int speex_resampler_reset_mem(SpeexResamplerState *st)
1109
for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
1111
return RESAMPLER_ERR_SUCCESS;
1114
EXPORT const char *speex_resampler_strerror(int err)
1118
case RESAMPLER_ERR_SUCCESS:
1120
case RESAMPLER_ERR_ALLOC_FAILED:
1121
return "Memory allocation failed.";
1122
case RESAMPLER_ERR_BAD_STATE:
1123
return "Bad resampler state.";
1124
case RESAMPLER_ERR_INVALID_ARG:
1125
return "Invalid argument.";
1126
case RESAMPLER_ERR_PTR_OVERLAP:
1127
return "Input and output buffers overlap.";
1129
return "Unknown error. Bad error code or strange version mismatch.";
added
removed
sflphone-common/libs/pjproject/third_party/speex/libspeex/resample.c
