sys/dev/sound/pcm/feeder_rate.c

   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 /* XXX: this should be settable by an user via a control tool, the sysadmin
 226    needs a max and min sysctl to limit what an user can do */
 227 SYSCTL_PROC(_hw_snd, OID_AUTO, _feeder_rate_buffersize, CTLTYPE_INT | CTLFLAG_RW,
 228         0, sizeof(int), sysctl_hw_snd_feeder_rate_buffersize, "I", "");
 229
 230 static void
 231 feed_speed_ratio(uint32_t x, uint32_t y, uint32_t *gx, uint32_t *gy)
 232 {
 233         uint32_t w, src = x, dst = y;
 234
 235         while (y != 0) {
 236                 w = x % y;
 237                 x = y;
 238                 y = w;
 239         }
 240         *gx = src / x;
 241         *gy = dst / x;
 242 }
 243
 244 static void
 245 feed_scale_roll(uint32_t dst, int32_t *scale, int32_t *roll, int32_t max)
 246 {
 247         int64_t k, tscale;
 248         int32_t j, troll;
 249
 250         *scale = *roll = -1;
 251         for (j = MAXGAIN; j >= MINGAIN; j -= 3) {
 252                 for (troll = 0; troll < max; troll++) {
 253                         tscale = (1 << troll) / dst;
 254                         k = (tscale * dst * 100) >> troll;
 255                         if (k > j && k <= 100) {
 256                                 *scale = tscale;
 257                                 *roll = troll;
 258                                 return;
 259                         }
 260                 }
 261         }
 262 }
 263
 264 static int
 265 feed_get_best_coef(uint32_t *src, uint32_t *dst, uint32_t *gx, uint32_t *gy,
 266                         int32_t *scale, int32_t *roll)
 267 {
 268         uint32_t tsrc, tdst, sscale, dscale;
 269         int32_t tscale, troll;
 270         int i, j, hzmin, hzmax;
 271
 272         *scale = *roll = -1;
 273         for (i = 0; i < 2; i++) {
 274                 hzmin = (ROUNDHZ * i) + 1;
 275                 hzmax = hzmin + ROUNDHZ;
 276                 for (j = hzmin; j < hzmax; j++) {
 277                         tsrc = *src - (*src % j);
 278                         tdst = *dst;
 279                         if (tsrc < 1 || tdst < 1)
 280                                 goto coef_failed;
 281                         feed_speed_ratio(tsrc, tdst, &sscale, &dscale);
 282                         feed_scale_roll(dscale, &tscale, &troll,
 283                                                 FEEDRATE_32_MAXROLL);
 284                         if (tscale != -1 && troll != -1) {
 285                                 *src = tsrc;
 286                                 *gx = sscale;
 287                                 *gy = dscale;
 288                                 *scale = tscale;
 289                                 *roll = troll;
 290                                 return j;
 291                         }
 292                 }
 293                 for (j = hzmin; j < hzmax; j++) {
 294                         tsrc = *src - (*src % j);
 295                         tdst = *dst - (*dst % j);
 296                         if (tsrc < 1 || tdst < 1)
 297                                 goto coef_failed;
 298                         feed_speed_ratio(tsrc, tdst, &sscale, &dscale);
 299                         feed_scale_roll(dscale, &tscale, &troll,
 300                                                 FEEDRATE_32_MAXROLL);
 301                         if (tscale != -1 && troll != -1) {
 302                                 *src = tsrc;
 303                                 *dst = tdst;
 304                                 *gx = sscale;
 305                                 *gy = dscale;
 306                                 *scale = tscale;
 307                                 *roll = troll;
 308                                 return j;
 309                         }
 310                 }
 311                 for (j = hzmin; j < hzmax; j++) {
 312                         tsrc = *src;
 313                         tdst = *dst - (*dst % j);
 314                         if (tsrc < 1 || tdst < 1)
 315                                 goto coef_failed;
 316                         feed_speed_ratio(tsrc, tdst, &sscale, &dscale);
 317                         feed_scale_roll(dscale, &tscale, &troll,
 318                                                 FEEDRATE_32_MAXROLL);
 319                         if (tscale != -1 && troll != -1) {
 320                                 *src = tsrc;
 321                                 *dst = tdst;
 322                                 *gx = sscale;
 323                                 *gy = dscale;
 324                                 *scale = tscale;
 325                                 *roll = troll;
 326                                 return j;
 327                         }
 328                 }
 329         }
 330 coef_failed:
 331         feed_speed_ratio(*src, *dst, gx, gy);
 332         feed_scale_roll(*gy, scale, roll, FEEDRATE_32_MAXROLL);
 333         return 0;
 334 }
 335
 336 static void
 337 feed_rate_reset(struct feed_rate_info *info)
 338 {
 339         info->scale = -1;
 340         info->roll = -1;
 341         info->src = info->rsrc;
 342         info->dst = info->rdst;
 343         info->gx = 0;
 344         info->gy = 0;
 345 }
 346
 347 static int
 348 feed_rate_setup(struct pcm_feeder *f)
 349 {
 350         struct feed_rate_info *info = f->data;
 351         int r = 0;
 352
 353         info->pos = 2;
 354         info->bpos = 4;
 355         info->alpha = 0;
 356         info->stray = 0;
 357         feed_rate_reset(info);
 358         if (info->src == info->dst) {
 359                 /*
 360                  * No conversion ever needed. Just do plain copy.
 361                  */
 362                 info->convert = feed_convert_plain;
 363                 info->gx = 1;
 364                 info->gy = 1;
 365         } else {
 366                 switch (feeder_rate_scaling) {
 367                         case FEEDRATE_CONVERT_64:
 368                                 feed_speed_ratio(info->src, info->dst,
 369                                         &info->gx, &info->gy);
 370                                 info->convert = feed_convert_64;
 371                                 break;
 372                         case FEEDRATE_CONVERT_SCALE64:
 373                                 feed_speed_ratio(info->src, info->dst,
 374                                         &info->gx, &info->gy);
 375                                 feed_scale_roll(info->gy, &info->scale,
 376                                         &info->roll, FEEDRATE_64_MAXROLL);
 377                                 if (info->scale == -1 || info->roll == -1)
 378                                         return -1;
 379                                 info->convert = feed_convert_scale64;
 380                                 break;
 381                         case FEEDRATE_CONVERT_SCALE32:
 382                                 r = feed_get_best_coef(&info->src, &info->dst,
 383                                         &info->gx, &info->gy, &info->scale,
 384                                         &info->roll);
 385                                 if (r == 0)
 386                                         return -1;
 387                                 info->convert = feed_convert_scale32;
 388                                 break;
 389                         case FEEDRATE_CONVERT_PLAIN:
 390                                 feed_speed_ratio(info->src, info->dst,
 391                                         &info->gx, &info->gy);
 392                                 info->convert = feed_convert_plain;
 393                                 break;
 394                         case FEEDRATE_CONVERT_FIXED:
 395                                 r = feed_get_best_coef(&info->src, &info->dst,
 396                                         &info->gx, &info->gy, &info->scale,
 397                                         &info->roll);
 398                                 if (r != 0 && info->src == info->rsrc &&
 399                                                 info->dst == info->rdst)
 400                                         info->convert = feed_convert_scale32;
 401                                 else {
 402                                         /* Fallback */
 403                                         feed_rate_reset(info);
 404                                         feed_speed_ratio(info->src, info->dst,
 405                                                 &info->gx, &info->gy);
 406                                         feed_scale_roll(info->gy, &info->scale,
 407                                                 &info->roll, FEEDRATE_64_MAXROLL);
 408                                         if (info->scale != -1 && info->roll != -1)
 409                                                 info->convert = feed_convert_scale64;
 410                                         else
 411                                                 info->convert = feed_convert_64;
 412                                 }
 413                                 break;
 414                         case FEEDRATE_CONVERT_OPTIMAL:
 415                                 r = feed_get_best_coef(&info->src, &info->dst,
 416                                         &info->gx, &info->gy, &info->scale,
 417                                         &info->roll);
 418                                 if (r != 0)
 419                                         info->convert = feed_convert_scale32;
 420                                 else {
 421                                         /* Fallback */
 422                                         feed_rate_reset(info);
 423                                         feed_speed_ratio(info->src, info->dst,
 424                                                 &info->gx, &info->gy);
 425                                         feed_scale_roll(info->gy, &info->scale,
 426                                                 &info->roll, FEEDRATE_64_MAXROLL);
 427                                         if (info->scale != -1 && info->roll != -1)
 428                                                 info->convert = feed_convert_scale64;
 429                                         else
 430                                                 info->convert = feed_convert_64;
 431                                 }
 432                                 break;
 433                         case FEEDRATE_CONVERT_WORST:
 434                                 r = feed_get_best_coef(&info->src, &info->dst,
 435                                         &info->gx, &info->gy, &info->scale,
 436                                         &info->roll);
 437                                 if (r != 0)
 438                                         info->convert = feed_convert_scale32;
 439                                 else {
 440                                         /* Fallback */
 441                                         feed_rate_reset(info);
 442                                         feed_speed_ratio(info->src, info->dst,
 443                                                 &info->gx, &info->gy);
 444                                         info->convert = feed_convert_plain;
 445                                 }
 446                                 break;
 447                         default:
 448                                 return -1;
 449                                 break;
 450                 }
 451                 /* No way! */
 452                 if (info->gx == 0 || info->gy == 0)
 453                         return -1;
 454                 /*
 455                  * No need to interpolate/decimate, just do plain copy.
 456                  * This probably caused by Hz roundup.
 457                  */
 458                 if (info->gx == info->gy)
 459                         info->convert = feed_convert_plain;
 460         }
 461         return 0;
 462 }
 463
 464 static int
 465 feed_rate_set(struct pcm_feeder *f, int what, int value)
 466 {
 467         struct feed_rate_info *info = f->data;
 468
 469         if (value < feeder_rate_ratemin || value > feeder_rate_ratemax)
 470                 return -1;
 471
 472         switch (what) {
 473                 case FEEDRATE_SRC:
 474                         info->rsrc = value;
 475                         break;
 476                 case FEEDRATE_DST:
 477                         info->rdst = value;
 478                         break;
 479                 default:
 480                         return -1;
 481         }
 482         return feed_rate_setup(f);
 483 }
 484
 485 static int
 486 feed_rate_get(struct pcm_feeder *f, int what)
 487 {
 488         struct feed_rate_info *info = f->data;
 489
 490         /*
 491          * Return *real* src/dst rate.
 492          */
 493         switch (what) {
 494                 case FEEDRATE_SRC:
 495                         return info->rsrc;
 496                 case FEEDRATE_DST:
 497                         return info->rdst;
 498                 default:
 499                         return -1;
 500         }
 501         return -1;
 502 }
 503
 504 static int
 505 feed_rate_init(struct pcm_feeder *f)
 506 {
 507         struct feed_rate_info *info;
 508
 509         info = malloc(sizeof(*info), M_RATEFEEDER, M_NOWAIT | M_ZERO);
 510         if (info == NULL)
 511                 return ENOMEM;
 512         /*
 513          * bufsz = sample from last cycle + conversion space
 514          */
 515         info->bufsz = 2 + feeder_rate_buffersize;
 516         info->buffer = malloc(sizeof(*info->buffer) * info->bufsz,
 517                                         M_RATEFEEDER, M_NOWAIT | M_ZERO);
 518         if (info->buffer == NULL) {
 519                 free(info, M_RATEFEEDER);
 520                 return ENOMEM;
 521         }
 522         info->rsrc = DSP_DEFAULT_SPEED;
 523         info->rdst = DSP_DEFAULT_SPEED;
 524         f->data = info;
 525         return feed_rate_setup(f);
 526 }
 527
 528 static int
 529 feed_rate_free(struct pcm_feeder *f)
 530 {
 531         struct feed_rate_info *info = f->data;
 532
 533         if (info) {
 534                 if (info->buffer)
 535                         free(info->buffer, M_RATEFEEDER);
 536                 free(info, M_RATEFEEDER);
 537         }
 538         f->data = NULL;
 539         return 0;
 540 }
 541
 542 static uint32_t
 543 feed_convert_64(struct feed_rate_info *info, int16_t *dst, uint32_t max)
 544 {
 545         int64_t x, alpha, distance;
 546         uint32_t ret;
 547         int32_t pos, bpos, gx, gy;
 548         int16_t *src;
 549         /*
 550          * Plain, straight forward 64bit arith. No bit-magic applied here.
 551          */
 552         ret = 0;
 553         alpha = info->alpha;
 554         gx = info->gx;
 555         gy = info->gy;
 556         pos = info->pos;
 557         bpos = info->bpos;
 558         src = info->buffer;
 559         for (;;) {
 560                 if (alpha < gx) {
 561                         alpha += gy;
 562                         pos += 2;
 563                         if (pos == bpos)
 564                                 break;
 565                 } else {
 566                         alpha -= gx;
 567                         distance = gy - alpha;
 568                         x = (alpha * src[pos - 2]) + (distance * src[pos]);
 569                         dst[ret++] = x / gy;
 570                         x = (alpha * src[pos - 1]) + (distance * src[pos + 1]);
 571                         dst[ret++] = x / gy;
 572                         if (ret == max)
 573                                 break;
 574                 }
 575         }
 576         info->alpha = alpha;
 577         info->pos = pos;
 578         return ret;
 579 }
 580
 581 static uint32_t
 582 feed_convert_scale64(struct feed_rate_info *info, int16_t *dst, uint32_t max)
 583 {
 584         int64_t x, alpha, distance;
 585         uint32_t ret;
 586         int32_t pos, bpos, gx, gy, roll;
 587         int16_t *src;
 588         /*
 589          * 64bit scaling.
 590          */
 591         ret = 0;
 592         roll = info->roll;
 593         alpha = info->alpha * info->scale;
 594         gx = info->gx * info->scale;
 595         gy = info->gy * info->scale;
 596         pos = info->pos;
 597         bpos = info->bpos;
 598         src = info->buffer;
 599         for (;;) {
 600                 if (alpha < gx) {
 601                         alpha += gy;
 602                         pos += 2;
 603                         if (pos == bpos)
 604                                 break;
 605                 } else {
 606                         alpha -= gx;
 607                         distance = gy - alpha;
 608                         x = (alpha * src[pos - 2]) + (distance * src[pos]);
 609                         dst[ret++] = x >> roll;
 610                         x = (alpha * src[pos - 1]) + (distance * src[pos + 1]);
 611                         dst[ret++] = x >> roll;
 612                         if (ret == max)
 613                                 break;
 614                 }
 615         }
 616         info->alpha = alpha / info->scale;
 617         info->pos = pos;
 618         return ret;
 619 }
 620
 621 static uint32_t
 622 feed_convert_scale32(struct feed_rate_info *info, int16_t *dst, uint32_t max)
 623 {
 624         uint32_t ret;
 625         int32_t x, pos, bpos, gx, gy, alpha, roll, distance;
 626         int16_t *src;
 627         /*
 628          * 32bit scaling.
 629          */
 630         ret = 0;
 631         roll = info->roll;
 632         alpha = info->alpha * info->scale;
 633         gx = info->gx * info->scale;
 634         gy = info->gy * info->scale;
 635         pos = info->pos;
 636         bpos = info->bpos;
 637         src = info->buffer;
 638         for (;;) {
 639                 if (alpha < gx) {
 640                         alpha += gy;
 641                         pos += 2;
 642                         if (pos == bpos)
 643                                 break;
 644                 } else {
 645                         alpha -= gx;
 646                         distance = gy - alpha;
 647                         x = (alpha * src[pos - 2]) + (distance * src[pos]);
 648                         dst[ret++] = x >> roll;
 649                         x = (alpha * src[pos - 1]) + (distance * src[pos + 1]);
 650                         dst[ret++] = x >> roll;
 651                         if (ret == max)
 652                                 break;
 653                 }
 654         }
 655         info->alpha = alpha / info->scale;
 656         info->pos = pos;
 657         return ret;
 658 }
 659
 660 static uint32_t
 661 feed_convert_plain(struct feed_rate_info *info, int16_t *dst, uint32_t max)
 662 {
 663         uint32_t ret;
 664         int32_t pos, bpos, gx, gy, alpha;
 665         int16_t *src;
 666         /*
 667          * Plain copy.
 668          */
 669         ret = 0;
 670         gx = info->gx;
 671         gy = info->gy;
 672         alpha = info->alpha;
 673         pos = info->pos;
 674         bpos = info->bpos;
 675         src = info->buffer;
 676         for (;;) {
 677                 if (alpha < gx) {
 678                         alpha += gy;
 679                         pos += 2;
 680                         if (pos == bpos)
 681                                 break;
 682                 } else {
 683                         alpha -= gx;
 684                         dst[ret++] = src[pos];
 685                         dst[ret++] = src[pos + 1];
 686                         if (ret == max)
 687                                 break;
 688                 }
 689         }
 690         info->pos = pos;
 691         info->alpha = alpha;
 692         return ret;
 693 }
 694
 695 static int32_t
 696 feed_rate(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
 697                         uint32_t count, void *source)
 698 {
 699         struct feed_rate_info *info = f->data;
 700         uint32_t i;
 701         int32_t fetch, slot;
 702         int16_t *dst = (int16_t *)b;
 703         /*
 704          * This loop has been optimized to generalize both up / down
 705          * sampling without causing missing samples or excessive buffer
 706          * feeding.
 707          */
 708         RATE_TEST(count >= 4 && (count & 3) == 0,
 709                 ("%s: Count size not byte integral (%d)\n", __func__, count));
 710         if (count < 4)
 711                 return 0;
 712         count >>= 1;
 713         count &= ~1;
 714         slot = (((info->gx * (count >> 1)) + info->gy - info->alpha - 1) / info->gy) << 1;
 715         RATE_TEST((slot & 1) == 0, ("%s: Slot count not sample integral (%d)\n",
 716                                                 __func__, slot));
 717         /*
 718          * Optimize buffer feeding aggressively to ensure calculated slot
 719          * can be fitted nicely into available buffer free space, hence
 720          * avoiding multiple feeding.
 721          */
 722         RATE_TEST(info->stray == 0, ("%s: [1] Stray bytes: %u\n",
 723                 __func__,info->stray));
 724         if (info->pos != 2 && info->bpos - info->pos == 2 &&
 725                         info->bpos + slot > info->bufsz) {
 726                 /*
 727                  * Copy last unit sample and its previous to
 728                  * beginning of buffer.
 729                  */
 730                 info->buffer[0] = info->buffer[info->pos - 2];
 731                 info->buffer[1] = info->buffer[info->pos - 1];
 732                 info->buffer[2] = info->buffer[info->pos];
 733                 info->buffer[3] = info->buffer[info->pos + 1];
 734                 info->pos = 2;
 735                 info->bpos = 4;
 736         }
 737         RATE_ASSERT(slot >= 0, ("%s: Negative Slot: %d\n",
 738                         __func__, slot));
 739         i = 0;
 740         for (;;) {
 741                 for (;;) {
 742                         fetch = (info->bufsz - info->bpos) << 1;
 743                         fetch -= info->stray;
 744                         RATE_ASSERT(fetch >= 0,
 745                                 ("%s: [1] Buffer overrun: %d > %d\n",
 746                                         __func__, info->bpos, info->bufsz));
 747                         if ((slot << 1) < fetch)
 748                                 fetch = slot << 1;
 749                         if (fetch > 0) {
 750                                 RATE_ASSERT(((info->bpos << 1) - info->stray) >= 0 &&
 751                                         ((info->bpos << 1) - info->stray) < (info->bufsz << 1),
 752                                         ("%s: DANGER - BUFFER OVERRUN! bufsz=%d, pos=%d\n", __func__,
 753                                         info->bufsz << 1, (info->bpos << 1) - info->stray));
 754                                 fetch = FEEDER_FEED(f->source, c,
 755                                                 (uint8_t *)(info->buffer) + (info->bpos << 1) - info->stray,
 756                                                 fetch, source);
 757                                 info->stray = 0;
 758                                 if (fetch == 0)
 759                                         break;
 760                                 RATE_TEST((fetch & 3) == 0,
 761                                         ("%s: Fetch size not byte integral (%d)\n",
 762                                         __func__, fetch));
 763                                 info->stray += fetch & 3;
 764                                 RATE_TEST(info->stray == 0,
 765                                         ("%s: Stray bytes detected (%d)\n",
 766                                         __func__, info->stray));
 767                                 fetch >>= 1;
 768                                 fetch &= ~1;
 769                                 info->bpos += fetch;
 770                                 slot -= fetch;
 771                                 RATE_ASSERT(slot >= 0,
 772                                         ("%s: Negative Slot: %d\n", __func__,
 773                                                 slot));
 774                                 if (slot == 0)
 775                                         break;
 776                                 if (info->bpos == info->bufsz)
 777                                         break;
 778                         } else
 779                                 break;
 780                 }
 781                 if (info->pos == info->bpos) {
 782                         RATE_TEST(info->pos == 2,
 783                                 ("%s: EOF while in progress\n", __func__));
 784                         break;
 785                 }
 786                 RATE_ASSERT(info->pos <= info->bpos,
 787                         ("%s: [2] Buffer overrun: %d > %d\n", __func__,
 788                         info->pos, info->bpos));
 789                 RATE_ASSERT(info->pos < info->bpos,
 790                         ("%s: Zero buffer!\n", __func__));
 791                 RATE_ASSERT(((info->bpos - info->pos) & 1) == 0,
 792                         ("%s: Buffer not sample integral (%d)\n",
 793                         __func__, info->bpos - info->pos));
 794                 i += info->convert(info, dst + i, count - i);
 795                 RATE_ASSERT(info->pos <= info->bpos,
 796                                 ("%s: [3] Buffer overrun: %d > %d\n",
 797                                         __func__, info->pos, info->bpos));
 798                 if (info->pos == info->bpos) {
 799                         /*
 800                          * End of buffer cycle. Copy last unit sample
 801                          * to beginning of buffer so next cycle can
 802                          * interpolate using it.
 803                          */
 804                         RATE_TEST(info->stray == 0, ("%s: [2] Stray bytes: %u\n", __func__, info->stray));
 805                         info->buffer[0] = info->buffer[info->pos - 2];
 806                         info->buffer[1] = info->buffer[info->pos - 1];
 807                         info->bpos = 2;
 808                         info->pos = 2;
 809                 }
 810                 if (i == count)
 811                         break;
 812         }
 813 #if 0
 814         RATE_TEST(count == i, ("Expect: %u , Got: %u\n", count << 1, i << 1));
 815 #endif
 816         RATE_TEST(info->stray == 0, ("%s: [3] Stray bytes: %u\n", __func__, info->stray));
 817         return i << 1;
 818 }
 819
 820 static struct pcm_feederdesc feeder_rate_desc[] = {
 821         {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
 822         {0, 0, 0, 0},
 823 };
 824 static kobj_method_t feeder_rate_methods[] = {
 825         KOBJMETHOD(feeder_init,         feed_rate_init),
 826         KOBJMETHOD(feeder_free,         feed_rate_free),
 827         KOBJMETHOD(feeder_set,          feed_rate_set),
 828         KOBJMETHOD(feeder_get,          feed_rate_get),
 829         KOBJMETHOD(feeder_feed,         feed_rate),
 830         {0, 0}
 831 };
 832 FEEDER_DECLARE(feeder_rate, 2, NULL);