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