1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2019 Google LLC, written by Richard Kralovic <riso@google.com>
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28 #include <sys/ioctl.h>
29 #include <sys/socket.h>
30 #include <sys/soundcard.h>
31 #include <sys/types.h>
32 #include <sys/un.h>
33
34 #include <err.h>
35 #include <errno.h>
36 #include <fcntl.h>
37 #include <fftw3.h>
38 #include <getopt.h>
39 #include <math.h>
40 #include <stdarg.h>
41 #include <stdint.h>
42 #include <stdlib.h>
43 #include <string.h>
44 #include <sysexits.h>
45 #include <unistd.h>
46
47 #include "virtual_oss.h"
48
49 struct Equalizer {
50 double rate;
51 int block_size;
52 int do_normalize;
53
54 /* (block_size * 2) elements, time domain */
55 double *fftw_time;
56
57 /* (block_size * 2) elements, half-complex, freq domain */
58 double *fftw_freq;
59
60 fftw_plan forward;
61 fftw_plan inverse;
62 };
63
64 static int be_silent = 0;
65
66 static void
message(const char * fmt,...)67 message(const char *fmt,...)
68 {
69 va_list list;
70
71 if (be_silent)
72 return;
73 va_start(list, fmt);
74 vfprintf(stderr, fmt, list);
75 va_end(list);
76 }
77
78 /*
79 * Masking window value for -1 < x < 1.
80 *
81 * Window must be symmetric, thus, this function is queried for x >= 0
82 * only. Currently a Hann window.
83 */
84 static double
equalizer_get_window(double x)85 equalizer_get_window(double x)
86 {
87 return (0.5 + 0.5 * cos(M_PI * x));
88 }
89
90 static int
equalizer_load_freq_amps(struct Equalizer * e,const char * config)91 equalizer_load_freq_amps(struct Equalizer *e, const char *config)
92 {
93 double prev_f = 0.0;
94 double prev_amp = 1.0;
95 double next_f = 0.0;
96 double next_amp = 1.0;
97 int i;
98
99 if (strncasecmp(config, "normalize", 4) == 0) {
100 while (*config != 0) {
101 if (*config == '\n') {
102 config++;
103 break;
104 }
105 config++;
106 }
107 e->do_normalize = 1;
108 } else {
109 e->do_normalize = 0;
110 }
111
112 for (i = 0; i <= (e->block_size / 2); ++i) {
113 const double f = (i * e->rate) / e->block_size;
114
115 while (f >= next_f) {
116 prev_f = next_f;
117 prev_amp = next_amp;
118
119 if (*config == 0) {
120 next_f = e->rate;
121 next_amp = prev_amp;
122 } else {
123 int len;
124
125 if (sscanf(config, "%lf %lf %n", &next_f, &next_amp, &len) == 2) {
126 config += len;
127 if (next_f < prev_f) {
128 message("Parse error: Nonincreasing sequence of frequencies.\n");
129 return (0);
130 }
131 } else {
132 message("Parse error.\n");
133 return (0);
134 }
135 }
136 if (prev_f == 0.0)
137 prev_amp = next_amp;
138 }
139 e->fftw_freq[i] = ((f - prev_f) / (next_f - prev_f)) * (next_amp - prev_amp) + prev_amp;
140 }
141 return (1);
142 }
143
144 static void
equalizer_init(struct Equalizer * e,int rate,int block_size)145 equalizer_init(struct Equalizer *e, int rate, int block_size)
146 {
147 size_t buffer_size;
148
149 e->rate = rate;
150 e->block_size = block_size;
151
152 buffer_size = sizeof(double) * e->block_size;
153
154 e->fftw_time = (double *)malloc(buffer_size);
155 e->fftw_freq = (double *)malloc(buffer_size);
156
157 e->forward = fftw_plan_r2r_1d(block_size, e->fftw_time, e->fftw_freq,
158 FFTW_R2HC, FFTW_MEASURE);
159 e->inverse = fftw_plan_r2r_1d(block_size, e->fftw_freq, e->fftw_time,
160 FFTW_HC2R, FFTW_MEASURE);
161 }
162
163 static int
equalizer_load(struct Equalizer * eq,const char * config)164 equalizer_load(struct Equalizer *eq, const char *config)
165 {
166 int retval = 0;
167 int N = eq->block_size;
168 int buffer_size = sizeof(double) * N;
169 int i;
170
171 memset(eq->fftw_freq, 0, buffer_size);
172
173 message("\n\nReloading amplification specifications:\n%s\n", config);
174
175 if (!equalizer_load_freq_amps(eq, config))
176 goto end;
177
178 double *requested_freq = (double *)malloc(buffer_size);
179
180 memcpy(requested_freq, eq->fftw_freq, buffer_size);
181
182 fftw_execute(eq->inverse);
183
184 /* Multiply by symmetric window and shift */
185 for (i = 0; i < (N / 2); ++i) {
186 double weight = equalizer_get_window(i / (double)(N / 2)) / N;
187
188 eq->fftw_time[N / 2 + i] = eq->fftw_time[i] * weight;
189 }
190 for (i = (N / 2 - 1); i > 0; --i) {
191 eq->fftw_time[i] = eq->fftw_time[N - i];
192 }
193 eq->fftw_time[0] = 0;
194
195 fftw_execute(eq->forward);
196 for (i = 0; i < N; ++i) {
197 eq->fftw_freq[i] /= (double)N;
198 }
199
200 /* Debug output */
201 for (i = 0; i <= (N / 2); ++i) {
202 double f = (eq->rate / N) * i;
203 double a = sqrt(pow(eq->fftw_freq[i], 2.0) +
204 ((i > 0 && i < N / 2) ? pow(eq->fftw_freq[N - i], 2.0) : 0));
205
206 a *= N;
207 double r = requested_freq[i];
208
209 message("%3.1lf Hz: requested %2.2lf, got %2.7lf (log10 = %.2lf), %3.7lfdb\n",
210 f, r, a, log(a) / log(10), (log(a / r) / log(10.0)) * 10.0);
211 }
212
213 /* Normalize FIR filter, if any */
214 if (eq->do_normalize) {
215 double sum = 0;
216
217 for (i = 0; i < N; ++i)
218 sum += fabs(eq->fftw_time[i]);
219 if (sum != 0.0) {
220 for (i = 0; i < N; ++i)
221 eq->fftw_time[i] /= sum;
222 }
223 }
224 for (i = 0; i < N; ++i) {
225 message("%.3lf ms: %.10lf\n", 1000.0 * i / eq->rate, eq->fftw_time[i]);
226 }
227
228 /* End of debug */
229
230 retval = 1;
231
232 free(requested_freq);
233 end:
234 return (retval);
235 }
236
237 static void
equalizer_done(struct Equalizer * eq)238 equalizer_done(struct Equalizer *eq)
239 {
240
241 fftw_destroy_plan(eq->forward);
242 fftw_destroy_plan(eq->inverse);
243 free(eq->fftw_time);
244 free(eq->fftw_freq);
245 }
246
247 static struct option equalizer_opts[] = {
248 {"device", required_argument, NULL, 'd'},
249 {"part", required_argument, NULL, 'p'},
250 {"channels", required_argument, NULL, 'c'},
251 {"what", required_argument, NULL, 'w'},
252 {"off", no_argument, NULL, 'o'},
253 {"quiet", no_argument, NULL, 'q'},
254 {"file", no_argument, NULL, 'f'},
255 {"help", no_argument, NULL, 'h'},
256 };
257
258 static void
usage(void)259 usage(void)
260 {
261 message("Usage: virtual_equalizer -d /dev/vdsp.ctl \n"
262 "\t -d, --device [control device]\n"
263 "\t -w, --what [rx_dev,tx_dev,rx_loop,tx_loop, default tx_dev]\n"
264 "\t -p, --part [part number, default 0]\n"
265 "\t -c, --channels [channels, default -1]\n"
266 "\t -f, --file [read input from file, default standard input]\n"
267 "\t -o, --off [disable equalizer]\n"
268 "\t -q, --quiet\n"
269 "\t -h, --help\n");
270 exit(EX_USAGE);
271 }
272
273 int
main(int argc,char ** argv)274 main(int argc, char **argv)
275 {
276 struct virtual_oss_fir_filter fir = {};
277 struct virtual_oss_io_info info = {};
278
279 struct Equalizer e;
280
281 char buffer[65536];
282 unsigned cmd_fir_set = VIRTUAL_OSS_SET_TX_DEV_FIR_FILTER;
283 unsigned cmd_fir_get = VIRTUAL_OSS_GET_TX_DEV_FIR_FILTER;
284 unsigned cmd_info = VIRTUAL_OSS_GET_DEV_INFO;
285 const char *dsp = NULL;
286 int rate;
287 int channels = -1;
288 int part = 0;
289 int opt;
290 int len;
291 int offset;
292 int disable = 0;
293 int f = STDIN_FILENO;
294
295 while ((opt = getopt_long(argc, argv, "d:c:f:op:w:qh",
296 equalizer_opts, NULL)) != -1) {
297 switch (opt) {
298 case 'd':
299 dsp = optarg;
300 break;
301 case 'c':
302 channels = atoi(optarg);
303 if (channels == 0) {
304 message("Wrong number of channels\n");
305 usage();
306 }
307 break;
308 case 'p':
309 part = atoi(optarg);
310 if (part < 0) {
311 message("Invalid part number\n");
312 usage();
313 }
314 break;
315 case 'w':
316 if (strcmp(optarg, "rx_dev") == 0) {
317 cmd_fir_set = VIRTUAL_OSS_SET_RX_DEV_FIR_FILTER;
318 cmd_fir_get = VIRTUAL_OSS_GET_RX_DEV_FIR_FILTER;
319 cmd_info = VIRTUAL_OSS_GET_DEV_INFO;
320 } else if (strcmp(optarg, "tx_dev") == 0) {
321 cmd_fir_set = VIRTUAL_OSS_SET_TX_DEV_FIR_FILTER;
322 cmd_fir_get = VIRTUAL_OSS_GET_TX_DEV_FIR_FILTER;
323 cmd_info = VIRTUAL_OSS_GET_DEV_INFO;
324 } else if (strcmp(optarg, "rx_loop") == 0) {
325 cmd_fir_set = VIRTUAL_OSS_SET_RX_LOOP_FIR_FILTER;
326 cmd_fir_get = VIRTUAL_OSS_GET_RX_LOOP_FIR_FILTER;
327 cmd_info = VIRTUAL_OSS_GET_LOOP_INFO;
328 } else if (strcmp(optarg, "tx_loop") == 0) {
329 cmd_fir_set = VIRTUAL_OSS_SET_TX_LOOP_FIR_FILTER;
330 cmd_fir_get = VIRTUAL_OSS_GET_TX_LOOP_FIR_FILTER;
331 cmd_info = VIRTUAL_OSS_GET_LOOP_INFO;
332 } else {
333 message("Bad -w argument not recognized\n");
334 usage();
335 }
336 break;
337 case 'f':
338 if (f != STDIN_FILENO) {
339 message("Can only specify one file\n");
340 usage();
341 }
342 f = open(optarg, O_RDONLY);
343 if (f < 0) {
344 message("Cannot open specified file\n");
345 usage();
346 }
347 break;
348 case 'o':
349 disable = 1;
350 break;
351 case 'q':
352 be_silent = 1;
353 break;
354 default:
355 usage();
356 }
357 }
358
359 fir.number = part;
360 info.number = part;
361
362 int fd = open(dsp, O_RDWR);
363
364 if (fd < 0) {
365 message("Cannot open DSP device\n");
366 return (EX_SOFTWARE);
367 }
368 if (ioctl(fd, VIRTUAL_OSS_GET_SAMPLE_RATE, &rate) < 0) {
369 message("Cannot get sample rate\n");
370 return (EX_SOFTWARE);
371 }
372 if (ioctl(fd, cmd_fir_get, &fir) < 0) {
373 message("Cannot get current FIR filter\n");
374 return (EX_SOFTWARE);
375 }
376 if (disable) {
377 for (fir.channel = 0; fir.channel != channels; fir.channel++) {
378 if (ioctl(fd, cmd_fir_set, &fir) < 0) {
379 if (fir.channel == 0) {
380 message("Cannot disable FIR filter\n");
381 return (EX_SOFTWARE);
382 }
383 break;
384 }
385 }
386 return (0);
387 }
388 equalizer_init(&e, rate, fir.filter_size);
389 equalizer_load(&e, "");
390
391 if (f == STDIN_FILENO) {
392 if (ioctl(fd, cmd_info, &info) < 0) {
393 message("Cannot read part information\n");
394 return (EX_SOFTWARE);
395 }
396 message("Please enter EQ layout for %s, <freq> <gain>:\n", info.name);
397 }
398 offset = 0;
399 while (1) {
400 if (offset == (int)(sizeof(buffer) - 1)) {
401 message("Too much input data\n");
402 return (EX_SOFTWARE);
403 }
404 len = read(f, buffer + offset, sizeof(buffer) - 1 - offset);
405 if (len <= 0)
406 break;
407 offset += len;
408 }
409 buffer[offset] = 0;
410 close(f);
411
412 if (f == STDIN_FILENO)
413 message("Loading new EQ layout\n");
414
415 if (equalizer_load(&e, buffer) == 0) {
416 message("Invalid equalizer data\n");
417 return (EX_SOFTWARE);
418 }
419 fir.filter_data = e.fftw_time;
420
421 for (fir.channel = 0; fir.channel != channels; fir.channel++) {
422 if (ioctl(fd, cmd_fir_set, &fir) < 0) {
423 if (fir.channel == 0)
424 message("Cannot set FIR filter on channel\n");
425 break;
426 }
427 }
428
429 close(fd);
430 equalizer_done(&e);
431
432 return (0);
433 }
434