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