From 83af30d7c0b0934772ad44ca6f38a94a675f7765 Mon Sep 17 00:00:00 2001 From: Matiq Date: Wed, 19 Aug 2026 22:00:22 +0300 Subject: [PATCH] P0: track twin.cpp/twin.hpp/rotor_kernel.hpp (were shadowed by .gitignore) --- dsp/rotor_kernel.hpp | 134 +++++++++++++++++++++++++++++++++++++++++++ dsp/twin.cpp | 115 +++++++++++++++++++++++++++++++++++++ dsp/twin.hpp | 49 ++++++++++++++++ 3 files changed, 298 insertions(+) create mode 100644 dsp/rotor_kernel.hpp create mode 100644 dsp/twin.cpp create mode 100644 dsp/twin.hpp diff --git a/dsp/rotor_kernel.hpp b/dsp/rotor_kernel.hpp new file mode 100644 index 0000000..10b2e44 --- /dev/null +++ b/dsp/rotor_kernel.hpp @@ -0,0 +1,134 @@ +#pragma once + +// Bit-exact transcription of the sincospi "rotor" kernel 0x180184900 (fast path), +// per-element scalar form, mirroring the exact VEX/FMA op order of big_184900.dis +// (main loop 0x180184a80-0x1801850d4 / masked tail 0x180185160-0x18018530c). +// +// Per input float x the kernel emits two floats: +// bufA[i] = sin(x) (buf1=rdx/arg at kernel entry) +// bufB[i] = cos(x) (buf2=r8 /arg) +// (verified: tail loop stores sin->[r11+rbx]=bufA via ymm5, cos->[r11+r12]=bufB via ymm2; +// main loop stores the 4 lane-group results ymm{7,6,4,1}->bufA, ymm{8,5,3,2}->bufB). +// +// The 8-lane AVX body interleaves four independent groups; all ops are per-lane +// (no cross-lane shuffle), so the scalar loop below reproduces each element exactly. +// +// The slow path (0x180186086, entered when any lane |x| >= 10000 or non-finite) and the +// inf/nan guard helper 0x180189d60 are NOT reproduced: for audio |x| <= 1 they never run. +// Contract: inputs must satisfy |x| < 10000 (checked in rotor_sincos via nan guard). + +#include +#include +#include + +namespace detkernel { + +// exact fused multiply-add with a single rounding (no re-contraction by the compiler) +static inline float fma_f(float a, float b, float c) { + return __builtin_fmaf(a, b, c); +} + +static inline uint32_t bit_cast_u32(float f) { + uint32_t u; + std::memcpy(&u, &f, sizeof(u)); + return u; +} + +static inline float bit_cast_f(uint32_t u) { + float f; + std::memcpy(&f, &u, sizeof(f)); + return f; +} + +// magic-rounding trick constants (from tables 0x181d1b3x0 / 0x181d1b0c0-0x181d1b300) +static constexpr uint32_t C_ABS_MASK = 0x7fffffffu; // 0x181d1ad40 +static constexpr float C_INV_PI = 0.318309873f; // 0x181d1b340 4b400000? no: 3ea2f983 +static constexpr float C_MAGIC = 12582912.0f; // 0x181d1b380 0x4b400000 +static constexpr float C_PI_HI = 3.141592741f; // 0x181d1b0c0 0x40490fdb +static constexpr float C_PI_LO = -8.742277658e-08f; // 0x181d1b100 0xb3bbbd2e +static constexpr float C_RED_CORR = -3.430249024e-15f; // 0x181d1b140 0xa7772ced (C0) +static constexpr float C_SIN_1 = 0.00833306462f; // 0x181d1b280 0x3c088768 (C1) +static constexpr float C_SIN_2 = -1.980916067e-04f; // 0x181d1b2c0 0xb94fb6cf (C2) +static constexpr float C_SIN_3 = 2.604164590e-06f; // 0x181d1b300 0x362ec335 (C3) +static constexpr float C_SIN_N6 = -0.166666612f; // 0x181d1b180 0xbe2aaaa7 +static constexpr uint32_t C_HALF_BITS = 0x3f000000u; // 0.5f 0x181d1b400 +static constexpr float C_ONE = 1.0f; // 0x181d1b440 +static constexpr float C_NOISE = 10000.0f; // 0x181d1ad80 (slow-path threshold) +static constexpr float C_MASK_DEF = 0.75f; // 0x181d1c280 (filled default lanes) + +// rotor_sincos(x, &sin, &cos) per 0x180184900 (elementwise, exact op order) +static inline void rotor_sincos(float x, float& s, float& c) { + uint32_t ux; + std::memcpy(&ux, &x, sizeof(ux)); + + // vmovups/ymm load + vandps abs mask + vfmadd231ps + uint32_t ua = ux & C_ABS_MASK; + float a; + std::memcpy(&a, &ua, sizeof(a)); + + float t = fma_f(a, C_INV_PI, C_MAGIC); // t = magic + a*(1/pi) + float k = t - C_MAGIC; // vsubps k = t - magic + uint32_t tk; + std::memcpy(&tk, &t, sizeof(tk)); + uint32_t sbt = tk & 1u; // vpslld t,31 -> bit0 (parity/round) + + float p = fma_f(-C_PI_HI, k, a); // vfnmadd231 p = a - pi_hi*k + p = fma_f(-C_PI_LO, k, p); // p -= pi_lo*k + uint32_t up; + std::memcpy(&up, &p, sizeof(up)); + uint32_t sbp = up & 0x80000000u; // vandps -0.0 -> sign bit of p + + uint32_t halfbits = sbp ^ C_HALF_BITS; // vxorps (-0.0&p) ^ 0.5 -> +-0.5 + float h; + std::memcpy(&h, &halfbits, sizeof(h)); + float q = k + h; // vaddps q = k +- 0.5 + + // vfnmadd213ps dest,src1,src2 = -(dest*src1) + src2 + float cc = fma_f(C_RED_CORR, -k, p); // cc = p - C0*k (sin arg core) + float rq = fma_f(-C_PI_HI, q, a); // rq = a - pi_hi*q + rq = fma_f(-C_PI_LO, q, rq); + float cq = fma_f(C_RED_CORR, -q, rq); // cq = rq - C0*q (cos arg core) + + uint32_t sbth = sbt ? 0x80000000u : 0u; // vpslld fully left -> 0x80000000/0 + uint32_t us1 = bit_cast_u32(cc) ^ sbth; // arg_sin = cc ^ sbt + // vxorps(-0.0,sbp)=0x80000000^sbp, then ^sbt -> arg_cos = cq ^ sbt ^ sbp ^ 0x80000000 + uint32_t us2 = bit_cast_u32(cq) ^ sbth ^ sbp ^ 0x80000000u; + + float sarg = bit_cast_f(us1); + float qarg = bit_cast_f(us2); + + float ss = sarg * sarg; // vmulps + float qs = qarg * qarg; + + float as = fma_f(C_SIN_3, ss, C_SIN_2); // Horner (vfmadd231 then vfmadd213 chain) + float aq = fma_f(C_SIN_3, qs, C_SIN_2); + as = fma_f(as, ss, C_SIN_1); + aq = fma_f(aq, qs, C_SIN_1); + as = fma_f(as, ss, C_SIN_N6); + aq = fma_f(aq, qs, C_SIN_N6); + + float os = ss * as; // vmulps + float oq = qs * aq; + float sins = fma_f(os, sarg, sarg); // vfmadd213 sin = os*sarg + sarg + float sinq = fma_f(oq, qarg, qarg); + + uint32_t usrc = ux & 0x80000000u; // vandnps ~abs & src -> sign bit of src + s = bit_cast_f(bit_cast_u32(sins) ^ usrc); // vxorps sin ^ sign(src) + + bool zero = (ux == 0u) || (ux == 0x80000000u); // vcmpeqps src == signbit(src) + c = zero ? C_ONE : sinq; // vblendvps -> 1.0 for zero lanes +} + +// 8-lane rotor; mirrors the AVX main loop over 32-float chunks implicitly (loop of 8) +// and supports the masked tail (partial) via `count`. When count < pushed, the masked +// lanes of the group are handled by the caller with bait; here we simply clamp. +static inline void rotor_batch(const float* src, float* sin_out, float* cos_out, size_t n) { + for (size_t i = 0; i < n; ++i) { + float si, co; + rotor_sincos(src[i], si, co); + sin_out[i] = si; + cos_out[i] = co; + } +} + +} // namespace detkernel \ No newline at end of file diff --git a/dsp/twin.cpp b/dsp/twin.cpp new file mode 100644 index 0000000..5fa6333 --- /dev/null +++ b/dsp/twin.cpp @@ -0,0 +1,115 @@ +#include "twin.hpp" + +#include +#include +#include + +#include "rotor_kernel.hpp" + +namespace detkernel { + +// --------------------------------------------------------------------------- +// Generator FUN_180533ec0 (double), then cvtpd2ps packing as in the twin prologue. +// --------------------------------------------------------------------------- +twin_coeff build_twin_coeff(double fs_total, double freq, double q, float sens_lin) { + float f1 = std::sqrt(sens_lin); + if (f1 <= 0.0f) f1 = 0.0f; + const double f1d = static_cast(f1); + + const double w0 = (freq < 2.0 ? 2.0 : freq) * 6.283185307179586 / fs_total; + const double s = std::sin(w0); + const double c2 = std::cos(w0) * -2.0; + const double p = (s * 0.5) / q; + + const double a0 = 1.0 + p * f1d; + const double a2 = 1.0 - p * f1d; + const double b0 = 1.0 + p / f1d; + const double b2 = 1.0 - p / f1d; + + twin_coeff c; + c.A[0] = static_cast(a0); + c.A[1] = static_cast(c2); + c.A[2] = static_cast(a2); + c.B[0] = static_cast(b0); + c.B[1] = static_cast(c2); + c.B[2] = static_cast(b2); + return c; +} + +// --------------------------------------------------------------------------- +// 0x181a77520: complex division num/den = (num*conj(den)) * ref +// per-lane: shufps-0x88/0xdd -> |den|^2; mulps bith; rcpps + 1 Newton step. +// --------------------------------------------------------------------------- +void cplx_div_exact(const cplxf& num, const cplxf& den, cplxf& out) { + const float ar = den.re, ai = den.im; + const float br = num.re, bi = num.im; + + const float ar2 = ar * ar; + const float ai2 = ai * ai; + const float den2 = ar2 + ai2; + + const float re = ar * br + ai * bi; // Re{num*conj(den)} + const float im = ar * bi - ai * br; // Im{num*conj(den)} + + if (den2 == 0.0f) { + const float qnan = 0.0f / 0.0f; + out.re = qnan; + out.im = qnan; + return; + } + + const float r0 = _mm_cvtss_f32(_mm_rcp_ss(_mm_set_ss(den2))); + const float t1 = den2 * r0; + const float t2 = 2.0f - t1; + const float ref = r0 * t2; + + out.re = re * ref; + out.im = im * ref; +} + +// --------------------------------------------------------------------------- +// 0x18000ad60 scalar complex multiply (body @0x18000ae00): +// t0=bi*ai; t1=bi*ar; re=fma(br,ar,-t0); im=fma(br,ai,+t1) +// --------------------------------------------------------------------------- +void cplx_mul_exact(const cplxf& a, const cplxf& b, cplxf& out) { + const float ar = a.re, ai = a.im; + const float br = b.re, bi = b.im; + + const float t0 = bi * ai; // mulps + const float t1 = bi * ar; // mulps + out.re = fma_f(br, ar, -t0); // vfmaddsub213ps lane0 (subtract) + out.im = fma_f(br, ai, t1); // lane1 (add) +} + +// --------------------------------------------------------------------------- +// 0x180535880 hot loop: seed A/B into acc, 2 Horner FMA stages, cplx-div, x2. +// `z` carries exp(+i*theta_k); the twin conjugates before use (0x1800018b0). +// --------------------------------------------------------------------------- +void twin_apply(const twin_coeff& c, const cplxf* z, size_t n, cplxf* out) { + for (size_t i = 0; i < n; ++i) { + // conj(z1) = conj(exp(+i*theta)) (0x1800018b0 negates imag) + const cplxf z1 = { z[i].re, -z[i].im }; + cplxf z2; + cplx_mul_exact(z1, z1, z2); // conj(z1)^2 + + // A accumulator (vfmadd213ss per scalar lane) + cplxf accA = { c.A[0], 0.0f }; + accA.re = fma_f(c.A[1], z1.re, accA.re); + accA.im = fma_f(c.A[1], z1.im, accA.im); + accA.re = fma_f(c.A[2], z2.re, accA.re); + accA.im = fma_f(c.A[2], z2.im, accA.im); + + // B accumulator + cplxf accB = { c.B[0], 0.0f }; + accB.re = fma_f(c.B[1], z1.re, accB.re); + accB.im = fma_f(c.B[1], z1.im, accB.im); + accB.re = fma_f(c.B[2], z2.re, accB.re); + accB.im = fma_f(c.B[2], z2.im, accB.im); + + cplx_div_exact(accB, accA, out[i]); // B/A + out[i].re *= 2.0f; // 2*B/A (caller-side scale, exact) + out[i].im *= 2.0f; + } +} + +} // namespace detkernel \ No newline at end of file diff --git a/dsp/twin.hpp b/dsp/twin.hpp new file mode 100644 index 0000000..a5b8d1e --- /dev/null +++ b/dsp/twin.hpp @@ -0,0 +1,49 @@ +#pragma once +#include +#include + +// Bit-exact transcription of the soothe2 "twin" resonance filter 0x180535880 +// (float sibling of the resonator 2nd-order detector core, NLS .sdk plugin). +// +// Pipeline per complex bin (all float32, op-order faithful to the disassembly): +// z1 = conj(e^{i*theta}) (rotor -> interleave, then 0x1800018b0 conj) +// z2 = cplx_mul(z1, z1) (0x18000ad60 scalar body @0x18000ae00) +// accA = A0; accA = fma(A1, z1, accA); accA = fma(A2, z2, accA) (vfmadd213ss) +// accB = B0; accB = fma(B1, z1, accB); accB = fma(B2, z2, accB) +// out = 2 * cplx_div(accB, accA) (0x181a77520: rcpps + 1 Newton step) +// +// Coefficients come from FUN_180533ec0 (double pipeline) + cvtpd2ps packing +// as done in the twin prologue (stack slots +0x28..+0x40). +// +// Constants locked in Phase 1: +// A0=B0=1+d, A1=B1=-2*cos(w0), A2=1-d, B2=1-d2, +// d = p*sqrtf(param_5), d2 = p/sqrtf(param_5), +// p = sin(w0)*0.5/Q, w0 = max(freq,2.0)*2*pi/fs_total. +// param_5 = 10^(sens_stored/20) with sens_stored~=24.65 dB (host-scaled ~=2*XML 12.0). + +namespace detkernel { + +struct cplxf { + float re, im; +}; + +struct twin_coeff { + float A[3]; // A0,A1,A2 (float32 after cvtpd2ps) + float B[3]; // B0,B1,B2 +}; + +// FUN_180533ec0 coefficients, packed to float32 like the twin prologue. +// sens_lin = param_5 (linear, before sqrtf) e.g. 10^(24.65/20). +twin_coeff build_twin_coeff(double fs_total, double freq, double q, float sens_lin); + +// 0x181a77520 complex division, scalar form (rcpps + Newton, NaN guard). +void cplx_div_exact(const cplxf& num, const cplxf& den, cplxf& out); + +// 0x18000ad60 scalar complex multiply (vfmaddsub213ps form). +void cplx_mul_exact(const cplxf& a, const cplxf& b, cplxf& out); + +// Full twin evaluation for `n` bins. `z` carries unit-magnitude twiddles +// exp(+i*theta_k) (the function applies the conjugate itself). +void twin_apply(const twin_coeff& c, const cplxf* z, size_t n, cplxf* out); + +} // namespace detkernel \ No newline at end of file