]>
Commit | Line | Data |
---|---|---|
8529da43 MG |
1 | /* |
2 | ** Copyright (c) 2002-2016, Erik de Castro Lopo <erikd@mega-nerd.com> | |
3 | ** All rights reserved. | |
4 | ** | |
5 | ** This code is released under 2-clause BSD license. Please see the | |
6 | ** file at : https://github.com/erikd/libsamplerate/blob/master/COPYING | |
7 | */ | |
8 | ||
9 | #include <stdio.h> | |
10 | #include <stdlib.h> | |
11 | #include <string.h> | |
12 | ||
13 | #include "config.h" | |
14 | #include "float_cast.h" | |
15 | #include "common.h" | |
16 | ||
17 | #define SINC_MAGIC_MARKER MAKE_MAGIC (' ', 's', 'i', 'n', 'c', ' ') | |
18 | ||
19 | /*======================================================================================== | |
20 | */ | |
21 | ||
22 | #define MAKE_INCREMENT_T(x) ((increment_t) (x)) | |
23 | ||
24 | #define SHIFT_BITS 12 | |
25 | #define FP_ONE ((double) (((increment_t) 1) << SHIFT_BITS)) | |
26 | #define INV_FP_ONE (1.0 / FP_ONE) | |
27 | ||
28 | /*======================================================================================== | |
29 | */ | |
30 | ||
31 | typedef int32_t increment_t ; | |
32 | typedef float coeff_t ; | |
33 | ||
34 | #include "fastest_coeffs.h" | |
35 | #include "mid_qual_coeffs.h" | |
36 | #include "high_qual_coeffs.h" | |
37 | ||
38 | typedef struct | |
39 | { int sinc_magic_marker ; | |
40 | ||
41 | int channels ; | |
42 | long in_count, in_used ; | |
43 | long out_count, out_gen ; | |
44 | ||
45 | int coeff_half_len, index_inc ; | |
46 | ||
47 | double src_ratio, input_index ; | |
48 | ||
49 | coeff_t const *coeffs ; | |
50 | ||
51 | int b_current, b_end, b_real_end, b_len ; | |
52 | ||
53 | /* Sure hope noone does more than 128 channels at once. */ | |
54 | double left_calc [128], right_calc [128] ; | |
55 | ||
56 | /* C99 struct flexible array. */ | |
57 | float buffer [] ; | |
58 | } SINC_FILTER ; | |
59 | ||
60 | static int sinc_multichan_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) ; | |
61 | static int sinc_hex_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) ; | |
62 | static int sinc_quad_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) ; | |
63 | static int sinc_stereo_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) ; | |
64 | static int sinc_mono_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) ; | |
65 | ||
66 | static int prepare_data (SINC_FILTER *filter, SRC_DATA *data, int half_filter_chan_len) WARN_UNUSED ; | |
67 | ||
68 | static void sinc_reset (SRC_PRIVATE *psrc) ; | |
69 | ||
70 | static inline increment_t | |
71 | double_to_fp (double x) | |
72 | { return (lrint ((x) * FP_ONE)) ; | |
73 | } /* double_to_fp */ | |
74 | ||
75 | static inline increment_t | |
76 | int_to_fp (int x) | |
77 | { return (((increment_t) (x)) << SHIFT_BITS) ; | |
78 | } /* int_to_fp */ | |
79 | ||
80 | static inline int | |
81 | fp_to_int (increment_t x) | |
82 | { return (((x) >> SHIFT_BITS)) ; | |
83 | } /* fp_to_int */ | |
84 | ||
85 | static inline increment_t | |
86 | fp_fraction_part (increment_t x) | |
87 | { return ((x) & ((((increment_t) 1) << SHIFT_BITS) - 1)) ; | |
88 | } /* fp_fraction_part */ | |
89 | ||
90 | static inline double | |
91 | fp_to_double (increment_t x) | |
92 | { return fp_fraction_part (x) * INV_FP_ONE ; | |
93 | } /* fp_to_double */ | |
94 | ||
95 | ||
96 | /*---------------------------------------------------------------------------------------- | |
97 | */ | |
98 | ||
99 | const char* | |
100 | sinc_get_name (int src_enum) | |
101 | { | |
102 | switch (src_enum) | |
103 | { case SRC_SINC_BEST_QUALITY : | |
104 | return "Best Sinc Interpolator" ; | |
105 | ||
106 | case SRC_SINC_MEDIUM_QUALITY : | |
107 | return "Medium Sinc Interpolator" ; | |
108 | ||
109 | case SRC_SINC_FASTEST : | |
110 | return "Fastest Sinc Interpolator" ; | |
111 | ||
112 | default: break ; | |
113 | } ; | |
114 | ||
115 | return NULL ; | |
116 | } /* sinc_get_descrition */ | |
117 | ||
118 | const char* | |
119 | sinc_get_description (int src_enum) | |
120 | { | |
121 | switch (src_enum) | |
122 | { case SRC_SINC_FASTEST : | |
123 | return "Band limited sinc interpolation, fastest, 97dB SNR, 80% BW." ; | |
124 | ||
125 | case SRC_SINC_MEDIUM_QUALITY : | |
126 | return "Band limited sinc interpolation, medium quality, 121dB SNR, 90% BW." ; | |
127 | ||
128 | case SRC_SINC_BEST_QUALITY : | |
129 | return "Band limited sinc interpolation, best quality, 144dB SNR, 96% BW." ; | |
130 | ||
131 | default : | |
132 | break ; | |
133 | } ; | |
134 | ||
135 | return NULL ; | |
136 | } /* sinc_get_descrition */ | |
137 | ||
138 | int | |
139 | sinc_set_converter (SRC_PRIVATE *psrc, int src_enum) | |
140 | { SINC_FILTER *filter, temp_filter ; | |
141 | increment_t count ; | |
142 | int bits ; | |
143 | ||
144 | /* Quick sanity check. */ | |
145 | if (SHIFT_BITS >= sizeof (increment_t) * 8 - 1) | |
146 | return SRC_ERR_SHIFT_BITS ; | |
147 | ||
148 | if (psrc->private_data != NULL) | |
149 | { free (psrc->private_data) ; | |
150 | psrc->private_data = NULL ; | |
151 | } ; | |
152 | ||
153 | memset (&temp_filter, 0, sizeof (temp_filter)) ; | |
154 | ||
155 | temp_filter.sinc_magic_marker = SINC_MAGIC_MARKER ; | |
156 | temp_filter.channels = psrc->channels ; | |
157 | ||
158 | if (psrc->channels > ARRAY_LEN (temp_filter.left_calc)) | |
159 | return SRC_ERR_BAD_CHANNEL_COUNT ; | |
160 | else if (psrc->channels == 1) | |
161 | { psrc->const_process = sinc_mono_vari_process ; | |
162 | psrc->vari_process = sinc_mono_vari_process ; | |
163 | } | |
164 | else | |
165 | if (psrc->channels == 2) | |
166 | { psrc->const_process = sinc_stereo_vari_process ; | |
167 | psrc->vari_process = sinc_stereo_vari_process ; | |
168 | } | |
169 | else | |
170 | if (psrc->channels == 4) | |
171 | { psrc->const_process = sinc_quad_vari_process ; | |
172 | psrc->vari_process = sinc_quad_vari_process ; | |
173 | } | |
174 | else | |
175 | if (psrc->channels == 6) | |
176 | { psrc->const_process = sinc_hex_vari_process ; | |
177 | psrc->vari_process = sinc_hex_vari_process ; | |
178 | } | |
179 | else | |
180 | { psrc->const_process = sinc_multichan_vari_process ; | |
181 | psrc->vari_process = sinc_multichan_vari_process ; | |
182 | } ; | |
183 | psrc->reset = sinc_reset ; | |
184 | ||
185 | switch (src_enum) | |
186 | { case SRC_SINC_FASTEST : | |
187 | temp_filter.coeffs = fastest_coeffs.coeffs ; | |
188 | temp_filter.coeff_half_len = ARRAY_LEN (fastest_coeffs.coeffs) - 2 ; | |
189 | temp_filter.index_inc = fastest_coeffs.increment ; | |
190 | break ; | |
191 | ||
192 | case SRC_SINC_MEDIUM_QUALITY : | |
193 | temp_filter.coeffs = slow_mid_qual_coeffs.coeffs ; | |
194 | temp_filter.coeff_half_len = ARRAY_LEN (slow_mid_qual_coeffs.coeffs) - 2 ; | |
195 | temp_filter.index_inc = slow_mid_qual_coeffs.increment ; | |
196 | break ; | |
197 | ||
198 | case SRC_SINC_BEST_QUALITY : | |
199 | temp_filter.coeffs = slow_high_qual_coeffs.coeffs ; | |
200 | temp_filter.coeff_half_len = ARRAY_LEN (slow_high_qual_coeffs.coeffs) - 2 ; | |
201 | temp_filter.index_inc = slow_high_qual_coeffs.increment ; | |
202 | break ; | |
203 | ||
204 | default : | |
205 | return SRC_ERR_BAD_CONVERTER ; | |
206 | } ; | |
207 | ||
208 | /* | |
209 | ** FIXME : This needs to be looked at more closely to see if there is | |
210 | ** a better way. Need to look at prepare_data () at the same time. | |
211 | */ | |
212 | ||
213 | temp_filter.b_len = lrint (2.5 * temp_filter.coeff_half_len / (temp_filter.index_inc * 1.0) * SRC_MAX_RATIO) ; | |
214 | temp_filter.b_len = MAX (temp_filter.b_len, 4096) ; | |
215 | temp_filter.b_len *= temp_filter.channels ; | |
216 | ||
217 | if ((filter = calloc (1, sizeof (SINC_FILTER) + sizeof (filter->buffer [0]) * (temp_filter.b_len + temp_filter.channels))) == NULL) | |
218 | return SRC_ERR_MALLOC_FAILED ; | |
219 | ||
220 | *filter = temp_filter ; | |
221 | memset (&temp_filter, 0xEE, sizeof (temp_filter)) ; | |
222 | ||
223 | psrc->private_data = filter ; | |
224 | ||
225 | sinc_reset (psrc) ; | |
226 | ||
227 | count = filter->coeff_half_len ; | |
228 | for (bits = 0 ; (MAKE_INCREMENT_T (1) << bits) < count ; bits++) | |
229 | count |= (MAKE_INCREMENT_T (1) << bits) ; | |
230 | ||
231 | if (bits + SHIFT_BITS - 1 >= (int) (sizeof (increment_t) * 8)) | |
232 | return SRC_ERR_FILTER_LEN ; | |
233 | ||
234 | return SRC_ERR_NO_ERROR ; | |
235 | } /* sinc_set_converter */ | |
236 | ||
237 | static void | |
238 | sinc_reset (SRC_PRIVATE *psrc) | |
239 | { SINC_FILTER *filter ; | |
240 | ||
241 | filter = (SINC_FILTER*) psrc->private_data ; | |
242 | if (filter == NULL) | |
243 | return ; | |
244 | ||
245 | filter->b_current = filter->b_end = 0 ; | |
246 | filter->b_real_end = -1 ; | |
247 | ||
248 | filter->src_ratio = filter->input_index = 0.0 ; | |
249 | ||
250 | memset (filter->buffer, 0, filter->b_len * sizeof (filter->buffer [0])) ; | |
251 | ||
252 | /* Set this for a sanity check */ | |
253 | memset (filter->buffer + filter->b_len, 0xAA, filter->channels * sizeof (filter->buffer [0])) ; | |
254 | } /* sinc_reset */ | |
255 | ||
256 | /*======================================================================================== | |
257 | ** Beware all ye who dare pass this point. There be dragons here. | |
258 | */ | |
259 | ||
260 | static inline double | |
261 | calc_output_single (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index) | |
262 | { double fraction, left, right, icoeff ; | |
263 | increment_t filter_index, max_filter_index ; | |
264 | int data_index, coeff_count, indx ; | |
265 | ||
266 | /* Convert input parameters into fixed point. */ | |
267 | max_filter_index = int_to_fp (filter->coeff_half_len) ; | |
268 | ||
269 | /* First apply the left half of the filter. */ | |
270 | filter_index = start_filter_index ; | |
271 | coeff_count = (max_filter_index - filter_index) / increment ; | |
272 | filter_index = filter_index + coeff_count * increment ; | |
273 | data_index = filter->b_current - coeff_count ; | |
274 | ||
275 | left = 0.0 ; | |
276 | do | |
277 | { fraction = fp_to_double (filter_index) ; | |
278 | indx = fp_to_int (filter_index) ; | |
279 | ||
280 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
281 | ||
282 | left += icoeff * filter->buffer [data_index] ; | |
283 | ||
284 | filter_index -= increment ; | |
285 | data_index = data_index + 1 ; | |
286 | } | |
287 | while (filter_index >= MAKE_INCREMENT_T (0)) ; | |
288 | ||
289 | /* Now apply the right half of the filter. */ | |
290 | filter_index = increment - start_filter_index ; | |
291 | coeff_count = (max_filter_index - filter_index) / increment ; | |
292 | filter_index = filter_index + coeff_count * increment ; | |
293 | data_index = filter->b_current + 1 + coeff_count ; | |
294 | ||
295 | right = 0.0 ; | |
296 | do | |
297 | { fraction = fp_to_double (filter_index) ; | |
298 | indx = fp_to_int (filter_index) ; | |
299 | ||
300 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
301 | ||
302 | right += icoeff * filter->buffer [data_index] ; | |
303 | ||
304 | filter_index -= increment ; | |
305 | data_index = data_index - 1 ; | |
306 | } | |
307 | while (filter_index > MAKE_INCREMENT_T (0)) ; | |
308 | ||
309 | return (left + right) ; | |
310 | } /* calc_output_single */ | |
311 | ||
312 | static int | |
313 | sinc_mono_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) | |
314 | { SINC_FILTER *filter ; | |
315 | double input_index, src_ratio, count, float_increment, terminate, rem ; | |
316 | increment_t increment, start_filter_index ; | |
317 | int half_filter_chan_len, samples_in_hand ; | |
318 | ||
319 | if (psrc->private_data == NULL) | |
320 | return SRC_ERR_NO_PRIVATE ; | |
321 | ||
322 | filter = (SINC_FILTER*) psrc->private_data ; | |
323 | ||
324 | /* If there is not a problem, this will be optimised out. */ | |
325 | if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0])) | |
326 | return SRC_ERR_SIZE_INCOMPATIBILITY ; | |
327 | ||
328 | filter->in_count = data->input_frames * filter->channels ; | |
329 | filter->out_count = data->output_frames * filter->channels ; | |
330 | filter->in_used = filter->out_gen = 0 ; | |
331 | ||
332 | src_ratio = psrc->last_ratio ; | |
333 | ||
334 | if (is_bad_src_ratio (src_ratio)) | |
335 | return SRC_ERR_BAD_INTERNAL_STATE ; | |
336 | ||
337 | /* Check the sample rate ratio wrt the buffer len. */ | |
338 | count = (filter->coeff_half_len + 2.0) / filter->index_inc ; | |
339 | if (MIN (psrc->last_ratio, data->src_ratio) < 1.0) | |
340 | count /= MIN (psrc->last_ratio, data->src_ratio) ; | |
341 | ||
342 | /* Maximum coefficientson either side of center point. */ | |
343 | half_filter_chan_len = filter->channels * (lrint (count) + 1) ; | |
344 | ||
345 | input_index = psrc->last_position ; | |
346 | float_increment = filter->index_inc ; | |
347 | ||
348 | rem = fmod_one (input_index) ; | |
349 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
350 | input_index = rem ; | |
351 | ||
352 | terminate = 1.0 / src_ratio + 1e-20 ; | |
353 | ||
354 | /* Main processing loop. */ | |
355 | while (filter->out_gen < filter->out_count) | |
356 | { | |
357 | /* Need to reload buffer? */ | |
358 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
359 | ||
360 | if (samples_in_hand <= half_filter_chan_len) | |
361 | { if ((psrc->error = prepare_data (filter, data, half_filter_chan_len)) != 0) | |
362 | return psrc->error ; | |
363 | ||
364 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
365 | if (samples_in_hand <= half_filter_chan_len) | |
366 | break ; | |
367 | } ; | |
368 | ||
369 | /* This is the termination condition. */ | |
370 | if (filter->b_real_end >= 0) | |
371 | { if (filter->b_current + input_index + terminate > filter->b_real_end) | |
372 | break ; | |
373 | } ; | |
374 | ||
375 | if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10) | |
376 | src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ; | |
377 | ||
378 | float_increment = filter->index_inc * (src_ratio < 1.0 ? src_ratio : 1.0) ; | |
379 | increment = double_to_fp (float_increment) ; | |
380 | ||
381 | start_filter_index = double_to_fp (input_index * float_increment) ; | |
382 | ||
383 | data->data_out [filter->out_gen] = (float) ((float_increment / filter->index_inc) * | |
384 | calc_output_single (filter, increment, start_filter_index)) ; | |
385 | filter->out_gen ++ ; | |
386 | ||
387 | /* Figure out the next index. */ | |
388 | input_index += 1.0 / src_ratio ; | |
389 | rem = fmod_one (input_index) ; | |
390 | ||
391 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
392 | input_index = rem ; | |
393 | } ; | |
394 | ||
395 | psrc->last_position = input_index ; | |
396 | ||
397 | /* Save current ratio rather then target ratio. */ | |
398 | psrc->last_ratio = src_ratio ; | |
399 | ||
400 | data->input_frames_used = filter->in_used / filter->channels ; | |
401 | data->output_frames_gen = filter->out_gen / filter->channels ; | |
402 | ||
403 | return SRC_ERR_NO_ERROR ; | |
404 | } /* sinc_mono_vari_process */ | |
405 | ||
406 | static inline void | |
407 | calc_output_stereo (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, double scale, float * output) | |
408 | { double fraction, left [2], right [2], icoeff ; | |
409 | increment_t filter_index, max_filter_index ; | |
410 | int data_index, coeff_count, indx ; | |
411 | ||
412 | /* Convert input parameters into fixed point. */ | |
413 | max_filter_index = int_to_fp (filter->coeff_half_len) ; | |
414 | ||
415 | /* First apply the left half of the filter. */ | |
416 | filter_index = start_filter_index ; | |
417 | coeff_count = (max_filter_index - filter_index) / increment ; | |
418 | filter_index = filter_index + coeff_count * increment ; | |
419 | data_index = filter->b_current - filter->channels * coeff_count ; | |
420 | ||
421 | left [0] = left [1] = 0.0 ; | |
422 | do | |
423 | { fraction = fp_to_double (filter_index) ; | |
424 | indx = fp_to_int (filter_index) ; | |
425 | ||
426 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
427 | ||
428 | left [0] += icoeff * filter->buffer [data_index] ; | |
429 | left [1] += icoeff * filter->buffer [data_index + 1] ; | |
430 | ||
431 | filter_index -= increment ; | |
432 | data_index = data_index + 2 ; | |
433 | } | |
434 | while (filter_index >= MAKE_INCREMENT_T (0)) ; | |
435 | ||
436 | /* Now apply the right half of the filter. */ | |
437 | filter_index = increment - start_filter_index ; | |
438 | coeff_count = (max_filter_index - filter_index) / increment ; | |
439 | filter_index = filter_index + coeff_count * increment ; | |
440 | data_index = filter->b_current + filter->channels * (1 + coeff_count) ; | |
441 | ||
442 | right [0] = right [1] = 0.0 ; | |
443 | do | |
444 | { fraction = fp_to_double (filter_index) ; | |
445 | indx = fp_to_int (filter_index) ; | |
446 | ||
447 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
448 | ||
449 | right [0] += icoeff * filter->buffer [data_index] ; | |
450 | right [1] += icoeff * filter->buffer [data_index + 1] ; | |
451 | ||
452 | filter_index -= increment ; | |
453 | data_index = data_index - 2 ; | |
454 | } | |
455 | while (filter_index > MAKE_INCREMENT_T (0)) ; | |
456 | ||
457 | output [0] = scale * (left [0] + right [0]) ; | |
458 | output [1] = scale * (left [1] + right [1]) ; | |
459 | } /* calc_output_stereo */ | |
460 | ||
461 | static int | |
462 | sinc_stereo_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) | |
463 | { SINC_FILTER *filter ; | |
464 | double input_index, src_ratio, count, float_increment, terminate, rem ; | |
465 | increment_t increment, start_filter_index ; | |
466 | int half_filter_chan_len, samples_in_hand ; | |
467 | ||
468 | if (psrc->private_data == NULL) | |
469 | return SRC_ERR_NO_PRIVATE ; | |
470 | ||
471 | filter = (SINC_FILTER*) psrc->private_data ; | |
472 | ||
473 | /* If there is not a problem, this will be optimised out. */ | |
474 | if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0])) | |
475 | return SRC_ERR_SIZE_INCOMPATIBILITY ; | |
476 | ||
477 | filter->in_count = data->input_frames * filter->channels ; | |
478 | filter->out_count = data->output_frames * filter->channels ; | |
479 | filter->in_used = filter->out_gen = 0 ; | |
480 | ||
481 | src_ratio = psrc->last_ratio ; | |
482 | ||
483 | if (is_bad_src_ratio (src_ratio)) | |
484 | return SRC_ERR_BAD_INTERNAL_STATE ; | |
485 | ||
486 | /* Check the sample rate ratio wrt the buffer len. */ | |
487 | count = (filter->coeff_half_len + 2.0) / filter->index_inc ; | |
488 | if (MIN (psrc->last_ratio, data->src_ratio) < 1.0) | |
489 | count /= MIN (psrc->last_ratio, data->src_ratio) ; | |
490 | ||
491 | /* Maximum coefficientson either side of center point. */ | |
492 | half_filter_chan_len = filter->channels * (lrint (count) + 1) ; | |
493 | ||
494 | input_index = psrc->last_position ; | |
495 | float_increment = filter->index_inc ; | |
496 | ||
497 | rem = fmod_one (input_index) ; | |
498 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
499 | input_index = rem ; | |
500 | ||
501 | terminate = 1.0 / src_ratio + 1e-20 ; | |
502 | ||
503 | /* Main processing loop. */ | |
504 | while (filter->out_gen < filter->out_count) | |
505 | { | |
506 | /* Need to reload buffer? */ | |
507 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
508 | ||
509 | if (samples_in_hand <= half_filter_chan_len) | |
510 | { if ((psrc->error = prepare_data (filter, data, half_filter_chan_len)) != 0) | |
511 | return psrc->error ; | |
512 | ||
513 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
514 | if (samples_in_hand <= half_filter_chan_len) | |
515 | break ; | |
516 | } ; | |
517 | ||
518 | /* This is the termination condition. */ | |
519 | if (filter->b_real_end >= 0) | |
520 | { if (filter->b_current + input_index + terminate >= filter->b_real_end) | |
521 | break ; | |
522 | } ; | |
523 | ||
524 | if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10) | |
525 | src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ; | |
526 | ||
527 | float_increment = filter->index_inc * (src_ratio < 1.0 ? src_ratio : 1.0) ; | |
528 | increment = double_to_fp (float_increment) ; | |
529 | ||
530 | start_filter_index = double_to_fp (input_index * float_increment) ; | |
531 | ||
532 | calc_output_stereo (filter, increment, start_filter_index, float_increment / filter->index_inc, data->data_out + filter->out_gen) ; | |
533 | filter->out_gen += 2 ; | |
534 | ||
535 | /* Figure out the next index. */ | |
536 | input_index += 1.0 / src_ratio ; | |
537 | rem = fmod_one (input_index) ; | |
538 | ||
539 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
540 | input_index = rem ; | |
541 | } ; | |
542 | ||
543 | psrc->last_position = input_index ; | |
544 | ||
545 | /* Save current ratio rather then target ratio. */ | |
546 | psrc->last_ratio = src_ratio ; | |
547 | ||
548 | data->input_frames_used = filter->in_used / filter->channels ; | |
549 | data->output_frames_gen = filter->out_gen / filter->channels ; | |
550 | ||
551 | return SRC_ERR_NO_ERROR ; | |
552 | } /* sinc_stereo_vari_process */ | |
553 | ||
554 | static inline void | |
555 | calc_output_quad (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, double scale, float * output) | |
556 | { double fraction, left [4], right [4], icoeff ; | |
557 | increment_t filter_index, max_filter_index ; | |
558 | int data_index, coeff_count, indx ; | |
559 | ||
560 | /* Convert input parameters into fixed point. */ | |
561 | max_filter_index = int_to_fp (filter->coeff_half_len) ; | |
562 | ||
563 | /* First apply the left half of the filter. */ | |
564 | filter_index = start_filter_index ; | |
565 | coeff_count = (max_filter_index - filter_index) / increment ; | |
566 | filter_index = filter_index + coeff_count * increment ; | |
567 | data_index = filter->b_current - filter->channels * coeff_count ; | |
568 | ||
569 | left [0] = left [1] = left [2] = left [3] = 0.0 ; | |
570 | do | |
571 | { fraction = fp_to_double (filter_index) ; | |
572 | indx = fp_to_int (filter_index) ; | |
573 | ||
574 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
575 | ||
576 | left [0] += icoeff * filter->buffer [data_index] ; | |
577 | left [1] += icoeff * filter->buffer [data_index + 1] ; | |
578 | left [2] += icoeff * filter->buffer [data_index + 2] ; | |
579 | left [3] += icoeff * filter->buffer [data_index + 3] ; | |
580 | ||
581 | filter_index -= increment ; | |
582 | data_index = data_index + 4 ; | |
583 | } | |
584 | while (filter_index >= MAKE_INCREMENT_T (0)) ; | |
585 | ||
586 | /* Now apply the right half of the filter. */ | |
587 | filter_index = increment - start_filter_index ; | |
588 | coeff_count = (max_filter_index - filter_index) / increment ; | |
589 | filter_index = filter_index + coeff_count * increment ; | |
590 | data_index = filter->b_current + filter->channels * (1 + coeff_count) ; | |
591 | ||
592 | right [0] = right [1] = right [2] = right [3] = 0.0 ; | |
593 | do | |
594 | { fraction = fp_to_double (filter_index) ; | |
595 | indx = fp_to_int (filter_index) ; | |
596 | ||
597 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
598 | ||
599 | right [0] += icoeff * filter->buffer [data_index] ; | |
600 | right [1] += icoeff * filter->buffer [data_index + 1] ; | |
601 | right [2] += icoeff * filter->buffer [data_index + 2] ; | |
602 | right [3] += icoeff * filter->buffer [data_index + 3] ; | |
603 | ||
604 | filter_index -= increment ; | |
605 | data_index = data_index - 4 ; | |
606 | } | |
607 | while (filter_index > MAKE_INCREMENT_T (0)) ; | |
608 | ||
609 | output [0] = scale * (left [0] + right [0]) ; | |
610 | output [1] = scale * (left [1] + right [1]) ; | |
611 | output [2] = scale * (left [2] + right [2]) ; | |
612 | output [3] = scale * (left [3] + right [3]) ; | |
613 | } /* calc_output_quad */ | |
614 | ||
615 | static int | |
616 | sinc_quad_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) | |
617 | { SINC_FILTER *filter ; | |
618 | double input_index, src_ratio, count, float_increment, terminate, rem ; | |
619 | increment_t increment, start_filter_index ; | |
620 | int half_filter_chan_len, samples_in_hand ; | |
621 | ||
622 | if (psrc->private_data == NULL) | |
623 | return SRC_ERR_NO_PRIVATE ; | |
624 | ||
625 | filter = (SINC_FILTER*) psrc->private_data ; | |
626 | ||
627 | /* If there is not a problem, this will be optimised out. */ | |
628 | if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0])) | |
629 | return SRC_ERR_SIZE_INCOMPATIBILITY ; | |
630 | ||
631 | filter->in_count = data->input_frames * filter->channels ; | |
632 | filter->out_count = data->output_frames * filter->channels ; | |
633 | filter->in_used = filter->out_gen = 0 ; | |
634 | ||
635 | src_ratio = psrc->last_ratio ; | |
636 | ||
637 | if (is_bad_src_ratio (src_ratio)) | |
638 | return SRC_ERR_BAD_INTERNAL_STATE ; | |
639 | ||
640 | /* Check the sample rate ratio wrt the buffer len. */ | |
641 | count = (filter->coeff_half_len + 2.0) / filter->index_inc ; | |
642 | if (MIN (psrc->last_ratio, data->src_ratio) < 1.0) | |
643 | count /= MIN (psrc->last_ratio, data->src_ratio) ; | |
644 | ||
645 | /* Maximum coefficientson either side of center point. */ | |
646 | half_filter_chan_len = filter->channels * (lrint (count) + 1) ; | |
647 | ||
648 | input_index = psrc->last_position ; | |
649 | float_increment = filter->index_inc ; | |
650 | ||
651 | rem = fmod_one (input_index) ; | |
652 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
653 | input_index = rem ; | |
654 | ||
655 | terminate = 1.0 / src_ratio + 1e-20 ; | |
656 | ||
657 | /* Main processing loop. */ | |
658 | while (filter->out_gen < filter->out_count) | |
659 | { | |
660 | /* Need to reload buffer? */ | |
661 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
662 | ||
663 | if (samples_in_hand <= half_filter_chan_len) | |
664 | { if ((psrc->error = prepare_data (filter, data, half_filter_chan_len)) != 0) | |
665 | return psrc->error ; | |
666 | ||
667 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
668 | if (samples_in_hand <= half_filter_chan_len) | |
669 | break ; | |
670 | } ; | |
671 | ||
672 | /* This is the termination condition. */ | |
673 | if (filter->b_real_end >= 0) | |
674 | { if (filter->b_current + input_index + terminate >= filter->b_real_end) | |
675 | break ; | |
676 | } ; | |
677 | ||
678 | if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10) | |
679 | src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ; | |
680 | ||
681 | float_increment = filter->index_inc * (src_ratio < 1.0 ? src_ratio : 1.0) ; | |
682 | increment = double_to_fp (float_increment) ; | |
683 | ||
684 | start_filter_index = double_to_fp (input_index * float_increment) ; | |
685 | ||
686 | calc_output_quad (filter, increment, start_filter_index, float_increment / filter->index_inc, data->data_out + filter->out_gen) ; | |
687 | filter->out_gen += 4 ; | |
688 | ||
689 | /* Figure out the next index. */ | |
690 | input_index += 1.0 / src_ratio ; | |
691 | rem = fmod_one (input_index) ; | |
692 | ||
693 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
694 | input_index = rem ; | |
695 | } ; | |
696 | ||
697 | psrc->last_position = input_index ; | |
698 | ||
699 | /* Save current ratio rather then target ratio. */ | |
700 | psrc->last_ratio = src_ratio ; | |
701 | ||
702 | data->input_frames_used = filter->in_used / filter->channels ; | |
703 | data->output_frames_gen = filter->out_gen / filter->channels ; | |
704 | ||
705 | return SRC_ERR_NO_ERROR ; | |
706 | } /* sinc_quad_vari_process */ | |
707 | ||
708 | static inline void | |
709 | calc_output_hex (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, double scale, float * output) | |
710 | { double fraction, left [6], right [6], icoeff ; | |
711 | increment_t filter_index, max_filter_index ; | |
712 | int data_index, coeff_count, indx ; | |
713 | ||
714 | /* Convert input parameters into fixed point. */ | |
715 | max_filter_index = int_to_fp (filter->coeff_half_len) ; | |
716 | ||
717 | /* First apply the left half of the filter. */ | |
718 | filter_index = start_filter_index ; | |
719 | coeff_count = (max_filter_index - filter_index) / increment ; | |
720 | filter_index = filter_index + coeff_count * increment ; | |
721 | data_index = filter->b_current - filter->channels * coeff_count ; | |
722 | ||
723 | left [0] = left [1] = left [2] = left [3] = left [4] = left [5] = 0.0 ; | |
724 | do | |
725 | { fraction = fp_to_double (filter_index) ; | |
726 | indx = fp_to_int (filter_index) ; | |
727 | ||
728 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
729 | ||
730 | left [0] += icoeff * filter->buffer [data_index] ; | |
731 | left [1] += icoeff * filter->buffer [data_index + 1] ; | |
732 | left [2] += icoeff * filter->buffer [data_index + 2] ; | |
733 | left [3] += icoeff * filter->buffer [data_index + 3] ; | |
734 | left [4] += icoeff * filter->buffer [data_index + 4] ; | |
735 | left [5] += icoeff * filter->buffer [data_index + 5] ; | |
736 | ||
737 | filter_index -= increment ; | |
738 | data_index = data_index + 6 ; | |
739 | } | |
740 | while (filter_index >= MAKE_INCREMENT_T (0)) ; | |
741 | ||
742 | /* Now apply the right half of the filter. */ | |
743 | filter_index = increment - start_filter_index ; | |
744 | coeff_count = (max_filter_index - filter_index) / increment ; | |
745 | filter_index = filter_index + coeff_count * increment ; | |
746 | data_index = filter->b_current + filter->channels * (1 + coeff_count) ; | |
747 | ||
748 | right [0] = right [1] = right [2] = right [3] = right [4] = right [5] = 0.0 ; | |
749 | do | |
750 | { fraction = fp_to_double (filter_index) ; | |
751 | indx = fp_to_int (filter_index) ; | |
752 | ||
753 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
754 | ||
755 | right [0] += icoeff * filter->buffer [data_index] ; | |
756 | right [1] += icoeff * filter->buffer [data_index + 1] ; | |
757 | right [2] += icoeff * filter->buffer [data_index + 2] ; | |
758 | right [3] += icoeff * filter->buffer [data_index + 3] ; | |
759 | right [4] += icoeff * filter->buffer [data_index + 4] ; | |
760 | right [5] += icoeff * filter->buffer [data_index + 5] ; | |
761 | ||
762 | filter_index -= increment ; | |
763 | data_index = data_index - 6 ; | |
764 | } | |
765 | while (filter_index > MAKE_INCREMENT_T (0)) ; | |
766 | ||
767 | output [0] = scale * (left [0] + right [0]) ; | |
768 | output [1] = scale * (left [1] + right [1]) ; | |
769 | output [2] = scale * (left [2] + right [2]) ; | |
770 | output [3] = scale * (left [3] + right [3]) ; | |
771 | output [4] = scale * (left [4] + right [4]) ; | |
772 | output [5] = scale * (left [5] + right [5]) ; | |
773 | } /* calc_output_hex */ | |
774 | ||
775 | static int | |
776 | sinc_hex_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) | |
777 | { SINC_FILTER *filter ; | |
778 | double input_index, src_ratio, count, float_increment, terminate, rem ; | |
779 | increment_t increment, start_filter_index ; | |
780 | int half_filter_chan_len, samples_in_hand ; | |
781 | ||
782 | if (psrc->private_data == NULL) | |
783 | return SRC_ERR_NO_PRIVATE ; | |
784 | ||
785 | filter = (SINC_FILTER*) psrc->private_data ; | |
786 | ||
787 | /* If there is not a problem, this will be optimised out. */ | |
788 | if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0])) | |
789 | return SRC_ERR_SIZE_INCOMPATIBILITY ; | |
790 | ||
791 | filter->in_count = data->input_frames * filter->channels ; | |
792 | filter->out_count = data->output_frames * filter->channels ; | |
793 | filter->in_used = filter->out_gen = 0 ; | |
794 | ||
795 | src_ratio = psrc->last_ratio ; | |
796 | ||
797 | if (is_bad_src_ratio (src_ratio)) | |
798 | return SRC_ERR_BAD_INTERNAL_STATE ; | |
799 | ||
800 | /* Check the sample rate ratio wrt the buffer len. */ | |
801 | count = (filter->coeff_half_len + 2.0) / filter->index_inc ; | |
802 | if (MIN (psrc->last_ratio, data->src_ratio) < 1.0) | |
803 | count /= MIN (psrc->last_ratio, data->src_ratio) ; | |
804 | ||
805 | /* Maximum coefficientson either side of center point. */ | |
806 | half_filter_chan_len = filter->channels * (lrint (count) + 1) ; | |
807 | ||
808 | input_index = psrc->last_position ; | |
809 | float_increment = filter->index_inc ; | |
810 | ||
811 | rem = fmod_one (input_index) ; | |
812 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
813 | input_index = rem ; | |
814 | ||
815 | terminate = 1.0 / src_ratio + 1e-20 ; | |
816 | ||
817 | /* Main processing loop. */ | |
818 | while (filter->out_gen < filter->out_count) | |
819 | { | |
820 | /* Need to reload buffer? */ | |
821 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
822 | ||
823 | if (samples_in_hand <= half_filter_chan_len) | |
824 | { if ((psrc->error = prepare_data (filter, data, half_filter_chan_len)) != 0) | |
825 | return psrc->error ; | |
826 | ||
827 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
828 | if (samples_in_hand <= half_filter_chan_len) | |
829 | break ; | |
830 | } ; | |
831 | ||
832 | /* This is the termination condition. */ | |
833 | if (filter->b_real_end >= 0) | |
834 | { if (filter->b_current + input_index + terminate >= filter->b_real_end) | |
835 | break ; | |
836 | } ; | |
837 | ||
838 | if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10) | |
839 | src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ; | |
840 | ||
841 | float_increment = filter->index_inc * (src_ratio < 1.0 ? src_ratio : 1.0) ; | |
842 | increment = double_to_fp (float_increment) ; | |
843 | ||
844 | start_filter_index = double_to_fp (input_index * float_increment) ; | |
845 | ||
846 | calc_output_hex (filter, increment, start_filter_index, float_increment / filter->index_inc, data->data_out + filter->out_gen) ; | |
847 | filter->out_gen += 6 ; | |
848 | ||
849 | /* Figure out the next index. */ | |
850 | input_index += 1.0 / src_ratio ; | |
851 | rem = fmod_one (input_index) ; | |
852 | ||
853 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
854 | input_index = rem ; | |
855 | } ; | |
856 | ||
857 | psrc->last_position = input_index ; | |
858 | ||
859 | /* Save current ratio rather then target ratio. */ | |
860 | psrc->last_ratio = src_ratio ; | |
861 | ||
862 | data->input_frames_used = filter->in_used / filter->channels ; | |
863 | data->output_frames_gen = filter->out_gen / filter->channels ; | |
864 | ||
865 | return SRC_ERR_NO_ERROR ; | |
866 | } /* sinc_hex_vari_process */ | |
867 | ||
868 | static inline void | |
869 | calc_output_multi (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int channels, double scale, float * output) | |
870 | { double fraction, icoeff ; | |
871 | /* The following line is 1999 ISO Standard C. If your compiler complains, get a better compiler. */ | |
872 | double *left, *right ; | |
873 | increment_t filter_index, max_filter_index ; | |
874 | int data_index, coeff_count, indx, ch ; | |
875 | ||
876 | left = filter->left_calc ; | |
877 | right = filter->right_calc ; | |
878 | ||
879 | /* Convert input parameters into fixed point. */ | |
880 | max_filter_index = int_to_fp (filter->coeff_half_len) ; | |
881 | ||
882 | /* First apply the left half of the filter. */ | |
883 | filter_index = start_filter_index ; | |
884 | coeff_count = (max_filter_index - filter_index) / increment ; | |
885 | filter_index = filter_index + coeff_count * increment ; | |
886 | data_index = filter->b_current - channels * coeff_count ; | |
887 | ||
888 | memset (left, 0, sizeof (left [0]) * channels) ; | |
889 | ||
890 | do | |
891 | { fraction = fp_to_double (filter_index) ; | |
892 | indx = fp_to_int (filter_index) ; | |
893 | ||
894 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
895 | ||
896 | /* | |
897 | ** Duff's Device. | |
898 | ** See : http://en.wikipedia.org/wiki/Duff's_device | |
899 | */ | |
900 | ch = channels ; | |
901 | do | |
902 | { | |
903 | switch (ch % 8) | |
904 | { default : | |
905 | ch -- ; | |
906 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
907 | case 7 : | |
908 | ch -- ; | |
909 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
910 | case 6 : | |
911 | ch -- ; | |
912 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
913 | case 5 : | |
914 | ch -- ; | |
915 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
916 | case 4 : | |
917 | ch -- ; | |
918 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
919 | case 3 : | |
920 | ch -- ; | |
921 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
922 | case 2 : | |
923 | ch -- ; | |
924 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
925 | case 1 : | |
926 | ch -- ; | |
927 | left [ch] += icoeff * filter->buffer [data_index + ch] ; | |
928 | } ; | |
929 | } | |
930 | while (ch > 0) ; | |
931 | ||
932 | filter_index -= increment ; | |
933 | data_index = data_index + channels ; | |
934 | } | |
935 | while (filter_index >= MAKE_INCREMENT_T (0)) ; | |
936 | ||
937 | /* Now apply the right half of the filter. */ | |
938 | filter_index = increment - start_filter_index ; | |
939 | coeff_count = (max_filter_index - filter_index) / increment ; | |
940 | filter_index = filter_index + coeff_count * increment ; | |
941 | data_index = filter->b_current + channels * (1 + coeff_count) ; | |
942 | ||
943 | memset (right, 0, sizeof (right [0]) * channels) ; | |
944 | do | |
945 | { fraction = fp_to_double (filter_index) ; | |
946 | indx = fp_to_int (filter_index) ; | |
947 | ||
948 | icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ; | |
949 | ||
950 | ch = channels ; | |
951 | do | |
952 | { | |
953 | switch (ch % 8) | |
954 | { default : | |
955 | ch -- ; | |
956 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
957 | case 7 : | |
958 | ch -- ; | |
959 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
960 | case 6 : | |
961 | ch -- ; | |
962 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
963 | case 5 : | |
964 | ch -- ; | |
965 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
966 | case 4 : | |
967 | ch -- ; | |
968 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
969 | case 3 : | |
970 | ch -- ; | |
971 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
972 | case 2 : | |
973 | ch -- ; | |
974 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
975 | case 1 : | |
976 | ch -- ; | |
977 | right [ch] += icoeff * filter->buffer [data_index + ch] ; | |
978 | } ; | |
979 | } | |
980 | while (ch > 0) ; | |
981 | ||
982 | filter_index -= increment ; | |
983 | data_index = data_index - channels ; | |
984 | } | |
985 | while (filter_index > MAKE_INCREMENT_T (0)) ; | |
986 | ||
987 | ch = channels ; | |
988 | do | |
989 | { | |
990 | switch (ch % 8) | |
991 | { default : | |
992 | ch -- ; | |
993 | output [ch] = scale * (left [ch] + right [ch]) ; | |
994 | case 7 : | |
995 | ch -- ; | |
996 | output [ch] = scale * (left [ch] + right [ch]) ; | |
997 | case 6 : | |
998 | ch -- ; | |
999 | output [ch] = scale * (left [ch] + right [ch]) ; | |
1000 | case 5 : | |
1001 | ch -- ; | |
1002 | output [ch] = scale * (left [ch] + right [ch]) ; | |
1003 | case 4 : | |
1004 | ch -- ; | |
1005 | output [ch] = scale * (left [ch] + right [ch]) ; | |
1006 | case 3 : | |
1007 | ch -- ; | |
1008 | output [ch] = scale * (left [ch] + right [ch]) ; | |
1009 | case 2 : | |
1010 | ch -- ; | |
1011 | output [ch] = scale * (left [ch] + right [ch]) ; | |
1012 | case 1 : | |
1013 | ch -- ; | |
1014 | output [ch] = scale * (left [ch] + right [ch]) ; | |
1015 | } ; | |
1016 | } | |
1017 | while (ch > 0) ; | |
1018 | ||
1019 | return ; | |
1020 | } /* calc_output_multi */ | |
1021 | ||
1022 | static int | |
1023 | sinc_multichan_vari_process (SRC_PRIVATE *psrc, SRC_DATA *data) | |
1024 | { SINC_FILTER *filter ; | |
1025 | double input_index, src_ratio, count, float_increment, terminate, rem ; | |
1026 | increment_t increment, start_filter_index ; | |
1027 | int half_filter_chan_len, samples_in_hand ; | |
1028 | ||
1029 | if (psrc->private_data == NULL) | |
1030 | return SRC_ERR_NO_PRIVATE ; | |
1031 | ||
1032 | filter = (SINC_FILTER*) psrc->private_data ; | |
1033 | ||
1034 | /* If there is not a problem, this will be optimised out. */ | |
1035 | if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0])) | |
1036 | return SRC_ERR_SIZE_INCOMPATIBILITY ; | |
1037 | ||
1038 | filter->in_count = data->input_frames * filter->channels ; | |
1039 | filter->out_count = data->output_frames * filter->channels ; | |
1040 | filter->in_used = filter->out_gen = 0 ; | |
1041 | ||
1042 | src_ratio = psrc->last_ratio ; | |
1043 | ||
1044 | if (is_bad_src_ratio (src_ratio)) | |
1045 | return SRC_ERR_BAD_INTERNAL_STATE ; | |
1046 | ||
1047 | /* Check the sample rate ratio wrt the buffer len. */ | |
1048 | count = (filter->coeff_half_len + 2.0) / filter->index_inc ; | |
1049 | if (MIN (psrc->last_ratio, data->src_ratio) < 1.0) | |
1050 | count /= MIN (psrc->last_ratio, data->src_ratio) ; | |
1051 | ||
1052 | /* Maximum coefficientson either side of center point. */ | |
1053 | half_filter_chan_len = filter->channels * (lrint (count) + 1) ; | |
1054 | ||
1055 | input_index = psrc->last_position ; | |
1056 | float_increment = filter->index_inc ; | |
1057 | ||
1058 | rem = fmod_one (input_index) ; | |
1059 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
1060 | input_index = rem ; | |
1061 | ||
1062 | terminate = 1.0 / src_ratio + 1e-20 ; | |
1063 | ||
1064 | /* Main processing loop. */ | |
1065 | while (filter->out_gen < filter->out_count) | |
1066 | { | |
1067 | /* Need to reload buffer? */ | |
1068 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
1069 | ||
1070 | if (samples_in_hand <= half_filter_chan_len) | |
1071 | { if ((psrc->error = prepare_data (filter, data, half_filter_chan_len)) != 0) | |
1072 | return psrc->error ; | |
1073 | ||
1074 | samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ; | |
1075 | if (samples_in_hand <= half_filter_chan_len) | |
1076 | break ; | |
1077 | } ; | |
1078 | ||
1079 | /* This is the termination condition. */ | |
1080 | if (filter->b_real_end >= 0) | |
1081 | { if (filter->b_current + input_index + terminate >= filter->b_real_end) | |
1082 | break ; | |
1083 | } ; | |
1084 | ||
1085 | if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10) | |
1086 | src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ; | |
1087 | ||
1088 | float_increment = filter->index_inc * (src_ratio < 1.0 ? src_ratio : 1.0) ; | |
1089 | increment = double_to_fp (float_increment) ; | |
1090 | ||
1091 | start_filter_index = double_to_fp (input_index * float_increment) ; | |
1092 | ||
1093 | calc_output_multi (filter, increment, start_filter_index, filter->channels, float_increment / filter->index_inc, data->data_out + filter->out_gen) ; | |
1094 | filter->out_gen += psrc->channels ; | |
1095 | ||
1096 | /* Figure out the next index. */ | |
1097 | input_index += 1.0 / src_ratio ; | |
1098 | rem = fmod_one (input_index) ; | |
1099 | ||
1100 | filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ; | |
1101 | input_index = rem ; | |
1102 | } ; | |
1103 | ||
1104 | psrc->last_position = input_index ; | |
1105 | ||
1106 | /* Save current ratio rather then target ratio. */ | |
1107 | psrc->last_ratio = src_ratio ; | |
1108 | ||
1109 | data->input_frames_used = filter->in_used / filter->channels ; | |
1110 | data->output_frames_gen = filter->out_gen / filter->channels ; | |
1111 | ||
1112 | return SRC_ERR_NO_ERROR ; | |
1113 | } /* sinc_multichan_vari_process */ | |
1114 | ||
1115 | /*---------------------------------------------------------------------------------------- | |
1116 | */ | |
1117 | ||
1118 | static int | |
1119 | prepare_data (SINC_FILTER *filter, SRC_DATA *data, int half_filter_chan_len) | |
1120 | { int len = 0 ; | |
1121 | ||
1122 | if (filter->b_real_end >= 0) | |
1123 | return 0 ; /* Should be terminating. Just return. */ | |
1124 | ||
1125 | if (filter->b_current == 0) | |
1126 | { /* Initial state. Set up zeros at the start of the buffer and | |
1127 | ** then load new data after that. | |
1128 | */ | |
1129 | len = filter->b_len - 2 * half_filter_chan_len ; | |
1130 | ||
1131 | filter->b_current = filter->b_end = half_filter_chan_len ; | |
1132 | } | |
1133 | else if (filter->b_end + half_filter_chan_len + filter->channels < filter->b_len) | |
1134 | { /* Load data at current end position. */ | |
1135 | len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0) ; | |
1136 | } | |
1137 | else | |
1138 | { /* Move data at end of buffer back to the start of the buffer. */ | |
1139 | len = filter->b_end - filter->b_current ; | |
1140 | memmove (filter->buffer, filter->buffer + filter->b_current - half_filter_chan_len, | |
1141 | (half_filter_chan_len + len) * sizeof (filter->buffer [0])) ; | |
1142 | ||
1143 | filter->b_current = half_filter_chan_len ; | |
1144 | filter->b_end = filter->b_current + len ; | |
1145 | ||
1146 | /* Now load data at current end of buffer. */ | |
1147 | len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0) ; | |
1148 | } ; | |
1149 | ||
1150 | len = MIN (filter->in_count - filter->in_used, len) ; | |
1151 | len -= (len % filter->channels) ; | |
1152 | ||
1153 | if (len < 0 || filter->b_end + len > filter->b_len) | |
1154 | return SRC_ERR_SINC_PREPARE_DATA_BAD_LEN ; | |
1155 | ||
1156 | memcpy (filter->buffer + filter->b_end, data->data_in + filter->in_used, | |
1157 | len * sizeof (filter->buffer [0])) ; | |
1158 | ||
1159 | filter->b_end += len ; | |
1160 | filter->in_used += len ; | |
1161 | ||
1162 | if (filter->in_used == filter->in_count && | |
1163 | filter->b_end - filter->b_current < 2 * half_filter_chan_len && data->end_of_input) | |
1164 | { /* Handle the case where all data in the current buffer has been | |
1165 | ** consumed and this is the last buffer. | |
1166 | */ | |
1167 | ||
1168 | if (filter->b_len - filter->b_end < half_filter_chan_len + 5) | |
1169 | { /* If necessary, move data down to the start of the buffer. */ | |
1170 | len = filter->b_end - filter->b_current ; | |
1171 | memmove (filter->buffer, filter->buffer + filter->b_current - half_filter_chan_len, | |
1172 | (half_filter_chan_len + len) * sizeof (filter->buffer [0])) ; | |
1173 | ||
1174 | filter->b_current = half_filter_chan_len ; | |
1175 | filter->b_end = filter->b_current + len ; | |
1176 | } ; | |
1177 | ||
1178 | filter->b_real_end = filter->b_end ; | |
1179 | len = half_filter_chan_len + 5 ; | |
1180 | ||
1181 | if (len < 0 || filter->b_end + len > filter->b_len) | |
1182 | len = filter->b_len - filter->b_end ; | |
1183 | ||
1184 | memset (filter->buffer + filter->b_end, 0, len * sizeof (filter->buffer [0])) ; | |
1185 | filter->b_end += len ; | |
1186 | } ; | |
1187 | ||
1188 | return 0 ; | |
1189 | } /* prepare_data */ | |
1190 | ||
1191 |