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