sys/dev/sound/pcm/feeder_rate.c

   1 /*
   2  * Copyright (c) 2003 Orion Hodson <orion@freebsd.org>
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  *
  26  * MAINTAINER: Orion Hodson <orion@freebsd.org>
  27  *
  28  * This rate conversion code uses linear interpolation without any
  29  * pre- or post- interpolation filtering to combat aliasing.  This
  30  * greatly limits the sound quality and should be addressed at some
  31  * stage in the future.
  32  *
  33  * Since this accuracy of interpolation is sensitive and examination
  34  * of the algorithm output is harder from the kernel, th code is
  35  * designed to be compiled in the kernel and in a userland test
  36  * harness.  This is done by selectively including and excluding code
  37  * with several portions based on whether _KERNEL is defined.  It's a
  38  * little ugly, but exceedingly useful.  The testsuite and its
  39  * revisions can be found at:
  40  *              http://people.freebsd.org/~orion/feedrate/
  41  *
  42  * Special thanks to Ken Marx for exposing flaws in the code and for
  43  * testing revisions.
  44  */
  45
  46 #ifdef _KERNEL
  47
  48 #include <dev/sound/pcm/sound.h>
  49 #include "feeder_if.h"
  50
  51 SND_DECLARE_FILE("$FreeBSD: src/sys/dev/sound/pcm/feeder_rate.c,v 1.2.2.3 2003/02/08 02:38:21 orion Exp $");
  52
  53 #endif /* _KERNEL */
  54
  55 MALLOC_DEFINE(M_RATEFEEDER, "ratefeed", "pcm rate feeder");
  56
  57 #ifndef RATE_ASSERT
  58 #define RATE_ASSERT(x, y) /* KASSERT(x) */
  59 #endif /* RATE_ASSERT */
  60
  61 #ifndef RATE_TRACE
  62 #define RATE_TRACE(x...)  /* printf(x) */
  63 #endif
  64
  65 /*****************************************************************************/
  66
  67 /* The following coefficients are coupled.  They are chosen to be
  68  * guarantee calculable factors for the interpolation routine.  They
  69  * have been tested over the range of RATEMIN-RATEMAX Hz.  Decreasing
  70  * the granularity increases the required buffer size and affects the
  71  * gain values at different points in the space.  These values were
  72  * found by running the test program with -p (probe) and some trial
  73  * and error.
  74  *
  75  * ROUNDHZ      the granularity of sample rates (fits n*11025 and n*8000).
  76  * FEEDBUFSZ    the amount of buffer space.
  77  * MINGAIN      the minimum acceptable gain in coefficients search.
  78  */
  79 #define ROUNDHZ                    25
  80 #define FEEDBUFSZ                8192
  81 #define MINGAIN                    92
  82
  83 #define RATEMIN                  4000
  84 #define RATEMAX                 48000
  85
  86 struct feed_rate_info;
  87
  88 typedef int (*rate_convert_method)(struct feed_rate_info *,
  89                                    uint32_t, uint32_t, int16_t *);
  90
  91 static int
  92 convert_stereo_up(struct feed_rate_info *info,
  93                   uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst);
  94
  95 static int
  96 convert_stereo_down(struct feed_rate_info *info,
  97                     uint32_t src_ticks, uint32_t dst_ticks, int16_t *dst);
  98
  99 struct feed_rate_info {
 100         uint32_t src, dst;      /* source and destination rates */
 101         uint16_t buffer_ticks;  /* number of available samples in buffer */
 102         uint16_t buffer_pos;    /* next available sample in buffer */
 103         uint16_t rounds;        /* maximum number of cycle rounds w buffer */
 104         uint16_t alpha;         /* interpolation distance */
 105         uint16_t sscale;        /* src clock scale */
 106         uint16_t dscale;        /* dst clock scale */
 107         uint16_t mscale;        /* scale factor to avoid divide per sample */
 108         uint16_t mroll;         /* roll to again avoid divide per sample */
 109         uint16_t channels;      /* 1 = mono, 2 = stereo */
 110
 111         rate_convert_method convert;
 112         int16_t  buffer[FEEDBUFSZ];
 113 };
 114
 115 #define bytes_per_sample                2
 116 #define src_ticks_per_cycle(info)       (info->dscale * info->rounds)
 117 #define dst_ticks_per_cycle(info)       (info->sscale * info->rounds)
 118 #define bytes_per_tick(info)            (info->channels * bytes_per_sample)
 119 #define src_bytes_per_cycle(info)                                             \
 120                         (src_ticks_per_cycle(info) * bytes_per_tick(info))
 121 #define dst_bytes_per_cycle(info)                                             \
 122                         (dst_ticks_per_cycle(info) * bytes_per_tick(info))
 123
 124 static uint32_t
 125 gcd(uint32_t x, uint32_t y)
 126 {
 127         uint32_t w;
 128         while (y != 0) {
 129                 w = x % y;
 130                 x = y;
 131                 y = w;
 132         }
 133         return x;
 134 }
 135
 136 static int
 137 feed_rate_setup(struct pcm_feeder *f)
 138 {
 139         struct feed_rate_info *info = f->data;
 140         uint32_t mscale, mroll, l, r, g;
 141
 142         /* Beat sample rates down by greatest common divisor */
 143         g = gcd(info->src, info->dst);
 144         info->sscale = info->dst / g;
 145         info->dscale = info->src / g;
 146
 147         info->alpha = 0;
 148         info->buffer_ticks = 0;
 149         info->buffer_pos = 0;
 150
 151         /* Pick suitable conversion routine */
 152         if (info->src > info->dst) {
 153                 info->convert = convert_stereo_down;
 154         } else {
 155                 info->convert = convert_stereo_up;
 156         }
 157
 158         /*
 159          * Determine number of conversion rounds that will fit into
 160          * buffer.  NB Must set info->rounds to one before using
 161          * src_ticks_per_cycle here since it used by src_ticks_per_cycle.
 162          */
 163         info->rounds = 1;
 164         r = (FEEDBUFSZ - bytes_per_tick(info)) /
 165                 (src_ticks_per_cycle(info) * bytes_per_tick(info));
 166         if (r == 0) {
 167                 RATE_TRACE("Insufficient buffer space for conversion %d -> %d "
 168                            "(%d < %d)\n", info->src, info->dst, FEEDBUFSZ,
 169                            src_ticks_per_cycle(info) * bytes_per_tick(info));
 170                 return -1;
 171         }
 172         info->rounds = r;
 173
 174         /*
 175          * Find scale and roll combination that allows us to trade
 176          * costly divide operations in the main loop for multiply-rolls.
 177          */
 178         for (l = 96; l >= MINGAIN; l -= 3) {
 179                 for (mroll = 0; mroll < 16; mroll ++) {
 180                         mscale = (1 << mroll) / info->sscale;
 181
 182                         r = (mscale * info->sscale * 100) >> mroll;
 183                         if (r > l && r <= 100) {
 184                                 info->mscale = mscale;
 185                                 info->mroll = mroll;
 186                                 RATE_TRACE("Converting %d to %d with "
 187                                            "mscale = %d and mroll = %d "
 188                                            "(gain = %d / 100)\n",
 189                                            info->src, info->dst,
 190                                            info->mscale, info->mroll, r);
 191                                 return 0;
 192                         }
 193                 }
 194         }
 195
 196         RATE_TRACE("Failed to find a converter within %d%% gain for "
 197                    "%d to %d.\n", l, info->src, info->dst);
 198
 199         return -2;
 200 }
 201
 202 static int
 203 feed_rate_set(struct pcm_feeder *f, int what, int value)
 204 {
 205         struct feed_rate_info *info = f->data;
 206         int rvalue;
 207
 208         if (value < RATEMIN || value > RATEMAX) {
 209                 return -1;
 210         }
 211
 212         rvalue = (value / ROUNDHZ) * ROUNDHZ;
 213         if (value - rvalue > ROUNDHZ / 2) {
 214             rvalue += ROUNDHZ;
 215         }
 216
 217         switch(what) {
 218         case FEEDRATE_SRC:
 219                 info->src = rvalue;
 220                 break;
 221         case FEEDRATE_DST:
 222                 info->dst = rvalue;
 223                 break;
 224         default:
 225                 return -1;
 226         }
 227
 228         return feed_rate_setup(f);
 229 }
 230
 231 static int
 232 feed_rate_get(struct pcm_feeder *f, int what)
 233 {
 234         struct feed_rate_info *info = f->data;
 235
 236         switch(what) {
 237         case FEEDRATE_SRC:
 238                 return info->src;
 239         case FEEDRATE_DST:
 240                 return info->dst;
 241         default:
 242                 return -1;
 243         }
 244         return -1;
 245 }
 246
 247 static int
 248 feed_rate_init(struct pcm_feeder *f)
 249 {
 250         struct feed_rate_info *info;
 251
 252         info = malloc(sizeof(*info), M_RATEFEEDER, M_WAITOK | M_ZERO);
 253         if (info == NULL)
 254                 return ENOMEM;
 255
 256         info->src = DSP_DEFAULT_SPEED;
 257         info->dst = DSP_DEFAULT_SPEED;
 258         info->channels = 2;
 259
 260         f->data = info;
 261         return 0;
 262 }
 263
 264 static int
 265 feed_rate_free(struct pcm_feeder *f)
 266 {
 267         struct feed_rate_info *info = f->data;
 268
 269         if (info) {
 270                 free(info, M_RATEFEEDER);
 271         }
 272         f->data = NULL;
 273         return 0;
 274 }
 275
 276 static int
 277 convert_stereo_up(struct feed_rate_info *info,
 278                   uint32_t               src_ticks,
 279                   uint32_t               dst_ticks,
 280                   int16_t               *dst)
 281 {
 282         uint32_t max_dst_ticks;
 283         int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o;
 284         int16_t *src;
 285
 286         sp = info->buffer_pos * 2;
 287         se = sp + src_ticks * 2;
 288
 289         src = info->buffer;
 290         alpha = info->alpha * info->mscale;
 291         dalpha = info->dscale * info->mscale; /* Alpha increment */
 292         malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */
 293         mroll = info->mroll;
 294
 295         /*
 296          * For efficiency the main conversion loop should only depend on
 297          * one variable.  We use the state to work out the maximum number
 298          * of output samples that are available and eliminate the checking of
 299          * sp from the loop.
 300          */
 301         max_dst_ticks = src_ticks * info->dst / info->src - alpha / dalpha;
 302         if (max_dst_ticks < dst_ticks) {
 303                 dst_ticks = max_dst_ticks;
 304         }
 305
 306         dp = 0;
 307         de = dst_ticks * 2;
 308         /*
 309          * Unrolling this loop manually does not help much here because
 310          * of the alpha, malpha comparison.
 311          */
 312         while (dp < de) {
 313                 o = malpha - alpha;
 314                 x = alpha * src[sp + 2] + o * src[sp];
 315                 dst[dp++] = x >> mroll;
 316                 x = alpha * src[sp + 3] + o * src[sp + 1];
 317                 dst[dp++] = x >> mroll;
 318                 alpha += dalpha;
 319                 if (alpha >= malpha) {
 320                         alpha -= malpha;
 321                         sp += 2;
 322                 }
 323         }
 324         RATE_ASSERT(sp <= se, ("%s: Source overrun\n", __func__));
 325
 326         info->buffer_pos = sp / info->channels;
 327         info->alpha = alpha / info->mscale;
 328
 329         return dp / info->channels;
 330 }
 331
 332 static int
 333 convert_stereo_down(struct feed_rate_info *info,
 334                     uint32_t               src_ticks,
 335                     uint32_t               dst_ticks,
 336                     int16_t               *dst)
 337 {
 338         int32_t alpha, dalpha, malpha, mroll, sp, dp, se, de, x, o, m,
 339                 mdalpha, mstep;
 340         int16_t *src;
 341
 342         sp = info->buffer_pos * 2;
 343         se = sp + src_ticks * 2;
 344
 345         src = info->buffer;
 346         alpha = info->alpha * info->mscale;
 347         dalpha = info->dscale * info->mscale; /* Alpha increment */
 348         malpha = info->sscale * info->mscale; /* Maximum allowed alpha value */
 349         mroll = info->mroll;
 350
 351         dp = 0;
 352         de = dst_ticks * 2;
 353
 354         m = dalpha / malpha;
 355         mstep = m * 2;
 356         mdalpha = dalpha - m * malpha;
 357
 358         /*
 359          * TODO: eliminate sp or dp from this loop comparison for a few
 360          * extra % performance.
 361          */
 362         while (sp < se && dp < de) {
 363                 o = malpha - alpha;
 364                 x = alpha * src[sp + 2] + o * src[sp];
 365                 dst[dp++] = x >> mroll;
 366                 x = alpha * src[sp + 3] + o * src[sp + 1];
 367                 dst[dp++] = x >> mroll;
 368
 369                 alpha += mdalpha;
 370                 sp += mstep;
 371                 if (alpha >= malpha) {
 372                         alpha -= malpha;
 373                         sp += 2;
 374                 }
 375         }
 376
 377         info->buffer_pos = sp / 2;
 378         info->alpha = alpha / info->mscale;
 379
 380         RATE_ASSERT(info->buffer_pos <= info->buffer_ticks,
 381                     ("%s: Source overrun\n", __func__));
 382
 383         return dp / 2;
 384 }
 385
 386 static int
 387 feed_rate(struct pcm_feeder     *f,
 388           struct pcm_channel    *c,
 389           uint8_t               *b,
 390           uint32_t               count,
 391           void                  *source)
 392 {
 393         struct feed_rate_info *info = f->data;
 394
 395         uint32_t done, s_ticks, d_ticks;
 396         done = 0;
 397
 398         RATE_ASSERT(info->channels == 2,
 399                     ("%s: channels (%d) != 2", __func__, info->channels));
 400
 401         while (done < count) {
 402                 /* Slurp in more data if input buffer is not full */
 403                 while (info->buffer_ticks < src_ticks_per_cycle(info)) {
 404                         uint8_t *u8b;
 405                         int      fetch;
 406                         fetch = src_bytes_per_cycle(info) -
 407                                 info->buffer_ticks * bytes_per_tick(info);
 408                         u8b = (uint8_t*)info->buffer +
 409                                 (info->buffer_ticks + 1) *
 410                                 bytes_per_tick(info);
 411                         fetch = FEEDER_FEED(f->source, c, u8b, fetch, source);
 412                         RATE_ASSERT(fetch % bytes_per_tick(info) == 0,
 413                                     ("%s: fetched unaligned bytes (%d)",
 414                                      __func__, fetch));
 415                         info->buffer_ticks += fetch / bytes_per_tick(info);
 416                         RATE_ASSERT(src_ticks_per_cycle(info) >=
 417                                     info->buffer_ticks,
 418                                     ("%s: buffer overfilled (%d > %d).",
 419                                      __func__, info->buffer_ticks,
 420                                  src_ticks_per_cycle(info)));
 421                         if (fetch == 0)
 422                                 break;
 423                 }
 424
 425                 /* Find amount of input buffer data that should be processed */
 426                 d_ticks = (count - done) / bytes_per_tick(info);
 427                 s_ticks = info->buffer_ticks - info->buffer_pos;
 428                 if (info->buffer_ticks != src_ticks_per_cycle(info)) {
 429                         if (s_ticks > 8)
 430                                 s_ticks -= 8;
 431                         else
 432                                 s_ticks = 0;
 433                 }
 434
 435                 d_ticks = info->convert(info, s_ticks, d_ticks,
 436                                         (int16_t*)(b + done));
 437                 if (d_ticks == 0)
 438                         break;
 439                 done += d_ticks * bytes_per_tick(info);
 440
 441                 RATE_ASSERT(info->buffer_pos <= info->buffer_ticks,
 442                             ("%s: buffer_ticks too big\n", __func__));
 443                 RATE_ASSERT(info->buffer_ticks <= src_ticks_per_cycle(info),
 444                             ("too many ticks %d /  %d\n",
 445                              info->buffer_ticks, src_ticks_per_cycle(info)));
 446                 RATE_TRACE("%s: ticks %5d / %d pos %d\n", __func__,
 447                            info->buffer_ticks, src_ticks_per_cycle(info),
 448                            info->buffer_pos);
 449
 450                 if (src_ticks_per_cycle(info) <= info->buffer_pos) {
 451                         /* End of cycle reached, copy last samples to start */
 452                         uint8_t *u8b;
 453                         u8b = (uint8_t*)info->buffer;
 454                         bcopy(u8b + src_bytes_per_cycle(info), u8b,
 455                               bytes_per_tick(info));
 456
 457                         RATE_ASSERT(info->alpha == 0,
 458                                     ("%s: completed cycle with "
 459                                      "alpha non-zero", __func__, info->alpha));
 460
 461                         info->buffer_pos = 0;
 462                         info->buffer_ticks = 0;
 463                 }
 464         }
 465
 466         RATE_ASSERT(count >= done,
 467                     ("%s: generated too many bytes of data (%d > %d).",
 468                      __func__, done, count));
 469
 470         if (done != count) {
 471                 RATE_TRACE("Only did %d of %d\n", done, count);
 472         }
 473
 474         return done;
 475 }
 476
 477 static struct pcm_feederdesc feeder_rate_desc[] = {
 478         {FEEDER_RATE, AFMT_S16_LE | AFMT_STEREO, AFMT_S16_LE | AFMT_STEREO, 0},
 479         {0, 0, 0, 0},
 480 };
 481 static kobj_method_t feeder_rate_methods[] = {
 482         KOBJMETHOD(feeder_init,         feed_rate_init),
 483         KOBJMETHOD(feeder_free,         feed_rate_free),
 484         KOBJMETHOD(feeder_set,          feed_rate_set),
 485         KOBJMETHOD(feeder_get,          feed_rate_get),
 486         KOBJMETHOD(feeder_feed,         feed_rate),
 487         {0, 0}
 488 };
 489 FEEDER_DECLARE(feeder_rate, 2, NULL);
 490