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 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 85 equalizer_get_window(double x) 86 { 87 return (0.5 + 0.5 * cos(M_PI * x)); 88 } 89 90 static int 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 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 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 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 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 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