feeder_rate.c revision 154684
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 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 154684 2006-01-22 15:06:49Z ariff $"); 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 75MALLOC_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 96struct 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 int32_t scale, roll; /* scale / roll factor */ 104 int16_t *buffer; 105 uint32_t (*convert)(struct feed_rate_info *, int16_t *, uint32_t); 106}; 107 108static uint32_t 109feed_convert_64(struct feed_rate_info *, int16_t *, uint32_t); 110static uint32_t 111feed_convert_scale64(struct feed_rate_info *, int16_t *, uint32_t); 112static uint32_t 113feed_convert_scale32(struct feed_rate_info *, int16_t *, uint32_t); 114static uint32_t 115feed_convert_plain(struct feed_rate_info *, int16_t *, uint32_t); 116 117int feeder_rate_ratemin = RATEMIN; 118int feeder_rate_ratemax = RATEMAX; 119/* 120 * See 'Feeder Scaling Type' below.. 121 */ 122static int feeder_rate_scaling = FEEDRATE_CONVERT_OPTIMAL; 123static int feeder_rate_buffersize = FEEDBUFSZ & ~1; 124 125/* 126 * sysctls.. I love sysctls.. 127 */ 128TUNABLE_INT("hw.snd.feeder_rate_ratemin", &feeder_rate_ratemin); 129TUNABLE_INT("hw.snd.feeder_rate_ratemax", &feeder_rate_ratemin); 130TUNABLE_INT("hw.snd.feeder_rate_scaling", &feeder_rate_scaling); 131TUNABLE_INT("hw.snd.feeder_rate_buffersize", &feeder_rate_buffersize); 132 133static int 134sysctl_hw_snd_feeder_rate_ratemin(SYSCTL_HANDLER_ARGS) 135{ 136 int err, val; 137 138 val = feeder_rate_ratemin; 139 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 140 if (val < 1 || val >= feeder_rate_ratemax) 141 err = EINVAL; 142 else 143 feeder_rate_ratemin = val; 144 return err; 145} 146SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemin, CTLTYPE_INT | CTLFLAG_RW, 147 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemin, "I", ""); 148 149static int 150sysctl_hw_snd_feeder_rate_ratemax(SYSCTL_HANDLER_ARGS) 151{ 152 int err, val; 153 154 val = feeder_rate_ratemax; 155 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 156 if (val <= feeder_rate_ratemin || val > 0x7fffff) 157 err = EINVAL; 158 else 159 feeder_rate_ratemax = val; 160 return err; 161} 162SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_ratemax, CTLTYPE_INT | CTLFLAG_RW, 163 0, sizeof(int), sysctl_hw_snd_feeder_rate_ratemax, "I", ""); 164 165static int 166sysctl_hw_snd_feeder_rate_scaling(SYSCTL_HANDLER_ARGS) 167{ 168 int err, val; 169 170 val = feeder_rate_scaling; 171 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 172 /* 173 * Feeder Scaling Type 174 * =================== 175 * 176 * 1. Plain 64bit (high precision) 177 * 2. 64bit scaling (high precision, CPU friendly, but can 178 * cause gain up/down). 179 * 3. 32bit scaling (somehow can cause hz roundup, gain 180 * up/down). 181 * 4. Plain copy (default if src == dst. Except if src == dst, 182 * this is the worst / silly conversion method!). 183 * 184 * Sysctl options:- 185 * 186 * 0 - Plain 64bit - no fallback. 187 * 1 - 64bit scaling - no fallback. 188 * 2 - 32bit scaling - no fallback. 189 * 3 - Plain copy - no fallback. 190 * 4 - Fixed rate. Means that, choose optimal conversion method 191 * without causing hz roundup. 192 * 32bit scaling (as long as hz roundup does not occur), 193 * 64bit scaling, Plain 64bit. 194 * 5 - Optimal / CPU friendly (DEFAULT). 195 * 32bit scaling, 64bit scaling, Plain 64bit 196 * 6 - Optimal to worst, no 64bit arithmetic involved. 197 * 32bit scaling, Plain copy. 198 */ 199 if (val < FEEDRATE_CONVERT_64 || val > FEEDRATE_CONVERT_WORST) 200 err = EINVAL; 201 else 202 feeder_rate_scaling = val; 203 return err; 204} 205SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_scaling, CTLTYPE_INT | CTLFLAG_RW, 206 0, sizeof(int), sysctl_hw_snd_feeder_rate_scaling, "I", ""); 207 208static int 209sysctl_hw_snd_feeder_rate_buffersize(SYSCTL_HANDLER_ARGS) 210{ 211 int err, val; 212 213 val = feeder_rate_buffersize; 214 err = sysctl_handle_int(oidp, &val, sizeof(val), req); 215 /* 216 * Don't waste too much kernel space 217 */ 218 if (val < 2 || val > 65536) 219 err = EINVAL; 220 else 221 feeder_rate_buffersize = val & ~1; 222 return err; 223} 224SYSCTL_PROC(_hw_snd, OID_AUTO, feeder_rate_buffersize, CTLTYPE_INT | CTLFLAG_RW, 225 0, sizeof(int), sysctl_hw_snd_feeder_rate_buffersize, "I", ""); 226 227static void 228feed_speed_ratio(uint32_t x, uint32_t y, uint32_t *gx, uint32_t *gy) 229{ 230 uint32_t w, src = x, dst = y; 231 232 while (y != 0) { 233 w = x % y; 234 x = y; 235 y = w; 236 } 237 *gx = src / x; 238 *gy = dst / x; 239} 240 241static void 242feed_scale_roll(uint32_t dst, int32_t *scale, int32_t *roll, int32_t max) 243{ 244 int64_t k, tscale; 245 int32_t j, troll; 246 247 *scale = *roll = -1; 248 for (j = MAXGAIN; j >= MINGAIN; j -= 3) { 249 for (troll = 0; troll < max; troll++) { 250 tscale = (1 << troll) / dst; 251 k = (tscale * dst * 100) >> troll; 252 if (k > j && k <= 100) { 253 *scale = tscale; 254 *roll = troll; 255 return; 256 } 257 } 258 } 259} 260 261static int 262feed_get_best_coef(uint32_t *src, uint32_t *dst, uint32_t *gx, uint32_t *gy, 263 int32_t *scale, int32_t *roll) 264{ 265 uint32_t tsrc, tdst, sscale, dscale; 266 int32_t tscale, troll; 267 int i, j, hzmin, hzmax; 268 269 *scale = *roll = -1; 270 for (i = 0; i < 2; i++) { 271 hzmin = (ROUNDHZ * i) + 1; 272 hzmax = hzmin + ROUNDHZ; 273 for (j = hzmin; j < hzmax; j++) { 274 tsrc = *src - (*src % j); 275 tdst = *dst; 276 if (tsrc < 1 || tdst < 1) 277 goto coef_failed; 278 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 279 feed_scale_roll(dscale, &tscale, &troll, 280 FEEDRATE_32_MAXROLL); 281 if (tscale != -1 && troll != -1) { 282 *src = tsrc; 283 *gx = sscale; 284 *gy = dscale; 285 *scale = tscale; 286 *roll = troll; 287 return j; 288 } 289 } 290 for (j = hzmin; j < hzmax; j++) { 291 tsrc = *src - (*src % j); 292 tdst = *dst - (*dst % j); 293 if (tsrc < 1 || tdst < 1) 294 goto coef_failed; 295 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 296 feed_scale_roll(dscale, &tscale, &troll, 297 FEEDRATE_32_MAXROLL); 298 if (tscale != -1 && troll != -1) { 299 *src = tsrc; 300 *dst = tdst; 301 *gx = sscale; 302 *gy = dscale; 303 *scale = tscale; 304 *roll = troll; 305 return j; 306 } 307 } 308 for (j = hzmin; j < hzmax; j++) { 309 tsrc = *src; 310 tdst = *dst - (*dst % j); 311 if (tsrc < 1 || tdst < 1) 312 goto coef_failed; 313 feed_speed_ratio(tsrc, tdst, &sscale, &dscale); 314 feed_scale_roll(dscale, &tscale, &troll, 315 FEEDRATE_32_MAXROLL); 316 if (tscale != -1 && troll != -1) { 317 *src = tsrc; 318 *dst = tdst; 319 *gx = sscale; 320 *gy = dscale; 321 *scale = tscale; 322 *roll = troll; 323 return j; 324 } 325 } 326 } 327coef_failed: 328 feed_speed_ratio(*src, *dst, gx, gy); 329 feed_scale_roll(*gy, scale, roll, FEEDRATE_32_MAXROLL); 330 return 0; 331} 332 333static void 334feed_rate_reset(struct feed_rate_info *info) 335{ 336 info->scale = -1; 337 info->roll = -1; 338 info->src = info->rsrc; 339 info->dst = info->rdst; 340 info->gx = 0; 341 info->gy = 0; 342} 343 344static int 345feed_rate_setup(struct pcm_feeder *f) 346{ 347 struct feed_rate_info *info = f->data; 348 int r = 0; 349 350 info->pos = 2; 351 info->bpos = 4; 352 info->alpha = 0; 353 feed_rate_reset(info); 354 if (info->src == info->dst) { 355 /* 356 * No conversion ever needed. Just do plain copy. 357 */ 358 info->convert = feed_convert_plain; 359 info->gx = 1; 360 info->gy = 1; 361 } else { 362 switch (feeder_rate_scaling) { 363 case FEEDRATE_CONVERT_64: 364 feed_speed_ratio(info->src, info->dst, 365 &info->gx, &info->gy); 366 info->convert = feed_convert_64; 367 break; 368 case FEEDRATE_CONVERT_SCALE64: 369 feed_speed_ratio(info->src, info->dst, 370 &info->gx, &info->gy); 371 feed_scale_roll(info->gy, &info->scale, 372 &info->roll, FEEDRATE_64_MAXROLL); 373 if (info->scale == -1 || info->roll == -1) 374 return -1; 375 info->convert = feed_convert_scale64; 376 break; 377 case FEEDRATE_CONVERT_SCALE32: 378 r = feed_get_best_coef(&info->src, &info->dst, 379 &info->gx, &info->gy, &info->scale, 380 &info->roll); 381 if (r == 0) 382 return -1; 383 info->convert = feed_convert_scale32; 384 break; 385 case FEEDRATE_CONVERT_PLAIN: 386 feed_speed_ratio(info->src, info->dst, 387 &info->gx, &info->gy); 388 info->convert = feed_convert_plain; 389 break; 390 case FEEDRATE_CONVERT_FIXED: 391 r = feed_get_best_coef(&info->src, &info->dst, 392 &info->gx, &info->gy, &info->scale, 393 &info->roll); 394 if (r != 0 && info->src == info->rsrc && 395 info->dst == info->rdst) 396 info->convert = feed_convert_scale32; 397 else { 398 /* Fallback */ 399 feed_rate_reset(info); 400 feed_speed_ratio(info->src, info->dst, 401 &info->gx, &info->gy); 402 feed_scale_roll(info->gy, &info->scale, 403 &info->roll, FEEDRATE_64_MAXROLL); 404 if (info->scale != -1 && info->roll != -1) 405 info->convert = feed_convert_scale64; 406 else 407 info->convert = feed_convert_64; 408 } 409 break; 410 case FEEDRATE_CONVERT_OPTIMAL: 411 r = feed_get_best_coef(&info->src, &info->dst, 412 &info->gx, &info->gy, &info->scale, 413 &info->roll); 414 if (r != 0) 415 info->convert = feed_convert_scale32; 416 else { 417 /* Fallback */ 418 feed_rate_reset(info); 419 feed_speed_ratio(info->src, info->dst, 420 &info->gx, &info->gy); 421 feed_scale_roll(info->gy, &info->scale, 422 &info->roll, FEEDRATE_64_MAXROLL); 423 if (info->scale != -1 && info->roll != -1) 424 info->convert = feed_convert_scale64; 425 else 426 info->convert = feed_convert_64; 427 } 428 break; 429 case FEEDRATE_CONVERT_WORST: 430 r = feed_get_best_coef(&info->src, &info->dst, 431 &info->gx, &info->gy, &info->scale, 432 &info->roll); 433 if (r != 0) 434 info->convert = feed_convert_scale32; 435 else { 436 /* Fallback */ 437 feed_rate_reset(info); 438 feed_speed_ratio(info->src, info->dst, 439 &info->gx, &info->gy); 440 info->convert = feed_convert_plain; 441 } 442 break; 443 default: 444 return -1; 445 break; 446 } 447 /* No way! */ 448 if (info->gx == 0 || info->gy == 0) 449 return -1; 450 /* 451 * No need to interpolate/decimate, just do plain copy. 452 * This probably caused by Hz roundup. 453 */ 454 if (info->gx == info->gy) 455 info->convert = feed_convert_plain; 456 } 457 return 0; 458} 459 460static int 461feed_rate_set(struct pcm_feeder *f, int what, int value) 462{ 463 struct feed_rate_info *info = f->data; 464 465 if (value < feeder_rate_ratemin || value > feeder_rate_ratemax) 466 return -1; 467 468 switch (what) { 469 case FEEDRATE_SRC: 470 info->rsrc = value; 471 break; 472 case FEEDRATE_DST: 473 info->rdst = value; 474 break; 475 default: 476 return -1; 477 } 478 return feed_rate_setup(f); 479} 480 481static int 482feed_rate_get(struct pcm_feeder *f, int what) 483{ 484 struct feed_rate_info *info = f->data; 485 486 /* 487 * Return *real* src/dst rate. 488 */ 489 switch (what) { 490 case FEEDRATE_SRC: 491 return info->rsrc; 492 case FEEDRATE_DST: 493 return info->rdst; 494 default: 495 return -1; 496 } 497 return -1; 498} 499 500static int 501feed_rate_init(struct pcm_feeder *f) 502{ 503 struct feed_rate_info *info; 504 505 info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO); 506 if (info == NULL) 507 return ENOMEM; 508 /* 509 * bufsz = sample from last cycle + conversion space 510 */ 511 info->bufsz = 2 + feeder_rate_buffersize; 512 info->buffer = malloc(sizeof(*info->buffer) * info->bufsz, 513 M_RATEFEEDER, M_NOWAIT | M_ZERO); 514 if (info->buffer == NULL) { 515 free(info, M_RATEFEEDER); 516 return ENOMEM; 517 } 518 info->rsrc = DSP_DEFAULT_SPEED; 519 info->rdst = DSP_DEFAULT_SPEED; 520 f->data = info; 521 return feed_rate_setup(f); 522} 523 524static int 525feed_rate_free(struct pcm_feeder *f) 526{ 527 struct feed_rate_info *info = f->data; 528 529 if (info) { 530 if (info->buffer) 531 free(info->buffer, M_RATEFEEDER); 532 free(info, M_RATEFEEDER); 533 } 534 f->data = NULL; 535 return 0; 536} 537 538static uint32_t 539feed_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 577static uint32_t 578feed_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 617static uint32_t 618feed_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 656static uint32_t 657feed_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 691static int32_t 692feed_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 slot &= ~1; 714 /* 715 * Optimize buffer feeding aggresively to ensure calculated slot 716 * can be fitted nicely into available buffer free space, hence 717 * avoiding multiple feeding. 718 */ 719 if (info->pos != 2 && info->bpos - info->pos == 2 && 720 info->bpos + slot > info->bufsz) { 721 /* 722 * Copy last unit sample and its previous to 723 * beginning of buffer. 724 */ 725 info->buffer[0] = info->buffer[info->pos - 2]; 726 info->buffer[1] = info->buffer[info->pos - 1]; 727 info->buffer[2] = info->buffer[info->pos]; 728 info->buffer[3] = info->buffer[info->pos + 1]; 729 info->pos = 2; 730 info->bpos = 4; 731 } 732 RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n", 733 __func__, slot)); 734 i = 0; 735 for (;;) { 736 for (;;) { 737 fetch = info->bufsz - info->bpos; 738 RATE_ASSERT(fetch >= 0, 739 ("%s: [1] Buffer overrun: %d > %d\n", 740 __func__, info->bpos, info->bufsz)); 741 if (slot < fetch) 742 fetch = slot; 743 fetch &= ~1; 744 if (fetch > 0) { 745 RATE_TEST((fetch & 1) == 0, 746 ("%s: Fetch size not sample integral (%d)\n", 747 __func__, fetch)); 748 fetch = FEEDER_FEED(f->source, c, 749 (uint8_t *)(info->buffer + info->bpos), 750 fetch << 1, source); 751 if (fetch == 0) 752 break; 753 RATE_TEST((fetch & 3) == 0, 754 ("%s: Fetch size not byte integral (%d)\n", 755 __func__, fetch)); 756 fetch >>= 1; 757 fetch &= ~1; 758 info->bpos += fetch; 759 slot -= fetch; 760 RATE_ASSERT(slot >= 0, 761 ("%s: Negative Slot: %d\n", __func__, 762 slot)); 763 if (slot == 0) 764 break; 765 if (info->bpos == info->bufsz) 766 break; 767 } else 768 break; 769 } 770 if (info->pos == info->bpos) { 771 RATE_TEST(info->pos == 2, 772 ("%s: EOF while in progress\n", __func__)); 773 break; 774 } 775 RATE_ASSERT(info->pos <= info->bpos, 776 ("%s: [2] Buffer overrun: %d > %d\n", __func__, 777 info->pos, info->bpos)); 778 RATE_ASSERT(info->pos < info->bpos, 779 ("%s: Zero buffer!\n", __func__)); 780 RATE_ASSERT(((info->bpos - info->pos) & 1) == 0, 781 ("%s: Buffer not sample integral (%d)\n", 782 __func__, info->bpos - info->pos)); 783 i += info->convert(info, dst + i, count - i); 784 RATE_ASSERT(info->pos <= info->bpos, 785 ("%s: [3] Buffer overrun: %d > %d\n", 786 __func__, info->pos, info->bpos)); 787 if (info->pos == info->bpos) { 788 /* 789 * End of buffer cycle. Copy last unit sample 790 * to beginning of buffer so next cycle can 791 * interpolate using it. 792 */ 793 info->buffer[0] = info->buffer[info->pos - 2]; 794 info->buffer[1] = info->buffer[info->pos - 1]; 795 info->bpos = 2; 796 info->pos = 2; 797 } 798 if (i == count) 799 break; 800 } 801 RATE_TEST(count == i, ("Expect: %u , Got: %u\n", count << 1, i << 1)); 802 return i << 1; 803} 804 805static struct pcm_feederdesc feeder_rate_desc[] = { 806 {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0}, 807 {0, 0, 0, 0}, 808}; 809static kobj_method_t feeder_rate_methods[] = { 810 KOBJMETHOD(feeder_init, feed_rate_init), 811 KOBJMETHOD(feeder_free, feed_rate_free), 812 KOBJMETHOD(feeder_set, feed_rate_set), 813 KOBJMETHOD(feeder_get, feed_rate_get), 814 KOBJMETHOD(feeder_feed, feed_rate), 815 {0, 0} 816}; 817FEEDER_DECLARE(feeder_rate, 2, NULL); 818