incomplete svf implementation

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
 

crap/eq_const_T420_svf.h

@@ -0,0 +1,84 @@
+#include <string.h>
+#include <stdio.h>
+
+#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]);
+	*/
+}

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;
 

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;
+	}
+}

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;
+}

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 v2sf __attribute__((vector_size(8), aligned(8)));
-typedef float v2df __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"