From c7a75ee2fc3f06492753e05623b1308d11b3f504 Mon Sep 17 00:00:00 2001
From: Jean Pierre Cimalando <jp-dev@inbox.ru>
Date: Mon, 1 Feb 2021 00:53:21 +0100
Subject: [PATCH] Accelerate windowed sinc SSE2

---
 src/sfizz/Interpolators.hpp | 21 ++++++++++++---------
 src/sfizz/WindowedSinc.h    | 13 +++++++++++--
 src/sfizz/WindowedSinc.hpp  | 30 ++++++++++++++++++++++++++++++
 3 files changed, 53 insertions(+), 11 deletions(-)

diff --git a/src/sfizz/Interpolators.hpp b/src/sfizz/Interpolators.hpp
index 9cef3d7d..a75c9608 100644
--- a/src/sfizz/Interpolators.hpp
+++ b/src/sfizz/Interpolators.hpp
@@ -190,7 +190,7 @@ class SincInterpolator;
 
 //------------------------------------------------------------------------------
 // Windowed sinc any order, SSE specialization
-#if SFIZZ_HAVE_SSE
+#if SFIZZ_HAVE_SSE2
 template <size_t Points>
 class SincInterpolator<float, Points>
 {
@@ -201,15 +201,18 @@ public:
     {
         const auto &ws = *SincInterpolatorTraits<Points>::windowedSinc;
 
-        int j0 = 1 - int(Points) / 2;
+        constexpr int j0 = 1 - int(Points) / 2;
+        float x0 = j0 - coeff;
 
-        __m128 h[Points / 4];
-        for (int i = 0; i < int(Points); ++i)
-            reinterpret_cast<float*>(h)[i] = ws.getUnchecked(j0 - coeff + i);
-
-        __m128 y = _mm_mul_ps(h[0], _mm_loadu_ps(&values[j0]));
-        for (int i = 1; i < int(Points / 4); ++i)
-            y = _mm_add_ps(y, _mm_mul_ps(h[i], _mm_loadu_ps(&values[j0 + 4 * i])));
+        __m128 y = _mm_set1_ps(0.0f);
+        __m128 x = _mm_add_ps(_mm_set1_ps(x0), _mm_setr_ps(0, 1, 2, 3));
+        size_t i = 0;
+        do {
+            __m128 h = ws.getUncheckedX4(x);
+            y = _mm_add_ps(y, _mm_mul_ps(h, _mm_loadu_ps(&values[j0 + i])));
+            x = _mm_add_ps(x, _mm_set1_ps(4.0f));
+            i += 4;
+        } while (i < Points);
 
         // sum 4 to 1
         __m128 xmm0 = y;
diff --git a/src/sfizz/WindowedSinc.h b/src/sfizz/WindowedSinc.h
index 94a8a9de..b8beb582 100644
--- a/src/sfizz/WindowedSinc.h
+++ b/src/sfizz/WindowedSinc.h
@@ -5,8 +5,12 @@
 // If not, contact the sfizz maintainers at https://github.com/sfztools/sfizz
 
 #pragma once
+#include "SIMDConfig.h"
 #include <absl/types/span.h>
 #include <memory>
+#if SFIZZ_HAVE_SSE2
+#include <xmmintrin.h>
+#endif
 
 namespace sfz {
 
@@ -26,6 +30,11 @@ public:
     // interpolate f(x), where x must be in domain [-Points/2:+Points/2]
     float getUnchecked(float x) const noexcept;
 
+#if SFIZZ_HAVE_SSE2
+    // interpolate f(x), 4 values at once
+    __m128 getUncheckedX4(__m128 x) const noexcept;
+#endif
+
     // calculate exact f(x), where x must be in domain [-Points/2:+Points/2]
     double getExact(double x) const noexcept;
 
@@ -35,8 +44,8 @@ public:
 protected:
     void fillTable() noexcept;
 
-    // allows interpolating f(Points/2), provided for safety
-    enum { TableExtra = 1 };
+    // allows interpolating f(Points/2), and SSE, provided for safety
+    enum { TableExtra = 4 };
 
 private:
     double beta_ {};
diff --git a/src/sfizz/WindowedSinc.hpp b/src/sfizz/WindowedSinc.hpp
index 7908b5aa..1bd4f19c 100644
--- a/src/sfizz/WindowedSinc.hpp
+++ b/src/sfizz/WindowedSinc.hpp
@@ -36,6 +36,36 @@ inline float AbstractWindowedSinc<T>::getUnchecked(float x) const noexcept
     return y0 + mu * dy;
 }
 
+#if SFIZZ_HAVE_SSE2
+template <class T>
+inline __m128 AbstractWindowedSinc<T>::getUncheckedX4(__m128 x) const noexcept
+{
+    const float* table = static_cast<const T*>(this)->getTablePointer();
+    size_t points = static_cast<const T*>(this)->getNumPoints();
+    size_t tableSize = static_cast<const T*>(this)->getTableSize();
+
+    __m128 ix = _mm_mul_ps(
+        _mm_add_ps(x, _mm_set1_ps(points / 2.0f)),
+        _mm_set1_ps((tableSize - 1) / points));
+    alignas(__m128i) int j0[4];
+    __m128i i0 = _mm_cvttps_epi32(ix);
+    _mm_store_si128((__m128i*)j0, i0);
+    __m128 mu = _mm_sub_ps(ix, _mm_cvtepi32_ps(i0));
+
+    // reference: Interpolated table lookups using SSE2 [2/2]
+    // https://rawstudio.org/blog/?p=482
+    __m128 p0p1 = _mm_castsi128_ps(_mm_loadl_epi64((__m128i*)&table[j0[0]]));
+    __m128 p2p3 = _mm_castsi128_ps(_mm_loadl_epi64((__m128i*)&table[j0[2]]));
+    p0p1 = _mm_loadh_pi(p0p1, (__m64*)&table[j0[1]]);
+    p2p3 = _mm_loadh_pi(p2p3, (__m64*)&table[j0[3]]);
+    __m128 y0 = _mm_shuffle_ps(p0p1, p2p3, _MM_SHUFFLE(2, 0, 2, 0));
+    __m128 y1 = _mm_shuffle_ps(p0p1, p2p3, _MM_SHUFFLE(3, 1, 3, 1));
+
+    __m128 dy = _mm_sub_ps(y1, y0);
+    return _mm_add_ps(y0, _mm_mul_ps(mu, dy));
+}
+#endif
+
 template <class T>
 inline double AbstractWindowedSinc<T>::getExact(double x) const noexcept
 {