5 files changed, 201 insertions, 52 deletions

diff --git a/Ryujinx.Audio/Renderer/Dsp/Command/UpsampleCommand.cs b/Ryujinx.Audio/Renderer/Dsp/Command/UpsampleCommand.cs
index 1617a6421..0870d59ce 100644
--- a/Ryujinx.Audio/Renderer/Dsp/Command/UpsampleCommand.cs
+++ b/Ryujinx.Audio/Renderer/Dsp/Command/UpsampleCommand.cs
@@ -40,6 +40,12 @@ namespace Ryujinx.Audio.Renderer.Dsp.Command
                 info.InputBufferIndices[i] = (ushort)(bufferOffset + inputBufferOffset[i]);
             }
 
+            if (info.BufferStates?.Length != (int)inputCount)
+            {
+                // Keep state if possible.
+                info.BufferStates = new UpsamplerBufferState[(int)inputCount];
+            }
+
             UpsamplerInfo = info;
         }
 
@@ -50,8 +56,6 @@ namespace Ryujinx.Audio.Renderer.Dsp.Command
 
         public void Process(CommandList context)
         {
-            float ratio = (float)InputSampleRate / Constants.TargetSampleRate;
-
             uint bufferCount = Math.Min(BufferCount, UpsamplerInfo.SourceSampleCount);
 
             for (int i = 0; i < bufferCount; i++)
@@ -59,9 +63,7 @@ namespace Ryujinx.Audio.Renderer.Dsp.Command
                 Span<float> inputBuffer = context.GetBuffer(UpsamplerInfo.InputBufferIndices[i]);
                 Span<float> outputBuffer = GetBuffer(UpsamplerInfo.InputBufferIndices[i], (int)UpsamplerInfo.SampleCount);
 
-                float fraction = 0.0f;
-
-                ResamplerHelper.ResampleForUpsampler(outputBuffer, inputBuffer, ratio, ref fraction, (int)(InputSampleCount / ratio));
+                UpsamplerHelper.Upsample(outputBuffer, inputBuffer, (int)UpsamplerInfo.SampleCount, (int)InputSampleCount, ref UpsamplerInfo.BufferStates[i]);
             }
         }
     }
diff --git a/Ryujinx.Audio/Renderer/Dsp/ResamplerHelper.cs b/Ryujinx.Audio/Renderer/Dsp/ResamplerHelper.cs
index 4de2e078a..b46a33fe0 100644
--- a/Ryujinx.Audio/Renderer/Dsp/ResamplerHelper.cs
+++ b/Ryujinx.Audio/Renderer/Dsp/ResamplerHelper.cs
@@ -579,52 +579,5 @@ namespace Ryujinx.Audio.Renderer.Dsp
                 fraction -= (int)fraction;
             }
         }
