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