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