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