libs/plugins/a-eq.lv2/a-eq.c

   1 /* a-eq
   2  * Copyright (C) 2016 Damien Zammit <damien@zamaudio.com>
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  */
  14
  15 #ifndef _GNU_SOURCE
  16 #define _GNU_SOURCE // needed for M_PI
  17 #endif
  18
  19 #include <math.h>
  20 #include <complex.h>
  21 #include <stdlib.h>
  22 #include <string.h>
  23 #include <stdbool.h>
  24 #include <stdio.h>
  25
  26 #ifdef COMPILER_MSVC
  27 #include <float.h>
  28 #define isfinite_local(val) (bool)_finite((double)val)
  29 #else
  30 #define isfinite_local isfinite
  31 #endif
  32
  33 #include "lv2/lv2plug.in/ns/lv2core/lv2.h"
  34
  35 #ifdef LV2_EXTENDED
  36 #include <cairo/cairo.h>
  37 #include "ardour/lv2_extensions.h"
  38 #endif
  39
  40 #define AEQ_URI "urn:ardour:a-eq"
  41 #define BANDS   6
  42 #ifndef MIN
  43 #define MIN(A,B) ((A) < (B)) ? (A) : (B)
  44 #endif
  45
  46 typedef enum {
  47         AEQ_FREQL = 0,
  48         AEQ_GAINL,
  49         AEQ_FREQ1,
  50         AEQ_GAIN1,
  51         AEQ_BW1,
  52         AEQ_FREQ2,
  53         AEQ_GAIN2,
  54         AEQ_BW2,
  55         AEQ_FREQ3,
  56         AEQ_GAIN3,
  57         AEQ_BW3,
  58         AEQ_FREQ4,
  59         AEQ_GAIN4,
  60         AEQ_BW4,
  61         AEQ_FREQH,
  62         AEQ_GAINH,
  63         AEQ_MASTER,
  64         AEQ_FILTOGL,
  65         AEQ_FILTOG1,
  66         AEQ_FILTOG2,
  67         AEQ_FILTOG3,
  68         AEQ_FILTOG4,
  69         AEQ_FILTOGH,
  70         AEQ_ENABLE,
  71         AEQ_INPUT,
  72         AEQ_OUTPUT,
  73 } PortIndex;
  74
  75 static inline double
  76 to_dB(double g) {
  77         return (20.0*log10(g));
  78 }
  79
  80 static inline double
  81 from_dB(double gdb) {
  82         return (exp(gdb/20.0*log(10.0)));
  83 }
  84
  85 static inline bool
  86 is_eq(float a, float b, float small) {
  87         return (fabsf(a - b) < small);
  88 }
  89
  90 struct linear_svf {
  91         double g, k;
  92         double a[3];
  93         double m[3];
  94         double s[2];
  95 };
  96
  97 static void linear_svf_reset(struct linear_svf *self)
  98 {
  99         self->s[0] = self->s[1] = 0.0;
 100 }
 101
 102 static void linear_svf_protect(struct linear_svf *self)
 103 {
 104         if (!isfinite_local (self->s[0]) || !isfinite_local (self->s[1])) {
 105                 linear_svf_reset (self);
 106         }
 107 }
 108
 109 typedef struct {
 110         float* f0[BANDS];
 111         float* g[BANDS];
 112         float* bw[BANDS];
 113         float* filtog[BANDS];
 114         float* master;
 115         float* enable;
 116
 117         float srate;
 118         float tau;
 119
 120         float* input;
 121         float* output;
 122
 123         struct linear_svf v_filter[BANDS];
 124         float v_g[BANDS];
 125         float v_bw[BANDS];
 126         float v_f0[BANDS];
 127         float v_master;
 128
 129         bool need_expose;
 130
 131 #ifdef LV2_EXTENDED
 132         LV2_Inline_Display_Image_Surface surf;
 133         cairo_surface_t*                 display;
 134         LV2_Inline_Display*              queue_draw;
 135         uint32_t                         w, h;
 136 #endif
 137 } Aeq;
 138
 139 static LV2_Handle
 140 instantiate(const LV2_Descriptor* descriptor,
 141             double rate,
 142             const char* bundle_path,
 143             const LV2_Feature* const* features)
 144 {
 145         Aeq* aeq = (Aeq*)calloc(1, sizeof(Aeq));
 146         aeq->srate = rate;
 147         aeq->tau = 1.0 - expf (-2.f * M_PI * 64.f * 25.f / aeq->srate); // 25Hz time constant @ 64fpp
 148
 149 #ifdef LV2_EXTENDED
 150         for (int i=0; features[i]; ++i) {
 151                 if (!strcmp(features[i]->URI, LV2_INLINEDISPLAY__queue_draw)) {
 152                         aeq->queue_draw = (LV2_Inline_Display*) features[i]->data;
 153                 }
 154         }
 155 #endif
 156
 157         for (int i = 0; i < BANDS; i++)
 158                 linear_svf_reset(&aeq->v_filter[i]);
 159
 160         aeq->need_expose = true;
 161 #ifdef LV2_EXTENDED
 162         aeq->display = NULL;
 163 #endif
 164
 165         return (LV2_Handle)aeq;
 166 }
 167
 168 static void
 169 connect_port(LV2_Handle instance,
 170              uint32_t port,
 171              void* data)
 172 {
 173         Aeq* aeq = (Aeq*)instance;
 174
 175         switch ((PortIndex)port) {
 176         case AEQ_ENABLE:
 177                 aeq->enable = (float*)data;
 178                 break;
 179         case AEQ_FREQL:
 180                 aeq->f0[0] = (float*)data;
 181                 break;
 182         case AEQ_GAINL:
 183                 aeq->g[0] = (float*)data;
 184                 break;
 185         case AEQ_FREQ1:
 186                 aeq->f0[1] = (float*)data;
 187                 break;
 188         case AEQ_GAIN1:
 189                 aeq->g[1] = (float*)data;
 190                 break;
 191         case AEQ_BW1:
 192                 aeq->bw[1] = (float*)data;
 193                 break;
 194         case AEQ_FREQ2:
 195                 aeq->f0[2] = (float*)data;
 196                 break;
 197         case AEQ_GAIN2:
 198                 aeq->g[2] = (float*)data;
 199                 break;
 200         case AEQ_BW2:
 201                 aeq->bw[2] = (float*)data;
 202                 break;
 203         case AEQ_FREQ3:
 204                 aeq->f0[3] = (float*)data;
 205                 break;
 206         case AEQ_GAIN3:
 207                 aeq->g[3] = (float*)data;
 208                 break;
 209         case AEQ_BW3:
 210                 aeq->bw[3] = (float*)data;
 211                 break;
 212         case AEQ_FREQ4:
 213                 aeq->f0[4] = (float*)data;
 214                 break;
 215         case AEQ_GAIN4:
 216                 aeq->g[4] = (float*)data;
 217                 break;
 218         case AEQ_BW4:
 219                 aeq->bw[4] = (float*)data;
 220                 break;
 221         case AEQ_FREQH:
 222                 aeq->f0[5] = (float*)data;
 223                 break;
 224         case AEQ_GAINH:
 225                 aeq->g[5] = (float*)data;
 226                 break;
 227         case AEQ_MASTER:
 228                 aeq->master = (float*)data;
 229                 break;
 230         case AEQ_FILTOGL:
 231                 aeq->filtog[0] = (float*)data;
 232                 break;
 233         case AEQ_FILTOG1:
 234                 aeq->filtog[1] = (float*)data;
 235                 break;
 236         case AEQ_FILTOG2:
 237                 aeq->filtog[2] = (float*)data;
 238                 break;
 239         case AEQ_FILTOG3:
 240                 aeq->filtog[3] = (float*)data;
 241                 break;
 242         case AEQ_FILTOG4:
 243                 aeq->filtog[4] = (float*)data;
 244                 break;
 245         case AEQ_FILTOGH:
 246                 aeq->filtog[5] = (float*)data;
 247                 break;
 248         case AEQ_INPUT:
 249                 aeq->input = (float*)data;
 250                 break;
 251         case AEQ_OUTPUT:
 252                 aeq->output = (float*)data;
 253                 break;
 254         }
 255 }
 256
 257 static void
 258 activate(LV2_Handle instance)
 259 {
 260         int i;
 261         Aeq* aeq = (Aeq*)instance;
 262
 263         for (i = 0; i < BANDS; i++)
 264                 linear_svf_reset(&aeq->v_filter[i]);
 265 }
 266
 267 // SVF filters
 268 // http://www.cytomic.com/files/dsp/SvfLinearTrapOptimised2.pdf
 269
 270 static void linear_svf_set_peq(struct linear_svf *self, float gdb, float sample_rate, float cutoff, float bandwidth)
 271 {
 272         double f0 = (double)cutoff;
 273         double q = (double)pow(2.0, 1.0 / bandwidth) / (pow(2.0, bandwidth) - 1.0);
 274         double sr = (double)sample_rate;
 275         double A = pow(10.0, gdb/40.0);
 276
 277         self->g = tan(M_PI * (f0 / sr));
 278         self->k = 1.0 / (q * A);
 279
 280         self->a[0] = 1.0 / (1.0 + self->g * (self->g + self->k));
 281         self->a[1] = self->g * self->a[0];
 282         self->a[2] = self->g * self->a[1];
 283
 284         self->m[0] = 1.0;
 285         self->m[1] = self->k * (A * A - 1.0);
 286         self->m[2] = 0.0;
 287 }
 288
 289 static void linear_svf_set_highshelf(struct linear_svf *self, float gdb, float sample_rate, float cutoff, float resonance)
 290 {
 291         double f0 = (double)cutoff;
 292         double q = (double)resonance;
 293         double sr = (double)sample_rate;
 294         double A = pow(10.0, gdb/40.0);
 295
 296         self->g = tan(M_PI * (f0 / sr));
 297         self->k = 1.0 / q;
 298
 299         self->a[0] = 1.0 / (1.0 + self->g * (self->g + self->k));
 300         self->a[1] = self->g * self->a[0];
 301         self->a[2] = self->g * self->a[1];
 302
 303         self->m[0] = A * A;
 304         self->m[1] = self->k * (1.0 - A) * A;
 305         self->m[2] = 1.0 - A * A;
 306 }
 307
 308 static void linear_svf_set_lowshelf(struct linear_svf *self, float gdb, float sample_rate, float cutoff, float resonance)
 309 {
 310         double f0 = (double)cutoff;
 311         double q = (double)resonance;
 312         double sr = (double)sample_rate;
 313         double A = pow(10.0, gdb/40.0);
 314
 315         self->g = tan(M_PI * (f0 / sr));
 316         self->k = 1.0 / q;
 317
 318         self->a[0] = 1.0 / (1.0 + self->g * (self->g + self->k));
 319         self->a[1] = self->g * self->a[0];
 320         self->a[2] = self->g * self->a[1];
 321
 322         self->m[0] = 1.0;
 323         self->m[1] = self->k * (A - 1.0);
 324         self->m[2] = A * A - 1.0;
 325 }
 326
 327 static float run_linear_svf(struct linear_svf *self, float in)
 328 {
 329         double v[3];
 330         double din = (double)in;
 331         double out;
 332
 333         v[2] = din - self->s[1];
 334         v[0] = (self->a[0] * self->s[0]) + (self->a[1] * v[2]);
 335         v[1] = self->s[1] + (self->a[1] * self->s[0]) + (self->a[2] * v[2]);
 336
 337         self->s[0] = (2.0 * v[0]) - self->s[0];
 338         self->s[1] = (2.0 * v[1]) - self->s[1];
 339
 340         out = (self->m[0] * din)
 341                 + (self->m[1] * v[0])
 342                 + (self->m[2] * v[1]);
 343
 344         return (float)out;
 345 }
 346
 347 static void set_params(LV2_Handle instance, int band) {
 348         Aeq* aeq = (Aeq*)instance;
 349
 350         switch (band) {
 351         case 0:
 352                 linear_svf_set_lowshelf(&aeq->v_filter[0], aeq->v_g[0], aeq->srate, aeq->v_f0[0], 0.7071068);
 353                 break;
 354         case 1:
 355         case 2:
 356         case 3:
 357         case 4:
 358                 linear_svf_set_peq(&aeq->v_filter[band], aeq->v_g[band], aeq->srate, aeq->v_f0[band], aeq->v_bw[band]);
 359                 break;
 360         case 5:
 361                 linear_svf_set_highshelf(&aeq->v_filter[5], aeq->v_g[5], aeq->srate, aeq->v_f0[5], 0.7071068);
 362                 break;
 363         }
 364 }
 365
 366 static void
 367 run(LV2_Handle instance, uint32_t n_samples)
 368 {
 369         Aeq* aeq = (Aeq*)instance;
 370
 371         const float* const input = aeq->input;
 372         float* const output = aeq->output;
 373
 374         const float tau = aeq->tau;
 375         uint32_t offset = 0;
 376
 377         const float target_gain = *aeq->enable <= 0 ? 0 : *aeq->master; // dB
 378
 379         while (n_samples > 0) {
 380                 uint32_t block = n_samples;
 381                 bool any_changed = false;
 382
 383                 if (!is_eq(aeq->v_master, target_gain, 0.1)) {
 384                         aeq->v_master += tau * (target_gain - aeq->v_master);
 385                         any_changed = true;
 386                 } else {
 387                         aeq->v_master = target_gain;
 388                 }
 389
 390                 for (int i = 0; i < BANDS; ++i) {
 391                         bool changed = false;
 392
 393                         if (!is_eq(aeq->v_f0[i], *aeq->f0[i], 0.1)) {
 394                                 aeq->v_f0[i] += tau * (*aeq->f0[i] - aeq->v_f0[i]);
 395                                 changed = true;
 396                         } else {
 397                                 aeq->v_f0[i] = *aeq->f0[i];
 398                         }
 399
 400                         if (*aeq->filtog[i] <= 0 || *aeq->enable <= 0) {
 401                                 if (!is_eq(aeq->v_g[i], 0.f, 0.05)) {
 402                                         aeq->v_g[i] += tau * (0.0 - aeq->v_g[i]);
 403                                         changed = true;
 404                                 } else {
 405                                         aeq->v_g[i] = 0.0;
 406                                 }
 407                         } else {
 408                                 if (!is_eq(aeq->v_g[i], *aeq->g[i], 0.05)) {
 409                                         aeq->v_g[i] += tau * (*aeq->g[i] - aeq->v_g[i]);
 410                                         changed = true;
 411                                 } else {
 412                                         aeq->v_g[i] = *aeq->g[i];
 413                                 }
 414                         }
 415
 416                         if (i != 0 && i != 5) {
 417                                 if (!is_eq(aeq->v_bw[i], *aeq->bw[i], 0.001)) {
 418                                         aeq->v_bw[i] += tau * (*aeq->bw[i] - aeq->v_bw[i]);
 419                                         changed = true;
 420                                 } else {
 421                                         aeq->v_bw[i] = *aeq->bw[i];
 422                                 }
 423                         }
 424
 425                         if (changed) {
 426                                 set_params(aeq, i);
 427                                 any_changed = true;
 428                         }
 429                 }
 430
 431                 if (any_changed) {
 432                         aeq->need_expose = true;
 433                         block = MIN (64, n_samples);
 434                 }
 435
 436                 for (uint32_t i = 0; i < block; ++i) {
 437                         float in0, out;
 438                         in0 = input[i + offset];
 439                         out = in0;
 440                         for (uint32_t j = 0; j < BANDS; j++) {
 441                                 out = run_linear_svf(&aeq->v_filter[j], out);
 442                         }
 443                         output[i + offset] = out * from_dB(aeq->v_master);
 444                 }
 445                 n_samples -= block;
 446                 offset += block;
 447         }
 448
 449         for (uint32_t j = 0; j < BANDS; j++) {
 450                 linear_svf_protect(&aeq->v_filter[j]);
 451         }
 452
 453 #ifdef LV2_EXTENDED
 454         if (aeq->need_expose && aeq->queue_draw) {
 455                 aeq->need_expose = false;
 456                 aeq->queue_draw->queue_draw (aeq->queue_draw->handle);
 457         }
 458 #endif
 459 }
 460
 461 static double
 462 calc_peq(Aeq* self, int i, double omega) {
 463         double complex H = 0.0;
 464         double complex z = cexp(I * omega);
 465         double complex zz = cexp(2. * I * omega);
 466         double complex zm = z - 1.0;
 467         double complex zp = z + 1.0;
 468         double complex zzm = zz - 1.0;
 469
 470         double A = pow(10.0, self->v_g[i]/40.0);
 471         double g = self->v_filter[i].g;
 472         double k = self->v_filter[i].k * A;
 473         double m1 = k * (A * A - 1.0) / A;
 474
 475         H = (g*k*zzm + A*(g*zp*(m1*zm) + (zm*zm + g*g*zp*zp))) / (g*k*zzm + A*(zm*zm + g*g*zp*zp));
 476         return cabs(H);
 477 }
 478
 479 static double
 480 calc_lowshelf(Aeq* self, double omega) {
 481         double complex H = 0.0;
 482         double complex z = cexp(I * omega);
 483         double complex zz = cexp(2. * I * omega);
 484         double complex zm = z - 1.0;
 485         double complex zp = z + 1.0;
 486         double complex zzm = zz - 1.0;
 487
 488         double A = pow(10.0, self->v_g[0]/40.0);
 489         double g = self->v_filter[0].g;
 490         double k = self->v_filter[0].k;
 491         double m0 = self->v_filter[0].m[0];
 492         double m1 = self->v_filter[0].m[1];
 493         double m2 = self->v_filter[0].m[2];
 494
 495         H = (A*m0*zm*zm + g*g*(m0+m2)*zp*zp + sqrt(A)*g*(k*m0+m1) * zzm) / (A*zm*zm + g*g*zp*zp + sqrt(A)*g*k*zzm);
 496         return cabs(H);
 497 }
 498
 499 static double
 500 calc_highshelf(Aeq* self, double omega) {
 501         double complex H = 0.0;
 502         double complex z = cexp(I * omega);
 503         double complex zz = cexp(2. * I * omega);
 504         double complex zm = z - 1.0;
 505         double complex zp = z + 1.0;
 506         double complex zzm = zz - 1.0;
 507
 508         double A = pow(10.0, self->v_g[5]/40.0);
 509         double g = self->v_filter[5].g;
 510         double k = self->v_filter[5].k;
 511         double m0 = self->v_filter[5].m[0];
 512         double m1 = self->v_filter[5].m[1];
 513         double m2 = self->v_filter[5].m[2];
 514
 515         H = ( sqrt(A) * g * zp * (m1 * zm + sqrt(A)*g*m2*zp) + m0 * ( zm*zm + A*g*g*zp*zp + sqrt(A)*g*k*zzm)) / (zm*zm + A*g*g*zp*zp + sqrt(A)*g*k*zzm);
 516         return cabs(H);
 517 }
 518
 519 #ifdef LV2_EXTENDED
 520 static float
 521 eq_curve (Aeq* self, float f) {
 522         double response = 1.0;
 523         double SR = (double)self->srate;
 524         double omega = f * 2. * M_PI / SR;
 525
 526         // lowshelf
 527         response *= calc_lowshelf(self, omega);
 528
 529         // peq 1 - 4:
 530         response *= calc_peq(self, 1, omega);
 531         response *= calc_peq(self, 2, omega);
 532         response *= calc_peq(self, 3, omega);
 533         response *= calc_peq(self, 4, omega);
 534
 535         // highshelf:
 536         response *= calc_highshelf(self, omega);
 537
 538         return (float)response;
 539 }
 540
 541 static LV2_Inline_Display_Image_Surface *
 542 render_inline (LV2_Handle instance, uint32_t w, uint32_t max_h)
 543 {
 544         Aeq* self = (Aeq*)instance;
 545         uint32_t h = MIN (1 | (uint32_t)ceilf (w * 9.f / 16.f), max_h);
 546
 547         if (!self->display || self->w != w || self->h != h) {
 548                 if (self->display) cairo_surface_destroy(self->display);
 549                 self->display = cairo_image_surface_create (CAIRO_FORMAT_ARGB32, w, h);
 550                 self->w = w;
 551                 self->h = h;
 552         }
 553
 554         cairo_t* cr = cairo_create (self->display);
 555
 556         // clear background
 557         cairo_rectangle (cr, 0, 0, w, h);
 558         cairo_set_source_rgba (cr, .2, .2, .2, 1.0);
 559         cairo_fill (cr);
 560
 561         cairo_set_line_width(cr, 1.0);
 562
 563         // prepare grid drawing
 564         cairo_save (cr);
 565         const double dash2[] = {1, 3};
 566         //cairo_set_line_cap(cr, CAIRO_LINE_CAP_ROUND);
 567         cairo_set_dash(cr, dash2, 2, 2);
 568         cairo_set_source_rgba (cr, 0.5, 0.5, 0.5, 0.5);
 569
 570         // draw x-grid 6dB steps
 571         for (int32_t d = -18; d <= 18; d+=6) {
 572                 float y = (float)h * (d / 40.0 + 0.5);
 573                 y = rint (y) - .5;
 574                 cairo_move_to (cr, 0, y);
 575                 cairo_line_to (cr, w, y);
 576                 cairo_stroke (cr);
 577         }
 578         // draw y-axis grid 100, 1k, 10K
 579         for (int32_t f = 100; f <= 10000; f *= 10) {
 580                 float x = w * log10 (f / 20.0) / log10 (1000.0);
 581                 x = rint (x) - .5;
 582                 cairo_move_to (cr, x, 0);
 583                 cairo_line_to (cr, x, h);
 584                 cairo_stroke (cr);
 585         }
 586
 587         cairo_restore (cr);
 588
 589
 590         // draw curve
 591         cairo_set_source_rgba (cr, .8, .8, .8, 1.0);
 592         cairo_move_to (cr, 0, h);
 593
 594         for (uint32_t x = 0; x < w; ++x) {
 595                 // plot 20..20kHz +-20dB
 596                 const float x_hz = 20.f * powf (1000.f, (float)x / (float)w);
 597                 const float y_db = to_dB(eq_curve(self, x_hz)) + self->v_master;
 598                 const float y = (float)h * (-y_db / 40.0 + 0.5);
 599                 cairo_line_to (cr, x, y);
 600         }
 601         cairo_stroke_preserve (cr);
 602
 603         cairo_line_to (cr, w, h);
 604         cairo_close_path (cr);
 605         cairo_clip (cr);
 606
 607         // create RGBA surface
 608         cairo_destroy (cr);
 609         cairo_surface_flush (self->display);
 610         self->surf.width = cairo_image_surface_get_width (self->display);
 611         self->surf.height = cairo_image_surface_get_height (self->display);
 612         self->surf.stride = cairo_image_surface_get_stride (self->display);
 613         self->surf.data = cairo_image_surface_get_data  (self->display);
 614
 615         return &self->surf;
 616 }
 617 #endif
 618
 619 static const void*
 620 extension_data(const char* uri)
 621 {
 622 #ifdef LV2_EXTENDED
 623         static const LV2_Inline_Display_Interface display  = { render_inline };
 624         if (!strcmp(uri, LV2_INLINEDISPLAY__interface)) {
 625                 return &display;
 626         }
 627 #endif
 628         return NULL;
 629 }
 630
 631 static void
 632 cleanup(LV2_Handle instance)
 633 {
 634 #ifdef LV2_EXTENDED
 635         Aeq* aeq = (Aeq*)instance;
 636         if (aeq->display) {
 637                 cairo_surface_destroy (aeq->display);
 638         }
 639 #endif
 640         free(instance);
 641 }
 642
 643 static const LV2_Descriptor descriptor = {
 644         AEQ_URI,
 645         instantiate,
 646         connect_port,
 647         activate,
 648         run,
 649         NULL,
 650         cleanup,
 651         extension_data
 652 };
 653
 654 LV2_SYMBOL_EXPORT
 655 const LV2_Descriptor*
 656 lv2_descriptor(uint32_t index)
 657 {
 658         switch (index) {
 659         case 0:
 660                 return &descriptor;
 661         default:
 662                 return NULL;
 663         }
 664 }