xref: /dragonfly/sys/dev/sound/pcm/feeder_eq.c (revision ed183f8c)
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