1 /*
2 ** Copyright (C) 2002-2008 Erik de Castro Lopo <erikd@mega-nerd.com>
3 **
4 ** This program is free software; you can redistribute it and/or modify
5 ** it under the terms of the GNU General Public License as published by
6 ** the Free Software Foundation; either version 2 of the License, or
7 ** (at your option) any later version.
8 **
9 ** This program is distributed in the hope that it will be useful,
10 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
11 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 ** GNU General Public License for more details.
13 **
14 ** You should have received a copy of the GNU General Public License
15 ** along with this program; if not, write to the Free Software
16 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
17 */
18
19 /*
20 ** This code is part of Secret Rabibt Code aka libsamplerate. A commercial
21 ** use license for this code is available, please see:
22 ** http://www.mega-nerd.com/SRC/procedure.html
23 */
24
25 #include <stdio.h>
26 #include <stdlib.h>
27 #include <string.h>
28
29 #include "config.h"
30 #include "common.h"
31
32 #define SINC_MAGIC_MARKER MAKE_MAGIC (' ', 's', 'i', 'n', 'c', ' ')
33
34 /*========================================================================================
35 */
36
37 #define MAKE_INCREMENT_T(x) ((increment_t) (x))
38
39 #define SHIFT_BITS 12
40 #define FP_ONE ((double) (((increment_t) 1) << SHIFT_BITS))
41 #define INV_FP_ONE (1.0 / FP_ONE)
42
43 /*========================================================================================
44 */
45
46 typedef int32_t increment_t ;
47 typedef double coeff_t ;
48
49 #include "fastest_coeffs.h"
50 #include "mid_qual_coeffs.h"
51 #include "high_qual_coeffs.h"
52
53 typedef struct
54 { int sinc_magic_marker ;
55
56 int channels ;
57 long in_count, in_used ;
58 long out_count, out_gen ;
59
60 int coeff_half_len, index_inc ;
61
62 double src_ratio, input_index ;
63
64 coeff_t const *coeffs ;
65
66 int b_current, b_end, b_real_end, b_len ;
67 int d;
68 float buffer_f [1] ;
69 double buffer_d [1] ;
70 } SINC_FILTER ;
71
72 static int sinc_vari_process_d (SRC_PRIVATE *psrc, SRC_DATA *data) ;
73 static int sinc_vari_process_f (SRC_PRIVATE *psrc, SRC_DATA *data) ;
74
75 static double calc_output_f (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int ch) ;
76 static double calc_output_d (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int ch) ;
77
78 static void prepare_data_f (SINC_FILTER *filter, SRC_DATA *data, int half_filter_chan_len) ;
79 static void prepare_data_d (SINC_FILTER *filter, SRC_DATA *data, int half_filter_chan_len) ;
80
81 static void sinc_reset (SRC_PRIVATE *psrc) ;
82
83 static inline increment_t
double_to_fp(double x)84 double_to_fp (double x)
85 { if (sizeof (increment_t) == 8)
86 return (llrint ((x) * FP_ONE)) ;
87 return (lrint ((x) * FP_ONE)) ;
88 } /* double_to_fp */
89
90 static inline increment_t
int_to_fp(int x)91 int_to_fp (int x)
92 { return (((increment_t) (x)) << SHIFT_BITS) ;
93 } /* int_to_fp */
94
95 static inline int
fp_to_int(increment_t x)96 fp_to_int (increment_t x)
97 { return (((x) >> SHIFT_BITS)) ;
98 } /* fp_to_int */
99
100 static inline increment_t
fp_fraction_part(increment_t x)101 fp_fraction_part (increment_t x)
102 { return ((x) & ((((increment_t) 1) << SHIFT_BITS) - 1)) ;
103 } /* fp_fraction_part */
104
105 static inline double
fp_to_double(increment_t x)106 fp_to_double (increment_t x)
107 { return fp_fraction_part (x) * INV_FP_ONE ;
108 } /* fp_to_double */
109
110
111 /*----------------------------------------------------------------------------------------
112 */
113
114 const char*
sinc_get_name(int src_enum)115 sinc_get_name (int src_enum)
116 {
117 switch (src_enum)
118 { case SRC_SINC_BEST_QUALITY :
119 return "Best Sinc Interpolator" ;
120
121 case SRC_SINC_MEDIUM_QUALITY :
122 return "Medium Sinc Interpolator" ;
123
124 case SRC_SINC_FASTEST :
125 return "Fastest Sinc Interpolator" ;
126
127 default: break ;
128 } ;
129
130 return NULL ;
131 } /* sinc_get_descrition */
132
133 const char*
sinc_get_description(int src_enum)134 sinc_get_description (int src_enum)
135 {
136 switch (src_enum)
137 { case SRC_SINC_FASTEST :
138 return "Band limited sinc interpolation, fastest, 97dB SNR, 80% BW." ;
139
140 case SRC_SINC_MEDIUM_QUALITY :
141 return "Band limited sinc interpolation, medium quality, 121dB SNR, 90% BW." ;
142
143 case SRC_SINC_BEST_QUALITY :
144 return "Band limited sinc interpolation, best quality, 145dB SNR, 96% BW." ;
145
146 default :
147 break ;
148 } ;
149
150 return NULL ;
151 } /* sinc_get_descrition */
152
153 int
gavl_sinc_set_converter(SRC_PRIVATE * psrc,int src_enum,int d)154 gavl_sinc_set_converter (SRC_PRIVATE *psrc, int src_enum, int d)
155 { SINC_FILTER *filter, temp_filter ;
156 increment_t count ;
157 int bits ;
158
159 /* Quick sanity check. */
160 if (SHIFT_BITS >= sizeof (increment_t) * 8 - 1)
161 return SRC_ERR_SHIFT_BITS ;
162
163 if (psrc->private_data != NULL)
164 { filter = (SINC_FILTER*) psrc->private_data ;
165 if (filter->sinc_magic_marker != SINC_MAGIC_MARKER)
166 { free (psrc->private_data) ;
167 psrc->private_data = NULL ;
168 } ;
169 } ;
170
171 memset (&temp_filter, 0, sizeof (temp_filter)) ;
172
173 temp_filter.sinc_magic_marker = SINC_MAGIC_MARKER ;
174 temp_filter.channels = psrc->channels ;
175 if(d)
176 {
177 psrc->const_process = sinc_vari_process_d ;
178 psrc->vari_process = sinc_vari_process_d ;
179 }
180 else
181 {
182 psrc->const_process = sinc_vari_process_f ;
183 psrc->vari_process = sinc_vari_process_f ;
184 }
185 psrc->reset = sinc_reset ;
186
187 switch (src_enum)
188 { case SRC_SINC_FASTEST :
189 temp_filter.coeffs = fastest_coeffs.coeffs ;
190 temp_filter.coeff_half_len = ARRAY_LEN (fastest_coeffs.coeffs) - 1 ;
191 temp_filter.index_inc = fastest_coeffs.increment ;
192 break ;
193
194 case SRC_SINC_MEDIUM_QUALITY :
195 temp_filter.coeffs = slow_mid_qual_coeffs.coeffs ;
196 temp_filter.coeff_half_len = ARRAY_LEN (slow_mid_qual_coeffs.coeffs) - 1 ;
197 temp_filter.index_inc = slow_mid_qual_coeffs.increment ;
198 break ;
199
200 case SRC_SINC_BEST_QUALITY :
201 temp_filter.coeffs = slow_high_qual_coeffs.coeffs ;
202 temp_filter.coeff_half_len = ARRAY_LEN (slow_high_qual_coeffs.coeffs) - 1 ;
203 temp_filter.index_inc = slow_high_qual_coeffs.increment ;
204 break ;
205
206 default :
207 return SRC_ERR_BAD_CONVERTER ;
208 } ;
209
210 /*
211 ** FIXME : This needs to be looked at more closely to see if there is
212 ** a better way. Need to look at prepare_data () at the same time.
213 */
214
215 temp_filter.b_len = 2 * lrint (1.0 + temp_filter.coeff_half_len / (temp_filter.index_inc * 1.0) * SRC_MAX_RATIO) ;
216 temp_filter.b_len = MAX (temp_filter.b_len, 4096) ;
217 temp_filter.b_len *= temp_filter.channels ;
218 temp_filter.d = d;
219 if(d)
220 {
221 if ((filter = calloc (1, sizeof (SINC_FILTER) + sizeof (filter->buffer_d [0]) * (temp_filter.b_len + temp_filter.channels))) == NULL)
222 return SRC_ERR_MALLOC_FAILED ;
223
224 }
225 else
226 {
227 if ((filter = calloc (1, sizeof (SINC_FILTER) + sizeof (filter->buffer_f [0]) * (temp_filter.b_len + temp_filter.channels))) == NULL)
228 return SRC_ERR_MALLOC_FAILED ;
229 }
230
231 *filter = temp_filter ;
232 memset (&temp_filter, 0xEE, sizeof (temp_filter)) ;
233
234 psrc->private_data = filter ;
235
236 sinc_reset (psrc) ;
237
238 count = filter->coeff_half_len ;
239 for (bits = 0 ; (MAKE_INCREMENT_T (1) << bits) < count ; bits++)
240 count |= (MAKE_INCREMENT_T (1) << bits) ;
241
242 if (bits + SHIFT_BITS - 1 >= (int) (sizeof (increment_t) * 8))
243 return SRC_ERR_FILTER_LEN ;
244
245 return SRC_ERR_NO_ERROR ;
246 } /* sinc_set_converter */
247
248 static void
sinc_reset(SRC_PRIVATE * psrc)249 sinc_reset (SRC_PRIVATE *psrc)
250 { SINC_FILTER *filter ;
251
252 filter = (SINC_FILTER*) psrc->private_data ;
253 if (filter == NULL)
254 return ;
255
256 filter->b_current = filter->b_end = 0 ;
257 filter->b_real_end = -1 ;
258
259 filter->src_ratio = filter->input_index = 0.0 ;
260 if(filter->d)
261 {
262 memset (filter->buffer_d, 0, filter->b_len * sizeof (filter->buffer_d [0])) ;
263 /* Set this for a sanity check */
264 memset (filter->buffer_d + filter->b_len, 0xAA, filter->channels * sizeof (filter->buffer_d [0])) ;
265 }
266 else
267 {
268 memset (filter->buffer_f, 0, filter->b_len * sizeof (filter->buffer_f [0])) ;
269 /* Set this for a sanity check */
270 memset (filter->buffer_f + filter->b_len, 0xAA, filter->channels * sizeof (filter->buffer_f [0])) ;
271 }
272
273 } /* sinc_reset */
274
275 /*========================================================================================
276 ** Beware all ye who dare pass this point. There be dragons here.
277 */
278
279 static int
sinc_vari_process_f(SRC_PRIVATE * psrc,SRC_DATA * data)280 sinc_vari_process_f (SRC_PRIVATE *psrc, SRC_DATA *data)
281 { SINC_FILTER *filter ;
282 double input_index, src_ratio, count, float_increment, terminate, rem ;
283 increment_t increment, start_filter_index ;
284 int half_filter_chan_len, samples_in_hand, ch ;
285
286 if (psrc->private_data == NULL)
287 return SRC_ERR_NO_PRIVATE ;
288
289 filter = (SINC_FILTER*) psrc->private_data ;
290 #if 0
291 /* If there is not a problem, this will be optimised out. */
292 if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
293 return SRC_ERR_SIZE_INCOMPATIBILITY ;
294 #endif
295 filter->in_count = data->input_frames * filter->channels ;
296 filter->out_count = data->output_frames * filter->channels ;
297 filter->in_used = filter->out_gen = 0 ;
298
299 src_ratio = psrc->last_ratio ;
300
301 /* Check the sample rate ratio wrt the buffer len. */
302 count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
303 if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
304 count /= MIN (psrc->last_ratio, data->src_ratio) ;
305
306 /* Maximum coefficientson either side of center point. */
307 half_filter_chan_len = filter->channels * (lrint (count) + 1) ;
308
309 input_index = psrc->last_position ;
310 float_increment = filter->index_inc ;
311
312 rem = fmod_one (input_index) ;
313 filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
314 input_index = rem ;
315
316 terminate = 1.0 / src_ratio + 1e-20 ;
317
318 /* Main processing loop. */
319 while (filter->out_gen < filter->out_count)
320 {
321 /* Need to reload buffer? */
322 samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
323
324 if (samples_in_hand <= half_filter_chan_len)
325 { prepare_data_f (filter, data, half_filter_chan_len) ;
326
327 samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
328 if (samples_in_hand <= half_filter_chan_len)
329 break ;
330 } ;
331
332 /* This is the termination condition. */
333 if (filter->b_real_end >= 0)
334 { if (filter->b_current + input_index + terminate >= filter->b_real_end)
335 break ;
336 } ;
337
338 if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
339 src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;
340
341 float_increment = filter->index_inc * 1.0 ;
342 if (src_ratio < 1.0)
343 float_increment = filter->index_inc * src_ratio ;
344
345 increment = double_to_fp (float_increment) ;
346
347 start_filter_index = double_to_fp (input_index * float_increment) ;
348
349 for (ch = 0 ; ch < filter->channels ; ch++)
350 { data->data_out_f [filter->out_gen] = (float) ((float_increment / filter->index_inc) *
351 calc_output_f (filter, increment, start_filter_index, ch)) ;
352 filter->out_gen ++ ;
353 } ;
354
355 /* Figure out the next index. */
356 input_index += 1.0 / src_ratio ;
357 rem = fmod_one (input_index) ;
358
359 filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
360 input_index = rem ;
361 } ;
362
363 psrc->last_position = input_index ;
364
365 /* Save current ratio rather then target ratio. */
366 psrc->last_ratio = src_ratio ;
367
368 data->input_frames_used = filter->in_used / filter->channels ;
369 data->output_frames_gen = filter->out_gen / filter->channels ;
370
371 return SRC_ERR_NO_ERROR ;
372 } /* sinc_vari_process_f */
373
374 static int
sinc_vari_process_d(SRC_PRIVATE * psrc,SRC_DATA * data)375 sinc_vari_process_d (SRC_PRIVATE *psrc, SRC_DATA *data)
376 { SINC_FILTER *filter ;
377 double input_index, src_ratio, count, float_increment, terminate, rem ;
378 increment_t increment, start_filter_index ;
379 int half_filter_chan_len, samples_in_hand, ch ;
380
381 if (psrc->private_data == NULL)
382 return SRC_ERR_NO_PRIVATE ;
383
384 filter = (SINC_FILTER*) psrc->private_data ;
385 #if 0
386 /* If there is not a problem, this will be optimised out. */
387 if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
388 return SRC_ERR_SIZE_INCOMPATIBILITY ;
389 #endif
390 filter->in_count = data->input_frames * filter->channels ;
391 filter->out_count = data->output_frames * filter->channels ;
392 filter->in_used = filter->out_gen = 0 ;
393
394 src_ratio = psrc->last_ratio ;
395
396 /* Check the sample rate ratio wrt the buffer len. */
397 count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
398 if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
399 count /= MIN (psrc->last_ratio, data->src_ratio) ;
400
401 /* Maximum coefficientson either side of center point. */
402 half_filter_chan_len = filter->channels * (lrint (count) + 1) ;
403
404 input_index = psrc->last_position ;
405 float_increment = filter->index_inc ;
406
407 rem = fmod_one (input_index) ;
408 filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
409 input_index = rem ;
410
411 terminate = 1.0 / src_ratio + 1e-20 ;
412
413 /* Main processing loop. */
414 while (filter->out_gen < filter->out_count)
415 {
416 /* Need to reload buffer? */
417 samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
418
419 if (samples_in_hand <= half_filter_chan_len)
420 { prepare_data_d (filter, data, half_filter_chan_len) ;
421
422 samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
423 if (samples_in_hand <= half_filter_chan_len)
424 break ;
425 } ;
426
427 /* This is the termination condition. */
428 if (filter->b_real_end >= 0)
429 { if (filter->b_current + input_index + terminate >= filter->b_real_end)
430 break ;
431 } ;
432
433 if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
434 src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;
435
436 float_increment = filter->index_inc * 1.0 ;
437 if (src_ratio < 1.0)
438 float_increment = filter->index_inc * src_ratio ;
439
440 increment = double_to_fp (float_increment) ;
441
442 start_filter_index = double_to_fp (input_index * float_increment) ;
443
444 for (ch = 0 ; ch < filter->channels ; ch++)
445 { data->data_out_d [filter->out_gen] = (float) ((float_increment / filter->index_inc) *
446 calc_output_d (filter, increment, start_filter_index, ch)) ;
447 filter->out_gen ++ ;
448 } ;
449
450 /* Figure out the next index. */
451 input_index += 1.0 / src_ratio ;
452 rem = fmod_one (input_index) ;
453
454 filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
455 input_index = rem ;
456 } ;
457
458 psrc->last_position = input_index ;
459
460 /* Save current ratio rather then target ratio. */
461 psrc->last_ratio = src_ratio ;
462
463 data->input_frames_used = filter->in_used / filter->channels ;
464 data->output_frames_gen = filter->out_gen / filter->channels ;
465
466 return SRC_ERR_NO_ERROR ;
467 } /* sinc_vari_process_d */
468
469
470 /*----------------------------------------------------------------------------------------
471 */
472
473 static void
prepare_data_f(SINC_FILTER * filter,SRC_DATA * data,int half_filter_chan_len)474 prepare_data_f (SINC_FILTER *filter, SRC_DATA *data, int half_filter_chan_len)
475 { int len = 0 ;
476
477 if (filter->b_real_end >= 0)
478 return ; /* This doesn't make sense, so return. */
479
480 if (filter->b_current == 0)
481 { /* Initial state. Set up zeros at the start of the buffer and
482 ** then load new data after that.
483 */
484 len = filter->b_len - 2 * half_filter_chan_len ;
485
486 filter->b_current = filter->b_end = half_filter_chan_len ;
487 }
488 else if (filter->b_end + half_filter_chan_len + filter->channels < filter->b_len)
489 { /* Load data at current end position. */
490 len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0) ;
491 }
492 else
493 { /* Move data at end of buffer back to the start of the buffer. */
494 len = filter->b_end - filter->b_current ;
495 memmove (filter->buffer_f, filter->buffer_f + filter->b_current - half_filter_chan_len,
496 (half_filter_chan_len + len) * sizeof (filter->buffer_f [0])) ;
497
498 filter->b_current = half_filter_chan_len ;
499 filter->b_end = filter->b_current + len ;
500
501 /* Now load data at current end of buffer. */
502 len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0) ;
503 } ;
504
505 len = MIN (filter->in_count - filter->in_used, len) ;
506 len -= (len % filter->channels) ;
507
508 memcpy (filter->buffer_f + filter->b_end, data->data_in_f + filter->in_used,
509 len * sizeof (filter->buffer_f [0])) ;
510
511 filter->b_end += len ;
512 filter->in_used += len ;
513
514 if (filter->in_used == filter->in_count &&
515 filter->b_end - filter->b_current < 2 * half_filter_chan_len && data->end_of_input)
516 { /* Handle the case where all data in the current buffer has been
517 ** consumed and this is the last buffer.
518 */
519
520 if (filter->b_len - filter->b_end < half_filter_chan_len + 5)
521 { /* If necessary, move data down to the start of the buffer. */
522 len = filter->b_end - filter->b_current ;
523 memmove (filter->buffer_f, filter->buffer_f + filter->b_current - half_filter_chan_len,
524 (half_filter_chan_len + len) * sizeof (filter->buffer_f [0])) ;
525
526 filter->b_current = half_filter_chan_len ;
527 filter->b_end = filter->b_current + len ;
528 } ;
529
530 filter->b_real_end = filter->b_end ;
531 len = half_filter_chan_len + 5 ;
532
533 memset (filter->buffer_f + filter->b_end, 0, len * sizeof (filter->buffer_f [0])) ;
534 filter->b_end += len ;
535 } ;
536
537 return ;
538 } /* prepare_data_f */
539
540 static void
prepare_data_d(SINC_FILTER * filter,SRC_DATA * data,int half_filter_chan_len)541 prepare_data_d (SINC_FILTER *filter, SRC_DATA *data, int half_filter_chan_len)
542 { int len = 0 ;
543
544 if (filter->b_real_end >= 0)
545 return ; /* This doesn't make sense, so return. */
546
547 if (filter->b_current == 0)
548 { /* Initial state. Set up zeros at the start of the buffer and
549 ** then load new data after that.
550 */
551 len = filter->b_len - 2 * half_filter_chan_len ;
552
553 filter->b_current = filter->b_end = half_filter_chan_len ;
554 }
555 else if (filter->b_end + half_filter_chan_len + filter->channels < filter->b_len)
556 { /* Load data at current end position. */
557 len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0) ;
558 }
559 else
560 { /* Move data at end of buffer back to the start of the buffer. */
561 len = filter->b_end - filter->b_current ;
562 memmove (filter->buffer_d, filter->buffer_d + filter->b_current - half_filter_chan_len,
563 (half_filter_chan_len + len) * sizeof (filter->buffer_d [0])) ;
564
565 filter->b_current = half_filter_chan_len ;
566 filter->b_end = filter->b_current + len ;
567
568 /* Now load data at current end of buffer. */
569 len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0) ;
570 } ;
571
572 len = MIN (filter->in_count - filter->in_used, len) ;
573 len -= (len % filter->channels) ;
574
575 memcpy (filter->buffer_d + filter->b_end, data->data_in_d + filter->in_used,
576 len * sizeof (filter->buffer_d [0])) ;
577
578 filter->b_end += len ;
579 filter->in_used += len ;
580
581 if (filter->in_used == filter->in_count &&
582 filter->b_end - filter->b_current < 2 * half_filter_chan_len && data->end_of_input)
583 { /* Handle the case where all data in the current buffer has been
584 ** consumed and this is the last buffer.
585 */
586
587 if (filter->b_len - filter->b_end < half_filter_chan_len + 5)
588 { /* If necessary, move data down to the start of the buffer. */
589 len = filter->b_end - filter->b_current ;
590 memmove (filter->buffer_d, filter->buffer_d + filter->b_current - half_filter_chan_len,
591 (half_filter_chan_len + len) * sizeof (filter->buffer_d [0])) ;
592
593 filter->b_current = half_filter_chan_len ;
594 filter->b_end = filter->b_current + len ;
595 } ;
596
597 filter->b_real_end = filter->b_end ;
598 len = half_filter_chan_len + 5 ;
599
600 memset (filter->buffer_d + filter->b_end, 0, len * sizeof (filter->buffer_d [0])) ;
601 filter->b_end += len ;
602 } ;
603
604 return ;
605 } /* prepare_data_d */
606
607
608
609 static double
calc_output_f(SINC_FILTER * filter,increment_t increment,increment_t start_filter_index,int ch)610 calc_output_f (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int ch)
611 { double fraction, left, right, icoeff ;
612 increment_t filter_index, max_filter_index ;
613 int data_index, coeff_count, indx ;
614
615 /* Convert input parameters into fixed point. */
616 max_filter_index = int_to_fp (filter->coeff_half_len) ;
617
618 /* First apply the left half of the filter. */
619 filter_index = start_filter_index ;
620 coeff_count = (max_filter_index - filter_index) / increment ;
621 filter_index = filter_index + coeff_count * increment ;
622 data_index = filter->b_current - filter->channels * coeff_count + ch ;
623
624 left = 0.0 ;
625 do
626 { fraction = fp_to_double (filter_index) ;
627 indx = fp_to_int (filter_index) ;
628
629 icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
630
631 left += icoeff * filter->buffer_f [data_index] ;
632
633 filter_index -= increment ;
634 data_index = data_index + filter->channels ;
635 }
636 while (filter_index >= MAKE_INCREMENT_T (0)) ;
637
638 /* Now apply the right half of the filter. */
639 filter_index = increment - start_filter_index ;
640 coeff_count = (max_filter_index - filter_index) / increment ;
641 filter_index = filter_index + coeff_count * increment ;
642 data_index = filter->b_current + filter->channels * (1 + coeff_count) + ch ;
643
644 right = 0.0 ;
645 do
646 { fraction = fp_to_double (filter_index) ;
647 indx = fp_to_int (filter_index) ;
648
649 icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
650
651 right += icoeff * filter->buffer_f [data_index] ;
652
653 filter_index -= increment ;
654 data_index = data_index - filter->channels ;
655 }
656 while (filter_index > MAKE_INCREMENT_T (0)) ;
657
658 return (left + right) ;
659 } /* calc_output_f */
660
661 static double
calc_output_d(SINC_FILTER * filter,increment_t increment,increment_t start_filter_index,int ch)662 calc_output_d (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int ch)
663 { double fraction, left, right, icoeff ;
664 increment_t filter_index, max_filter_index ;
665 int data_index, coeff_count, indx ;
666
667 /* Convert input parameters into fixed point. */
668 max_filter_index = int_to_fp (filter->coeff_half_len) ;
669
670 /* First apply the left half of the filter. */
671 filter_index = start_filter_index ;
672 coeff_count = (max_filter_index - filter_index) / increment ;
673 filter_index = filter_index + coeff_count * increment ;
674 data_index = filter->b_current - filter->channels * coeff_count + ch ;
675
676 left = 0.0 ;
677 do
678 { fraction = fp_to_double (filter_index) ;
679 indx = fp_to_int (filter_index) ;
680
681 icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
682
683 left += icoeff * filter->buffer_d [data_index] ;
684
685 filter_index -= increment ;
686 data_index = data_index + filter->channels ;
687 }
688 while (filter_index >= MAKE_INCREMENT_T (0)) ;
689
690 /* Now apply the right half of the filter. */
691 filter_index = increment - start_filter_index ;
692 coeff_count = (max_filter_index - filter_index) / increment ;
693 filter_index = filter_index + coeff_count * increment ;
694 data_index = filter->b_current + filter->channels * (1 + coeff_count) + ch ;
695
696 right = 0.0 ;
697 do
698 { fraction = fp_to_double (filter_index) ;
699 indx = fp_to_int (filter_index) ;
700
701 icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
702
703 right += icoeff * filter->buffer_d [data_index] ;
704
705 filter_index -= increment ;
706 data_index = data_index - filter->channels ;
707 }
708 while (filter_index > MAKE_INCREMENT_T (0)) ;
709
710 return (left + right) ;
711 } /* calc_output_d */
712
713