Fix hwopus DecodeInterleaved implementation (#786)

* Fix hwopus DecodeInterleaved implementation

Also implement new variants of this api.

This should fix #763

* Sample rate shouldn't be hardcoded

This fix issues while opening Pokémon Let's Go pause menu.

* Apply Ac_K's suggestion about EndianSwap

* Address gdkchan's comment

* Address Ac_k's comment

1 files changed, 186 insertions, 35 deletions

diff --git a/Ryujinx.HLE/HOS/Services/Audio/HardwareOpusDecoderManager/IHardwareOpusDecoder.cs b/Ryujinx.HLE/HOS/Services/Audio/HardwareOpusDecoderManager/IHardwareOpusDecoder.cs
index e23398df..079f2ae7 100644
--- a/Ryujinx.HLE/HOS/Services/Audio/HardwareOpusDecoderManager/IHardwareOpusDecoder.cs
+++ b/Ryujinx.HLE/HOS/Services/Audio/HardwareOpusDecoderManager/IHardwareOpusDecoder.cs
@@ -1,13 +1,18 @@
+using Concentus;
+using Concentus.Enums;
 using Concentus.Structs;
+using Ryujinx.HLE.HOS.Services.Audio.Types;
+using System;
+using System.IO;
+using System.Runtime.InteropServices;
 
 namespace Ryujinx.HLE.HOS.Services.Audio.HardwareOpusDecoderManager
 {
     class IHardwareOpusDecoder : IpcService
     {
-        private const int FixedSampleRate = 48000;
-
-        private int _sampleRate;
-        private int _channelsCount;
+        private int  _sampleRate;
+        private int  _channelsCount;
+        private bool _reset;
 
         private OpusDecoder _decoder;
 
@@ -15,65 +20,211 @@ namespace Ryujinx.HLE.HOS.Services.Audio.HardwareOpusDecoderManager
         {
             _sampleRate    = sampleRate;
             _channelsCount = channelsCount;
+            _reset         = false;
 
-            _decoder = new OpusDecoder(FixedSampleRate, channelsCount);
+            _decoder = new OpusDecoder(sampleRate, channelsCount);
         }
 
-        [Command(0)]
-        // DecodeInterleaved(buffer<unknown, 5>) -> (u32, u32, buffer<unknown, 6>)
-        public ResultCode DecodeInterleaved(ServiceCtx context)
+        private ResultCode GetPacketNumSamples(out int numSamples, byte[] packet)
         {
-            long inPosition = context.Request.SendBuff[0].Position;
-            long inSize     = context.Request.SendBuff[0].Size;
+            int result = OpusPacketInfo.GetNumSamples(_decoder, packet, 0, packet.Length);
 
-            if (inSize < 8)
+            numSamples = result;
+
+            if (result == OpusError.OPUS_INVALID_PACKET)
+            {
+                return ResultCode.OpusInvalidInput;
+            }
+            else if (result == OpusError.OPUS_BAD_ARG)
             {
                 return ResultCode.OpusInvalidInput;
             }
 
-            long outPosition = context.Request.ReceiveBuff[0].Position;
-            long outSize     = context.Request.ReceiveBuff[0].Size;
+            return ResultCode.Success;
+        }
 
-            byte[] opusData = context.Memory.ReadBytes(inPosition, inSize);
+        private ResultCode DecodeInterleavedInternal(BinaryReader input, out short[] outPcmData, long outputSize, out uint outConsumed, out int outSamples)
+        {
+            outPcmData  = null;
+            outConsumed = 0;
+            outSamples  = 0;
 
-            int processed = ((opusData[0] << 24) |
-                             (opusData[1] << 16) |
-                             (opusData[2] << 8)  |
-                             (opusData[3] << 0)) + 8;
+            long streamSize = input.BaseStream.Length;
 
-            if ((uint)processed > (ulong)inSize)
+            if (streamSize < Marshal.SizeOf<OpusPacketHeader>())
             {
                 return ResultCode.OpusInvalidInput;
             }
 
-            short[] pcm = new short[outSize / 2];
+            OpusPacketHeader header = OpusPacketHeader.FromStream(input);
+
+            uint totalSize = header.length + (uint)Marshal.SizeOf<OpusPacketHeader>();
+
+            if (totalSize > streamSize)
+            {
+                return ResultCode.OpusInvalidInput;
+            }
 
-            int frameSize = pcm.Length / (_channelsCount * 2);
+            byte[] opusData = input.ReadBytes((int)header.length);
 
-            int samples = _decoder.Decode(opusData, 0, opusData.Length, pcm, 0, frameSize);
+            ResultCode result = GetPacketNumSamples(out int numSamples, opusData);
 
-            foreach (short sample in pcm)
+            if (result == ResultCode.Success)
             {
-                context.Memory.WriteInt16(outPosition, sample);
+                if ((uint)numSamples * (uint)_channelsCount * sizeof(short) > outputSize)
+                {
+                    return ResultCode.OpusInvalidInput;
+                }
+
+                outPcmData = new short[numSamples * _channelsCount];
+
+                if (_reset)
+                {
+                    _reset = false;
+
+                    _decoder.ResetState();
+                }
+
+                try
+                {
+                    outSamples  = _decoder.Decode(opusData, 0, opusData.Length, outPcmData, 0, outPcmData.Length / _channelsCount);
+                    outConsumed = totalSize;
+                }
+                catch (OpusException)
+                {
+                    // TODO: as OpusException doesn't provide us the exact error code, this is kind of inaccurate in some cases...
+                    return ResultCode.OpusInvalidInput;
+                }
+            }
+
+            return ResultCode.Success;
+        }
+
+        [Command(0)]
+        // DecodeInterleaved(buffer<unknown, 5>) -> (u32, u32, buffer<unknown, 6>)
+        public ResultCode DecodeInterleavedOriginal(ServiceCtx context)
+        {
+            ResultCode result;
 
-                outPosition += 2;
+            long inPosition     = context.Request.SendBuff[0].Position;
+            long inSize         = context.Request.SendBuff[0].Size;
+            long outputPosition = context.Request.ReceiveBuff[0].Position;
+            long outputSize     = context.Request.ReceiveBuff[0].Size;
+
+            using (BinaryReader inputStream = new BinaryReader(new MemoryStream(context.Memory.ReadBytes(inPosition, inSize))))
+            {
+                result = DecodeInterleavedInternal(inputStream, out short[] outPcmData, outputSize, out uint outConsumed, out int outSamples);
+
+                if (result == ResultCode.Success)
+                {
+                    byte[] pcmDataBytes = new byte[outPcmData.Length * sizeof(short)];
+                    Buffer.BlockCopy(outPcmData, 0, pcmDataBytes, 0, pcmDataBytes.Length);
+                    context.Memory.WriteBytes(outputPosition, pcmDataBytes);
+
+                    context.ResponseData.Write(outConsumed);
+                    context.ResponseData.Write(outSamples);
+                }
             }
 
-            context.ResponseData.Write(processed);
-            context.ResponseData.Write(samples);
+            return result;
+        }
 
-            return ResultCode.Success;
+        [Command(4)] // 6.0.0+
+        // DecodeInterleavedWithPerfOld(buffer<unknown, 5>) -> (u32, u32, u64, buffer<unknown, 0x46>)
+        public ResultCode DecodeInterleavedWithPerfOld(ServiceCtx context)
+        {
+            ResultCode result;
+
+            long inPosition     = context.Request.SendBuff[0].Position;
+            long inSize         = context.Request.SendBuff[0].Size;
+            long outputPosition = context.Request.ReceiveBuff[0].Position;
+            long outputSize     = context.Request.ReceiveBuff[0].Size;
+
+            using (BinaryReader inputStream = new BinaryReader(new MemoryStream(context.Memory.ReadBytes(inPosition, inSize))))
+            {
+                result = DecodeInterleavedInternal(inputStream, out short[] outPcmData, outputSize, out uint outConsumed, out int outSamples);
+
+                if (result == ResultCode.Success)
+                {
+                    byte[] pcmDataBytes = new byte[outPcmData.Length * sizeof(short)];
+                    Buffer.BlockCopy(outPcmData, 0, pcmDataBytes, 0, pcmDataBytes.Length);
+                    context.Memory.WriteBytes(outputPosition, pcmDataBytes);
+
+                    context.ResponseData.Write(outConsumed);
+                    context.ResponseData.Write(outSamples);
+
+                    // This is the time the DSP took to process the request, TODO: fill this.
+                    context.ResponseData.Write(0);
+                }
+            }
+
+            return result;
         }
 
-        [Command(4)]
-        // DecodeInterleavedWithPerf(buffer<unknown, 5>) -> (u32, u32, u64, buffer<unknown, 0x46>)
-        public ResultCode DecodeInterleavedWithPerf(ServiceCtx context)
+        [Command(6)] // 6.0.0+
+        // DecodeInterleavedOld(bool reset, buffer<unknown, 5>) -> (u32, u32, u64, buffer<unknown, 0x46>)
+        public ResultCode DecodeInterleavedOld(ServiceCtx context)
         {
-            ResultCode result = DecodeInterleaved(context);
+            ResultCode result;
+
+            _reset = context.RequestData.ReadBoolean();
+
+            long inPosition     = context.Request.SendBuff[0].Position;
+            long inSize         = context.Request.SendBuff[0].Size;
+            long outputPosition = context.Request.ReceiveBuff[0].Position;
+            long outputSize     = context.Request.ReceiveBuff[0].Size;
+
+            using (BinaryReader inputStream = new BinaryReader(new MemoryStream(context.Memory.ReadBytes(inPosition, inSize))))
+            {
+                result = DecodeInterleavedInternal(inputStream, out short[] outPcmData, outputSize, out uint outConsumed, out int outSamples);
+
+                if (result == ResultCode.Success)
+                {
+                    byte[] pcmDataBytes = new byte[outPcmData.Length * sizeof(short)];
+                    Buffer.BlockCopy(outPcmData, 0, pcmDataBytes, 0, pcmDataBytes.Length);
+                    context.Memory.WriteBytes(outputPosition, pcmDataBytes);
+
+                    context.ResponseData.Write(outConsumed);
+                    context.ResponseData.Write(outSamples);
 
-            // TODO: Figure out what this value is.
-            // According to switchbrew, it is now used.
-            context.ResponseData.Write(0L);
+                    // This is the time the DSP took to process the request, TODO: fill this.
+                    context.ResponseData.Write(0);
+                }
+            }
+
+            return result;
+        }
+
+        [Command(8)] // 7.0.0+
+        // DecodeInterleaved(bool reset, buffer<unknown, 0x45>) -> (u32, u32, u64, buffer<unknown, 0x46>)
+        public ResultCode DecodeInterleaved(ServiceCtx context)
+        {
+            ResultCode result;
+
+            _reset = context.RequestData.ReadBoolean();
+
+            long inPosition     = context.Request.SendBuff[0].Position;
+            long inSize         = context.Request.SendBuff[0].Size;
+            long outputPosition = context.Request.ReceiveBuff[0].Position;
+            long outputSize     = context.Request.ReceiveBuff[0].Size;
+
+            using (BinaryReader inputStream = new BinaryReader(new MemoryStream(context.Memory.ReadBytes(inPosition, inSize))))
+            {
+                result = DecodeInterleavedInternal(inputStream, out short[] outPcmData, outputSize, out uint outConsumed, out int outSamples);
+
+                if (result == ResultCode.Success)
+                {
+                    byte[] pcmDataBytes = new byte[outPcmData.Length * sizeof(short)];
+                    Buffer.BlockCopy(outPcmData, 0, pcmDataBytes, 0, pcmDataBytes.Length);
+                    context.Memory.WriteBytes(outputPosition, pcmDataBytes);
+
+                    context.ResponseData.Write(outConsumed);
+                    context.ResponseData.Write(outSamples);
+
+                    // This is the time the DSP took to process the request, TODO: fill this.
+                    context.ResponseData.Write(0);
+                }
+            }
 
             return result;
         }