// SafeC Standard Library — General FIR/IIR filter implementation (see filter.h)
#pragma once
#include <std/dsp/dsp.h>
#include <std/dsp/filter.h>
#include <std/dsp/fixed.h>
namespace std {
// ── FIR ──────────────────────────────────────────────────────────────────────
inline struct FirFilter FirFilter::new([]const double coeffs, []double history) {
struct FirFilter f;
unsafe { f.coeffs = (const &double)coeffs.ptr; f.history = (&double)history.ptr; }
f.numTaps = coeffs.len < history.len ? coeffs.len : history.len;
f.pos = 0UL;
return f;
}
double FirFilter::process(double x) {
unsafe { self.history[self.pos] = x; }
// y = sum_k coeffs[k] * history[(pos - k) mod numTaps], as two runs
// without the modulo: k = 0..pos walks history down from pos, then
// k = pos+1..numTaps-1 walks it down from the end. Four partial sums
// keep the multiply-adds independent.
double s0 = 0.0; double s1 = 0.0; double s2 = 0.0; double s3 = 0.0;
unsafe {
const double* c = (const double*)self.coeffs;
const double* h = (const double*)self.history;
unsigned long k = 0UL;
unsigned long last = self.pos; // first run: k <= pos
while (k + 4UL <= last + 1UL) {
s0 = __builtin_fmuladd(c[k], h[self.pos - k], s0);
s1 = __builtin_fmuladd(c[k + 1UL], h[self.pos - k - 1UL], s1);
s2 = __builtin_fmuladd(c[k + 2UL], h[self.pos - k - 2UL], s2);
s3 = __builtin_fmuladd(c[k + 3UL], h[self.pos - k - 3UL], s3);
k = k + 4UL;
}
while (k <= last) { s0 = __builtin_fmuladd(c[k], h[self.pos - k], s0); k = k + 1UL; }
unsigned long top = self.pos + self.numTaps; // second run: index top - k
while (k + 4UL <= self.numTaps) {
s0 = __builtin_fmuladd(c[k], h[top - k], s0);
s1 = __builtin_fmuladd(c[k + 1UL], h[top - k - 1UL], s1);
s2 = __builtin_fmuladd(c[k + 2UL], h[top - k - 2UL], s2);
s3 = __builtin_fmuladd(c[k + 3UL], h[top - k - 3UL], s3);
k = k + 4UL;
}
while (k < self.numTaps) { s0 = __builtin_fmuladd(c[k], h[top - k], s0); k = k + 1UL; }
}
self.pos = self.pos + 1UL == self.numTaps ? 0UL : self.pos + 1UL;
return (s0 + s1) + (s2 + s3);
}
void FirFilter::reset() {
unsigned long i = 0UL;
while (i < self.numTaps) {
unsafe { self.history[i] = 0.0; }
i = i + 1UL;
}
self.pos = 0UL;
}
// Blocks go through a small window on the stack -- the last numTaps - 1
// inputs in time order, then up to FIR_CHUNK new ones -- so each output is
// a dot product with the reversed taps, many outputs per vector
// (dsp_sliding_dot_f64)
// instead of a walk around the ring. No allocation, so still real-time
// safe. Filters longer than FIR_STACK_TAPS run sample by sample.
#ifdef __SAFEC_FREESTANDING__
#define FIR_STACK_TAPS 64UL
#define FIR_CHUNK 64UL
#else
#define FIR_STACK_TAPS 256UL
#define FIR_CHUNK 512UL
#endif
void FirFilter::process_block([]const double in_s, []double out_s) {
unsigned long n = in_s.len < out_s.len ? in_s.len : out_s.len;
if (n == 0UL) return;
const &double in;
&double out;
unsafe { in = (const &double)in_s.ptr; out = (&double)out_s.ptr; }
unsigned long T = self.numTaps;
if (T == 0UL || T > FIR_STACK_TAPS) {
filter_process_each_n_(self, in, out, n);
return;
}
double crev[FIR_STACK_TAPS];
double win[FIR_STACK_TAPS + FIR_CHUNK];
unsafe {
unsigned long t = 0UL;
while (t < T) { crev[t] = self.coeffs[T - 1UL - t]; t = t + 1UL; }
// win[0 .. T-1) = x[n-(T-1)] .. x[n-1], from the ring (the sample at
// ring index pos - j is x[n-j]).
unsigned long j = 0UL;
while (j + 1UL < T) {
unsigned long back = T - 1UL - j; // x[n - back]
win[j] = self.history[(self.pos + T - back) % T];
j = j + 1UL;
}
unsigned long done = 0UL;
while (done < n) {
unsigned long len = n - done < FIR_CHUNK ? n - done : FIR_CHUNK;
unsigned long q = 0UL;
while (q < len) { win[T - 1UL + q] = in[done + q]; q = q + 1UL; }
dsp_sliding_dot_f64_n_((const double*)&crev[0], (const double*)&win[0], &out[done], len, T);
// keep the newest T - 1 samples at the front
q = 0UL;
while (q + 1UL < T) { win[q] = win[len + q]; q = q + 1UL; }
done = done + len;
}
// Back into the ring with pos = 0: x[n-j] at index T - j.
j = 1UL;
while (j < T) { self.history[T - j] = win[T - 1UL - j]; j = j + 1UL; }
}
self.pos = 0UL;
}
// The float FIR: the same code on floats (dsp_dot_f32 for blocks).
inline struct FirFilter32 FirFilter32::new([]const float coeffs, []float history) {
struct FirFilter32 f;
unsafe { f.coeffs = (const &float)coeffs.ptr; f.history = (&float)history.ptr; }
f.numTaps = coeffs.len < history.len ? coeffs.len : history.len;
f.pos = 0UL;
return f;
}
float FirFilter32::process(float x) {
unsafe { self.history[self.pos] = x; }
// y = sum_k coeffs[k] * history[(pos - k) mod numTaps], as two runs
// without the modulo: k = 0..pos walks history down from pos, then
// k = pos+1..numTaps-1 walks it down from the end. Four partial sums
// keep the multiply-adds independent.
float s0 = 0.0; float s1 = 0.0; float s2 = 0.0; float s3 = 0.0;
unsafe {
const float* c = (const float*)self.coeffs;
const float* h = (const float*)self.history;
unsigned long k = 0UL;
unsigned long last = self.pos; // first run: k <= pos
while (k + 4UL <= last + 1UL) {
s0 = __builtin_fmuladd(c[k], h[self.pos - k], s0);
s1 = __builtin_fmuladd(c[k + 1UL], h[self.pos - k - 1UL], s1);
s2 = __builtin_fmuladd(c[k + 2UL], h[self.pos - k - 2UL], s2);
s3 = __builtin_fmuladd(c[k + 3UL], h[self.pos - k - 3UL], s3);
k = k + 4UL;
}
while (k <= last) { s0 = __builtin_fmuladd(c[k], h[self.pos - k], s0); k = k + 1UL; }
unsigned long top = self.pos + self.numTaps; // second run: index top - k
while (k + 4UL <= self.numTaps) {
s0 = __builtin_fmuladd(c[k], h[top - k], s0);
s1 = __builtin_fmuladd(c[k + 1UL], h[top - k - 1UL], s1);
s2 = __builtin_fmuladd(c[k + 2UL], h[top - k - 2UL], s2);
s3 = __builtin_fmuladd(c[k + 3UL], h[top - k - 3UL], s3);
k = k + 4UL;
}
while (k < self.numTaps) { s0 = __builtin_fmuladd(c[k], h[top - k], s0); k = k + 1UL; }
}
self.pos = self.pos + 1UL == self.numTaps ? 0UL : self.pos + 1UL;
return (s0 + s1) + (s2 + s3);
}
void FirFilter32::reset() {
unsigned long i = 0UL;
while (i < self.numTaps) {
unsafe { self.history[i] = 0.0; }
i = i + 1UL;
}
self.pos = 0UL;
}
void FirFilter32::process_block([]const float in_s, []float out_s) {
unsigned long n = in_s.len < out_s.len ? in_s.len : out_s.len;
if (n == 0UL) return;
const &float in;
&float out;
unsafe { in = (const &float)in_s.ptr; out = (&float)out_s.ptr; }
unsigned long T = self.numTaps;
if (T == 0UL || T > FIR_STACK_TAPS) {
filter_process_each_n_(self, in, out, n);
return;
}
float crev[FIR_STACK_TAPS];
float win[FIR_STACK_TAPS + FIR_CHUNK];
unsafe {
unsigned long t = 0UL;
while (t < T) { crev[t] = self.coeffs[T - 1UL - t]; t = t + 1UL; }
// win[0 .. T-1) = x[n-(T-1)] .. x[n-1], from the ring (the sample at
// ring index pos - j is x[n-j]).
unsigned long j = 0UL;
while (j + 1UL < T) {
unsigned long back = T - 1UL - j; // x[n - back]
win[j] = self.history[(self.pos + T - back) % T];
j = j + 1UL;
}
unsigned long done = 0UL;
while (done < n) {
unsigned long len = n - done < FIR_CHUNK ? n - done : FIR_CHUNK;
unsigned long q = 0UL;
while (q < len) { win[T - 1UL + q] = in[done + q]; q = q + 1UL; }
dsp_sliding_dot_f32_n_((const float*)&crev[0], (const float*)&win[0], &out[done], len, T);
// keep the newest T - 1 samples at the front
q = 0UL;
while (q + 1UL < T) { win[q] = win[len + q]; q = q + 1UL; }
done = done + len;
}
// Back into the ring with pos = 0: x[n-j] at index T - j.
j = 1UL;
while (j < T) { self.history[T - j] = win[T - 1UL - j]; j = j + 1UL; }
}
self.pos = 0UL;
}
inline struct FFirFilter FFirFilter::new([]const Fixed coeffs, []Fixed history) {
struct FFirFilter f;
unsafe { f.coeffs = (const &Fixed)coeffs.ptr; f.history = (&Fixed)history.ptr; }
f.numTaps = coeffs.len < history.len ? coeffs.len : history.len;
f.pos = 0UL;
return f;
}
Fixed FFirFilter::process(Fixed x) {
unsafe { self.history[self.pos] = x; }
Fixed y = (Fixed)0;
unsigned long k = 0UL;
while (k < self.numTaps) {
unsigned long idx = (self.pos + self.numTaps - k) % self.numTaps;
unsafe { y = fixed_add(y, fixed_mul(self.coeffs[k], self.history[idx])); }
k = k + 1UL;
}
self.pos = (self.pos + 1UL) % self.numTaps;
return y;
}
void FFirFilter::reset() {
unsigned long i = 0UL;
while (i < self.numTaps) {
unsafe { self.history[i] = (Fixed)0; }
i = i + 1UL;
}
self.pos = 0UL;
}
void FFirFilter::process_block([]const Fixed in_s, []Fixed out_s) {
unsigned long n = in_s.len < out_s.len ? in_s.len : out_s.len;
if (n == 0UL) return;
const &Fixed in;
&Fixed out;
unsafe { in = (const &Fixed)in_s.ptr; out = (&Fixed)out_s.ptr; }
filter_process_each_n_(self, in, out, n);
}
// ── IIR (general order, direct form I) ────────────────────────────────────────
inline struct IirFilter IirFilter::new([]const double b, []const double a,
[]double xHistory, []double yHistory) {
struct IirFilter f;
unsafe {
f.b = (const &double)b.ptr; f.a = (const &double)a.ptr;
f.xHistory = (&double)xHistory.ptr; f.yHistory = (&double)yHistory.ptr;
}
f.numB = b.len < xHistory.len ? b.len : xHistory.len;
f.numA = a.len < yHistory.len ? a.len : yHistory.len;
f.xPos = 0UL;
f.yPos = 0UL;
return f;
}
double IirFilter::process(double x) {
unsafe { self.xHistory[self.xPos] = x; }
double ffwd = 0.0;
unsigned long k = 0UL;
while (k < self.numB) {
unsigned long idx = (self.xPos + self.numB - k) % self.numB;
unsafe { ffwd = ffwd + self.b[k] * self.xHistory[idx]; }
k = k + 1UL;
}
double fback = 0.0;
k = 1UL;
while (k < self.numA) {
unsigned long idx = (self.yPos + self.numA - k) % self.numA;
unsafe { fback = fback + self.a[k] * self.yHistory[idx]; }
k = k + 1UL;
}
double y = ffwd - fback;
unsafe { self.yHistory[self.yPos] = y; }
self.xPos = (self.xPos + 1UL) % self.numB;
self.yPos = (self.yPos + 1UL) % self.numA;
return y;
}
void IirFilter::reset() {
unsigned long i = 0UL;
while (i < self.numB) { unsafe { self.xHistory[i] = 0.0; } i = i + 1UL; }
i = 0UL;
while (i < self.numA) { unsafe { self.yHistory[i] = 0.0; } i = i + 1UL; }
self.xPos = 0UL;
self.yPos = 0UL;
}
void IirFilter::process_block([]const double in_s, []double out_s) {
unsigned long n = in_s.len < out_s.len ? in_s.len : out_s.len;
if (n == 0UL) return;
const &double in;
&double out;
unsafe { in = (const &double)in_s.ptr; out = (&double)out_s.ptr; }
filter_process_each_n_(self, in, out, n);
}
// The float IIR (high orders lose precision in float: prefer Biquad32
// sections for those).
inline struct IirFilter32 IirFilter32::new([]const float b, []const float a,
[]float xHistory, []float yHistory) {
struct IirFilter32 f;
unsafe {
f.b = (const &float)b.ptr; f.a = (const &float)a.ptr;
f.xHistory = (&float)xHistory.ptr; f.yHistory = (&float)yHistory.ptr;
}
f.numB = b.len < xHistory.len ? b.len : xHistory.len;
f.numA = a.len < yHistory.len ? a.len : yHistory.len;
f.xPos = 0UL;
f.yPos = 0UL;
return f;
}
float IirFilter32::process(float x) {
unsafe { self.xHistory[self.xPos] = x; }
float ffwd = (float)0.0;
unsigned long k = 0UL;
while (k < self.numB) {
unsigned long idx = (self.xPos + self.numB - k) % self.numB;
unsafe { ffwd = ffwd + self.b[k] * self.xHistory[idx]; }
k = k + 1UL;
}
float fback = (float)0.0;
k = 1UL;
while (k < self.numA) {
unsigned long idx = (self.yPos + self.numA - k) % self.numA;
unsafe { fback = fback + self.a[k] * self.yHistory[idx]; }
k = k + 1UL;
}
float y = ffwd - fback;
unsafe { self.yHistory[self.yPos] = y; }
self.xPos = (self.xPos + 1UL) % self.numB;
self.yPos = (self.yPos + 1UL) % self.numA;
return y;
}
void IirFilter32::reset() {
unsigned long i = 0UL;
while (i < self.numB) { unsafe { self.xHistory[i] = (float)0.0; } i = i + 1UL; }
i = 0UL;
while (i < self.numA) { unsafe { self.yHistory[i] = (float)0.0; } i = i + 1UL; }
self.xPos = 0UL;
self.yPos = 0UL;
}
void IirFilter32::process_block([]const float in_s, []float out_s) {
unsigned long n = in_s.len < out_s.len ? in_s.len : out_s.len;
if (n == 0UL) return;
const &float in;
&float out;
unsafe { in = (const &float)in_s.ptr; out = (&float)out_s.ptr; }
filter_process_each_n_(self, in, out, n);
}
inline struct FIirFilter FIirFilter::new([]const Fixed b, []const Fixed a,
[]Fixed xHistory, []Fixed yHistory) {
struct FIirFilter f;
unsafe {
f.b = (const &Fixed)b.ptr; f.a = (const &Fixed)a.ptr;
f.xHistory = (&Fixed)xHistory.ptr; f.yHistory = (&Fixed)yHistory.ptr;
}
f.numB = b.len < xHistory.len ? b.len : xHistory.len;
f.numA = a.len < yHistory.len ? a.len : yHistory.len;
f.xPos = 0UL;
f.yPos = 0UL;
return f;
}
Fixed FIirFilter::process(Fixed x) {
unsafe { self.xHistory[self.xPos] = x; }
Fixed ffwd = (Fixed)0;
unsigned long k = 0UL;
while (k < self.numB) {
unsigned long idx = (self.xPos + self.numB - k) % self.numB;
unsafe { ffwd = fixed_add(ffwd, fixed_mul(self.b[k], self.xHistory[idx])); }
k = k + 1UL;
}
Fixed fback = (Fixed)0;
k = 1UL;
while (k < self.numA) {
unsigned long idx = (self.yPos + self.numA - k) % self.numA;
unsafe { fback = fixed_add(fback, fixed_mul(self.a[k], self.yHistory[idx])); }
k = k + 1UL;
}
Fixed y = fixed_sub(ffwd, fback);
unsafe { self.yHistory[self.yPos] = y; }
self.xPos = (self.xPos + 1UL) % self.numB;
self.yPos = (self.yPos + 1UL) % self.numA;
return y;
}
void FIirFilter::reset() {
unsigned long i = 0UL;
while (i < self.numB) { unsafe { self.xHistory[i] = (Fixed)0; } i = i + 1UL; }
i = 0UL;
while (i < self.numA) { unsafe { self.yHistory[i] = (Fixed)0; } i = i + 1UL; }
self.xPos = 0UL;
self.yPos = 0UL;
}
void FIirFilter::process_block([]const Fixed in_s, []Fixed out_s) {
unsigned long n = in_s.len < out_s.len ? in_s.len : out_s.len;
if (n == 0UL) return;
const &Fixed in;
&Fixed out;
unsafe { in = (const &Fixed)in_s.ptr; out = (&Fixed)out_s.ptr; }
filter_process_each_n_(self, in, out, n);
}
} // namespace std