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