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