From bd9ac0fdaadd233e778a872c48f7f628b5a68c93 Mon Sep 17 00:00:00 2001
From: merry <git@mary.rs>
Date: Fri, 4 Mar 2022 22:05:08 +0000
Subject: [PATCH] T32: Implement B, B.cond, BL, BLX (#3155)

* Decoders: Make IsThumb a function of OpCode32

* OpCode32: Fix GetPc

* T32: Implement B, B.cond, BL, BLX

* rm usings
---
 ARMeilleure/Decoders/OpCode32.cs              |  16 +-
 ARMeilleure/Decoders/OpCodeT32BImm20.cs       |  29 +++
 ARMeilleure/Decoders/OpCodeT32BImm24.cs       |  35 ++++
 ARMeilleure/Decoders/OpCodeTable.cs           |   4 +
 ARMeilleure/Instructions/InstEmitAluHelper.cs |   2 +-
 ARMeilleure/Instructions/InstEmitFlow32.cs    |   4 +-
 ARMeilleure/Instructions/InstEmitHelper.cs    |   5 -
 Ryujinx.Tests/Cpu/CpuTestT32Flow.cs           | 167 ++++++++++++++++++
 8 files changed, 253 insertions(+), 9 deletions(-)
 create mode 100644 ARMeilleure/Decoders/OpCodeT32BImm20.cs
 create mode 100644 ARMeilleure/Decoders/OpCodeT32BImm24.cs
 create mode 100644 Ryujinx.Tests/Cpu/CpuTestT32Flow.cs

diff --git a/ARMeilleure/Decoders/OpCode32.cs b/ARMeilleure/Decoders/OpCode32.cs
index 0d8ad1fd..92487c6e 100644
--- a/ARMeilleure/Decoders/OpCode32.cs
+++ b/ARMeilleure/Decoders/OpCode32.cs
@@ -13,11 +13,25 @@ namespace ARMeilleure.Decoders
             Cond = (Condition)((uint)opCode >> 28);
         }
 
+        public bool IsThumb()
+        {
+            return this is OpCodeT16 || this is OpCodeT32;
+        }
+
         public uint GetPc()
         {
             // Due to backwards compatibility and legacy behavior of ARMv4 CPUs pipeline,
             // the PC actually points 2 instructions ahead.
-            return (uint)Address + (uint)OpCodeSizeInBytes * 2;
+            if (IsThumb())
+            {
+                // PC is ahead by 4 in thumb mode whether or not the current instruction
+                // is 16 or 32 bit.
+                return (uint)Address + 4u;
+            }
+            else
+            {
+                return (uint)Address + 8u;
+            }
         }
     }
 }
