Combine some duplicate code to mix each channel

1 files changed, 75 insertions, 136 deletions

diff --git a/core/mixer/mixer_sse.cpp b/core/mixer/mixer_sse.cpp
index 89968fc8..702f9bd4 100644
--- a/core/mixer/mixer_sse.cpp
+++ b/core/mixer/mixer_sse.cpp
@@ -73,6 +73,77 @@ inline void ApplyCoeffs(float2 *RESTRICT Values, const size_t IrSize, const Cons
     }
 }
 
+inline void MixLine(const al::span<const float> InSamples, float *RESTRICT dst, float &CurrentGain,
+    const float TargetGain, const float delta, const size_t min_len, const size_t aligned_len,
+    size_t Counter)
+{
+    float gain{CurrentGain};
+    const float step{(TargetGain-gain) * delta};
+
+    size_t pos{0};
+    if(!(std::abs(step) > std::numeric_limits<float>::epsilon()))
+        gain = TargetGain;
+    else
+    {
+        float step_count{0.0f};
+        /* Mix with applying gain steps in aligned multiples of 4. */
+        if(size_t todo{min_len >> 2})
+        {
+            const __m128 four4{_mm_set1_ps(4.0f)};
+            const __m128 step4{_mm_set1_ps(step)};
+            const __m128 gain4{_mm_set1_ps(gain)};
+            __m128 step_count4{_mm_setr_ps(0.0f, 1.0f, 2.0f, 3.0f)};
+            do {
+                const __m128 val4{_mm_load_ps(&InSamples[pos])};
+                __m128 dry4{_mm_load_ps(&dst[pos])};
+
+                /* dry += val * (gain + step*step_count) */
+                dry4 = MLA4(dry4, val4, MLA4(gain4, step4, step_count4));
+
+                _mm_store_ps(&dst[pos], dry4);
+                step_count4 = _mm_add_ps(step_count4, four4);
+                pos += 4;
+            } while(--todo);
+            /* NOTE: step_count4 now represents the next four counts after the
+             * last four mixed samples, so the lowest element represents the
+             * next step count to apply.
+             */
+            step_count = _mm_cvtss_f32(step_count4);
+        }
+        /* Mix with applying left over gain steps that aren't aligned multiples of 4. */
+        for(size_t leftover{min_len&3};leftover;++pos,--leftover)
+        {
+            dst[pos] += InSamples[pos] * (gain + step*step_count);
+            step_count += 1.0f;
+        }
+        if(pos == Counter)
+            gain = TargetGain;
+        else
+            gain += step*step_count;
+
+        /* Mix until pos is aligned with 4 or the mix is done. */
+        for(size_t leftover{aligned_len&3};leftover;++pos,--leftover)
+            dst[pos] += InSamples[pos] * gain;
+    }
+    CurrentGain = gain;
+
+    if(!(std::abs(gain) > GainSilenceThreshold))
+        return;
+    if(size_t todo{(InSamples.size()-pos) >> 2})
+    {
+        const __m128 gain4{_mm_set1_ps(gain)};
+        do {
+            const __m128 val4{_mm_load_ps(&InSamples[pos])};
+            __m128 dry4{_mm_load_ps(&dst[pos])};
+            dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4));
+            _mm_store_ps(&dst[pos], dry4);
+            pos += 4;
+        } while(--todo);
+    }
+    for(size_t leftover{(InSamples.size()-pos)&3};leftover;++pos,--leftover)
+        dst[pos] += InSamples[pos] * gain;
+}
+
 } // namespace
 
 template<>
