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