sys/dev/sound/pcm/feeder_eq.c

   1 /*-
   2  * Copyright (c) 2008-2009 Ariff Abdullah <ariff@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
  27 /*
  28  * feeder_eq: Parametric (compile time) Software Equalizer. Though accidental,
  29  *            it proves good enough for educational and general consumption.
  30  *
  31  * "Cookbook formulae for audio EQ biquad filter coefficients"
  32  *    by Robert Bristow-Johnson  <rbj@audioimagination.com>
  33  *    -  http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
  34  */
  35
  36 #ifdef _KERNEL
  37 #ifdef HAVE_KERNEL_OPTION_HEADERS
  38 #include "opt_snd.h"
  39 #endif
  40 #include <dev/sound/pcm/sound.h>
  41 #include <dev/sound/pcm/pcm.h>
  42 #include "feeder_if.h"
  43
  44 #define SND_USE_FXDIV
  45 #include "snd_fxdiv_gen.h"
  46
  47 SND_DECLARE_FILE("$FreeBSD$");
  48 #endif
  49
  50 #include "feeder_eq_gen.h"
  51
  52 #define FEEDEQ_LEVELS                                                   \
  53         (((FEEDEQ_GAIN_MAX - FEEDEQ_GAIN_MIN) *                         \
  54         (FEEDEQ_GAIN_DIV / FEEDEQ_GAIN_STEP)) + 1)
  55
  56 #define FEEDEQ_L2GAIN(v)                                                \
  57         ((int)min(((v) * FEEDEQ_LEVELS) / 100, FEEDEQ_LEVELS - 1))
  58
  59 #define FEEDEQ_PREAMP_IPART(x)          (abs(x) >> FEEDEQ_GAIN_SHIFT)
  60 #define FEEDEQ_PREAMP_FPART(x)          (abs(x) & FEEDEQ_GAIN_FMASK)
  61 #define FEEDEQ_PREAMP_SIGNVAL(x)        ((x) < 0 ? -1 : 1)
  62 #define FEEDEQ_PREAMP_SIGNMARK(x)       (((x) < 0) ? '-' : '+')
  63
  64 #define FEEDEQ_PREAMP_IMIN      -192
  65 #define FEEDEQ_PREAMP_IMAX      192
  66 #define FEEDEQ_PREAMP_FMIN      0
  67 #define FEEDEQ_PREAMP_FMAX      9
  68
  69 #define FEEDEQ_PREAMP_INVALID   INT_MAX
  70
  71 #define FEEDEQ_IF2PREAMP(i, f)                                          \
  72         ((abs(i) << FEEDEQ_GAIN_SHIFT) |                                \
  73         (((abs(f) / FEEDEQ_GAIN_STEP) * FEEDEQ_GAIN_STEP) &             \
  74         FEEDEQ_GAIN_FMASK))
  75
  76 #define FEEDEQ_PREAMP_MIN                                               \
  77         (FEEDEQ_PREAMP_SIGNVAL(FEEDEQ_GAIN_MIN) *                       \
  78         FEEDEQ_IF2PREAMP(FEEDEQ_GAIN_MIN, 0))
  79
  80 #define FEEDEQ_PREAMP_MAX                                               \
  81         (FEEDEQ_PREAMP_SIGNVAL(FEEDEQ_GAIN_MAX) *                       \
  82         FEEDEQ_IF2PREAMP(FEEDEQ_GAIN_MAX, 0))
  83
  84 #define FEEDEQ_PREAMP_DEFAULT   FEEDEQ_IF2PREAMP(0, 0)
  85
  86 #define FEEDEQ_PREAMP2IDX(v)                                            \
  87         ((int32_t)((FEEDEQ_GAIN_MAX * (FEEDEQ_GAIN_DIV /                \
  88         FEEDEQ_GAIN_STEP)) + (FEEDEQ_PREAMP_SIGNVAL(v) *                \
  89         FEEDEQ_PREAMP_IPART(v) * (FEEDEQ_GAIN_DIV /                     \
  90         FEEDEQ_GAIN_STEP)) + (FEEDEQ_PREAMP_SIGNVAL(v) *                \
  91         (FEEDEQ_PREAMP_FPART(v) / FEEDEQ_GAIN_STEP))))
  92
  93 static int feeder_eq_exact_rate = 0;
  94
  95 #ifdef _KERNEL
  96 static char feeder_eq_presets[] = FEEDER_EQ_PRESETS;
  97 SYSCTL_STRING(_hw_snd, OID_AUTO, feeder_eq_presets, CTLFLAG_RD,
  98     &feeder_eq_presets, 0, "compile-time eq presets");
  99
 100 TUNABLE_INT("hw.snd.feeder_eq_exact_rate", &feeder_eq_exact_rate);
 101 SYSCTL_INT(_hw_snd, OID_AUTO, feeder_eq_exact_rate, CTLFLAG_RW,
 102     &feeder_eq_exact_rate, 0, "force exact rate validation");
 103 #endif
 104
 105 struct feed_eq_info;
 106
 107 typedef void (*feed_eq_t)(struct feed_eq_info *, uint8_t *, uint32_t);
 108
 109 struct feed_eq_tone {
 110         intpcm_t o1[SND_CHN_MAX];
 111         intpcm_t o2[SND_CHN_MAX];
 112         intpcm_t i1[SND_CHN_MAX];
 113         intpcm_t i2[SND_CHN_MAX];
 114         int gain;
 115 };
 116
 117 struct feed_eq_info {
 118         struct feed_eq_tone treble;
 119         struct feed_eq_tone bass;
 120         struct feed_eq_coeff *coeff;
 121         feed_eq_t biquad;
 122         uint32_t channels;
 123         uint32_t rate;
 124         uint32_t align;
 125         int32_t preamp;
 126         int state;
 127 };
 128
 129 #if !defined(_KERNEL) && defined(FEEDEQ_ERR_CLIP)
 130 #define FEEDEQ_ERR_CLIP_CHECK(t, v)     do {                            \
 131         if ((v) < PCM_S32_MIN || (v) > PCM_S32_MAX)                     \
 132                 errx(1, "\n\n%s(): ["#t"] Sample clipping: %jd\n",      \
 133                     __func__, (intmax_t)(v));                           \
 134 } while (0)
 135 #else
 136 #define FEEDEQ_ERR_CLIP_CHECK(...)
 137 #endif
 138
 139 #define FEEDEQ_CLAMP(v)         (((v) > PCM_S32_MAX) ? PCM_S32_MAX :    \
 140                                 (((v) < PCM_S32_MIN) ? PCM_S32_MIN :    \
 141                                   (v)))
 142
 143 #define FEEDEQ_DECLARE(SIGN, BIT, ENDIAN)                                       \
 144 static void                                                                     \
 145 feed_eq_biquad_##SIGN##BIT##ENDIAN(struct feed_eq_info *info,                   \
 146     uint8_t *dst, uint32_t count)                                               \
 147 {                                                                               \
 148         struct feed_eq_coeff_tone *treble, *bass;                               \
 149         intpcm64_t w;                                                           \
 150         intpcm_t v;                                                             \
 151         uint32_t i, j;                                                          \
 152         int32_t pmul, pshift;                                                   \
 153                                                                                 \
 154         pmul = feed_eq_preamp[info->preamp].mul;                                \
 155         pshift = feed_eq_preamp[info->preamp].shift;                            \
 156                                                                                 \
 157         if (info->state == FEEDEQ_DISABLE) {                                    \
 158                 j = count * info->channels;                                     \
 159                 dst += j * PCM_##BIT##_BPS;                                     \
 160                 do {                                                            \
 161                         dst -= PCM_##BIT##_BPS;                                 \
 162                         v = _PCM_READ_##SIGN##BIT##_##ENDIAN(dst);              \
 163                         v = ((intpcm64_t)pmul * v) >> pshift;                   \
 164                         _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, v);              \
 165                 } while (--j != 0);                                             \
 166                                                                                 \
 167                 return;                                                         \
 168         }                                                                       \
 169                                                                                 \
 170         treble = &(info->coeff[info->treble.gain].treble);                      \
 171         bass   = &(info->coeff[info->bass.gain].bass);                          \
 172                                                                                 \
 173         do {                                                                    \
 174                 i = 0;                                                          \
 175                 j = info->channels;                                             \
 176                 do {                                                            \
 177                         v = _PCM_READ_##SIGN##BIT##_##ENDIAN(dst);              \
 178                         v <<= 32 - BIT;                                         \
 179                         v = ((intpcm64_t)pmul * v) >> pshift;                   \
 180                                                                                 \
 181                         w  = (intpcm64_t)v * treble->b0;                        \
 182                         w += (intpcm64_t)info->treble.i1[i] * treble->b1;       \
 183                         w += (intpcm64_t)info->treble.i2[i] * treble->b2;       \
 184                         w -= (intpcm64_t)info->treble.o1[i] * treble->a1;       \
 185                         w -= (intpcm64_t)info->treble.o2[i] * treble->a2;       \
 186                         info->treble.i2[i] = info->treble.i1[i];                \
 187                         info->treble.i1[i] = v;                                 \
 188                         info->treble.o2[i] = info->treble.o1[i];                \
 189                         w >>= FEEDEQ_COEFF_SHIFT;                               \
 190                         FEEDEQ_ERR_CLIP_CHECK(treble, w);                       \
 191                         v = FEEDEQ_CLAMP(w);                                    \
 192                         info->treble.o1[i] = v;                                 \
 193                                                                                 \
 194                         w  = (intpcm64_t)v * bass->b0;                          \
 195                         w += (intpcm64_t)info->bass.i1[i] * bass->b1;           \
 196                         w += (intpcm64_t)info->bass.i2[i] * bass->b2;           \
 197                         w -= (intpcm64_t)info->bass.o1[i] * bass->a1;           \
 198                         w -= (intpcm64_t)info->bass.o2[i] * bass->a2;           \
 199                         info->bass.i2[i] = info->bass.i1[i];                    \
 200                         info->bass.i1[i] = v;                                   \
 201                         info->bass.o2[i] = info->bass.o1[i];                    \
 202                         w >>= FEEDEQ_COEFF_SHIFT;                               \
 203                         FEEDEQ_ERR_CLIP_CHECK(bass, w);                         \
 204                         v = FEEDEQ_CLAMP(w);                                    \
 205                         info->bass.o1[i] = v;                                   \
 206                                                                                 \
 207                         v >>= 32 - BIT;                                         \
 208                         _PCM_WRITE_##SIGN##BIT##_##ENDIAN(dst, v);              \
 209                         dst += PCM_##BIT##_BPS;                                 \
 210                         i++;                                                    \
 211                 } while (--j != 0);                                             \
 212         } while (--count != 0);                                                 \
 213 }
 214
 215 #if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 216 FEEDEQ_DECLARE(S, 16, LE)
 217 FEEDEQ_DECLARE(S, 32, LE)
 218 #endif
 219 #if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 220 FEEDEQ_DECLARE(S, 16, BE)
 221 FEEDEQ_DECLARE(S, 32, BE)
 222 #endif
 223 #ifdef SND_FEEDER_MULTIFORMAT
 224 FEEDEQ_DECLARE(S,  8, NE)
 225 FEEDEQ_DECLARE(S, 24, LE)
 226 FEEDEQ_DECLARE(S, 24, BE)
 227 FEEDEQ_DECLARE(U,  8, NE)
 228 FEEDEQ_DECLARE(U, 16, LE)
 229 FEEDEQ_DECLARE(U, 24, LE)
 230 FEEDEQ_DECLARE(U, 32, LE)
 231 FEEDEQ_DECLARE(U, 16, BE)
 232 FEEDEQ_DECLARE(U, 24, BE)
 233 FEEDEQ_DECLARE(U, 32, BE)
 234 #endif
 235
 236 #define FEEDEQ_ENTRY(SIGN, BIT, ENDIAN)                                 \
 237         {                                                               \
 238                 AFMT_##SIGN##BIT##_##ENDIAN,                            \
 239                 feed_eq_biquad_##SIGN##BIT##ENDIAN                      \
 240         }
 241
 242
 243 static const struct {
 244         uint32_t format;
 245         feed_eq_t biquad;
 246 } feed_eq_biquad_tab[] = {
 247 #if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 248         FEEDEQ_ENTRY(S, 16, LE),
 249         FEEDEQ_ENTRY(S, 32, LE),
 250 #endif
 251 #if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT)
 252         FEEDEQ_ENTRY(S, 16, BE),
 253         FEEDEQ_ENTRY(S, 32, BE),
 254 #endif
 255 #ifdef SND_FEEDER_MULTIFORMAT
 256         FEEDEQ_ENTRY(S,  8, NE),
 257         FEEDEQ_ENTRY(S, 24, LE),
 258         FEEDEQ_ENTRY(S, 24, BE),
 259         FEEDEQ_ENTRY(U,  8, NE),
 260         FEEDEQ_ENTRY(U, 16, LE),
 261         FEEDEQ_ENTRY(U, 24, LE),
 262         FEEDEQ_ENTRY(U, 32, LE),
 263         FEEDEQ_ENTRY(U, 16, BE),
 264         FEEDEQ_ENTRY(U, 24, BE),
 265         FEEDEQ_ENTRY(U, 32, BE)
 266 #endif
 267 };
 268
 269 #define FEEDEQ_BIQUAD_TAB_SIZE                                          \
 270         ((int32_t)(sizeof(feed_eq_biquad_tab) / sizeof(feed_eq_biquad_tab[0])))
 271
 272 static struct feed_eq_coeff *
 273 feed_eq_coeff_rate(uint32_t rate)
 274 {
 275         uint32_t spd, threshold;
 276         int i;
 277
 278         if (rate < FEEDEQ_RATE_MIN || rate > FEEDEQ_RATE_MAX)
 279                 return (NULL);
 280
 281         /*
 282          * Not all rates are supported. Choose the best rate that we can to
 283          * allow 'sloppy' conversion. Good enough for naive listeners.
 284          */
 285         for (i = 0; i < FEEDEQ_TAB_SIZE; i++) {
 286                 spd = feed_eq_tab[i].rate;
 287                 threshold = spd + ((i < (FEEDEQ_TAB_SIZE - 1) &&
 288                     feed_eq_tab[i + 1].rate > spd) ?
 289                     ((feed_eq_tab[i + 1].rate - spd) >> 1) : 0);
 290                 if (rate == spd ||
 291                     (feeder_eq_exact_rate == 0 && rate <= threshold))
 292                         return (feed_eq_tab[i].coeff);
 293         }
 294
 295         return (NULL);
 296 }
 297
 298 int
 299 feeder_eq_validrate(uint32_t rate)
 300 {
 301
 302         if (feed_eq_coeff_rate(rate) != NULL)
 303                 return (1);
 304
 305         return (0);
 306 }
 307
 308 static void
 309 feed_eq_reset(struct feed_eq_info *info)
 310 {
 311         uint32_t i;
 312
 313         for (i = 0; i < info->channels; i++) {
 314                 info->treble.i1[i] = 0;
 315                 info->treble.i2[i] = 0;
 316                 info->treble.o1[i] = 0;
 317                 info->treble.o2[i] = 0;
 318                 info->bass.i1[i] = 0;
 319                 info->bass.i2[i] = 0;
 320                 info->bass.o1[i] = 0;
 321                 info->bass.o2[i] = 0;
 322         }
 323 }
 324
 325 static int
 326 feed_eq_setup(struct feed_eq_info *info)
 327 {
 328
 329         info->coeff = feed_eq_coeff_rate(info->rate);
 330         if (info->coeff == NULL)
 331                 return (EINVAL);
 332
 333         feed_eq_reset(info);
 334
 335         return (0);
 336 }
 337
 338 static int
 339 feed_eq_init(struct pcm_feeder *f)
 340 {
 341         struct feed_eq_info *info;
 342         feed_eq_t biquad_op;
 343         int i;
 344
 345         if (f->desc->in != f->desc->out)
 346                 return (EINVAL);
 347
 348         biquad_op = NULL;
 349
 350         for (i = 0; i < FEEDEQ_BIQUAD_TAB_SIZE && biquad_op == NULL; i++) {
 351                 if (AFMT_ENCODING(f->desc->in) == feed_eq_biquad_tab[i].format)
 352                         biquad_op = feed_eq_biquad_tab[i].biquad;
 353         }
 354
 355         if (biquad_op == NULL)
 356                 return (EINVAL);
 357
 358         info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO);
 359         if (info == NULL)
 360                 return (ENOMEM);
 361
 362         info->channels = AFMT_CHANNEL(f->desc->in);
 363         info->align = info->channels * AFMT_BPS(f->desc->in);
 364
 365         info->rate = FEEDEQ_RATE_MIN;
 366         info->treble.gain = FEEDEQ_L2GAIN(50);
 367         info->bass.gain = FEEDEQ_L2GAIN(50);
 368         info->preamp = FEEDEQ_PREAMP2IDX(FEEDEQ_PREAMP_DEFAULT);
 369         info->state = FEEDEQ_UNKNOWN;
 370
 371         info->biquad = biquad_op;
 372
 373         f->data = info;
 374
 375         return (feed_eq_setup(info));
 376 }
 377
 378 static int
 379 feed_eq_set(struct pcm_feeder *f, int what, int value)
 380 {
 381         struct feed_eq_info *info;
 382
 383         info = f->data;
 384
 385         switch (what) {
 386         case FEEDEQ_CHANNELS:
 387                 if (value < SND_CHN_MIN || value > SND_CHN_MAX)
 388                         return (EINVAL);
 389                 info->channels = (uint32_t)value;
 390                 info->align = info->channels * AFMT_BPS(f->desc->in);
 391                 feed_eq_reset(info);
 392                 break;
 393         case FEEDEQ_RATE:
 394                 if (feeder_eq_validrate(value) == 0)
 395                         return (EINVAL);
 396                 info->rate = (uint32_t)value;
 397                 if (info->state == FEEDEQ_UNKNOWN)
 398                         info->state = FEEDEQ_ENABLE;
 399                 return (feed_eq_setup(info));
 400                 break;
 401         case FEEDEQ_TREBLE:
 402         case FEEDEQ_BASS:
 403                 if (value < 0 || value > 100)
 404                         return (EINVAL);
 405                 if (what == FEEDEQ_TREBLE)
 406                         info->treble.gain = FEEDEQ_L2GAIN(value);
 407                 else
 408                         info->bass.gain = FEEDEQ_L2GAIN(value);
 409                 break;
 410         case FEEDEQ_PREAMP:
 411                 if (value < FEEDEQ_PREAMP_MIN || value > FEEDEQ_PREAMP_MAX)
 412                         return (EINVAL);
 413                 info->preamp = FEEDEQ_PREAMP2IDX(value);
 414                 break;
 415         case FEEDEQ_STATE:
 416                 if (!(value == FEEDEQ_BYPASS || value == FEEDEQ_ENABLE ||
 417                     value == FEEDEQ_DISABLE))
 418                         return (EINVAL);
 419                 info->state = value;
 420                 feed_eq_reset(info);
 421                 break;
 422         default:
 423                 return (EINVAL);
 424                 break;
 425         }
 426
 427         return (0);
 428 }
 429
 430 static int
 431 feed_eq_free(struct pcm_feeder *f)
 432 {
 433         struct feed_eq_info *info;
 434
 435         info = f->data;
 436         if (info != NULL)
 437                 free(info, M_DEVBUF);
 438
 439         f->data = NULL;
 440
 441         return (0);
 442 }
 443
 444 static int
 445 feed_eq_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
 446     uint32_t count, void *source)
 447 {
 448         struct feed_eq_info *info;
 449         uint32_t j;
 450         uint8_t *dst;
 451
 452         info = f->data;
 453
 454         /*
 455          * 3 major states:
 456          *      FEEDEQ_BYPASS  - Bypass entirely, nothing happened.
 457          *      FEEDEQ_ENABLE  - Preamp+biquad filtering.
 458          *      FEEDEQ_DISABLE - Preamp only.
 459          */
 460         if (info->state == FEEDEQ_BYPASS)
 461                 return (FEEDER_FEED(f->source, c, b, count, source));
 462
 463         dst = b;
 464         count = SND_FXROUND(count, info->align);
 465
 466         do {
 467                 if (count < info->align)
 468                         break;
 469
 470                 j = SND_FXDIV(FEEDER_FEED(f->source, c, dst, count, source),
 471                     info->align);
 472                 if (j == 0)
 473                         break;
 474
 475                 info->biquad(info, dst, j);
 476
 477                 j *= info->align;
 478                 dst += j;
 479                 count -= j;
 480
 481         } while (count != 0);
 482
 483         return (dst - b);
 484 }
 485
 486 static struct pcm_feederdesc feeder_eq_desc[] = {
 487         { FEEDER_EQ, 0, 0, 0, 0 },
 488         { 0, 0, 0, 0, 0 }
 489 };
 490
 491 static kobj_method_t feeder_eq_methods[] = {
 492         KOBJMETHOD(feeder_init,         feed_eq_init),
 493         KOBJMETHOD(feeder_free,         feed_eq_free),
 494         KOBJMETHOD(feeder_set,          feed_eq_set),
 495         KOBJMETHOD(feeder_feed,         feed_eq_feed),
 496         KOBJMETHOD_END
 497 };
 498
 499 FEEDER_DECLARE(feeder_eq, NULL);
 500
 501 static int32_t
 502 feed_eq_scan_preamp_arg(const char *s)
 503 {
 504         int r, i, f;
 505         size_t len;
 506         char buf[32];
 507
 508         bzero(buf, sizeof(buf));
 509
 510         /* XXX kind of ugly, but works for now.. */
 511
 512         r = sscanf(s, "%d.%d", &i, &f);
 513
 514         if (r == 1 && !(i < FEEDEQ_PREAMP_IMIN || i > FEEDEQ_PREAMP_IMAX)) {
 515                 snprintf(buf, sizeof(buf), "%c%d",
 516                     FEEDEQ_PREAMP_SIGNMARK(i), abs(i));
 517                 f = 0;
 518         } else if (r == 2 &&
 519             !(i < FEEDEQ_PREAMP_IMIN || i > FEEDEQ_PREAMP_IMAX ||
 520             f < FEEDEQ_PREAMP_FMIN || f > FEEDEQ_PREAMP_FMAX))
 521                 snprintf(buf, sizeof(buf), "%c%d.%d",
 522                     FEEDEQ_PREAMP_SIGNMARK(i), abs(i), f);
 523         else
 524                 return (FEEDEQ_PREAMP_INVALID);
 525
 526         len = strlen(s);
 527         if (len > 2 && strcasecmp(s + len - 2, "dB") == 0)
 528                 strlcat(buf, "dB", sizeof(buf));
 529
 530         if (i == 0 && *s == '-')
 531                 *buf = '-';
 532
 533         if (strcasecmp(buf + ((*s >= '0' && *s <= '9') ? 1 : 0), s) != 0)
 534                 return (FEEDEQ_PREAMP_INVALID);
 535
 536         while ((f / FEEDEQ_GAIN_DIV) > 0)
 537                 f /= FEEDEQ_GAIN_DIV;
 538
 539         return (((i < 0 || *buf == '-') ? -1 : 1) * FEEDEQ_IF2PREAMP(i, f));
 540 }
 541
 542 #ifdef _KERNEL
 543 static int
 544 sysctl_dev_pcm_eq(SYSCTL_HANDLER_ARGS)
 545 {
 546         struct snddev_info *d;
 547         struct pcm_channel *c;
 548         struct pcm_feeder *f;
 549         int err, val, oval;
 550
 551         d = oidp->oid_arg1;
 552         if (!PCM_REGISTERED(d))
 553                 return (ENODEV);
 554
 555         PCM_LOCK(d);
 556         PCM_WAIT(d);
 557         if (d->flags & SD_F_EQ_BYPASSED)
 558                 val = 2;
 559         else if (d->flags & SD_F_EQ_ENABLED)
 560                 val = 1;
 561         else
 562                 val = 0;
 563         PCM_ACQUIRE(d);
 564         PCM_UNLOCK(d);
 565
 566         oval = val;
 567         err = sysctl_handle_int(oidp, &val, 0, req);
 568
 569         if (err == 0 && req->newptr != NULL && val != oval) {
 570                 if (!(val == 0 || val == 1 || val == 2)) {
 571                         PCM_RELEASE_QUICK(d);
 572                         return (EINVAL);
 573                 }
 574
 575                 PCM_LOCK(d);
 576
 577                 d->flags &= ~(SD_F_EQ_ENABLED | SD_F_EQ_BYPASSED);
 578                 if (val == 2) {
 579                         val = FEEDEQ_BYPASS;
 580                         d->flags |= SD_F_EQ_BYPASSED;
 581                 } else if (val == 1) {
 582                         val = FEEDEQ_ENABLE;
 583                         d->flags |= SD_F_EQ_ENABLED;
 584                 } else
 585                         val = FEEDEQ_DISABLE;
 586
 587                 CHN_FOREACH(c, d, channels.pcm.busy) {
 588                         CHN_LOCK(c);
 589                         f = chn_findfeeder(c, FEEDER_EQ);
 590                         if (f != NULL)
 591                                 (void)FEEDER_SET(f, FEEDEQ_STATE, val);
 592                         CHN_UNLOCK(c);
 593                 }
 594
 595                 PCM_RELEASE(d);
 596                 PCM_UNLOCK(d);
 597         } else
 598                 PCM_RELEASE_QUICK(d);
 599
 600         return (err);
 601 }
 602
 603 static int
 604 sysctl_dev_pcm_eq_preamp(SYSCTL_HANDLER_ARGS)
 605 {
 606         struct snddev_info *d;
 607         struct pcm_channel *c;
 608         struct pcm_feeder *f;
 609         int err, val, oval;
 610         char buf[32];
 611
 612         d = oidp->oid_arg1;
 613         if (!PCM_REGISTERED(d))
 614                 return (ENODEV);
 615
 616         PCM_LOCK(d);
 617         PCM_WAIT(d);
 618         val = d->eqpreamp;
 619         bzero(buf, sizeof(buf));
 620         (void)snprintf(buf, sizeof(buf), "%c%d.%ddB",
 621             FEEDEQ_PREAMP_SIGNMARK(val), FEEDEQ_PREAMP_IPART(val),
 622             FEEDEQ_PREAMP_FPART(val));
 623         PCM_ACQUIRE(d);
 624         PCM_UNLOCK(d);
 625
 626         oval = val;
 627         err = sysctl_handle_string(oidp, buf, sizeof(buf), req);
 628
 629         if (err == 0 && req->newptr != NULL) {
 630                 val = feed_eq_scan_preamp_arg(buf);
 631                 if (val == FEEDEQ_PREAMP_INVALID) {
 632                         PCM_RELEASE_QUICK(d);
 633                         return (EINVAL);
 634                 }
 635
 636                 PCM_LOCK(d);
 637
 638                 if (val != oval) {
 639                         if (val < FEEDEQ_PREAMP_MIN)
 640                                 val = FEEDEQ_PREAMP_MIN;
 641                         else if (val > FEEDEQ_PREAMP_MAX)
 642                                 val = FEEDEQ_PREAMP_MAX;
 643
 644                         d->eqpreamp = val;
 645
 646                         CHN_FOREACH(c, d, channels.pcm.busy) {
 647                                 CHN_LOCK(c);
 648                                 f = chn_findfeeder(c, FEEDER_EQ);
 649                                 if (f != NULL)
 650                                         (void)FEEDER_SET(f, FEEDEQ_PREAMP, val);
 651                                 CHN_UNLOCK(c);
 652                         }
 653
 654                 }
 655
 656                 PCM_RELEASE(d);
 657                 PCM_UNLOCK(d);
 658         } else
 659                 PCM_RELEASE_QUICK(d);
 660
 661         return (err);
 662 }
 663
 664 void
 665 feeder_eq_initsys(device_t dev)
 666 {
 667         struct snddev_info *d;
 668         const char *preamp;
 669         char buf[64];
 670
 671         d = device_get_softc(dev);
 672
 673         if (!(resource_string_value(device_get_name(dev), device_get_unit(dev),
 674             "eq_preamp", &preamp) == 0 &&
 675             (d->eqpreamp = feed_eq_scan_preamp_arg(preamp)) !=
 676             FEEDEQ_PREAMP_INVALID))
 677                 d->eqpreamp = FEEDEQ_PREAMP_DEFAULT;
 678
 679         if (d->eqpreamp < FEEDEQ_PREAMP_MIN)
 680                 d->eqpreamp = FEEDEQ_PREAMP_MIN;
 681         else if (d->eqpreamp > FEEDEQ_PREAMP_MAX)
 682                 d->eqpreamp = FEEDEQ_PREAMP_MAX;
 683
 684         SYSCTL_ADD_PROC(device_get_sysctl_ctx(dev),
 685             SYSCTL_CHILDREN(device_get_sysctl_tree(dev)), OID_AUTO,
 686             "eq", CTLTYPE_INT | CTLFLAG_RW, d, sizeof(d),
 687             sysctl_dev_pcm_eq, "I",
 688             "Bass/Treble Equalizer (0=disable, 1=enable, 2=bypass)");
 689
 690         bzero(buf, sizeof(buf));
 691
 692         (void)snprintf(buf, sizeof(buf), "Bass/Treble Equalizer Preamp "
 693             "(-/+ %d.0dB , %d.%ddB step)",
 694             FEEDEQ_GAIN_MAX, FEEDEQ_GAIN_STEP / FEEDEQ_GAIN_DIV,
 695             FEEDEQ_GAIN_STEP - ((FEEDEQ_GAIN_STEP / FEEDEQ_GAIN_DIV) *
 696             FEEDEQ_GAIN_DIV));
 697
 698         SYSCTL_ADD_PROC(device_get_sysctl_ctx(dev),
 699             SYSCTL_CHILDREN(device_get_sysctl_tree(dev)), OID_AUTO,
 700             "eq_preamp", CTLTYPE_STRING | CTLFLAG_RW, d, sizeof(d),
 701             sysctl_dev_pcm_eq_preamp, "A", buf);
 702 }
 703 #endif