@@ -199,76 +270,8 @@ void Mix_<SSETag>(const al::span<const float> InSamples, const al::span<FloatBuf
     const auto aligned_len = minz((min_len+3) & ~size_t{3}, InSamples.size()) - min_len;
 
     for(FloatBufferLine &output : OutBuffer)
-    {
-        float *RESTRICT dst{al::assume_aligned<16>(output.data()+OutPos)};
-        float gain{*CurrentGains};
-        const float step{(*TargetGains-gain) * delta};
-
-        size_t pos{0};
-        if(!(std::abs(step) > std::numeric_limits<float>::epsilon()))
-            gain = *TargetGains;
-        else
-        {
-            float step_count{0.0f};
-            /* Mix with applying gain steps in aligned multiples of 4. */
-            if(size_t todo{min_len >> 2})
-            {
-                const __m128 four4{_mm_set1_ps(4.0f)};
-                const __m128 step4{_mm_set1_ps(step)};
-                const __m128 gain4{_mm_set1_ps(gain)};
-                __m128 step_count4{_mm_setr_ps(0.0f, 1.0f, 2.0f, 3.0f)};
-                do {
-                    const __m128 val4{_mm_load_ps(&InSamples[pos])};
-                    __m128 dry4{_mm_load_ps(&dst[pos])};
-
-                    /* dry += val * (gain + step*step_count) */
-                    dry4 = MLA4(dry4, val4, MLA4(gain4, step4, step_count4));
-
-                    _mm_store_ps(&dst[pos], dry4);
-                    step_count4 = _mm_add_ps(step_count4, four4);
-                    pos += 4;
-                } while(--todo);
-                /* NOTE: step_count4 now represents the next four counts after
-                 * the last four mixed samples, so the lowest element
-                 * represents the next step count to apply.
-                 */
-                step_count = _mm_cvtss_f32(step_count4);
-            }
-            /* Mix with applying left over gain steps that aren't aligned multiples of 4. */
-            for(size_t leftover{min_len&3};leftover;++pos,--leftover)
-            {
-                dst[pos] += InSamples[pos] * (gain + step*step_count);
-                step_count += 1.0f;
-            }
-            if(pos == Counter)
-                gain = *TargetGains;
-            else
-                gain += step*step_count;
-
-            /* Mix until pos is aligned with 4 or the mix is done. */
-            for(size_t leftover{aligned_len&3};leftover;++pos,--leftover)
-                dst[pos] += InSamples[pos] * gain;
-        }
-        *CurrentGains = gain;
-        ++CurrentGains;
-        ++TargetGains;
-
-        if(!(std::abs(gain) > GainSilenceThreshold))
-            continue;
-        if(size_t todo{(InSamples.size()-pos) >> 2})
-        {
-            const __m128 gain4{_mm_set1_ps(gain)};
-            do {
-                const __m128 val4{_mm_load_ps(&InSamples[pos])};
-                __m128 dry4{_mm_load_ps(&dst[pos])};
-                dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4));
-                _mm_store_ps(&dst[pos], dry4);
-                pos += 4;
-            } while(--todo);
-        }
-        for(size_t leftover{(InSamples.size()-pos)&3};leftover;++pos,--leftover)
-            dst[pos] += InSamples[pos] * gain;
-    }
+        MixLine(InSamples, al::assume_aligned<16>(output.data()+OutPos), *CurrentGains++,
+            *TargetGains++, delta, min_len, aligned_len, Counter);
 }
 
 template<>
@@ -279,70 +282,6 @@ void Mix_<SSETag>(const al::span<const float> InSamples, float *OutBuffer, float
     const auto min_len = minz(Counter, InSamples.size());
     const auto aligned_len = minz((min_len+3) & ~size_t{3}, InSamples.size()) - min_len;
 
-    float *RESTRICT dst{al::assume_aligned<16>(OutBuffer)};
-    float gain{CurrentGain};
-    const float step{(TargetGain-gain) * delta};
-
-    size_t pos{0};
-    if(!(std::abs(step) > std::numeric_limits<float>::epsilon()))
-        gain = TargetGain;
-    else
-    {
-        float step_count{0.0f};
-        /* Mix with applying gain steps in aligned multiples of 4. */
-        if(size_t todo{min_len >> 2})
-        {
-            const __m128 four4{_mm_set1_ps(4.0f)};
-            const __m128 step4{_mm_set1_ps(step)};
-            const __m128 gain4{_mm_set1_ps(gain)};
-            __m128 step_count4{_mm_setr_ps(0.0f, 1.0f, 2.0f, 3.0f)};
-            do {
-                const __m128 val4{_mm_load_ps(&InSamples[pos])};
-                __m128 dry4{_mm_load_ps(&dst[pos])};
-
-                /* dry += val * (gain + step*step_count) */
-                dry4 = MLA4(dry4, val4, MLA4(gain4, step4, step_count4));
-
-                _mm_store_ps(&dst[pos], dry4);
-                step_count4 = _mm_add_ps(step_count4, four4);
-                pos += 4;
-            } while(--todo);
-            /* NOTE: step_count4 now represents the next four counts after the
-             * last four mixed samples, so the lowest element represents the
-             * next step count to apply.
-             */
-            step_count = _mm_cvtss_f32(step_count4);
-        }
-        /* Mix with applying left over gain steps that aren't aligned multiples of 4. */
-        for(size_t leftover{min_len&3};leftover;++pos,--leftover)
-        {
-            dst[pos] += InSamples[pos] * (gain + step*step_count);
-            step_count += 1.0f;
-        }
-        if(pos == Counter)
-            gain = TargetGain;
-        else
-            gain += step*step_count;
-
-        /* Mix until pos is aligned with 4 or the mix is done. */
-        for(size_t leftover{aligned_len&3};leftover;++pos,--leftover)
-            dst[pos] += InSamples[pos] * gain;
-    }
-    CurrentGain = gain;
-
-    if(!(std::abs(gain) > GainSilenceThreshold))
-        return;
-    if(size_t todo{(InSamples.size()-pos) >> 2})
-    {
-        const __m128 gain4{_mm_set1_ps(gain)};
-        do {
-            const __m128 val4{_mm_load_ps(&InSamples[pos])};
-            __m128 dry4{_mm_load_ps(&dst[pos])};
-            dry4 = _mm_add_ps(dry4, _mm_mul_ps(val4, gain4));
-            _mm_store_ps(&dst[pos], dry4);
-            pos += 4;
-        } while(--todo);
-    }
-    for(size_t leftover{(InSamples.size()-pos)&3};leftover;++pos,--leftover)
-        dst[pos] += InSamples[pos] * gain;
+    MixLine(InSamples, al::assume_aligned<16>(OutBuffer), CurrentGain, TargetGain, delta, min_len,
+        aligned_len, Counter);
 }