\ No newline at end of file
diff --git a/ARMeilleure/Decoders/OpCodeT32BImm20.cs b/ARMeilleure/Decoders/OpCodeT32BImm20.cs
new file mode 100644
index 00000000..8ed4b4b1
--- /dev/null
+++ b/ARMeilleure/Decoders/OpCodeT32BImm20.cs
@@ -0,0 +1,29 @@
+using ARMeilleure.Instructions;
+
+namespace ARMeilleure.Decoders
+{
+    class OpCodeT32BImm20 : OpCodeT32, IOpCode32BImm
+    {
+        public long Immediate { get; }
+
+        public new static OpCode Create(InstDescriptor inst, ulong address, int opCode) => new OpCodeT32BImm20(inst, address, opCode);
+
+        public OpCodeT32BImm20(InstDescriptor inst, ulong address, int opCode) : base(inst, address, opCode)
+        {
+            uint pc = GetPc();
+
+            int imm11 = (opCode >> 0) & 0x7ff;
+            int j2 = (opCode >> 11) & 1;
+            int j1 = (opCode >> 13) & 1;
+            int imm6 = (opCode >> 16) & 0x3f;
+            int s = (opCode >> 26) & 1;
+
+            int imm32 = imm11 | (imm6 << 11) | (j1 << 17) | (j2 << 18) | (s << 19);
+            imm32 = (imm32 << 13) >> 12;
+
+            Immediate = pc + imm32;
+
+            Cond = (Condition)((opCode >> 22) & 0xf);
+        }
+    }
+}
\ No newline at end of file
diff --git a/ARMeilleure/Decoders/OpCodeT32BImm24.cs b/ARMeilleure/Decoders/OpCodeT32BImm24.cs
new file mode 100644
index 00000000..4381be47
--- /dev/null
+++ b/ARMeilleure/Decoders/OpCodeT32BImm24.cs
@@ -0,0 +1,35 @@
+using ARMeilleure.Instructions;
+
+namespace ARMeilleure.Decoders
+{
+    class OpCodeT32BImm24 : OpCodeT32, IOpCode32BImm
+    {
+        public long Immediate { get; }
+
+        public new static OpCode Create(InstDescriptor inst, ulong address, int opCode) => new OpCodeT32BImm24(inst, address, opCode);
+
+        public OpCodeT32BImm24(InstDescriptor inst, ulong address, int opCode) : base(inst, address, opCode)
+        {
+            uint pc = GetPc();
+
+            if (inst.Name == InstName.Blx)
+            {
+                pc &= ~3u;
+            }
+
+            int imm11 = (opCode >> 0) & 0x7ff;
+            int j2 = (opCode >> 11) & 1;
+            int j1 = (opCode >> 13) & 1;
+            int imm10 = (opCode >> 16) & 0x3ff;
+            int s = (opCode >> 26) & 1;
+
+            int i1 = j1 ^ s ^ 1;
+            int i2 = j2 ^ s ^ 1;
+
+            int imm32 = imm11 | (imm10 << 11) | (i2 << 21) | (i1 << 22) | (s << 23);
+            imm32 = (imm32 << 9) >> 8;
+
+            Immediate = pc + imm32;
+        }
+    }
+}
\ No newline at end of file
diff --git a/ARMeilleure/Decoders/OpCodeTable.cs b/ARMeilleure/Decoders/OpCodeTable.cs
index d290e554..ad696104 100644
--- a/ARMeilleure/Decoders/OpCodeTable.cs
+++ b/ARMeilleure/Decoders/OpCodeTable.cs
@@ -1050,7 +1050,11 @@ namespace ARMeilleure.Decoders
             SetT32("11101011010xxxxx0xxxxxxxxxxxxxxx", InstName.Adc,      InstEmit32.Adc,      OpCodeT32AluRsImm.Create);
             SetT32("11101011000<xxxx0xxx<<<<xxxxxxxx", InstName.Add,      InstEmit32.Add,      OpCodeT32AluRsImm.Create);
             SetT32("11101010000<xxxx0xxx<<<<xxxxxxxx", InstName.And,      InstEmit32.And,      OpCodeT32AluRsImm.Create);
+            SetT32("11110x<<<xxxxxxx10x0xxxxxxxxxxxx", InstName.B,        InstEmit32.B,        OpCodeT32BImm20.Create);
+            SetT32("11110xxxxxxxxxxx10x1xxxxxxxxxxxx", InstName.B,        InstEmit32.B,        OpCodeT32BImm24.Create);
             SetT32("11101010001xxxxx0xxxxxxxxxxxxxxx", InstName.Bic,      InstEmit32.Bic,      OpCodeT32AluRsImm.Create);
+            SetT32("11110xxxxxxxxxxx11x1xxxxxxxxxxxx", InstName.Bl,       InstEmit32.Bl,       OpCodeT32BImm24.Create);
+            SetT32("11110xxxxxxxxxxx11x0xxxxxxxxxxx0", InstName.Blx,      InstEmit32.Blx,      OpCodeT32BImm24.Create);
             SetT32("111010110001xxxx0xxx1111xxxxxxxx", InstName.Cmn,      InstEmit32.Cmn,      OpCodeT32AluRsImm.Create);
             SetT32("111010111011xxxx0xxx1111xxxxxxxx", InstName.Cmp,      InstEmit32.Cmp,      OpCodeT32AluRsImm.Create);
             SetT32("11101010100<xxxx0xxx<<<<xxxxxxxx", InstName.Eor,      InstEmit32.Eor,      OpCodeT32AluRsImm.Create);
