From 79fe905ec54db9bf801e63f5f909190b3bef58a0 Mon Sep 17 00:00:00 2001 From: Connor Olding Date: Thu, 28 May 2015 16:45:14 -0700 Subject: [PATCH] incomplete svf implementation --- Makefile | 4 +- crap/eq_const_T420_svf.h | 84 ++++++++++++++++++++++++++ include/biquad.h | 15 +---- include/process_svfs.h | 40 ++++++++++++ include/svf.h | 127 +++++++++++++++++++++++++++++++++++++++ include/util.h | 22 ++++++- 6 files changed, 273 insertions(+), 19 deletions(-) create mode 100644 crap/eq_const_T420_svf.h create mode 100644 include/process_svfs.h create mode 100644 include/svf.h diff --git a/Makefile b/Makefile index b6af27d..5461c5e 100644 --- a/Makefile +++ b/Makefile @@ -5,12 +5,12 @@ FULLNAME = $(DISTNAME)-$(VERSION) BIN ?= ./bin VST_SDK_DIR ?= . -BOTH = eq eq_const eq_const_T420 noise tube +BOTH = eq eq_const eq_const_T420 eq_const_T420_svf noise tube LADSPA = $(BOTH) VST = $(BOTH) delay_test UTILS = design bench -INCLUDES = util biquad param os6iir os2piir +INCLUDES = util biquad svf param os6iir os2piir BENCH_AGAINST = eq_const diff --git a/crap/eq_const_T420_svf.h b/crap/eq_const_T420_svf.h new file mode 100644 index 0000000..5c88cfc --- /dev/null +++ b/crap/eq_const_T420_svf.h @@ -0,0 +1,84 @@ +#include +#include + +#define ID (0x0DEFACED+420+1337) +#define LABEL "crap_eq_const_T420_svf" +#define NAME "crap T420 Speaker Compensation (SVF)" +#define AUTHOR "Connor Olding" +#define COPYRIGHT "MIT" +#define PARAMETERS 0 + +#define BLOCK_SIZE 256 + +#include "util.h" + +#define BANDS 16 +typedef struct { + svf filters[2][BANDS]; +} personal; + +static void +process_double(personal *data, + double *in_L, double *in_R, + double *out_L, double *out_R, + unsigned long count) +#include "process_svfs.h" + +static void +process(personal *data, + float *in_L, float *in_R, + float *out_L, float *out_R, + ulong count) +#include "process_svfs.h" + +INNER void +construct(personal *data) +{} + +INNER void +destruct(personal *data) +{} + +INNER void +resume(personal *data) +{ + svf *filters = data->filters[0]; + for (int i = 0; i < BANDS; i++) { + filters[i].memory[0] = 0; + filters[i].memory[1] = 0; + } + memcpy(data->filters[1], filters, BANDS*sizeof(svf)); +} + +INNER void +pause(personal *data) +{} + +INNER void +adjust(personal *data, ulong fs) +{ + svf *filters = data->filters[0]; + filters[ 0] = svf_gen(FILT_PEAKING, 180., 11., 1.40, fs); + filters[ 1] = svf_gen(FILT_PEAKING, 740., 5.5, 0.70, fs); + filters[ 2] = svf_gen(FILT_PEAKING, 1220, -12., 0.70, fs); + filters[ 3] = svf_gen(FILT_PEAKING, 1580, 7.0, 0.25, fs); + filters[ 4] = svf_gen(FILT_PEAKING, 2080, -2.5, 0.30, fs); + filters[ 5] = svf_gen(FILT_PEAKING, 2270, 6.0, 0.20, fs); + filters[ 6] = svf_gen(FILT_PEAKING, 2470, -2.0, 0.18, fs); + filters[ 7] = svf_gen(FILT_PEAKING, 3700, -5.0, 0.32, fs); + filters[ 8] = svf_gen(FILT_PEAKING, 6200, -3.5, 0.25, fs); + filters[ 9] = svf_gen(FILT_PEAKING, 6000, -11., 3.66, fs); + filters[10] = svf_gen(FILT_HIGHSHELF, 11500, 4.0, 0.40, fs); + filters[11] = svf_gen(FILT_HIGHPASS, 150, 0.0, 1.00, fs); + filters[12] = svf_gen(FILT_PEAKING, 1775, -2.0, 0.18, fs); + filters[13] = svf_gen(FILT_PEAKING, 490, -1.5, 0.23, fs); + filters[14] = svf_gen(FILT_PEAKING, 3100, 5.0, 0.33, fs); + filters[15] = svf_gen(FILT_LOWPASS, 14000, 0.0, 0.40, fs); + /* debugging + svf *f = &filters[10]; + printf("A = [%.8f %.8f]\n", f->A0[0], f->A0[1]); + printf(" [%.8f %.8f]\n", f->A1[0], f->A1[1]); + printf("B = {%.8f, %.8f}\n", f->B[0], f->B[1]); + printf("C = {%.8f, %.8f, %9.7f}\n", f->C[0], f->C[1], f->C[2]); + */ +} diff --git a/include/biquad.h b/include/biquad.h index 421d7a9..b33fc03 100644 --- a/include/biquad.h +++ b/include/biquad.h @@ -1,18 +1,5 @@ /* used to resemble https://github.com/swh/ladspa/blob/master/util/biquad.h */ -typedef enum { - FILT_PEAKING, - FILT_LOWSHELF, - FILT_HIGHSHELF, - FILT_LOWPASS, - FILT_HIGHPASS, - FILT_ALLPASS, - FILT_BANDPASS, - FILT_BANDPASS_2, - FILT_NOTCH, - FILT_GAIN -} filter_t; - typedef struct { double a1, a2, b0, b1, b2, x1, x2, y1, y2; } biquad; @@ -42,6 +29,7 @@ design(double cw, double sw, }; } +// TODO: rename to biquad_gen_raw, fix up parameters like you did with svf static biquad biquad_gen(filter_t type, double fc, double gain, double bw, double fs) { @@ -52,7 +40,6 @@ biquad_gen(filter_t type, double fc, double gain, double bw, double fs) A = DB2LIN(gain/2); As = sqrt(A); Q = M_SQRT1_2*(1 - (w0/M_PI)*(w0/M_PI))/bw; - /* skip = (fabs(A - 1) <= TINY); */ biquad_interim bqi; diff --git a/include/process_svfs.h b/include/process_svfs.h new file mode 100644 index 0000000..8871dba --- /dev/null +++ b/include/process_svfs.h @@ -0,0 +1,40 @@ +{ + disable_denormals(); + + float buf_L[BLOCK_SIZE]; + float buf_R[BLOCK_SIZE]; + + svf *f0, *f1; + + for (ulong pos = 0; pos < count; pos += BLOCK_SIZE) { + ulong rem = BLOCK_SIZE; + if (pos + BLOCK_SIZE > count) + rem = count - pos; + + for (ulong i = 0; i < rem; i++) { + buf_L[i] = in_L[i]; + buf_R[i] = in_R[i]; + } + + f0 = data->filters[0]; + f1 = data->filters[1]; + for (ulong i = 0; i < BANDS; i++) { + for (ulong j = 0; j < rem; j++) + buf_L[j] = svf_run(f0, buf_L[j]); + for (ulong j = 0; j < rem; j++) + buf_R[j] = svf_run(f1, buf_R[j]); + f0++; + f1++; + } + + for (ulong i = 0; i < rem; i++) { + out_L[i] = buf_L[i]; + out_R[i] = buf_R[i]; + } + + in_L += BLOCK_SIZE; + in_R += BLOCK_SIZE; + out_L += BLOCK_SIZE; + out_R += BLOCK_SIZE; + } +} diff --git a/include/svf.h b/include/svf.h new file mode 100644 index 0000000..7ab01b9 --- /dev/null +++ b/include/svf.h @@ -0,0 +1,127 @@ +/* via http://nbviewer.ipython.org/urls/music-synthesizer-for-android.googlecode.com/git/lab/Second%20order%20sections%20in%20matrix%20form.ipynb */ + +typedef struct { + float A0[2], A1[2], B[2], C[3], memory[2]; +} svf; + +typedef struct { + double A0[2], A1[2], B[2], C[3]; +} svf_interim; + +typedef struct { + v4sf a, b, c, d, memory; +} svf_matrix; + +static svf_interim +svf_design(double w0, double Q, double c0, double c1, double c2, double gc) +{ + svf_interim svfi; + double g = tan(0.5*w0)*gc; + double a1 = 1/(1 + g*(g + 1/Q)); + double a2 = g*a1; + double a3 = g*a2; + svfi.A0[0] = 2*a1 - 1; + svfi.A0[1] = -2*a2; + svfi.A1[0] = 2*a2; + svfi.A1[1] = 1 - 2*a3; + svfi.B[0] = 2*a2; + svfi.B[1] = 2*a3; + double v0[3] = {1, 0, 0}; + double v1[3] = {a2, a1, -a2}; + double v2[3] = {a3, a2, 1 - a3}; + svfi.C[0] = v0[0]*c0 + v1[0]*c1 + v2[0]*c2; + svfi.C[1] = v0[1]*c0 + v1[1]*c1 + v2[1]*c2; + svfi.C[2] = v0[2]*c0 + v1[2]*c1 + v2[2]*c2; + return svfi; +} + +static svf +svf_gen_raw(filter_t type, double w0, double A, double Q) +{ + double As = sqrt(A); + + svf_interim svfi; + + #define d(Q,c0,c1,c2,gc) svfi = svf_design(w0,Q,c0,c1,c2,gc) + switch (type) { + case FILT_PEAKING: d(Q*A, 1, (A*A - 1)/A/Q, 0, 1); break; + case FILT_LOWSHELF: d(Q, 1, (A - 1)/Q, A*A - 1, 1/As); break; + case FILT_HIGHSHELF: d(Q, A*A, (1 - A)*A/Q, 1 - A*A, As); break; + case FILT_LOWPASS: d(Q, 0, 0, 1, 1); break; + case FILT_HIGHPASS: d(Q, 1, -1/Q, -1, 1); break; + case FILT_ALLPASS: d(Q, 0, 0, 0, 1); break; // TODO: implement + case FILT_BANDPASS: d(Q, 0, 1, 0, 1); break; // FIXME: no gain + case FILT_BANDPASS_2: d(Q, 0, 0, 0, 1); break; // TODO: implement + case FILT_NOTCH: d(Q, 1, -1/Q, 0, 1); break; + case FILT_GAIN: d(Q, 0, 0, 0, 1); break; // TODO: implement + } + #undef d + + svf s; + s.A0[0] = svfi.A0[0]; + s.A0[1] = svfi.A0[1]; + s.A1[0] = svfi.A1[0]; + s.A1[1] = svfi.A1[1]; + s.B[0] = svfi.B[0]; + s.B[1] = svfi.B[1]; + s.C[0] = svfi.C[0]; + s.C[1] = svfi.C[1]; + s.C[2] = svfi.C[2]; + s.memory[0] = 0; + s.memory[1] = 0; + return s; +} + +static svf +svf_gen(filter_t type, double fc, double gain, double bw, double fs) +{ + double w0 = ANGULAR_LIM(fc, fs); + double A = DB2LIN(gain/2); + //double Q = M_SQRT1_2/bw; + double Q = M_SQRT1_2*(1 - (w0/M_PI)*(w0/M_PI))/bw; + return svf_gen_raw(type, w0, A, Q); +} + +INNER float +svf_run(svf *s, float x) +{ + float y = s->C[0]*x + s->C[1]*s->memory[0] + s->C[2]*s->memory[1]; + float temp = s->memory[0]; + s->memory[0] = s->B[0]*x + s->A0[0]*temp + s->A0[1]*s->memory[1]; + s->memory[1] = s->B[1]*x + s->A1[0]*temp + s->A1[1]*s->memory[1]; + return y; +} + +static svf_matrix +svf_gen_matrix(svf s) +{ + // note: does not copy memory + /* + AA = dot(A, A); + AB = dot(A, B); + CA = dot(C[1:], A); + cb = dot(C[1:], B); + return [[ C[0], 0, C[1], C[2]], + [ cb, C[0], CA[0], CA[1]], + [AB[0], B[0], AA[0][0], AA[0][1]], + [AB[1], B[1], AA[1][0], AA[1][1]]]; + */ + float AA0[2], AA1[2], AB[2], CA[2], cb; + AA0[0] = s.A0[0]*s.A0[0] + s.A0[1]*s.A1[0]; + AA1[0] = s.A1[0]*s.A0[0] + s.A1[1]*s.A1[0]; + AA0[1] = s.A0[0]*s.A0[1] + s.A0[1]*s.A1[1]; + AA1[1] = s.A1[0]*s.A0[1] + s.A1[1]*s.A1[1]; + AB[0] = s.A0[0]*s.B[0] + s.A0[1]*s.B[1]; + AB[1] = s.A1[0]*s.B[0] + s.A1[1]*s.B[1]; + CA[0] = s.A0[0]*s.C[1] + s.A0[1]*s.C[2]; + CA[1] = s.A1[0]*s.C[1] + s.A1[1]*s.C[2]; + cb = s.C[1]*s.B[0] + s.C[2]*s.B[1]; + + svf_matrix mat; + mat.memory = (v4sf){0, 0, 0, 0}; + mat.a = (v4sf){s.C[0], 0, s.C[1], s.C[2]}; + mat.b = (v4sf){ cb, s.C[0], CA[0], CA[1]}; + mat.c = (v4sf){ AB[0], s.B[0], AA0[0], AA0[1]}; + mat.d = (v4sf){ AB[1], s.B[1], AA1[0], AA1[1]}; + return mat; +} diff --git a/include/util.h b/include/util.h index 0500f0a..0224383 100644 --- a/include/util.h +++ b/include/util.h @@ -10,10 +10,12 @@ #define PURE __attribute__((pure)) #define CONST __attribute__((const)) -#ifndef FORCE_SINGLE typedef double v2df __attribute__((vector_size(16), aligned(16))); -#else -typedef float v2df __attribute__((vector_size(8), aligned(8))); +typedef float v2sf __attribute__((vector_size(8), aligned(8))); +typedef float v4sf __attribute__((vector_size(16), aligned(16))); + +#ifndef FORCE_SINGLE +#define v2df v2sf #endif typedef float v4sf __attribute__((vector_size(16), aligned(16))); @@ -51,4 +53,18 @@ whitenoise() return white.f - 3; } +typedef enum { + FILT_PEAKING, + FILT_LOWSHELF, + FILT_HIGHSHELF, + FILT_LOWPASS, + FILT_HIGHPASS, + FILT_ALLPASS, + FILT_BANDPASS, + FILT_BANDPASS_2, + FILT_NOTCH, + FILT_GAIN +} filter_t; + #include "biquad.h" +#include "svf.h"