1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2008-2009 Ariff Abdullah <ariff@FreeBSD.org>
5 * All rights reserved.
6 * Copyright (c) 2024-2026 The FreeBSD Foundation
7 *
8 * Portions of this software were developed by Christos Margiolis
9 * <christos@FreeBSD.org> under sponsorship from the FreeBSD Foundation.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
14 * 1. Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * 2. Redistributions in binary form must reproduce the above copyright
17 * notice, this list of conditions and the following disclaimer in the
18 * documentation and/or other materials provided with the distribution.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
30 * SUCH DAMAGE.
31 */
32
33 /*
34 * feeder_eq: Parametric (compile time) Software Equalizer. Though accidental,
35 * it proves good enough for educational and general consumption.
36 *
37 * "Cookbook formulae for audio EQ biquad filter coefficients"
38 * by Robert Bristow-Johnson <rbj@audioimagination.com>
39 * - http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
40 */
41
42 #ifdef _KERNEL
43 #ifdef HAVE_KERNEL_OPTION_HEADERS
44 #include "opt_snd.h"
45 #endif
46 #include <dev/sound/pcm/sound.h>
47 #include <dev/sound/pcm/pcm.h>
48 #include "feeder_if.h"
49
50 #define SND_USE_FXDIV
51 #include "snd_fxdiv_gen.h"
52 #endif
53
54 #include "feeder_eq_gen.h"
55
56 #define FEEDEQ_LEVELS \
57 (((FEEDEQ_GAIN_MAX - FEEDEQ_GAIN_MIN) * \
58 (FEEDEQ_GAIN_DIV / FEEDEQ_GAIN_STEP)) + 1)
59
60 #define FEEDEQ_L2GAIN(v) \
61 ((int)min(((v) * FEEDEQ_LEVELS) / 100, FEEDEQ_LEVELS - 1))
62
63 #define FEEDEQ_PREAMP_IPART(x) (abs(x) >> FEEDEQ_GAIN_SHIFT)
64 #define FEEDEQ_PREAMP_FPART(x) (abs(x) & FEEDEQ_GAIN_FMASK)
65 #define FEEDEQ_PREAMP_SIGNVAL(x) ((x) < 0 ? -1 : 1)
66 #define FEEDEQ_PREAMP_SIGNMARK(x) (((x) < 0) ? '-' : '+')
67
68 #define FEEDEQ_PREAMP_IMIN -192
69 #define FEEDEQ_PREAMP_IMAX 192
70 #define FEEDEQ_PREAMP_FMIN 0
71 #define FEEDEQ_PREAMP_FMAX 9
72
73 #define FEEDEQ_PREAMP_INVALID INT_MAX
74
75 #define FEEDEQ_IF2PREAMP(i, f) \
76 ((abs(i) << FEEDEQ_GAIN_SHIFT) | \
77 (((abs(f) / FEEDEQ_GAIN_STEP) * FEEDEQ_GAIN_STEP) & \
78 FEEDEQ_GAIN_FMASK))
79
80 #define FEEDEQ_PREAMP_MIN \
81 (FEEDEQ_PREAMP_SIGNVAL(FEEDEQ_GAIN_MIN) * \
82 FEEDEQ_IF2PREAMP(FEEDEQ_GAIN_MIN, 0))
83
84 #define FEEDEQ_PREAMP_MAX \
85 (FEEDEQ_PREAMP_SIGNVAL(FEEDEQ_GAIN_MAX) * \
86 FEEDEQ_IF2PREAMP(FEEDEQ_GAIN_MAX, 0))
87
88 #define FEEDEQ_PREAMP_DEFAULT FEEDEQ_IF2PREAMP(0, 0)
89
90 #define FEEDEQ_PREAMP2IDX(v) \
91 ((int32_t)((FEEDEQ_GAIN_MAX * (FEEDEQ_GAIN_DIV / \
92 FEEDEQ_GAIN_STEP)) + (FEEDEQ_PREAMP_SIGNVAL(v) * \
93 FEEDEQ_PREAMP_IPART(v) * (FEEDEQ_GAIN_DIV / \
94 FEEDEQ_GAIN_STEP)) + (FEEDEQ_PREAMP_SIGNVAL(v) * \
95 (FEEDEQ_PREAMP_FPART(v) / FEEDEQ_GAIN_STEP))))
96
97 static int feeder_eq_exact_rate = 0;
98
99 #ifdef _KERNEL
100 static char feeder_eq_presets[] = FEEDER_EQ_PRESETS;
101 SYSCTL_STRING(_hw_snd, OID_AUTO, feeder_eq_presets, CTLFLAG_RD,
102 &feeder_eq_presets, 0, "compile-time eq presets");
103
104 SYSCTL_INT(_hw_snd, OID_AUTO, feeder_eq_exact_rate, CTLFLAG_RWTUN,
105 &feeder_eq_exact_rate, 0, "force exact rate validation");
106 #endif
107
108 struct feed_eq_tone {
109 intpcm_t o1[SND_CHN_MAX];
110 intpcm_t o2[SND_CHN_MAX];
111 intpcm_t i1[SND_CHN_MAX];
112 intpcm_t i2[SND_CHN_MAX];
113 int gain;
114 };
115
116 struct feed_eq_info {
117 struct feed_eq_tone treble;
118 struct feed_eq_tone bass;
119 struct feed_eq_coeff *coeff;
120 uint32_t fmt;
121 uint32_t channels;
122 uint32_t rate;
123 uint32_t align;
124 int32_t preamp;
125 };
126
127 #if !defined(_KERNEL) && defined(FEEDEQ_ERR_CLIP)
128 #define FEEDEQ_ERR_CLIP_CHECK(t, v) do { \
129 if ((v) < PCM_S32_MIN || (v) > PCM_S32_MAX) \
130 errx(1, "\n\n%s(): ["#t"] Sample clipping: %jd\n", \
131 __func__, (intmax_t)(v)); \
132 } while (0)
133 #else
134 #define FEEDEQ_ERR_CLIP_CHECK(...)
135 #endif
136
137 __always_inline static void
feed_eq_biquad(struct feed_eq_info * info,uint8_t * dst,uint32_t count,const uint32_t fmt)138 feed_eq_biquad(struct feed_eq_info *info, uint8_t *dst, uint32_t count,
139 const uint32_t fmt)
140 {
141 struct feed_eq_coeff_tone *treble, *bass;
142 intpcm64_t w;
143 intpcm_t v;
144 uint32_t i, j;
145 int32_t pmul, pshift;
146
147 pmul = feed_eq_preamp[info->preamp].mul;
148 pshift = feed_eq_preamp[info->preamp].shift;
149
150 treble = &(info->coeff[info->treble.gain].treble);
151 bass = &(info->coeff[info->bass.gain].bass);
152
153 do {
154 i = 0;
155 j = info->channels;
156 do {
157 v = pcm_sample_read_norm(dst, fmt);
158 v = ((intpcm64_t)pmul * v) >> pshift;
159
160 w = (intpcm64_t)v * treble->b0;
161 w += (intpcm64_t)info->treble.i1[i] * treble->b1;
162 w += (intpcm64_t)info->treble.i2[i] * treble->b2;
163 w -= (intpcm64_t)info->treble.o1[i] * treble->a1;
164 w -= (intpcm64_t)info->treble.o2[i] * treble->a2;
165 info->treble.i2[i] = info->treble.i1[i];
166 info->treble.i1[i] = v;
167 info->treble.o2[i] = info->treble.o1[i];
168 w >>= FEEDEQ_COEFF_SHIFT;
169 FEEDEQ_ERR_CLIP_CHECK(treble, w);
170 v = pcm_clamp(w, AFMT_S32_NE);
171 info->treble.o1[i] = v;
172
173 w = (intpcm64_t)v * bass->b0;
174 w += (intpcm64_t)info->bass.i1[i] * bass->b1;
175 w += (intpcm64_t)info->bass.i2[i] * bass->b2;
176 w -= (intpcm64_t)info->bass.o1[i] * bass->a1;
177 w -= (intpcm64_t)info->bass.o2[i] * bass->a2;
178 info->bass.i2[i] = info->bass.i1[i];
179 info->bass.i1[i] = v;
180 info->bass.o2[i] = info->bass.o1[i];
181 w >>= FEEDEQ_COEFF_SHIFT;
182 FEEDEQ_ERR_CLIP_CHECK(bass, w);
183 v = pcm_clamp(w, AFMT_S32_NE);
184 info->bass.o1[i] = v;
185
186 pcm_sample_write_norm(dst, v, fmt);
187 dst += AFMT_BPS(fmt);
188 i++;
189 } while (--j != 0);
190 } while (--count != 0);
191 }
192
193 static struct feed_eq_coeff *
feed_eq_coeff_rate(uint32_t rate)194 feed_eq_coeff_rate(uint32_t rate)
195 {
196 uint32_t spd, threshold;
197 int i;
198
199 if (rate < FEEDEQ_RATE_MIN || rate > FEEDEQ_RATE_MAX)
200 return (NULL);
201
202 /*
203 * Not all rates are supported. Choose the best rate that we can to
204 * allow 'sloppy' conversion. Good enough for naive listeners.
205 */
206 for (i = 0; i < FEEDEQ_TAB_SIZE; i++) {
207 spd = feed_eq_tab[i].rate;
208 threshold = spd + ((i < (FEEDEQ_TAB_SIZE - 1) &&
209 feed_eq_tab[i + 1].rate > spd) ?
210 ((feed_eq_tab[i + 1].rate - spd) >> 1) : 0);
211 if (rate == spd ||
212 (feeder_eq_exact_rate == 0 && rate <= threshold))
213 return (feed_eq_tab[i].coeff);
214 }
215
216 return (NULL);
217 }
218
219 int
feeder_eq_validrate(uint32_t rate)220 feeder_eq_validrate(uint32_t rate)
221 {
222
223 if (feed_eq_coeff_rate(rate) != NULL)
224 return (1);
225
226 return (0);
227 }
228
229 static void
feed_eq_reset(struct feed_eq_info * info)230 feed_eq_reset(struct feed_eq_info *info)
231 {
232 uint32_t i;
233
234 for (i = 0; i < info->channels; i++) {
235 info->treble.i1[i] = 0;
236 info->treble.i2[i] = 0;
237 info->treble.o1[i] = 0;
238 info->treble.o2[i] = 0;
239 info->bass.i1[i] = 0;
240 info->bass.i2[i] = 0;
241 info->bass.o1[i] = 0;
242 info->bass.o2[i] = 0;
243 }
244 }
245
246 static int
feed_eq_setup(struct feed_eq_info * info)247 feed_eq_setup(struct feed_eq_info *info)
248 {
249
250 info->coeff = feed_eq_coeff_rate(info->rate);
251 if (info->coeff == NULL)
252 return (EINVAL);
253
254 feed_eq_reset(info);
255
256 return (0);
257 }
258
259 static int
feed_eq_init(struct pcm_feeder * f)260 feed_eq_init(struct pcm_feeder *f)
261 {
262 struct feed_eq_info *info;
263
264 if (f->desc.in != f->desc.out)
265 return (EINVAL);
266
267 info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO);
268 if (info == NULL)
269 return (ENOMEM);
270
271 info->fmt = AFMT_ENCODING(f->desc.in);
272 info->channels = AFMT_CHANNEL(f->desc.in);
273 info->align = info->channels * AFMT_BPS(f->desc.in);
274
275 info->rate = FEEDEQ_RATE_MIN;
276 info->treble.gain = FEEDEQ_L2GAIN(50);
277 info->bass.gain = FEEDEQ_L2GAIN(50);
278 info->preamp = FEEDEQ_PREAMP2IDX(FEEDEQ_PREAMP_DEFAULT);
279
280 f->data = info;
281
282 return (feed_eq_setup(info));
283 }
284
285 static int
feed_eq_set(struct pcm_feeder * f,int what,int value)286 feed_eq_set(struct pcm_feeder *f, int what, int value)
287 {
288 struct feed_eq_info *info;
289
290 info = f->data;
291
292 switch (what) {
293 case FEEDEQ_CHANNELS:
294 if (value < SND_CHN_MIN || value > SND_CHN_MAX)
295 return (EINVAL);
296 info->channels = (uint32_t)value;
297 info->align = info->channels * AFMT_BPS(f->desc.in);
298 feed_eq_reset(info);
299 break;
300 case FEEDEQ_RATE:
301 if (feeder_eq_validrate(value) == 0)
302 return (EINVAL);
303 info->rate = (uint32_t)value;
304 return (feed_eq_setup(info));
305 case FEEDEQ_TREBLE:
306 case FEEDEQ_BASS:
307 if (value < 0 || value > 100)
308 return (EINVAL);
309 if (what == FEEDEQ_TREBLE)
310 info->treble.gain = FEEDEQ_L2GAIN(value);
311 else
312 info->bass.gain = FEEDEQ_L2GAIN(value);
313 break;
314 case FEEDEQ_PREAMP:
315 if (value < FEEDEQ_PREAMP_MIN || value > FEEDEQ_PREAMP_MAX)
316 return (EINVAL);
317 info->preamp = FEEDEQ_PREAMP2IDX(value);
318 break;
319 default:
320 return (EINVAL);
321 }
322
323 return (0);
324 }
325
326 static int
feed_eq_free(struct pcm_feeder * f)327 feed_eq_free(struct pcm_feeder *f)
328 {
329 struct feed_eq_info *info;
330
331 info = f->data;
332 free(info, M_DEVBUF);
333
334 f->data = NULL;
335
336 return (0);
337 }
338
339 static int
feed_eq_feed(struct pcm_feeder * f,struct pcm_channel * c,uint8_t * b,uint32_t count,void * source)340 feed_eq_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
341 uint32_t count, void *source)
342 {
343 struct feed_eq_info *info;
344 uint32_t j;
345 uint8_t *dst;
346
347 info = f->data;
348
349 dst = b;
350 count = SND_FXROUND(count, info->align);
351
352 do {
353 if (count < info->align)
354 break;
355
356 j = SND_FXDIV(FEEDER_FEED(f->source, c, dst, count, source),
357 info->align);
358 if (j == 0)
359 break;
360
361 /* Optimize some common formats. */
362 switch (info->fmt) {
363 case AFMT_S16_NE:
364 feed_eq_biquad(info, dst, j, AFMT_S16_NE);
365 break;
366 case AFMT_S24_NE:
367 feed_eq_biquad(info, dst, j, AFMT_S24_NE);
368 break;
369 case AFMT_S32_NE:
370 feed_eq_biquad(info, dst, j, AFMT_S32_NE);
371 break;
372 default:
373 feed_eq_biquad(info, dst, j, info->fmt);
374 break;
375 }
376
377 j *= info->align;
378 dst += j;
379 count -= j;
380
381 } while (count != 0);
382
383 return (dst - b);
384 }
385
386 static kobj_method_t feeder_eq_methods[] = {
387 KOBJMETHOD(feeder_init, feed_eq_init),
388 KOBJMETHOD(feeder_free, feed_eq_free),
389 KOBJMETHOD(feeder_set, feed_eq_set),
390 KOBJMETHOD(feeder_feed, feed_eq_feed),
391 KOBJMETHOD_END
392 };
393
394 FEEDER_DECLARE(feeder_eq, FEEDER_EQ);
395
396 static int32_t
feed_eq_scan_preamp_arg(const char * s)397 feed_eq_scan_preamp_arg(const char *s)
398 {
399 int r, i, f;
400 size_t len;
401 char buf[32];
402
403 bzero(buf, sizeof(buf));
404
405 /* XXX kind of ugly, but works for now.. */
406
407 r = sscanf(s, "%d.%d", &i, &f);
408
409 if (r == 1 && !(i < FEEDEQ_PREAMP_IMIN || i > FEEDEQ_PREAMP_IMAX)) {
410 snprintf(buf, sizeof(buf), "%c%d",
411 FEEDEQ_PREAMP_SIGNMARK(i), abs(i));
412 f = 0;
413 } else if (r == 2 &&
414 !(i < FEEDEQ_PREAMP_IMIN || i > FEEDEQ_PREAMP_IMAX ||
415 f < FEEDEQ_PREAMP_FMIN || f > FEEDEQ_PREAMP_FMAX))
416 snprintf(buf, sizeof(buf), "%c%d.%d",
417 FEEDEQ_PREAMP_SIGNMARK(i), abs(i), f);
418 else
419 return (FEEDEQ_PREAMP_INVALID);
420
421 len = strlen(s);
422 if (len > 2 && strcasecmp(s + len - 2, "dB") == 0)
423 strlcat(buf, "dB", sizeof(buf));
424
425 if (i == 0 && *s == '-')
426 *buf = '-';
427
428 if (strcasecmp(buf + ((*s >= '0' && *s <= '9') ? 1 : 0), s) != 0)
429 return (FEEDEQ_PREAMP_INVALID);
430
431 while ((f / FEEDEQ_GAIN_DIV) > 0)
432 f /= FEEDEQ_GAIN_DIV;
433
434 return (((i < 0 || *buf == '-') ? -1 : 1) * FEEDEQ_IF2PREAMP(i, f));
435 }
436
437 #ifdef _KERNEL
438 static int
sysctl_dev_pcm_eq(SYSCTL_HANDLER_ARGS)439 sysctl_dev_pcm_eq(SYSCTL_HANDLER_ARGS)
440 {
441 struct snddev_info *d;
442 int err, val, oval;
443
444 d = oidp->oid_arg1;
445 if (!PCM_REGISTERED(d))
446 return (ENODEV);
447
448 PCM_LOCK(d);
449 PCM_WAIT(d);
450 if (d->flags & SD_F_EQ_ENABLED)
451 val = 1;
452 else
453 val = 0;
454 PCM_ACQUIRE(d);
455 PCM_UNLOCK(d);
456
457 oval = val;
458 err = sysctl_handle_int(oidp, &val, 0, req);
459
460 if (err == 0 && req->newptr != NULL && val != oval) {
461 if (!(val == 0 || val == 1)) {
462 PCM_RELEASE_QUICK(d);
463 return (EINVAL);
464 }
465
466 PCM_LOCK(d);
467
468 if (val == 1)
469 d->flags |= SD_F_EQ_ENABLED;
470 else
471 d->flags &= ~SD_F_EQ_ENABLED;
472
473 PCM_RELEASE(d);
474 PCM_UNLOCK(d);
475 } else
476 PCM_RELEASE_QUICK(d);
477
478 return (err);
479 }
480
481 static int
sysctl_dev_pcm_eq_preamp(SYSCTL_HANDLER_ARGS)482 sysctl_dev_pcm_eq_preamp(SYSCTL_HANDLER_ARGS)
483 {
484 struct snddev_info *d;
485 struct pcm_channel *c;
486 struct pcm_feeder *f;
487 int err, val, oval;
488 char buf[32];
489
490 d = oidp->oid_arg1;
491 if (!PCM_REGISTERED(d))
492 return (ENODEV);
493
494 PCM_LOCK(d);
495 PCM_WAIT(d);
496 val = d->eqpreamp;
497 bzero(buf, sizeof(buf));
498 (void)snprintf(buf, sizeof(buf), "%c%d.%ddB",
499 FEEDEQ_PREAMP_SIGNMARK(val), FEEDEQ_PREAMP_IPART(val),
500 FEEDEQ_PREAMP_FPART(val));
501 PCM_ACQUIRE(d);
502 PCM_UNLOCK(d);
503
504 oval = val;
505 err = sysctl_handle_string(oidp, buf, sizeof(buf), req);
506
507 if (err == 0 && req->newptr != NULL) {
508 val = feed_eq_scan_preamp_arg(buf);
509 if (val == FEEDEQ_PREAMP_INVALID) {
510 PCM_RELEASE_QUICK(d);
511 return (EINVAL);
512 }
513
514 PCM_LOCK(d);
515
516 if (val != oval) {
517 if (val < FEEDEQ_PREAMP_MIN)
518 val = FEEDEQ_PREAMP_MIN;
519 else if (val > FEEDEQ_PREAMP_MAX)
520 val = FEEDEQ_PREAMP_MAX;
521
522 d->eqpreamp = val;
523
524 CHN_FOREACH(c, d, channels.pcm.busy) {
525 CHN_LOCK(c);
526 f = feeder_find(c, FEEDER_EQ);
527 if (f != NULL)
528 (void)FEEDER_SET(f, FEEDEQ_PREAMP, val);
529 CHN_UNLOCK(c);
530 }
531 }
532
533 PCM_RELEASE(d);
534 PCM_UNLOCK(d);
535 } else
536 PCM_RELEASE_QUICK(d);
537
538 return (err);
539 }
540
541 void
feeder_eq_initsys(device_t dev)542 feeder_eq_initsys(device_t dev)
543 {
544 struct snddev_info *d;
545 char buf[64];
546
547 d = device_get_softc(dev);
548
549 d->eqpreamp = FEEDEQ_PREAMP_DEFAULT;
550 if (d->eqpreamp < FEEDEQ_PREAMP_MIN)
551 d->eqpreamp = FEEDEQ_PREAMP_MIN;
552 else if (d->eqpreamp > FEEDEQ_PREAMP_MAX)
553 d->eqpreamp = FEEDEQ_PREAMP_MAX;
554
555 SYSCTL_ADD_PROC(device_get_sysctl_ctx(dev),
556 SYSCTL_CHILDREN(device_get_sysctl_tree(dev)), OID_AUTO,
557 "eq", CTLTYPE_INT | CTLFLAG_RWTUN | CTLFLAG_MPSAFE, d,
558 sizeof(d), sysctl_dev_pcm_eq, "I",
559 "Bass/Treble Equalizer (0=disable, 1=enable)");
560
561 (void)snprintf(buf, sizeof(buf), "Bass/Treble Equalizer Preamp "
562 "(-/+ %d.0dB , %d.%ddB step)",
563 FEEDEQ_GAIN_MAX, FEEDEQ_GAIN_STEP / FEEDEQ_GAIN_DIV,
564 FEEDEQ_GAIN_STEP - ((FEEDEQ_GAIN_STEP / FEEDEQ_GAIN_DIV) *
565 FEEDEQ_GAIN_DIV));
566
567 SYSCTL_ADD_PROC(device_get_sysctl_ctx(dev),
568 SYSCTL_CHILDREN(device_get_sysctl_tree(dev)), OID_AUTO,
569 "eq_preamp", CTLTYPE_STRING | CTLFLAG_RWTUN | CTLFLAG_MPSAFE,
570 d, sizeof(d), sysctl_dev_pcm_eq_preamp, "A", buf);
571 }
572 #endif
573