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 * $FreeBSD: src/sys/dev/sound/pcm/feeder_rate.c,v 1.11.2.2 2006/01/29 02:27:28 ariff Exp $ 66 */ 67 68 #include <dev/sound/pcm/sound.h> 69 #include "feeder_if.h" 70 71 SND_DECLARE_FILE("$DragonFly: src/sys/dev/sound/pcm/feeder_rate.c,v 1.5 2007/01/04 21:47:03 corecode Exp $"); 72 73 #define RATE_ASSERT(x, y) /* KASSERT(x,y) */ 74 #define RATE_TEST(x, y) /* if (!(x)) printf y */ 75 #define RATE_TRACE(x...) /* printf(x) */ 76 77 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder"); 78 79 #define FEEDBUFSZ 8192 80 #define ROUNDHZ 25 81 #define RATEMIN 4000 82 /* 8000 * 138 or 11025 * 100 . This is insane, indeed! */ 83 #define RATEMAX 1102500 84 #define MINGAIN 92 85 #define MAXGAIN 96 86 87 #define FEEDRATE_CONVERT_64 0 88 #define FEEDRATE_CONVERT_SCALE64 1 89 #define FEEDRATE_CONVERT_SCALE32 2 90 #define FEEDRATE_CONVERT_PLAIN 3 91 #define FEEDRATE_CONVERT_FIXED 4 92 #define FEEDRATE_CONVERT_OPTIMAL 5 93 #define FEEDRATE_CONVERT_WORST 6 94 95 #define FEEDRATE_64_MAXROLL 32 96 #define FEEDRATE_32_MAXROLL 16 97 98 struct feed_rate_info { 99 uint32_t src, dst; /* rounded source / destination rates */ 100 uint32_t rsrc, rdst; /* original source / destination rates */ 101 uint32_t gx, gy; /* interpolation / decimation ratio */ 102 uint32_t alpha; /* interpolation distance */ 103 uint32_t pos, bpos; /* current sample / buffer positions */ 104 uint32_t bufsz; /* total buffer size */ 105 uint32_t stray; /* stray bytes */ 106 int32_t scale, roll; /* scale / roll factor */ 107 int16_t *buffer; 108 uint32_t (*convert)(struct feed_rate_info *, int16_t *, uint32_t); 109 }; 110 111 static uint32_t 112 feed_convert_64(struct feed_rate_info *, int16_t *, uint32_t); 113 static uint32_t 114 feed_convert_scale64(struct feed_rate_info *, int16_t *, uint32_t); 115 static uint32_t 116 feed_convert_scale32(struct feed_rate_info *, int16_t *, uint32_t); 117 static uint32_t 118 feed_convert_plain(struct feed_rate_info *, int16_t *, uint32_t); 119 120 int feeder_rate_ratemin = RATEMIN; 121 int feeder_rate_ratemax = RATEMAX; 122 /* 123 * See 'Feeder Scaling Type' below.. 124 */ 125 static int feeder_rate_scaling = FEEDRATE_CONVERT_OPTIMAL; 126 static int feeder_rate_buffersize = FEEDBUFSZ & ~1; 127 128 #if 0 129 /* 130 * sysctls.. I love sysctls.. 131 */ 132 TUNABLE_INT("hw.snd.feeder_rate_ratemin", &feeder_rate_ratemin); 133 TUNABLE_INT("hw.snd.feeder_rate_ratemax", &feeder_rate_ratemin); 134 TUNABLE_INT("hw.snd.feeder_rate_scaling", &feeder_rate_scaling); 135 TUNABLE_INT("hw.snd.feeder_rate_buffersize", &feeder_rate_buffersize); 136 137 static int 138 sysctl_hw_snd_feeder_rate_ratemin(SYSCTL_HANDLER_ARGS) 139 { 140 int err, val; 141 142 val = feeder_rate_ratemin; 143 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 144 if (val < 1 || val >= feeder_rate_ratemax) 145 err = EINVAL; 146 else 147 feeder_rate_ratemin = val; 148 return err; 149 } 150 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemin, CTLTYPE_INT | CTLFLAG_RW, 151 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemin, "I", ""); 152 153 static int 154 sysctl_hw_snd_feeder_rate_ratemax(SYSCTL_HANDLER_ARGS) 155 { 156 int err, val; 157 158 val = feeder_rate_ratemax; 159 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 160 if (val <= feeder_rate_ratemin || val > 0x7fffff) 161 err = EINVAL; 162 else 163 feeder_rate_ratemax = val; 164 return err; 165 } 166 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemax, CTLTYPE_INT | CTLFLAG_RW, 167 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemax, "I", ""); 168 169 static int 170 sysctl_hw_snd_feeder_rate_scaling(SYSCTL_HANDLER_ARGS) 171 { 172 int err, val; 173 174 val = feeder_rate_scaling; 175 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 176 /* 177 * Feeder Scaling Type 178 * =================== 179 * 180 * 1. Plain 64bit (high precision) 181 * 2. 64bit scaling (high precision, CPU friendly, but can 182 * cause gain up/down). 183 * 3. 32bit scaling (somehow can cause hz roundup, gain 184 * up/down). 185 * 4. Plain copy (default if src == dst. Except if src == dst, 186 * this is the worst / silly conversion method!). 187 * 188 * Sysctl options:- 189 * 190 * 0 - Plain 64bit - no fallback. 191 * 1 - 64bit scaling - no fallback. 192 * 2 - 32bit scaling - no fallback. 193 * 3 - Plain copy - no fallback. 194 * 4 - Fixed rate. Means that, choose optimal conversion method 195 * without causing hz roundup. 196 * 32bit scaling (as long as hz roundup does not occur), 197 * 64bit scaling, Plain 64bit. 198 * 5 - Optimal / CPU friendly (DEFAULT). 199 * 32bit scaling, 64bit scaling, Plain 64bit 200 * 6 - Optimal to worst, no 64bit arithmetic involved. 201 * 32bit scaling, Plain copy. 202 */ 203 if (val < FEEDRATE_CONVERT_64 || val > FEEDRATE_CONVERT_WORST) 204 err = EINVAL; 205 else 206 feeder_rate_scaling = val; 207 return err; 208 } 209 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_scaling, CTLTYPE_INT | CTLFLAG_RW, 210 0, sizeof(int), sysctl_hw_snd_feeder_rate_scaling, "I", ""); 211 212 static int 213 sysctl_hw_snd_feeder_rate_buffersize(SYSCTL_HANDLER_ARGS) 214 { 215 int err, val; 216 217 val = feeder_rate_buffersize; 218 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 219 /* 220 * Don't waste too much kernel space 221 */ 222 if (val < 2 || val > 65536) 223 err = EINVAL; 224 else 225 feeder_rate_buffersize = val & ~1; 226 return err; 227 } 228 SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_buffersize, CTLTYPE_INT | CTLFLAG_RW, 229 0, sizeof(int), sysctl_hw_snd_feeder_rate_buffersize, "I", ""); 230 #endif 231 232 static void 233 feed_speed_ratio(uint32_t x, uint32_t y, uint32_t *gx, uint32_t *gy) 234 { 235 uint32_t w, src = x, dst = y; 236 237 while (y != 0) { 238 w = x % y; 239 x = y; 240 y = w; 241 } 242 *gx = src / x; 243 *gy = dst / x; 244 } 245 246 static void 247 feed_scale_roll(uint32_t dst, int32_t *scale, int32_t *roll, int32_t max) 248 { 249 int64_t k, tscale; 250 int32_t j, troll; 251 252 *scale = *roll = -1; 253 for (j = MAXGAIN; j >= MINGAIN; j -= 3) { 254 for (troll = 0; troll < max; troll++) { 255 tscale = (1 << troll) / dst; 256 k = (tscale * dst * 100) >> troll; 257 if (k > j && k <= 100) { 258 *scale = tscale; 259 *roll = troll; 260 return; 261 } 262 } 263 } 264 } 265 266 static int 267 feed_get_best_coef(uint32_t *src, uint32_t *dst, uint32_t *gx, uint32_t *gy, 268 int32_t *scale, int32_t *roll) 269 { 270 uint32_t tsrc, tdst, sscale, dscale; 271 int32_t tscale, troll; 272 int i, j, hzmin, hzmax; 273 274 *scale = *roll = -1; 275 for (i = 0; i < 2; i++) { 276 hzmin = (ROUNDHZ * i) + 1; 277 hzmax = hzmin + ROUNDHZ; 278 for (j = hzmin; j < hzmax; j++) { 279 tsrc = *src - (*src % j); 280 tdst = *dst; 281 if (tsrc < 1 || tdst < 1) 282 goto coef_failed; 283 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 284 feed_scale_roll(dscale, &tscale, &troll, 285 FEEDRATE_32_MAXROLL); 286 if (tscale != -1 && troll != -1) { 287 *src = tsrc; 288 *gx = sscale; 289 *gy = dscale; 290 *scale = tscale; 291 *roll = troll; 292 return j; 293 } 294 } 295 for (j = hzmin; j < hzmax; j++) { 296 tsrc = *src - (*src % j); 297 tdst = *dst - (*dst % j); 298 if (tsrc < 1 || tdst < 1) 299 goto coef_failed; 300 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 301 feed_scale_roll(dscale, &tscale, &troll, 302 FEEDRATE_32_MAXROLL); 303 if (tscale != -1 && troll != -1) { 304 *src = tsrc; 305 *dst = tdst; 306 *gx = sscale; 307 *gy = dscale; 308 *scale = tscale; 309 *roll = troll; 310 return j; 311 } 312 } 313 for (j = hzmin; j < hzmax; j++) { 314 tsrc = *src; 315 tdst = *dst - (*dst % j); 316 if (tsrc < 1 || tdst < 1) 317 goto coef_failed; 318 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 319 feed_scale_roll(dscale, &tscale, &troll, 320 FEEDRATE_32_MAXROLL); 321 if (tscale != -1 && troll != -1) { 322 *src = tsrc; 323 *dst = tdst; 324 *gx = sscale; 325 *gy = dscale; 326 *scale = tscale; 327 *roll = troll; 328 return j; 329 } 330 } 331 } 332 coef_failed: 333 feed_speed_ratio(*src, *dst, gx, gy); 334 feed_scale_roll(*gy, scale, roll, FEEDRATE_32_MAXROLL); 335 return 0; 336 } 337 338 static void 339 feed_rate_reset(struct feed_rate_info *info) 340 { 341 info->scale = -1; 342 info->roll = -1; 343 info->src = info->rsrc; 344 info->dst = info->rdst; 345 info->gx = 0; 346 info->gy = 0; 347 } 348 349 static int 350 feed_rate_setup(struct pcm_feeder *f) 351 { 352 struct feed_rate_info *info = f->data; 353 int r = 0; 354 355 info->pos = 2; 356 info->bpos = 4; 357 info->alpha = 0; 358 info->stray = 0; 359 feed_rate_reset(info); 360 if (info->src == info->dst) { 361 /* 362 * No conversion ever needed. Just do plain copy. 363 */ 364 info->convert = feed_convert_plain; 365 info->gx = 1; 366 info->gy = 1; 367 } else { 368 switch (feeder_rate_scaling) { 369 case FEEDRATE_CONVERT_64: 370 feed_speed_ratio(info->src, info->dst, 371 &info->gx, &info->gy); 372 info->convert = feed_convert_64; 373 break; 374 case FEEDRATE_CONVERT_SCALE64: 375 feed_speed_ratio(info->src, info->dst, 376 &info->gx, &info->gy); 377 feed_scale_roll(info->gy, &info->scale, 378 &info->roll, FEEDRATE_64_MAXROLL); 379 if (info->scale == -1 || info->roll == -1) 380 return -1; 381 info->convert = feed_convert_scale64; 382 break; 383 case FEEDRATE_CONVERT_SCALE32: 384 r = feed_get_best_coef(&info->src, &info->dst, 385 &info->gx, &info->gy, &info->scale, 386 &info->roll); 387 if (r == 0) 388 return -1; 389 info->convert = feed_convert_scale32; 390 break; 391 case FEEDRATE_CONVERT_PLAIN: 392 feed_speed_ratio(info->src, info->dst, 393 &info->gx, &info->gy); 394 info->convert = feed_convert_plain; 395 break; 396 case FEEDRATE_CONVERT_FIXED: 397 r = feed_get_best_coef(&info->src, &info->dst, 398 &info->gx, &info->gy, &info->scale, 399 &info->roll); 400 if (r != 0 && info->src == info->rsrc && 401 info->dst == info->rdst) 402 info->convert = feed_convert_scale32; 403 else { 404 /* Fallback */ 405 feed_rate_reset(info); 406 feed_speed_ratio(info->src, info->dst, 407 &info->gx, &info->gy); 408 feed_scale_roll(info->gy, &info->scale, 409 &info->roll, FEEDRATE_64_MAXROLL); 410 if (info->scale != -1 && info->roll != -1) 411 info->convert = feed_convert_scale64; 412 else 413 info->convert = feed_convert_64; 414 } 415 break; 416 case FEEDRATE_CONVERT_OPTIMAL: 417 r = feed_get_best_coef(&info->src, &info->dst, 418 &info->gx, &info->gy, &info->scale, 419 &info->roll); 420 if (r != 0) 421 info->convert = feed_convert_scale32; 422 else { 423 /* Fallback */ 424 feed_rate_reset(info); 425 feed_speed_ratio(info->src, info->dst, 426 &info->gx, &info->gy); 427 feed_scale_roll(info->gy, &info->scale, 428 &info->roll, FEEDRATE_64_MAXROLL); 429 if (info->scale != -1 && info->roll != -1) 430 info->convert = feed_convert_scale64; 431 else 432 info->convert = feed_convert_64; 433 } 434 break; 435 case FEEDRATE_CONVERT_WORST: 436 r = feed_get_best_coef(&info->src, &info->dst, 437 &info->gx, &info->gy, &info->scale, 438 &info->roll); 439 if (r != 0) 440 info->convert = feed_convert_scale32; 441 else { 442 /* Fallback */ 443 feed_rate_reset(info); 444 feed_speed_ratio(info->src, info->dst, 445 &info->gx, &info->gy); 446 info->convert = feed_convert_plain; 447 } 448 break; 449 default: 450 return -1; 451 break; 452 } 453 /* No way! */ 454 if (info->gx == 0 || info->gy == 0) 455 return -1; 456 /* 457 * No need to interpolate/decimate, just do plain copy. 458 * This probably caused by Hz roundup. 459 */ 460 if (info->gx == info->gy) 461 info->convert = feed_convert_plain; 462 } 463 return 0; 464 } 465 466 static int 467 feed_rate_set(struct pcm_feeder *f, int what, int value) 468 { 469 struct feed_rate_info *info = f->data; 470 471 if (value < feeder_rate_ratemin || value > feeder_rate_ratemax) 472 return -1; 473 474 switch (what) { 475 case FEEDRATE_SRC: 476 info->rsrc = value; 477 break; 478 case FEEDRATE_DST: 479 info->rdst = value; 480 break; 481 default: 482 return -1; 483 } 484 return feed_rate_setup(f); 485 } 486 487 static int 488 feed_rate_get(struct pcm_feeder *f, int what) 489 { 490 struct feed_rate_info *info = f->data; 491 492 /* 493 * Return *real* src/dst rate. 494 */ 495 switch (what) { 496 case FEEDRATE_SRC: 497 return info->rsrc; 498 case FEEDRATE_DST: 499 return info->rdst; 500 default: 501 return -1; 502 } 503 return -1; 504 } 505 506 static int 507 feed_rate_init(struct pcm_feeder *f) 508 { 509 struct feed_rate_info *info; 510 511 info = kmalloc(sizeof(*info), M_RATEFEEDER, M_WAITOK | M_ZERO); 512 /* 513 * bufsz = sample from last cycle + conversion space 514 */ 515 info->bufsz = 2 + feeder_rate_buffersize; 516 info->buffer = kmalloc(sizeof(*info->buffer) * info->bufsz, 517 M_RATEFEEDER, M_WAITOK | M_ZERO); 518 info->rsrc = DSP_DEFAULT_SPEED; 519 info->rdst = DSP_DEFAULT_SPEED; 520 f->data = info; 521 return feed_rate_setup(f); 522 } 523 524 static int 525 feed_rate_free(struct pcm_feeder *f) 526 { 527 struct feed_rate_info *info = f->data; 528 529 if (info) { 530 if (info->buffer) 531 kfree(info->buffer, M_RATEFEEDER); 532 kfree(info, M_RATEFEEDER); 533 } 534 f->data = NULL; 535 return 0; 536 } 537 538 static uint32_t 539 feed_convert_64(struct feed_rate_info *info, int16_t *dst, uint32_t max) 540 { 541 int64_t x, alpha, distance; 542 uint32_t ret; 543 int32_t pos, bpos, gx, gy; 544 int16_t *src; 545 /* 546 * Plain, straight forward 64bit arith. No bit-magic applied here. 547 */ 548 ret = 0; 549 alpha = info->alpha; 550 gx = info->gx; 551 gy = info->gy; 552 pos = info->pos; 553 bpos = info->bpos; 554 src = info->buffer; 555 for (;;) { 556 if (alpha < gx) { 557 alpha += gy; 558 pos += 2; 559 if (pos == bpos) 560 break; 561 } else { 562 alpha -= gx; 563 distance = gy - alpha; 564 x = (alpha * src[pos - 2]) + (distance * src[pos]); 565 dst[ret++] = x / gy; 566 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]); 567 dst[ret++] = x / gy; 568 if (ret == max) 569 break; 570 } 571 } 572 info->alpha = alpha; 573 info->pos = pos; 574 return ret; 575 } 576 577 static uint32_t 578 feed_convert_scale64(struct feed_rate_info *info, int16_t *dst, uint32_t max) 579 { 580 int64_t x, alpha, distance; 581 uint32_t ret; 582 int32_t pos, bpos, gx, gy, roll; 583 int16_t *src; 584 /* 585 * 64bit scaling. 586 */ 587 ret = 0; 588 roll = info->roll; 589 alpha = info->alpha * info->scale; 590 gx = info->gx * info->scale; 591 gy = info->gy * info->scale; 592 pos = info->pos; 593 bpos = info->bpos; 594 src = info->buffer; 595 for (;;) { 596 if (alpha < gx) { 597 alpha += gy; 598 pos += 2; 599 if (pos == bpos) 600 break; 601 } else { 602 alpha -= gx; 603 distance = gy - alpha; 604 x = (alpha * src[pos - 2]) + (distance * src[pos]); 605 dst[ret++] = x >> roll; 606 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]); 607 dst[ret++] = x >> roll; 608 if (ret == max) 609 break; 610 } 611 } 612 info->alpha = alpha / info->scale; 613 info->pos = pos; 614 return ret; 615 } 616 617 static uint32_t 618 feed_convert_scale32(struct feed_rate_info *info, int16_t *dst, uint32_t max) 619 { 620 uint32_t ret; 621 int32_t x, pos, bpos, gx, gy, alpha, roll, distance; 622 int16_t *src; 623 /* 624 * 32bit scaling. 625 */ 626 ret = 0; 627 roll = info->roll; 628 alpha = info->alpha * info->scale; 629 gx = info->gx * info->scale; 630 gy = info->gy * info->scale; 631 pos = info->pos; 632 bpos = info->bpos; 633 src = info->buffer; 634 for (;;) { 635 if (alpha < gx) { 636 alpha += gy; 637 pos += 2; 638 if (pos == bpos) 639 break; 640 } else { 641 alpha -= gx; 642 distance = gy - alpha; 643 x = (alpha * src[pos - 2]) + (distance * src[pos]); 644 dst[ret++] = x >> roll; 645 x = (alpha * src[pos - 1]) + (distance * src[pos + 1]); 646 dst[ret++] = x >> roll; 647 if (ret == max) 648 break; 649 } 650 } 651 info->alpha = alpha / info->scale; 652 info->pos = pos; 653 return ret; 654 } 655 656 static uint32_t 657 feed_convert_plain(struct feed_rate_info *info, int16_t *dst, uint32_t max) 658 { 659 uint32_t ret; 660 int32_t pos, bpos, gx, gy, alpha; 661 int16_t *src; 662 /* 663 * Plain copy. 664 */ 665 ret = 0; 666 gx = info->gx; 667 gy = info->gy; 668 alpha = info->alpha; 669 pos = info->pos; 670 bpos = info->bpos; 671 src = info->buffer; 672 for (;;) { 673 if (alpha < gx) { 674 alpha += gy; 675 pos += 2; 676 if (pos == bpos) 677 break; 678 } else { 679 alpha -= gx; 680 dst[ret++] = src[pos]; 681 dst[ret++] = src[pos + 1]; 682 if (ret == max) 683 break; 684 } 685 } 686 info->pos = pos; 687 info->alpha = alpha; 688 return ret; 689 } 690 691 static int32_t 692 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b, 693 uint32_t count, void *source) 694 { 695 struct feed_rate_info *info = f->data; 696 uint32_t i; 697 int32_t fetch, slot; 698 int16_t *dst = (int16_t *)b; 699 /* 700 * This loop has been optimized to generalize both up / down 701 * sampling without causing missing samples or excessive buffer 702 * feeding. 703 */ 704 RATE_TEST(count >= 4 && (count & 3) == 0, 705 ("%s: Count size not byte integral (%d)\n", __func__, count)); 706 if (count < 4) 707 return 0; 708 count >>= 1; 709 count &= ~1; 710 slot = (((info->gx * (count >> 1)) + info->gy - info->alpha - 1) / info->gy) << 1; 711 RATE_TEST((slot & 1) == 0, ("%s: Slot count not sample integral (%d)\n", 712 __func__, slot)); 713 /* 714 * Optimize buffer feeding aggressively to ensure calculated slot 715 * can be fitted nicely into available buffer free space, hence 716 * avoiding multiple feeding. 717 */ 718 RATE_TEST(info->stray == 0, ("%s: [1] Stray bytes: %u\n", 719 __func__,info->stray)); 720 if (info->pos != 2 && info->bpos - info->pos == 2 && 721 info->bpos + slot > info->bufsz) { 722 /* 723 * Copy last unit sample and its previous to 724 * beginning of buffer. 725 */ 726 info->buffer[0] = info->buffer[info->pos - 2]; 727 info->buffer[1] = info->buffer[info->pos - 1]; 728 info->buffer[2] = info->buffer[info->pos]; 729 info->buffer[3] = info->buffer[info->pos + 1]; 730 info->pos = 2; 731 info->bpos = 4; 732 } 733 RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n", 734 __func__, slot)); 735 i = 0; 736 for (;;) { 737 for (;;) { 738 fetch = (info->bufsz - info->bpos) << 1; 739 fetch -= info->stray; 740 RATE_ASSERT(fetch >= 0, 741 ("%s: [1] Buffer overrun: %d > %d\n", 742 __func__, info->bpos, info->bufsz)); 743 if ((slot << 1) < fetch) 744 fetch = slot << 1; 745 if (fetch > 0) { 746 RATE_ASSERT(((info->bpos << 1) - info->stray) >= 0 && 747 ((info->bpos << 1) - info->stray) < (info->bufsz << 1), 748 ("%s: DANGER - BUFFER OVERRUN! bufsz=%d, pos=%d\n", __func__, 749 info->bufsz << 1, (info->bpos << 1) - info->stray)); 750 fetch = FEEDER_FEED(f->source, c, 751 (uint8_t *)(info->buffer) + (info->bpos << 1) - info->stray, 752 fetch, source); 753 info->stray = 0; 754 if (fetch == 0) 755 break; 756 RATE_TEST((fetch & 3) == 0, 757 ("%s: Fetch size not byte integral (%d)\n", 758 __func__, fetch)); 759 info->stray += fetch & 3; 760 RATE_TEST(info->stray == 0, 761 ("%s: Stray bytes detected (%d)\n", 762 __func__, info->stray)); 763 fetch >>= 1; 764 fetch &= ~1; 765 info->bpos += fetch; 766 slot -= fetch; 767 RATE_ASSERT(slot >= 0, 768 ("%s: Negative Slot: %d\n", __func__, 769 slot)); 770 if (slot == 0) 771 break; 772 if (info->bpos == info->bufsz) 773 break; 774 } else 775 break; 776 } 777 if (info->pos == info->bpos) { 778 RATE_TEST(info->pos == 2, 779 ("%s: EOF while in progress\n", __func__)); 780 break; 781 } 782 RATE_ASSERT(info->pos <= info->bpos, 783 ("%s: [2] Buffer overrun: %d > %d\n", __func__, 784 info->pos, info->bpos)); 785 RATE_ASSERT(info->pos < info->bpos, 786 ("%s: Zero buffer!\n", __func__)); 787 RATE_ASSERT(((info->bpos - info->pos) & 1) == 0, 788 ("%s: Buffer not sample integral (%d)\n", 789 __func__, info->bpos - info->pos)); 790 i += info->convert(info, dst + i, count - i); 791 RATE_ASSERT(info->pos <= info->bpos, 792 ("%s: [3] Buffer overrun: %d > %d\n", 793 __func__, info->pos, info->bpos)); 794 if (info->pos == info->bpos) { 795 /* 796 * End of buffer cycle. Copy last unit sample 797 * to beginning of buffer so next cycle can 798 * interpolate using it. 799 */ 800 RATE_TEST(info->stray == 0, ("%s: [2] Stray bytes: %u\n", __func__, info->stray)); 801 info->buffer[0] = info->buffer[info->pos - 2]; 802 info->buffer[1] = info->buffer[info->pos - 1]; 803 info->bpos = 2; 804 info->pos = 2; 805 } 806 if (i == count) 807 break; 808 } 809 #if 0 810 RATE_TEST(count == i, ("Expect: %u , Got: %u\n", count << 1, i << 1)); 811 #endif 812 RATE_TEST(info->stray == 0, ("%s: [3] Stray bytes: %u\n", __func__, info->stray)); 813 return i << 1; 814 } 815 816 static struct pcm_feederdesc feeder_rate_desc[] = { 817 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, 818 {0, 0, 0, 0}, 819 }; 820 static kobj_method_t feeder_rate_methods[] = { 821 KOBJMETHOD(feeder_init, feed_rate_init), 822 KOBJMETHOD(feeder_free, feed_rate_free), 823 KOBJMETHOD(feeder_set, feed_rate_set), 824 KOBJMETHOD(feeder_get, feed_rate_get), 825 KOBJMETHOD(feeder_feed, feed_rate), 826 KOBJMETHOD_END 827 }; 828 FEEDER_DECLARE(feeder_rate, 2, NULL); 829