projects / audio-libsamplerate.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
Bundle libsamplerate
[audio-libsamplerate.git] / libsamplerate / src / src_sinc.c
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
interface to Secret Rabbit Code audio sample rate converter
This page took 0.063611 seconds and 4 git commands to generate.