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  * $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_ratemax);
 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, 0, 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, 0, 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, 0, 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, 0, 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);