diff --git a/ARMeilleure/Instructions/InstEmitAluHelper.cs b/ARMeilleure/Instructions/InstEmitAluHelper.cs
index 8bf53ed4..67d9e915 100644
--- a/ARMeilleure/Instructions/InstEmitAluHelper.cs
+++ b/ARMeilleure/Instructions/InstEmitAluHelper.cs
@@ -128,7 +128,7 @@ namespace ARMeilleure.Instructions
         {
             Debug.Assert(value.Type == OperandType.I32);
 
-            if (IsThumb(context.CurrOp))
+            if (((OpCode32)context.CurrOp).IsThumb())
             {
                 bool isReturn = IsA32Return(context);
                 if (!isReturn)
diff --git a/ARMeilleure/Instructions/InstEmitFlow32.cs b/ARMeilleure/Instructions/InstEmitFlow32.cs
index 2b052505..064aeb36 100644
--- a/ARMeilleure/Instructions/InstEmitFlow32.cs
+++ b/ARMeilleure/Instructions/InstEmitFlow32.cs
@@ -34,7 +34,7 @@ namespace ARMeilleure.Instructions
 
             uint pc = op.GetPc();
 
-            bool isThumb = IsThumb(context.CurrOp);
+            bool isThumb = ((OpCode32)context.CurrOp).IsThumb();
 
             uint currentPc = isThumb
                 ? pc | 1
@@ -61,7 +61,7 @@ namespace ARMeilleure.Instructions
             Operand addr = context.Copy(GetIntA32(context, op.Rm));
             Operand bitOne = context.BitwiseAnd(addr, Const(1));
 
-            bool isThumb = IsThumb(context.CurrOp);
+            bool isThumb = ((OpCode32)context.CurrOp).IsThumb();
 
             uint currentPc = isThumb
                 ? (pc - 2) | 1
diff --git a/ARMeilleure/Instructions/InstEmitHelper.cs b/ARMeilleure/Instructions/InstEmitHelper.cs
index 773f6bd6..a22bb3fb 100644
--- a/ARMeilleure/Instructions/InstEmitHelper.cs
+++ b/ARMeilleure/Instructions/InstEmitHelper.cs
@@ -10,11 +10,6 @@ namespace ARMeilleure.Instructions
 {
     static class InstEmitHelper
     {
-        public static bool IsThumb(OpCode op)
-        {
-            return op is OpCodeT16 || op is OpCodeT32;
-        }
-
         public static Operand GetExtendedM(ArmEmitterContext context, int rm, IntType type)
         {
             Operand value = GetIntOrZR(context, rm);
diff --git a/Ryujinx.Tests/Cpu/CpuTestT32Flow.cs b/Ryujinx.Tests/Cpu/CpuTestT32Flow.cs
new file mode 100644
index 00000000..2c83b01d
--- /dev/null
+++ b/Ryujinx.Tests/Cpu/CpuTestT32Flow.cs
@@ -0,0 +1,167 @@
+using ARMeilleure.State;
+using NUnit.Framework;
+
+namespace Ryujinx.Tests.Cpu
+{
+    [Category("T32Flow")]
+    public sealed class CpuTestT32Flow : CpuTest32
+    {
+        [Test]
+        public void TestT32B1()
+        {
+            // BNE label
+            ThumbOpcode(0xf040);
+            ThumbOpcode(0x8240);
+            for (int i = 0; i < 576; i++)
+            {
+                ThumbOpcode(0xe7fe);
+            }
+            // label: BX LR
+            ThumbOpcode(0x4770);
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+        }
+
+        [Test]
+        public void TestT32B2()
+        {
+            // BNE label1
+            ThumbOpcode(0xf040);
+            ThumbOpcode(0x8242);
+            // label2: BNE label3
+            ThumbOpcode(0xf040);
+            ThumbOpcode(0x8242);
+            for (int i = 0; i < 576; i++)
+            {
+                ThumbOpcode(0xe7fe);
+            }
+            // label1: BNE label2
+            ThumbOpcode(0xf47f);
+            ThumbOpcode(0xadbc);
+            // label3: BX LR
+            ThumbOpcode(0x4770);
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+        }
+
+        [Test]
+        public void TestT32B3()
+        {
+            // B.W label
+            ThumbOpcode(0xf000);
+            ThumbOpcode(0xba40);
+            for (int i = 0; i < 576; i++)
+            {
+                ThumbOpcode(0xe7fe);
+            }
+            // label: BX LR
+            ThumbOpcode(0x4770);
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+        }
+
+        [Test]
+        public void TestT32B4()
+        {
+            // B.W label1
+            ThumbOpcode(0xf000);
+            ThumbOpcode(0xba42);
+            // label2: B.W label3
+            ThumbOpcode(0xf000);
+            ThumbOpcode(0xba42);
+            for (int i = 0; i < 576; i++)
+            {
+                ThumbOpcode(0xe7fe);
+            }
+            // label1: B.W label2
+            ThumbOpcode(0xf7ff);
+            ThumbOpcode(0xbdbc);
+            // label3: BX LR
+            ThumbOpcode(0x4770);
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+        }
+
+        [Test]
+        public void TestT32Bl()
+        {
+            // BL label
+            ThumbOpcode(0xf000);
+            ThumbOpcode(0xf840);
+            for (int i = 0; i < 64; i++)
+            {
+                ThumbOpcode(0xe7fe);
+            }
+            ThumbOpcode(0x4670); // label: MOV R0, LR
+            ThumbOpcode(0x2100); //        MOVS R1, #0
+            ThumbOpcode(0x468e); //        MOV LR, R1
+            ThumbOpcode(0x4770); //        BX LR
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+
+            Assert.That(GetContext().GetX(0), Is.EqualTo(0x1005));
+        }
+
+        [Test]
+        public void TestT32Blx1()
+        {
+            // BLX label
+            ThumbOpcode(0xf000);
+            ThumbOpcode(0xe840);
+            for (int i = 0; i < 64; i++)
+            {
+                ThumbOpcode(0x4770);
+            }
+            // .arm ; label: MOV R0, LR
+            Opcode(0xe1a0000e);
+            // MOV LR, #0
+            Opcode(0xe3a0e000);
+            // BX LR
+            Opcode(0xe12fff1e);
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+
+            Assert.That(GetContext().GetX(0), Is.EqualTo(0x1005));
+            Assert.That(GetContext().GetPstateFlag(PState.TFlag), Is.EqualTo(false));
+        }
+
+        [Test]
+        public void TestT32Blx2()
+        {
+            // NOP
+            ThumbOpcode(0xbf00);
+            // BLX label
+            ThumbOpcode(0xf000);
+            ThumbOpcode(0xe840);
+            for (int i = 0; i < 63; i++)
+            {
+                ThumbOpcode(0x4770);
+            }
+            // .arm ; label: MOV R0, LR
+            Opcode(0xe1a0000e);
+            // MOV LR, #0
+            Opcode(0xe3a0e000);
+            // BX LR
+            Opcode(0xe12fff1e);
+
+            GetContext().SetPstateFlag(PState.TFlag, true);
+
+            ExecuteOpcodes(runUnicorn: false);
+
+            Assert.That(GetContext().GetX(0), Is.EqualTo(0x1007));
+            Assert.That(GetContext().GetPstateFlag(PState.TFlag), Is.EqualTo(false));
+        }
+    }
+}
\ No newline at end of file