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: head/sys/dev/sound/pcm/feeder_eq.c 267992 2014-06-28 03:56:17Z hselasky $");
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 (rounddown(abs(f), 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 *
feed_eq_coeff_rate(uint32_t rate)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
feeder_eq_validrate(uint32_t rate)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
feed_eq_reset(struct feed_eq_info * info)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
feed_eq_setup(struct feed_eq_info * info)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
feed_eq_init(struct pcm_feeder * f)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 = kmalloc(sizeof(*info), M_DEVBUF, M_WAITOK | 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
feed_eq_set(struct pcm_feeder * f,int what,int value)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
feed_eq_free(struct pcm_feeder * f)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 kfree(info, M_DEVBUF);
438
439 f->data = NULL;
440
441 return (0);
442 }
443
444 static int
feed_eq_feed(struct pcm_feeder * f,struct pcm_channel * c,uint8_t * b,uint32_t count,void * source)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
feed_eq_scan_preamp_arg(const char * s)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 = ksscanf(s, "%d.%d", &i, &f);
513
514 if (r == 1 && !(i < FEEDEQ_PREAMP_IMIN || i > FEEDEQ_PREAMP_IMAX)) {
515 ksnprintf(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 ksnprintf(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
sysctl_dev_pcm_eq(SYSCTL_HANDLER_ARGS)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
sysctl_dev_pcm_eq_preamp(SYSCTL_HANDLER_ARGS)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)ksnprintf(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
feeder_eq_initsys(device_t dev)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)ksnprintf(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 - rounddown(FEEDEQ_GAIN_STEP, FEEDEQ_GAIN_DIV));
696
697 SYSCTL_ADD_PROC(device_get_sysctl_ctx(dev),
698 SYSCTL_CHILDREN(device_get_sysctl_tree(dev)), OID_AUTO,
699 "eq_preamp", CTLTYPE_STRING | CTLFLAG_RW, d, sizeof(d),
700 sysctl_dev_pcm_eq_preamp, "A", buf);
701 }
702 #endif
703