1 /*- 2 * Copyright (c) 1999 Cameron Grant <cg@FreeBSD.org> 3 * Copyright (c) 2003 Orion Hodson <orion@FreeBSD.org> 4 * Copyright (c) 2005 Ariff Abdullah <ariff@FreeBSD.org> 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 * 28 * 2005-06-11: 29 * ========== 30 * 31 * *New* and rewritten soft sample rate converter supporting arbitrary sample 32 * rates, fine grained scaling/coefficients and a unified up/down stereo 33 * converter. Most of the disclaimers from orion's notes also applies 34 * here, regarding linear interpolation deficiencies and pre/post 35 * anti-aliasing filtering issues. This version comes with a much simpler and 36 * tighter interface, although it works almost exactly like the older one. 37 * 38 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 39 * * 40 * This new implementation is fully dedicated in memory of Cameron Grant, * 41 * the creator of the magnificent, highly addictive feeder infrastructure. * 42 * * 43 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 44 * 45 * Orion's notes: 46 * ============= 47 * 48 * This rate conversion code uses linear interpolation without any 49 * pre- or post- interpolation filtering to combat aliasing. This 50 * greatly limits the sound quality and should be addressed at some 51 * stage in the future. 52 * 53 * Since this accuracy of interpolation is sensitive and examination 54 * of the algorithm output is harder from the kernel, the code is 55 * designed to be compiled in the kernel and in a userland test 56 * harness. This is done by selectively including and excluding code 57 * with several portions based on whether _KERNEL is defined. It's a 58 * little ugly, but exceedingly useful. The testsuite and its 59 * revisions can be found at: 60 * http://people.freebsd.org/~orion/files/feedrate/ 61 * 62 * Special thanks to Ken Marx for exposing flaws in the code and for 63 * testing revisions. 64 */ 65 66 #include <dev/sound/pcm/sound.h> 67 #include "feeder_if.h" 68 69 SND_DECLARE_FILE("$FreeBSD$"); 70 71 #define RATE_ASSERT(x, y) /* KASSERT(x,y) */ 72 #define RATE_TEST(x, y) /* if (!(x)) printf y */ 73 #define RATE_TRACE(x...) /* printf(x) */ 74 75 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder"); 76 77 #define FEEDBUFSZ 8192 78 #define ROUNDHZ 25 79 #define RATEMIN 4000 80 /* 8000 * 138 or 11025 * 100 . This is insane, indeed! */ 81 #define RATEMAX 1102500 82 #define MINGAIN 92 83 #define MAXGAIN 96 84 85 #define FEEDRATE_CONVERT_64 0 86 #define FEEDRATE_CONVERT_SCALE64 1 87 #define FEEDRATE_CONVERT_SCALE32 2 88 #define FEEDRATE_CONVERT_PLAIN 3 89 #define FEEDRATE_CONVERT_FIXED 4 90 #define FEEDRATE_CONVERT_OPTIMAL 5 91 #define FEEDRATE_CONVERT_WORST 6 92 93 #define FEEDRATE_64_MAXROLL 32 94 #define FEEDRATE_32_MAXROLL 16 95 96 struct feed_rate_info { 97 uint32_t src, dst; /* rounded source / destination rates */ 98 uint32_t rsrc, rdst; /* original source / destination rates */ 99 uint32_t gx, gy; /* interpolation / decimation ratio */ 100 uint32_t alpha; /* interpolation distance */ 101 uint32_t pos, bpos; /* current sample / buffer positions */ 102 uint32_t bufsz; /* total buffer size */ 103 uint32_t stray; /* stray bytes */ 104 int32_t scale, roll; /* scale / roll factor */ 105 int16_t *buffer; 106 uint32_t (*convert)(struct feed_rate_info *, int16_t *, uint32_t); 107 }; 108 109 static uint32_t 110 feed_convert_64(struct feed_rate_info *, int16_t *, uint32_t); 111 static uint32_t 112 feed_convert_scale64(struct feed_rate_info *, int16_t *, uint32_t); 113 static uint32_t 114 feed_convert_scale32(struct feed_rate_info *, int16_t *, uint32_t); 115 static uint32_t 116 feed_convert_plain(struct feed_rate_info *, int16_t *, uint32_t); 117 118 int feeder_rate_ratemin = RATEMIN; 119 int feeder_rate_ratemax = RATEMAX; 120 /* 121 * See 'Feeder Scaling Type' below.. 122 */ 123 static int feeder_rate_scaling = FEEDRATE_CONVERT_OPTIMAL; 124 static int feeder_rate_buffersize = FEEDBUFSZ & ~1; 125 126 /* 127 * sysctls.. I love sysctls.. 128 */ 129 TUNABLE_INT("hw.snd.feeder_rate_ratemin", &feeder_rate_ratemin); 130 TUNABLE_INT("hw.snd.feeder_rate_ratemax", &feeder_rate_ratemin); 131 TUNABLE_INT("hw.snd.feeder_rate_scaling", &feeder_rate_scaling); 132 TUNABLE_INT("hw.snd.feeder_rate_buffersize", &feeder_rate_buffersize); 133 134 static int 135 sysctl_hw_snd_feeder_rate_ratemin(SYSCTL_HANDLER_ARGS) 136 { 137 int err, val; 138 139 val = feeder_rate_ratemin; 140 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 141 if (val < 1 || val >= feeder_rate_ratemax) 142 err = EINVAL; 143 else 144 feeder_rate_ratemin = val; 145 return err; 146 } 147 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemin, CTLTYPE_INT | CTLFLAG_RW, 148 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemin, "I", ""); 149 150 static int 151 sysctl_hw_snd_feeder_rate_ratemax(SYSCTL_HANDLER_ARGS) 152 { 153 int err, val; 154 155 val = feeder_rate_ratemax; 156 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 157 if (val <= feeder_rate_ratemin || val > 0x7fffff) 158 err = EINVAL; 159 else 160 feeder_rate_ratemax = val; 161 return err; 162 } 163 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemax, CTLTYPE_INT | CTLFLAG_RW, 164 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemax, "I", ""); 165 166 static int 167 sysctl_hw_snd_feeder_rate_scaling(SYSCTL_HANDLER_ARGS) 168 { 169 int err, val; 170 171 val = feeder_rate_scaling; 172 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 173 /* 174 * Feeder Scaling Type 175 * =================== 176 * 177 * 1. Plain 64bit (high precision) 178 * 2. 64bit scaling (high precision, CPU friendly, but can 179 * cause gain up/down). 180 * 3. 32bit scaling (somehow can cause hz roundup, gain 181 * up/down). 182 * 4. Plain copy (default if src == dst. Except if src == dst, 183 * this is the worst / silly conversion method!). 184 * 185 * Sysctl options:- 186 * 187 * 0 - Plain 64bit - no fallback. 188 * 1 - 64bit scaling - no fallback. 189 * 2 - 32bit scaling - no fallback. 190 * 3 - Plain copy - no fallback. 191 * 4 - Fixed rate. Means that, choose optimal conversion method 192 * without causing hz roundup. 193 * 32bit scaling (as long as hz roundup does not occur), 194 * 64bit scaling, Plain 64bit. 195 * 5 - Optimal / CPU friendly (DEFAULT). 196 * 32bit scaling, 64bit scaling, Plain 64bit 197 * 6 - Optimal to worst, no 64bit arithmetic involved. 198 * 32bit scaling, Plain copy. 199 */ 200 if (val < FEEDRATE_CONVERT_64 || val > FEEDRATE_CONVERT_WORST) 201 err = EINVAL; 202 else 203 feeder_rate_scaling = val; 204 return err; 205 } 206 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_scaling, CTLTYPE_INT | CTLFLAG_RW, 207 0, sizeof(int), sysctl_hw_snd_feeder_rate_scaling, "I", ""); 208 209 static int 210 sysctl_hw_snd_feeder_rate_buffersize(SYSCTL_HANDLER_ARGS) 211 { 212 int err, val; 213 214 val = feeder_rate_buffersize; 215 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 216 /* 217 * Don't waste too much kernel space 218 */ 219 if (val < 2 || val > 65536) 220 err = EINVAL; 221 else 222 feeder_rate_buffersize = val & ~1; 223 return err; 224 } 225 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_buffersize, CTLTYPE_INT | CTLFLAG_RW, 226 0, sizeof(int), sysctl_hw_snd_feeder_rate_buffersize, "I", ""); 227 228 static void 229 feed_speed_ratio(uint32_t x, uint32_t y, uint32_t *gx, uint32_t *gy) 230 { 231 uint32_t w, src = x, dst = y; 232 233 while (y != 0) { 234 w = x % y; 235 x = y; 236 y = w; 237 } 238 *gx = src / x; 239 *gy = dst / x; 240 } 241 242 static void 243 feed_scale_roll(uint32_t dst, int32_t *scale, int32_t *roll, int32_t max) 244 { 245 int64_t k, tscale; 246 int32_t j, troll; 247 248 *scale = *roll = -1; 249 for (j = MAXGAIN; j >= MINGAIN; j -= 3) { 250 for (troll = 0; troll < max; troll++) { 251 tscale = (1 << troll) / dst; 252 k = (tscale * dst * 100) >> troll; 253 if (k > j && k <= 100) { 254 *scale = tscale; 255 *roll = troll; 256 return; 257 } 258 } 259 } 260 } 261 262 static int 263 feed_get_best_coef(uint32_t *src, uint32_t *dst, uint32_t *gx, uint32_t *gy, 264 int32_t *scale, int32_t *roll) 265 { 266 uint32_t tsrc, tdst, sscale, dscale; 267 int32_t tscale, troll; 268 int i, j, hzmin, hzmax; 269 270 *scale = *roll = -1; 271 for (i = 0; i < 2; i++) { 272 hzmin = (ROUNDHZ * i) + 1; 273 hzmax = hzmin + ROUNDHZ; 274 for (j = hzmin; j < hzmax; j++) { 275 tsrc = *src - (*src % j); 276 tdst = *dst; 277 if (tsrc < 1 || tdst < 1) 278 goto coef_failed; 279 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 280 feed_scale_roll(dscale, &tscale, &troll, 281 FEEDRATE_32_MAXROLL); 282 if (tscale != -1 && troll != -1) { 283 *src = tsrc; 284 *gx = sscale; 285 *gy = dscale; 286 *scale = tscale; 287 *roll = troll; 288 return j; 289 } 290 } 291 for (j = hzmin; j < hzmax; j++) { 292 tsrc = *src - (*src % j); 293 tdst = *dst - (*dst % j); 294 if (tsrc < 1 || tdst < 1) 295 goto coef_failed; 296 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 297 feed_scale_roll(dscale, &tscale, &troll, 298 FEEDRATE_32_MAXROLL); 299 if (tscale != -1 && troll != -1) { 300 *src = tsrc; 301 *dst = tdst; 302 *gx = sscale; 303 *gy = dscale; 304 *scale = tscale; 305 *roll = troll; 306 return j; 307 } 308 } 309 for (j = hzmin; j < hzmax; j++) { 310 tsrc = *src; 311 tdst = *dst - (*dst % j); 312 if (tsrc < 1 || tdst < 1) 313 goto coef_failed; 314 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 315 feed_scale_roll(dscale, &tscale, &troll, 316 FEEDRATE_32_MAXROLL); 317 if (tscale != -1 && troll != -1) { 318 *src = tsrc; 319 *dst = tdst; 320 *gx = sscale; 321 *gy = dscale; 322 *scale = tscale; 323 *roll = troll; 324 return j; 325 } 326 } 327 } 328 coef_failed: 329 feed_speed_ratio(*src, *dst, gx, gy); 330 feed_scale_roll(*gy, scale, roll, FEEDRATE_32_MAXROLL); 331 return 0; 332 } 333 334 static void 335 feed_rate_reset(struct feed_rate_info *info) 336 { 337 info->scale = -1; 338 info->roll = -1; 339 info->src = info->rsrc; 340 info->dst = info->rdst; 341 info->gx = 0; 342 info->gy = 0; 343 } 344 345 static int 346 feed_rate_setup(struct pcm_feeder *f) 347 { 348 struct feed_rate_info *info = f->data; 349 int r = 0; 350 351 info->pos = 2; 352 info->bpos = 4; 353 info->alpha = 0; 354 info->stray = 0; 355 feed_rate_reset(info); 356 if (info->src == info->dst) { 357 /* 358 * No conversion ever needed. Just do plain copy. 359 */ 360 info->convert = feed_convert_plain; 361 info->gx = 1; 362 info->gy = 1; 363 } else { 364 switch (feeder_rate_scaling) { 365 case FEEDRATE_CONVERT_64: 366 feed_speed_ratio(info->src, info->dst, 367 &info->gx, &info->gy); 368 info->convert = feed_convert_64; 369 break; 370 case FEEDRATE_CONVERT_SCALE64: 371 feed_speed_ratio(info->src, info->dst, 372 &info->gx, &info->gy); 373 feed_scale_roll(info->gy, &info->scale, 374 &info->roll, FEEDRATE_64_MAXROLL); 375 if (info->scale == -1 || info->roll == -1) 376 return -1; 377 info->convert = feed_convert_scale64; 378 break; 379 case FEEDRATE_CONVERT_SCALE32: 380 r = feed_get_best_coef(&info->src, &info->dst, 381 &info->gx, &info->gy, &info->scale, 382 &info->roll); 383 if (r == 0) 384 return -1; 385 info->convert = feed_convert_scale32; 386 break; 387 case FEEDRATE_CONVERT_PLAIN: 388 feed_speed_ratio(info->src, info->dst, 389 &info->gx, &info->gy); 390 info->convert = feed_convert_plain; 391 break; 392 case FEEDRATE_CONVERT_FIXED: 393 r = feed_get_best_coef(&info->src, &info->dst, 394 &info->gx, &info->gy, &info->scale, 395 &info->roll); 396 if (r != 0 && info->src == info->rsrc && 397 info->dst == info->rdst) 398 info->convert = feed_convert_scale32; 399 else { 400 /* Fallback */ 401 feed_rate_reset(info); 402 feed_speed_ratio(info->src, info->dst, 403 &info->gx, &info->gy); 404 feed_scale_roll(info->gy, &info->scale, 405 &info->roll, FEEDRATE_64_MAXROLL); 406 if (info->scale != -1 && info->roll != -1) 407 info->convert = feed_convert_scale64; 408 else 409 info->convert = feed_convert_64; 410 } 411 break; 412 case FEEDRATE_CONVERT_OPTIMAL: 413 r = feed_get_best_coef(&info->src, &info->dst, 414 &info->gx, &info->gy, &info->scale, 415 &info->roll); 416 if (r != 0) 417 info->convert = feed_convert_scale32; 418 else { 419 /* Fallback */ 420 feed_rate_reset(info); 421 feed_speed_ratio(info->src, info->dst, 422 &info->gx, &info->gy); 423 feed_scale_roll(info->gy, &info->scale, 424 &info->roll, FEEDRATE_64_MAXROLL); 425 if (info->scale != -1 && info->roll != -1) 426 info->convert = feed_convert_scale64; 427 else 428 info->convert = feed_convert_64; 429 } 430 break; 431 case FEEDRATE_CONVERT_WORST: 432 r = feed_get_best_coef(&info->src, &info->dst, 433 &info->gx, &info->gy, &info->scale, 434 &info->roll); 435 if (r != 0) 436 info->convert = feed_convert_scale32; 437 else { 438 /* Fallback */ 439 feed_rate_reset(info); 440 feed_speed_ratio(info->src, info->dst, 441 &info->gx, &info->gy); 442 info->convert = feed_convert_plain; 443 } 444 break; 445 default: 446 return -1; 447 break; 448 } 449 /* No way! */ 450 if (info->gx == 0 || info->gy == 0) 451 return -1; 452 /* 453 * No need to interpolate/decimate, just do plain copy. 454 * This probably caused by Hz roundup. 455 */ 456 if (info->gx == info->gy) 457 info->convert = feed_convert_plain; 458 } 459 return 0; 460 } 461 462 static int 463 feed_rate_set(struct pcm_feeder *f, int what, int value) 464 { 465 struct feed_rate_info *info = f->data; 466 467 if (value < feeder_rate_ratemin || value > feeder_rate_ratemax) 468 return -1; 469 470 switch (what) { 471 case FEEDRATE_SRC: 472 info->rsrc = value; 473 break; 474 case FEEDRATE_DST: 475 info->rdst = value; 476 break; 477 default: 478 return -1; 479 } 480 return feed_rate_setup(f); 481 } 482 483 static int 484 feed_rate_get(struct pcm_feeder *f, int what) 485 { 486 struct feed_rate_info *info = f->data; 487 488 /* 489 * Return *real* src/dst rate. 490 */ 491 switch (what) { 492 case FEEDRATE_SRC: 493 return info->rsrc; 494 case FEEDRATE_DST: 495 return info->rdst; 496 default: 497 return -1; 498 } 499 return -1; 500 } 501 502 static int 503 feed_rate_init(struct pcm_feeder *f) 504 { 505 struct feed_rate_info *info; 506 507 info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO); 508 if (info == NULL) 509 return ENOMEM; 510 /* 511 * bufsz = sample from last cycle + conversion space 512 */ 513 info->bufsz = 2 + feeder_rate_buffersize; 514 info->buffer = malloc(sizeof(*info->buffer) * info->bufsz, 515 M_RATEFEEDER, M_NOWAIT | M_ZERO); 516 if (info->buffer == NULL) { 517 free(info, M_RATEFEEDER); 518 return ENOMEM; 519 } 520 info->rsrc = DSP_DEFAULT_SPEED; 521 info->rdst = DSP_DEFAULT_SPEED; 522 f->data = info; 523 return feed_rate_setup(f); 524 } 525 526 static int 527 feed_rate_free(struct pcm_feeder *f) 528 { 529 struct feed_rate_info *info = f->data; 530 531 if (info) { 532 if (info->buffer) 533 free(info->buffer, M_RATEFEEDER); 534 free(info, M_RATEFEEDER); 535 } 536 f->data = NULL; 537 return 0; 538 } 539 540 static uint32_t 541 feed_convert_64(struct feed_rate_info *info, int16_t *dst, uint32_t max) 542 { 543 int64_t x, alpha, distance; 544 uint32_t ret; 545 int32_t pos, bpos, gx, gy; 546 int16_t *src; 547 /* 548 * Plain, straight forward 64bit arith. No bit-magic applied here. 549 */ 550 ret = 0; 551 alpha = info->alpha; 552 gx = info->gx; 553 gy = info->gy; 554 pos = info->pos; 555 bpos = info->bpos; 556 src = info->buffer; 557 for (;;) { 558 if (alpha < gx) { 559 alpha += gy; 560 pos += 2; 561 if (pos == bpos) 562 break; 563 } else { 564 alpha -= gx; 565 distance = gy - alpha; 566 x = (alpha * src[pos - 2]) + (distance * src[pos]); 567 dst[ret++] = x / gy; 568 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]); 569 dst[ret++] = x / gy; 570 if (ret == max) 571 break; 572 } 573 } 574 info->alpha = alpha; 575 info->pos = pos; 576 return ret; 577 } 578 579 static uint32_t 580 feed_convert_scale64(struct feed_rate_info *info, int16_t *dst, uint32_t max) 581 { 582 int64_t x, alpha, distance; 583 uint32_t ret; 584 int32_t pos, bpos, gx, gy, roll; 585 int16_t *src; 586 /* 587 * 64bit scaling. 588 */ 589 ret = 0; 590 roll = info->roll; 591 alpha = info->alpha * info->scale; 592 gx = info->gx * info->scale; 593 gy = info->gy * info->scale; 594 pos = info->pos; 595 bpos = info->bpos; 596 src = info->buffer; 597 for (;;) { 598 if (alpha < gx) { 599 alpha += gy; 600 pos += 2; 601 if (pos == bpos) 602 break; 603 } else { 604 alpha -= gx; 605 distance = gy - alpha; 606 x = (alpha * src[pos - 2]) + (distance * src[pos]); 607 dst[ret++] = x >> roll; 608 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]); 609 dst[ret++] = x >> roll; 610 if (ret == max) 611 break; 612 } 613 } 614 info->alpha = alpha / info->scale; 615 info->pos = pos; 616 return ret; 617 } 618 619 static uint32_t 620 feed_convert_scale32(struct feed_rate_info *info, int16_t *dst, uint32_t max) 621 { 622 uint32_t ret; 623 int32_t x, pos, bpos, gx, gy, alpha, roll, distance; 624 int16_t *src; 625 /* 626 * 32bit scaling. 627 */ 628 ret = 0; 629 roll = info->roll; 630 alpha = info->alpha * info->scale; 631 gx = info->gx * info->scale; 632 gy = info->gy * info->scale; 633 pos = info->pos; 634 bpos = info->bpos; 635 src = info->buffer; 636 for (;;) { 637 if (alpha < gx) { 638 alpha += gy; 639 pos += 2; 640 if (pos == bpos) 641 break; 642 } else { 643 alpha -= gx; 644 distance = gy - alpha; 645 x = (alpha * src[pos - 2]) + (distance * src[pos]); 646 dst[ret++] = x >> roll; 647 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]); 648 dst[ret++] = x >> roll; 649 if (ret == max) 650 break; 651 } 652 } 653 info->alpha = alpha / info->scale; 654 info->pos = pos; 655 return ret; 656 } 657 658 static uint32_t 659 feed_convert_plain(struct feed_rate_info *info, int16_t *dst, uint32_t max) 660 { 661 uint32_t ret; 662 int32_t pos, bpos, gx, gy, alpha; 663 int16_t *src; 664 /* 665 * Plain copy. 666 */ 667 ret = 0; 668 gx = info->gx; 669 gy = info->gy; 670 alpha = info->alpha; 671 pos = info->pos; 672 bpos = info->bpos; 673 src = info->buffer; 674 for (;;) { 675 if (alpha < gx) { 676 alpha += gy; 677 pos += 2; 678 if (pos == bpos) 679 break; 680 } else { 681 alpha -= gx; 682 dst[ret++] = src[pos]; 683 dst[ret++] = src[pos + 1]; 684 if (ret == max) 685 break; 686 } 687 } 688 info->pos = pos; 689 info->alpha = alpha; 690 return ret; 691 } 692 693 static int32_t 694 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b, 695 uint32_t count, void *source) 696 { 697 struct feed_rate_info *info = f->data; 698 uint32_t i; 699 int32_t fetch, slot; 700 int16_t *dst = (int16_t *)b; 701 /* 702 * This loop has been optimized to generalize both up / down 703 * sampling without causing missing samples or excessive buffer 704 * feeding. 705 */ 706 RATE_TEST(count >= 4 && (count & 3) == 0, 707 ("%s: Count size not byte integral (%d)\n", __func__, count)); 708 if (count < 4) 709 return 0; 710 count >>= 1; 711 count &= ~1; 712 slot = (((info->gx * (count >> 1)) + info->gy - info->alpha - 1) / info->gy) << 1; 713 RATE_TEST((slot & 1) == 0, ("%s: Slot count not sample integral (%d)\n", 714 __func__, slot)); 715 /* 716 * Optimize buffer feeding aggressively to ensure calculated slot 717 * can be fitted nicely into available buffer free space, hence 718 * avoiding multiple feeding. 719 */ 720 RATE_TEST(info->stray == 0, ("%s: [1] Stray bytes: %u\n", 721 __func__,info->stray)); 722 if (info->pos != 2 && info->bpos - info->pos == 2 && 723 info->bpos + slot > info->bufsz) { 724 /* 725 * Copy last unit sample and its previous to 726 * beginning of buffer. 727 */ 728 info->buffer[0] = info->buffer[info->pos - 2]; 729 info->buffer[1] = info->buffer[info->pos - 1]; 730 info->buffer[2] = info->buffer[info->pos]; 731 info->buffer[3] = info->buffer[info->pos + 1]; 732 info->pos = 2; 733 info->bpos = 4; 734 } 735 RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n", 736 __func__, slot)); 737 i = 0; 738 for (;;) { 739 for (;;) { 740 fetch = (info->bufsz - info->bpos) << 1; 741 fetch -= info->stray; 742 RATE_ASSERT(fetch >= 0, 743 ("%s: [1] Buffer overrun: %d > %d\n", 744 __func__, info->bpos, info->bufsz)); 745 if ((slot << 1) < fetch) 746 fetch = slot << 1; 747 if (fetch > 0) { 748 RATE_ASSERT(((info->bpos << 1) - info->stray) >= 0 && 749 ((info->bpos << 1) - info->stray) < (info->bufsz << 1), 750 ("%s: DANGER - BUFFER OVERRUN! bufsz=%d, pos=%d\n", __func__, 751 info->bufsz << 1, (info->bpos << 1) - info->stray)); 752 fetch = FEEDER_FEED(f->source, c, 753 (uint8_t *)(info->buffer) + (info->bpos << 1) - info->stray, 754 fetch, source); 755 info->stray = 0; 756 if (fetch == 0) 757 break; 758 RATE_TEST((fetch & 3) == 0, 759 ("%s: Fetch size not byte integral (%d)\n", 760 __func__, fetch)); 761 info->stray += fetch & 3; 762 RATE_TEST(info->stray == 0, 763 ("%s: Stray bytes detected (%d)\n", 764 __func__, info->stray)); 765 fetch >>= 1; 766 fetch &= ~1; 767 info->bpos += fetch; 768 slot -= fetch; 769 RATE_ASSERT(slot >= 0, 770 ("%s: Negative Slot: %d\n", __func__, 771 slot)); 772 if (slot == 0) 773 break; 774 if (info->bpos == info->bufsz) 775 break; 776 } else 777 break; 778 } 779 if (info->pos == info->bpos) { 780 RATE_TEST(info->pos == 2, 781 ("%s: EOF while in progress\n", __func__)); 782 break; 783 } 784 RATE_ASSERT(info->pos <= info->bpos, 785 ("%s: [2] Buffer overrun: %d > %d\n", __func__, 786 info->pos, info->bpos)); 787 RATE_ASSERT(info->pos < info->bpos, 788 ("%s: Zero buffer!\n", __func__)); 789 RATE_ASSERT(((info->bpos - info->pos) & 1) == 0, 790 ("%s: Buffer not sample integral (%d)\n", 791 __func__, info->bpos - info->pos)); 792 i += info->convert(info, dst + i, count - i); 793 RATE_ASSERT(info->pos <= info->bpos, 794 ("%s: [3] Buffer overrun: %d > %d\n", 795 __func__, info->pos, info->bpos)); 796 if (info->pos == info->bpos) { 797 /* 798 * End of buffer cycle. Copy last unit sample 799 * to beginning of buffer so next cycle can 800 * interpolate using it. 801 */ 802 RATE_TEST(info->stray == 0, ("%s: [2] Stray bytes: %u\n", __func__, info->stray)); 803 info->buffer[0] = info->buffer[info->pos - 2]; 804 info->buffer[1] = info->buffer[info->pos - 1]; 805 info->bpos = 2; 806 info->pos = 2; 807 } 808 if (i == count) 809 break; 810 } 811 #if 0 812 RATE_TEST(count == i, ("Expect: %u , Got: %u\n", count << 1, i << 1)); 813 #endif 814 RATE_TEST(info->stray == 0, ("%s: [3] Stray bytes: %u\n", __func__, info->stray)); 815 return i << 1; 816 } 817 818 static struct pcm_feederdesc feeder_rate_desc[] = { 819 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, 820 {0, 0, 0, 0}, 821 }; 822 static kobj_method_t feeder_rate_methods[] = { 823 KOBJMETHOD(feeder_init, feed_rate_init), 824 KOBJMETHOD(feeder_free, feed_rate_free), 825 KOBJMETHOD(feeder_set, feed_rate_set), 826 KOBJMETHOD(feeder_get, feed_rate_get), 827 KOBJMETHOD(feeder_feed, feed_rate), 828 {0, 0} 829 }; 830 FEEDER_DECLARE(feeder_rate, 2, NULL); 831