synthio: Fix multichannel biquad filtering

jepler · jepler · commit a891e149a662 · 2023-05-29T11:23:07.000-05:00
diff --git a/shared-module/synthio/Biquad.c b/shared-module/synthio/Biquad.c
@@ -110,30 +110,35 @@ void synthio_biquad_filter_assign(biquad_filter_state *st, mp_obj_t biquad_obj)
     }
 }
 
-void synthio_biquad_filter_samples(biquad_filter_state *st, int32_t *out, const int32_t *in, size_t n, size_t stride) {
+void synthio_biquad_filter_samples(biquad_filter_state *st, int32_t *out0, const int32_t *in0, size_t n0, size_t n_channels) {
     int32_t a1 = st->a1;
     int32_t a2 = st->a2;
     int32_t b0 = st->b0;
     int32_t b1 = st->b1;
     int32_t b2 = st->b2;
 
-    int32_t x0 = st->x[0];
-    int32_t x1 = st->x[1];
-    int32_t y0 = st->y[0];
-    int32_t y1 = st->y[1];
+    for (size_t i = 0; i < n_channels; i++) {
+        const int32_t *in = in0 + i;
+        int32_t *out = out0 + i;
 
-    for (; n; --n, in += stride, out += stride) {
-        int16_t input = *in;
-        int32_t output = (b0 * input + b1 * x0 + b2 * x1 - a1 * y0 - a2 * y1) >> BIQUAD_SHIFT;
+        int32_t x0 = st->x[i][0];
+        int32_t x1 = st->x[i][1];
+        int32_t y0 = st->y[i][0];
+        int32_t y1 = st->y[i][1];
 
-        x1 = x0;
-        x0 = input;
-        y1 = y0;
-        y0 = output;
-        *out = output;
+        for (size_t n = n0; n; --n, in += n_channels, out += n_channels) {
+            int32_t input = *in;
+            int32_t output = (b0 * input + b1 * x0 + b2 * x1 - a1 * y0 - a2 * y1) >> BIQUAD_SHIFT;
+
+            x1 = x0;
+            x0 = input;
+            y1 = y0;
+            y0 = output;
+            *out = output;
+        }
+        st->x[i][0] = x0;
+        st->x[i][1] = x1;
+        st->y[i][0] = y0;
+        st->y[i][1] = y1;
     }
-    st->x[0] = x0;
-    st->x[1] = x1;
-    st->y[0] = y0;
-    st->y[1] = y1;
 }
diff --git a/shared-module/synthio/Biquad.h b/shared-module/synthio/Biquad.h
@@ -30,8 +30,8 @@
 
 typedef struct {
     int32_t a1, a2, b0, b1, b2;
-    int32_t x[2], y[2];
+    int32_t x[2][2], y[2][2];
 } biquad_filter_state;
 
 void synthio_biquad_filter_assign(biquad_filter_state *st, mp_obj_t biquad_obj);
-void synthio_biquad_filter_samples(biquad_filter_state *st, int32_t *out, const int32_t *in, size_t n, size_t stride);
+void synthio_biquad_filter_samples(biquad_filter_state *st, int32_t *out, const int32_t *in, size_t n_samples, size_t n_channels);
diff --git a/shared-module/synthio/__init__.c b/shared-module/synthio/__init__.c
@@ -352,10 +352,7 @@ void synthio_synth_synthesize(synthio_synth_t *synth, uint8_t **bufptr, uint32_t
             memset(filter_buffer32, 0, sizeof(filter_buffer32));
 
             synth_note_into_buffer(synth, chan, filter_buffer32, dur);
-            int synth_chan = synth->channel_count;
-            for (int i = 0; i < synth_chan; i++) {
-                synthio_biquad_filter_samples(&note->filter_state, &out_buffer32[i], &filter_buffer32[i], dur, i);
-            }
+            synthio_biquad_filter_samples(&note->filter_state, out_buffer32, filter_buffer32, dur, synth->channel_count);
         }
     }
 

Original file line number	Diff line number	Diff line change
`@@ -352,10 +352,7 @@ void synthio_synth_synthesize(synthio_synth_t synth, uint8_t *bufptr, uint32_t`
`352`	`352`	`memset(filter_buffer32, 0, sizeof(filter_buffer32));`
`353`	`353`
`354`	`354`	`synth_note_into_buffer(synth, chan, filter_buffer32, dur);`
`355`		`- int synth_chan = synth->channel_count;`
`356`		`- for (int i = 0; i < synth_chan; i++) {`
`357`		`- synthio_biquad_filter_samples(&note->filter_state, &out_buffer32[i], &filter_buffer32[i], dur, i);`
`358`		`- }`
	`355`	`+ synthio_biquad_filter_samples(&note->filter_state, out_buffer32, filter_buffer32, dur, synth->channel_count);`
`359`	`356`	`}`
`360`	`357`	`}`
`361`	`358`