// safeguard: link macos blas (vDSP_conv, weak: Accelerate unless [build] blas says otherwise)
#pragma once
#include <std/dsp/dsp.h>
#include <std/simd/simd.h>
#include <std/simd/cpu.h>
// ── dsp_dot ──────────────────────────────────────────────────────────────────
// Accumulate fixed-point dot product.
namespace std {
Fixed dsp_dot_n_(const &Fixed a, const &Fixed b, unsigned long n) {
Fixed acc = (Fixed)0;
unsigned long i = 0UL;
while (i < n) {
unsafe {
const Fixed* pa = (const Fixed*)a;
const Fixed* pb = (const Fixed*)b;
acc = fixed_add(acc, fixed_mul(pa[i], pb[i]));
}
i = i + 1UL;
}
return acc;
}
// ── dsp_scale ─────────────────────────────────────────────────────────────────
// Multiply every element by scale in-place.
void dsp_scale_n_(&Fixed a, Fixed scale, unsigned long n) {
unsigned long i = 0UL;
while (i < n) {
unsafe {
const Fixed* pa = (const Fixed*)a;
pa[i] = fixed_mul(pa[i], scale);
}
i = i + 1UL;
}
}
// ── dsp_add ───────────────────────────────────────────────────────────────────
// Element-wise addition: out[i] = a[i] + b[i].
void dsp_add_n_(const &Fixed a, const &Fixed b, &Fixed out, unsigned long n) {
unsigned long i = 0UL;
while (i < n) {
unsafe {
const Fixed* pa = (const Fixed*)a;
const Fixed* pb = (const Fixed*)b;
Fixed* po = (Fixed*)out;
po[i] = fixed_add(pa[i], pb[i]);
}
i = i + 1UL;
}
}
// ── dsp_clip ──────────────────────────────────────────────────────────────────
// Clamp every element to [lo, hi].
void dsp_clip_n_(&Fixed a, Fixed lo, Fixed hi, unsigned long n) {
unsigned long i = 0UL;
while (i < n) {
Fixed v;
unsafe {
const Fixed* pa = (const Fixed*)a;
v = pa[i];
}
if ((int)v < (int)lo) {
v = lo;
} else if ((int)v > (int)hi) {
v = hi;
}
unsafe {
const Fixed* pa = (const Fixed*)a;
pa[i] = v;
}
i = i + 1UL;
}
}
// ── dsp_peak ──────────────────────────────────────────────────────────────────
// Return maximum absolute value in array.
Fixed dsp_peak_n_(const &Fixed a, unsigned long n) {
Fixed peak = (Fixed)0;
unsigned long i = 0UL;
while (i < n) {
Fixed v;
unsafe {
const Fixed* pa = (const Fixed*)a;
v = pa[i];
}
Fixed av = fixed_abs(v);
if ((int)av > (int)peak) {
peak = av;
}
i = i + 1UL;
}
return peak;
}
// ── dsp_rms ───────────────────────────────────────────────────────────────────
// Root mean square: sqrt( sum(x[i]^2) / n ).
Fixed dsp_rms_n_(&Fixed a, unsigned long n) {
if (n == 0UL) {
return (Fixed)0;
}
Fixed sum = (Fixed)0;
unsigned long i = 0UL;
while (i < n) {
Fixed v;
unsafe {
const Fixed* pa = (const Fixed*)a;
v = pa[i];
}
sum = fixed_add(sum, fixed_mul(v, v));
i = i + 1UL;
}
// Divide by n.
Fixed inv_n = fixed_div((Fixed)FIXED_ONE, fixed_from_int((int)n));
Fixed mean_sq = fixed_mul(sum, inv_n);
return fixed_sqrt(mean_sq);
}
// ── dsp_dot_f64 ──────────────────────────────────────────────────────────────
// SIMD FMA dot product (see dsp.h for why this stays float64-only).
static double dsp_dot_body_(const double* a, const double* b, unsigned long n) __attribute__((always_inline)) {
// Two independent f64x4 accumulators hide the multiply-add latency.
f64x4 acc0;
f64x4 acc1;
unsigned long i = 0UL;
unsafe {
while (i + 8UL <= n) {
acc0 = __builtin_fmuladd(*(const f64x4*)&a[i], *(const f64x4*)&b[i], acc0);
acc1 = __builtin_fmuladd(*(const f64x4*)&a[i + 4UL], *(const f64x4*)&b[i + 4UL], acc1);
i = i + 8UL;
}
if (i + 4UL <= n) {
acc0 = __builtin_fmuladd(*(const f64x4*)&a[i], *(const f64x4*)&b[i], acc0);
i = i + 4UL;
}
}
f64x4 acc = acc0 + acc1;
double sum = (acc[0] + acc[1]) + (acc[2] + acc[3]);
while (i < n) {
unsafe { sum = sum + a[i] * b[i]; }
i = i + 1UL;
}
return sum;
}
// The same body per ISA level (std/simd/cpu.h): FMA from AVX2 on.
static double dsp_dot_base_(const double* a, const double* b, unsigned long n) { return dsp_dot_body_(a, b, n); }
#if defined(__x86_64__)
static double dsp_dot_avx_(const double* a, const double* b, unsigned long n)
__attribute__((target("avx"))) { return dsp_dot_body_(a, b, n); }
static double dsp_dot_avx2_(const double* a, const double* b, unsigned long n)
__attribute__((target("avx2,fma"))) { return dsp_dot_body_(a, b, n); }
#endif
double dsp_dot_f64_n_(const &double a_ref, const &double b_ref, unsigned long n) {
unsafe {
const double* a = (const double*)a_ref;
const double* b = (const double*)b_ref;
#if defined(__x86_64__)
int l = simd_level();
if (l >= SIMD_AVX2) return dsp_dot_avx2_(a, b, n);
if (l >= SIMD_AVX) return dsp_dot_avx_(a, b, n);
#endif
return dsp_dot_base_(a, b, n);
}
}
// ── dsp_dot_f32 ──────────────────────────────────────────────────────────────
// The float version: f32x8 accumulators, so twice the values per register.
static float dsp_dot32_body_(const float* a, const float* b, unsigned long n) __attribute__((always_inline)) {
f32x8 acc0;
f32x8 acc1;
unsigned long i = 0UL;
unsafe {
while (i + 16UL <= n) {
acc0 = __builtin_fmuladd(*(const f32x8*)&a[i], *(const f32x8*)&b[i], acc0);
acc1 = __builtin_fmuladd(*(const f32x8*)&a[i + 8UL], *(const f32x8*)&b[i + 8UL], acc1);
i = i + 16UL;
}
if (i + 8UL <= n) {
acc0 = __builtin_fmuladd(*(const f32x8*)&a[i], *(const f32x8*)&b[i], acc0);
i = i + 8UL;
}
}
f32x8 acc = acc0 + acc1;
float sum = ((acc[0] + acc[1]) + (acc[2] + acc[3])) + ((acc[4] + acc[5]) + (acc[6] + acc[7]));
while (i < n) {
unsafe { sum = __builtin_fmuladd(a[i], b[i], sum); }
i = i + 1UL;
}
return sum;
}
static float dsp_dot32_base_(const float* a, const float* b, unsigned long n) { return dsp_dot32_body_(a, b, n); }
#if defined(__x86_64__)
static float dsp_dot32_avx_(const float* a, const float* b, unsigned long n)
__attribute__((target("avx"))) { return dsp_dot32_body_(a, b, n); }
static float dsp_dot32_avx2_(const float* a, const float* b, unsigned long n)
__attribute__((target("avx2,fma"))) { return dsp_dot32_body_(a, b, n); }
#endif
float dsp_dot_f32_n_(const &float a_ref, const &float b_ref, unsigned long n) {
unsafe {
const float* a = (const float*)a_ref;
const float* b = (const float*)b_ref;
#if defined(__x86_64__)
int l = simd_level();
if (l >= SIMD_AVX2) return dsp_dot32_avx2_(a, b, n);
if (l >= SIMD_AVX) return dsp_dot32_avx_(a, b, n);
#endif
return dsp_dot32_base_(a, b, n);
}
}
// ── dsp_sliding_dot ──────────────────────────────────────────────────────────
// out[q] = sum_t a[t] * x[q + t] for q < nOut: 32 (double) or 64 (float)
// outputs at a time, each tap broadcast once and multiplied into four
// accumulator vectors of consecutive outputs. No horizontal sums, and one
// load per multiply-add instead of two; the leftovers use dsp_dot.
//
// On Apple targets a program that links Accelerate (safeguard's default
// there, see std/simd/gemm.h) hands the whole product to vDSP_conv /
// vDSP_convD instead: on an M1 Pro core they run a 127-tap filter over 2^20
// samples in 5.6 ms (double) / 3.3 ms (float) against 8.0 / 4.1 ms here,
// and keep that lead with 8 threads calling at once. This kernel already
// sits at the NEON ceiling there (about 34 GFLOPS in double whatever the
// accumulator count, and building the odd-offset windows with shuffles
// instead of loads gained nothing). The references are weak, as for
// cblas_sgemm: without Accelerate they are null (safeguard passes -U for
// them) and the kernel below runs.
#if defined(__APPLE__) && !defined(__SAFEC_FREESTANDING__)
#define DSP_VDSP 1
extern void vDSP_convD(const double* a, long ia, const double* f, long ifs, double* c, long ic,
unsigned long n, unsigned long p) __attribute__((weak));
extern void vDSP_conv(const float* a, long ia, const float* f, long ifs, float* c, long ic,
unsigned long n, unsigned long p) __attribute__((weak));
#else
#define DSP_VDSP 0
#endif
// Below this many multiply-adds the call costs more than it saves.
#define DSP_VDSP_MIN 4096UL
typedef vec<double, 8> dsp_f64x8_;
typedef vec<float, 16> dsp_f32x16_;
static dsp_f64x8_ dsp_splat8_(double v) __attribute__((always_inline)) {
dsp_f64x8_ r;
r[0] = v; r[1] = v; r[2] = v; r[3] = v; r[4] = v; r[5] = v; r[6] = v; r[7] = v;
return r;
}
static dsp_f32x16_ dsp_splat16_(float v) __attribute__((always_inline)) {
dsp_f32x16_ r;
r[0] = v; r[1] = v; r[2] = v; r[3] = v; r[4] = v; r[5] = v; r[6] = v; r[7] = v;
r[8] = v; r[9] = v; r[10] = v; r[11] = v; r[12] = v; r[13] = v; r[14] = v; r[15] = v;
return r;
}
static unsigned long dsp_sdot64_body_(const double* a, const double* x, double* out, unsigned long nOut, unsigned long n)
__attribute__((always_inline)) {
unsigned long q = 0UL;
unsafe {
while (q + 32UL <= nOut) {
const double* xp = &x[q];
dsp_f64x8_ s0 = dsp_splat8_(0.0);
dsp_f64x8_ s1 = s0;
dsp_f64x8_ s2 = s0;
dsp_f64x8_ s3 = s0;
unsigned long t = 0UL;
while (t < n) {
dsp_f64x8_ c = dsp_splat8_(a[t]);
s0 = __builtin_fmuladd(c, *(const dsp_f64x8_*)&xp[t], s0);
s1 = __builtin_fmuladd(c, *(const dsp_f64x8_*)&xp[t + 8UL], s1);
s2 = __builtin_fmuladd(c, *(const dsp_f64x8_*)&xp[t + 16UL], s2);
s3 = __builtin_fmuladd(c, *(const dsp_f64x8_*)&xp[t + 24UL], s3);
t = t + 1UL;
}
*(dsp_f64x8_*)&out[q] = s0;
*(dsp_f64x8_*)&out[q + 8UL] = s1;
*(dsp_f64x8_*)&out[q + 16UL] = s2;
*(dsp_f64x8_*)&out[q + 24UL] = s3;
q = q + 32UL;
}
}
return q;
}
static unsigned long dsp_sdot32_body_(const float* a, const float* x, float* out, unsigned long nOut, unsigned long n)
__attribute__((always_inline)) {
unsigned long q = 0UL;
unsafe {
while (q + 64UL <= nOut) {
const float* xp = &x[q];
dsp_f32x16_ s0 = dsp_splat16_((float)0.0);
dsp_f32x16_ s1 = s0;
dsp_f32x16_ s2 = s0;
dsp_f32x16_ s3 = s0;
unsigned long t = 0UL;
while (t < n) {
dsp_f32x16_ c = dsp_splat16_(a[t]);
s0 = __builtin_fmuladd(c, *(const dsp_f32x16_*)&xp[t], s0);
s1 = __builtin_fmuladd(c, *(const dsp_f32x16_*)&xp[t + 16UL], s1);
s2 = __builtin_fmuladd(c, *(const dsp_f32x16_*)&xp[t + 32UL], s2);
s3 = __builtin_fmuladd(c, *(const dsp_f32x16_*)&xp[t + 48UL], s3);
t = t + 1UL;
}
*(dsp_f32x16_*)&out[q] = s0;
*(dsp_f32x16_*)&out[q + 16UL] = s1;
*(dsp_f32x16_*)&out[q + 32UL] = s2;
*(dsp_f32x16_*)&out[q + 48UL] = s3;
q = q + 64UL;
}
}
return q;
}
static unsigned long dsp_sdot64_base_(const double* a, const double* x, double* out, unsigned long nOut, unsigned long n) {
return dsp_sdot64_body_(a, x, out, nOut, n);
}
static unsigned long dsp_sdot32_base_(const float* a, const float* x, float* out, unsigned long nOut, unsigned long n) {
return dsp_sdot32_body_(a, x, out, nOut, n);
}
#if defined(__x86_64__)
static unsigned long dsp_sdot64_avx2_(const double* a, const double* x, double* out, unsigned long nOut, unsigned long n)
__attribute__((target("avx2,fma"))) { return dsp_sdot64_body_(a, x, out, nOut, n); }
static unsigned long dsp_sdot64_avx512_(const double* a, const double* x, double* out, unsigned long nOut, unsigned long n)
__attribute__((target("avx512f,avx512vl,avx2,fma"))) { return dsp_sdot64_body_(a, x, out, nOut, n); }
static unsigned long dsp_sdot32_avx2_(const float* a, const float* x, float* out, unsigned long nOut, unsigned long n)
__attribute__((target("avx2,fma"))) { return dsp_sdot32_body_(a, x, out, nOut, n); }
static unsigned long dsp_sdot32_avx512_(const float* a, const float* x, float* out, unsigned long nOut, unsigned long n)
__attribute__((target("avx512f,avx512vl,avx2,fma"))) { return dsp_sdot32_body_(a, x, out, nOut, n); }
#endif
void dsp_sliding_dot_f64_n_(const &double a_ref, const &double x_ref, &double out_ref, unsigned long nOut, unsigned long n) {
unsafe {
const double* a = (const double*)a_ref;
const double* x = (const double*)x_ref;
double* out = (double*)out_ref;
unsigned long q = 0UL;
#if DSP_VDSP
// vDSP_convD with a positive filter stride is this correlation.
if ((unsigned long)vDSP_convD != 0UL && nOut * n >= DSP_VDSP_MIN) {
vDSP_convD(x, 1L, a, 1L, out, 1L, nOut, n);
return;
}
#endif
#if defined(__x86_64__)
int l = simd_level();
if (l >= SIMD_AVX512) q = dsp_sdot64_avx512_(a, x, out, nOut, n);
else if (l >= SIMD_AVX2) q = dsp_sdot64_avx2_(a, x, out, nOut, n);
else
#endif
q = dsp_sdot64_base_(a, x, out, nOut, n);
while (q < nOut) { out[q] = dsp_dot_f64_n_(a, &x[q], n); q = q + 1UL; }
}
}
void dsp_sliding_dot_f32_n_(const &float a_ref, const &float x_ref, &float out_ref, unsigned long nOut, unsigned long n) {
unsafe {
const float* a = (const float*)a_ref;
const float* x = (const float*)x_ref;
float* out = (float*)out_ref;
unsigned long q = 0UL;
#if DSP_VDSP
if ((unsigned long)vDSP_conv != 0UL && nOut * n >= DSP_VDSP_MIN) {
vDSP_conv(x, 1L, a, 1L, out, 1L, nOut, n);
return;
}
#endif
#if defined(__x86_64__)
int l = simd_level();
if (l >= SIMD_AVX512) q = dsp_sdot32_avx512_(a, x, out, nOut, n);
else if (l >= SIMD_AVX2) q = dsp_sdot32_avx2_(a, x, out, nOut, n);
else
#endif
q = dsp_sdot32_base_(a, x, out, nOut, n);
while (q < nOut) { out[q] = dsp_dot_f32_n_(a, &x[q], n); q = q + 1UL; }
}
}
// ── Slice API (the public signatures; the (reference, length) cores above
// are the *_n_ functions) ─────────────────────────────────────────────────────
Fixed dsp_dot([]const Fixed a, []const Fixed b) {
unsigned long n = a.len < b.len ? a.len : b.len;
if (n == 0UL) return (Fixed)0;
unsafe { return dsp_dot_n_((const &Fixed)a.ptr, (const &Fixed)b.ptr, n); }
}
void dsp_scale([]Fixed a, Fixed scale) {
if (a.len == 0UL) return;
unsafe { dsp_scale_n_((&Fixed)a.ptr, scale, a.len); }
}
unsigned long dsp_add([]const Fixed a, []const Fixed b, []Fixed out) {
unsigned long n = a.len < b.len ? a.len : b.len;
if (out.len < n) n = out.len;
if (n == 0UL) return 0UL;
unsafe { dsp_add_n_((const &Fixed)a.ptr, (const &Fixed)b.ptr, (&Fixed)out.ptr, n); }
return n;
}
void dsp_clip([]Fixed a, Fixed lo, Fixed hi) {
if (a.len == 0UL) return;
unsafe { dsp_clip_n_((&Fixed)a.ptr, lo, hi, a.len); }
}
Fixed dsp_peak([]const Fixed a) {
if (a.len == 0UL) return (Fixed)0;
unsafe { return dsp_peak_n_((const &Fixed)a.ptr, a.len); }
}
Fixed dsp_rms([]const Fixed a) {
if (a.len == 0UL) return (Fixed)0;
unsafe { return dsp_rms_n_((&Fixed)(const Fixed*)a.ptr, a.len); }
}
double dsp_dot_f64([]const double a, []const double b) {
unsigned long n = a.len < b.len ? a.len : b.len;
if (n == 0UL) return (double)0;
unsafe { return dsp_dot_f64_n_((const &double)a.ptr, (const &double)b.ptr, n); }
}
float dsp_dot_f32([]const float a, []const float b) {
unsigned long n = a.len < b.len ? a.len : b.len;
if (n == 0UL) return (float)0;
unsafe { return dsp_dot_f32_n_((const &float)a.ptr, (const &float)b.ptr, n); }
}
int dsp_sliding_dot_f64([]const double a, []const double x, []double out) {
if (a.len == 0UL || out.len == 0UL || x.len < out.len + a.len - 1UL) return 0;
unsafe { dsp_sliding_dot_f64_n_((const &double)a.ptr, (const &double)x.ptr, (&double)out.ptr, out.len, a.len); }
return 1;
}
int dsp_sliding_dot_f32([]const float a, []const float x, []float out) {
if (a.len == 0UL || out.len == 0UL || x.len < out.len + a.len - 1UL) return 0;
unsafe { dsp_sliding_dot_f32_n_((const &float)a.ptr, (const &float)x.ptr, (&float)out.ptr, out.len, a.len); }
return 1;
}
} // namespace std