P0: track twin.cpp/twin.hpp/rotor_kernel.hpp (were shadowed by .gitignore)

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 <cstdint>
+#include <cstring>
+#include <cmath>
+
+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

dsp/twin.cpp

@@ -0,0 +1,115 @@
+#include "twin.hpp"
+
+#include <cmath>
+#include <cstring>
+#include <xmmintrin.h>
+
+#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<double>(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<float>(a0);
+    c.A[1] = static_cast<float>(c2);
+    c.A[2] = static_cast<float>(a2);
+    c.B[0] = static_cast<float>(b0);
+    c.B[1] = static_cast<float>(c2);
+    c.B[2] = static_cast<float>(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

dsp/twin.hpp

@@ -0,0 +1,49 @@
+#pragma once
+#include <cstddef>
+#include <cstdint>
+
+// 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