-
-        [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ResampleForUpsampler(Span<float> outputBuffer, ReadOnlySpan<float> inputBuffer, float ratio, ref float fraction, int sampleCount)
-        {
-            // Currently a simple cubic interpolation, assuming duplicated values at edges.
-            // TODO: Discover and use algorithm that the switch uses.
-
-            int inputBufferIndex = 0;
-            int maxIndex = inputBuffer.Length - 1;
-            int cubicEnd = inputBuffer.Length - 3;
-
-            for (int i = 0; i < sampleCount; i++)
-            {
-                float s0, s1, s2, s3;
-
-                s1 = inputBuffer[inputBufferIndex];
-
-                if (inputBufferIndex == 0 || inputBufferIndex > cubicEnd)
-                {
-                    // Clamp interplation values at the ends of the input buffer.
-                    s0 = inputBuffer[Math.Max(0, inputBufferIndex - 1)];
-                    s2 = inputBuffer[Math.Min(maxIndex, inputBufferIndex + 1)];
-                    s3 = inputBuffer[Math.Min(maxIndex, inputBufferIndex + 2)];
-                }
-                else
-                {
-                    s0 = inputBuffer[inputBufferIndex - 1];
-                    s2 = inputBuffer[inputBufferIndex + 1];
-                    s3 = inputBuffer[inputBufferIndex + 2];
-                }
-
-                float a = s3 - s2 - s0 + s1;
-                float b = s0 - s1 - a;
-                float c = s2 - s0;
-                float d = s1;
-
-                float f2 = fraction * fraction;
-                float f3 = f2 * fraction;
-
-                outputBuffer[i] = a * f3 + b * f2 + c * fraction + d;
-
-                fraction += ratio;
-                inputBufferIndex += (int)MathF.Truncate(fraction);
-
-                fraction -= (int)fraction;
-            }
-        }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx.Audio/Renderer/Dsp/UpsamplerHelper.cs b/Ryujinx.Audio/Renderer/Dsp/UpsamplerHelper.cs
new file mode 100644
index 000000000..847acec2e
--- /dev/null
+++ b/Ryujinx.Audio/Renderer/Dsp/UpsamplerHelper.cs
@@ -0,0 +1,175 @@
+using Ryujinx.Audio.Renderer.Server.Upsampler;
+using Ryujinx.Common.Memory;
+using System;
+using System.Diagnostics;
+using System.Runtime.CompilerServices;
+
+namespace Ryujinx.Audio.Renderer.Dsp
+{
+    public class UpsamplerHelper
+    {
+        private const int HistoryLength = UpsamplerBufferState.HistoryLength;
+        private const int FilterBankLength = 20;
+        // Bank0 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
+        private const int Bank0CenterIndex = 9;
+        private static readonly Array20<float> Bank1 = PrecomputeFilterBank(1.0f / 6.0f);
+        private static readonly Array20<float> Bank2 = PrecomputeFilterBank(2.0f / 6.0f);
+        private static readonly Array20<float> Bank3 = PrecomputeFilterBank(3.0f / 6.0f);
+        private static readonly Array20<float> Bank4 = PrecomputeFilterBank(4.0f / 6.0f);
+        private static readonly Array20<float> Bank5 = PrecomputeFilterBank(5.0f / 6.0f);
+
+        private static Array20<float> PrecomputeFilterBank(float offset)
+        {
+            float Sinc(float x)
+            {
+                if (x == 0)
+                {
+                    return 1.0f;
+                }
+                return (MathF.Sin(MathF.PI * x) / (MathF.PI * x));
+            }
+
+            float BlackmanWindow(float x)
+            {
+                const float a = 0.18f;
+                const float a0 = 0.5f - 0.5f * a;
+                const float a1 = -0.5f;
+                const float a2 = 0.5f * a;
+                return a0 + a1 * MathF.Cos(2 * MathF.PI * x) + a2 * MathF.Cos(4 * MathF.PI * x);
+            }
+            
+            Array20<float> result = new Array20<float>();
+
+            for (int i = 0; i < FilterBankLength; i++)
+            {
+                float x = (Bank0CenterIndex - i) + offset;
+                result[i] = Sinc(x) * BlackmanWindow(x / FilterBankLength + 0.5f);
+            }
+
+            return result;
+        }
+
+        // Polyphase upsampling algorithm
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static void Upsample(Span<float> outputBuffer, ReadOnlySpan<float> inputBuffer, int outputSampleCount, int inputSampleCount, ref UpsamplerBufferState state)
+        {
+            if (!state.Initialized)
+            {
+                state.Scale = inputSampleCount switch
+                {
+                    40  => 6.0f,
+                    80  => 3.0f,
+                    160 => 1.5f,
+                    _   => throw new ArgumentOutOfRangeException()
+                };
+                state.Initialized = true;
+            }
+
+            if (outputSampleCount == 0)
+            {
+                return;
+            }
+
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            float DoFilterBank(ref UpsamplerBufferState state, in Array20<float> bank)
+            {
+                float result = 0.0f;
+
+                Debug.Assert(state.History.Length == HistoryLength);
+                Debug.Assert(bank.Length == FilterBankLength);
+                for (int j = 0; j < FilterBankLength; j++)
+                {
+                    result += bank[j] * state.History[j];
+                }
+
+                return result;
+            }
+
+            [MethodImpl(MethodImplOptions.AggressiveInlining)]
+            void NextInput(ref UpsamplerBufferState state, float input)
+            {
+                state.History.AsSpan().Slice(1).CopyTo(state.History.AsSpan());
+                state.History[HistoryLength - 1] = input;
+            }
+
+            int inputBufferIndex = 0;
+
+            switch (state.Scale)
+            { 
+                case 6.0f:
+                    for (int i = 0; i < outputSampleCount; i++)
+                    {
+                        switch (state.Phase)
+                        {
+                            case 0:
+                                NextInput(ref state, inputBuffer[inputBufferIndex++]);
+                                outputBuffer[i] = state.History[Bank0CenterIndex];
+                                break;
+                            case 1:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank1);
+                                break;
+                            case 2:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank2);
+                                break;
+                            case 3:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank3);
+                                break;
+                            case 4:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank4);
+                                break;
+                            case 5:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank5);
+                                break;
+                        }
+
+                        state.Phase = (state.Phase + 1) % 6;
+                    }
+                    break;
+                case 3.0f:
+                    for (int i = 0; i < outputSampleCount; i++)
+                    {
+                        switch (state.Phase)
+                        {
+                            case 0:
+                                NextInput(ref state, inputBuffer[inputBufferIndex++]);
+                                outputBuffer[i] = state.History[Bank0CenterIndex];
+                                break;
+                            case 1:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank2);
+                                break;
+                            case 2:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank4);
+                                break;
+                        }
+
+                        state.Phase = (state.Phase + 1) % 3;
+                    }
+                    break;
+                case 1.5f:
+                    // Upsample by 3 then decimate by 2.
+                    for (int i = 0; i < outputSampleCount; i++)
+                    {
+                        switch (state.Phase)
+                        {
+                            case 0:
+                                NextInput(ref state, inputBuffer[inputBufferIndex++]);
+                                outputBuffer[i] = state.History[Bank0CenterIndex];
+                                break;
+                            case 1:
+                                outputBuffer[i] = DoFilterBank(ref state, Bank4);
+                                break;
+                            case 2:
+                                NextInput(ref state, inputBuffer[inputBufferIndex++]);
+                                outputBuffer[i] = DoFilterBank(ref state, Bank2);
+                                break;
+                        }
+
+                        state.Phase = (state.Phase + 1) % 3;
+                    }
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException();
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerBufferState.cs b/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerBufferState.cs
new file mode 100644
index 000000000..a45fa8e5b
--- /dev/null
+++ b/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerBufferState.cs
@@ -0,0 +1,14 @@
+using Ryujinx.Common.Memory;
+
+namespace Ryujinx.Audio.Renderer.Server.Upsampler
+{
+    public struct UpsamplerBufferState
+    {
+        public const int HistoryLength = 20;
+
+        public float Scale;
+        public Array20<float> History;
+        public bool Initialized;
+        public int Phase;
+    }
+}
\ No newline at end of file
diff --git a/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerState.cs b/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerState.cs
index 065e4838c..e508f35b4 100644
--- a/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerState.cs
+++ b/Ryujinx.Audio/Renderer/Server/Upsampler/UpsamplerState.cs
@@ -38,6 +38,11 @@ namespace Ryujinx.Audio.Renderer.Server.Upsampler
         public ushort[] InputBufferIndices;
 
         /// <summary>
+        /// State of each input buffer index kept across invocations of the upsampler.
+        /// </summary>
+        public UpsamplerBufferState[] BufferStates;
+
+        /// <summary>
         /// Create a new <see cref="UpsamplerState"/>.
         /// </summary>
         /// <param name="manager">The upsampler manager.</param>