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