Fir2

fir2.c

@@ -0,0 +1,69 @@
+#ifndef FIR_FILTER_H
+#define FIR_FILTER_H
+
+#include <stdint.h>
+#include <string.h>
+
+/* This needs to be a power of two for the bitmask optimizations to work */
+/* If a non power of two length is desired, change to modulo */
+#define FIR_LEN (1u << 5)
+_Static_assert((FIR_LEN & (FIR_LEN - 1u)) == 0, "FIR_LEN must be a power of two");
+
+typedef struct {
+    uint_fast8_t idx;
+    float        buf[FIR_LEN];
+    const float *coeffs;
+} FIRFilter;
+
+void  FIRFilter_init(FIRFilter *fir, const float *coef_array);
+float FIRFilter_update(FIRFilter *fir, float inp);
+
+#endif // FIR_FILTER_H
+
+// #include "fir_filter.h"
+
+void FIRFilter_init(FIRFilter *fir, const float *coeffs) {
+    memset(fir, 0, sizeof(FIRFilter));
+    fir->coeffs = coeffs;
+}
+
+float FIRFilter_update(FIRFilter *fir, float inp) {
+    float out = 0.f;
+    fir->buf[fir->idx] = inp;
+    fir->idx = (fir->idx + 1) & (FIR_LEN - 1);
+
+    // Compute the latest output sample by applying a convolution
+    uint8_t sum_idx = fir->idx;
+
+#ifndef CONFIG_FIR_UNROLL
+
+    for (size_t i = 0; i < FIR_LEN; i++) {
+        sum_idx = (sum_idx - 1) & (FIR_LEN - 1);
+        out += fir->coeffs[i] * fir->buf[sum_idx];
+    }
+
+#else // CONFIG_FIR_UNROLL
+
+    int          count = FIR_LEN / 8;
+    int          leftover = FIR_LEN % 8;
+    const float *c = fir->coeffs;
+
+    switch (leftover) {
+    case 0:
+        do {
+            sum_idx = (sum_idx - 1) & (FIR_LEN - 1);
+            out += *c++ * fir->buf[sum_idx];
+        case 7: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        case 6: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        case 5: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        case 4: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        case 3: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        case 2: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        case 1: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx];
+        } while (--count > 0);
+    }
+
+#endif // CONFIG_FIR_UNROLL
+
+    return out;
+}