From 1cb07ffc143bd8197d2665c2643be76dda0e8ef0 Mon Sep 17 00:00:00 2001
From: Ryan Houdek <Sonicadvance1@gmail.com>
Date: Sun, 21 Sep 2014 14:12:26 -0500
Subject: [PATCH] [AArch64] Implement twi and tw.

---
 Source/Core/Core/PowerPC/JitArm64/Jit.h       |  1 +
 .../JitArm64/JitArm64_SystemRegisters.cpp     | 62 +++++++++++++++++++
 .../Core/PowerPC/JitArm64/JitArm64_Tables.cpp |  4 +-
 3 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/Source/Core/Core/PowerPC/JitArm64/Jit.h b/Source/Core/Core/PowerPC/JitArm64/Jit.h
index 2779f9414b..c8b2c7a1eb 100644
--- a/Source/Core/Core/PowerPC/JitArm64/Jit.h
+++ b/Source/Core/Core/PowerPC/JitArm64/Jit.h
@@ -92,6 +92,7 @@ public:
 	void mtsr(UGeckoInstruction inst);
 	void mfsrin(UGeckoInstruction inst);
 	void mtsrin(UGeckoInstruction inst);
+	void twx(UGeckoInstruction inst);
 
 	// LoadStore
 	void icbi(UGeckoInstruction inst);
diff --git a/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp b/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp
index 82eb5cc908..800c58044a 100644
--- a/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp
+++ b/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp
@@ -129,3 +129,65 @@ void JitArm64::mtsrin(UGeckoInstruction inst)
 
 	gpr.Unlock(index);
 }
+
+void JitArm64::twx(UGeckoInstruction inst)
+{
+	INSTRUCTION_START
+	JITDISABLE(bJITIntegerOff);
+
+	gpr.Flush(FlushMode::FLUSH_ALL);
+	fpr.Flush(FlushMode::FLUSH_ALL);
+
+	s32 a = inst.RA;
+
+	ARM64Reg WA = gpr.GetReg();
+
+	if (inst.OPCD == 3) // twi
+	{
+		if (inst.SIMM_16 >= 0 && inst.SIMM_16 < 4096)
+		{
+			// Can fit in immediate in to the instruction encoding
+			CMP(gpr.R(a), inst.SIMM_16);
+		}
+		else
+		{
+			MOVI2R(WA, (s32)(s16)inst.SIMM_16);
+			CMP(gpr.R(a), WA);
+		}
+	}
+	else // tw
+	{
+		CMP(gpr.R(a), gpr.R(inst.RB));
+	}
+
+	std::vector<FixupBranch> fixups;
+	CCFlags conditions[] = { CC_LT, CC_GT, CC_EQ, CC_VC, CC_VS };
+
+	for (int i = 0; i < 5; i++)
+	{
+		if (inst.TO & (1 << i))
+		{
+			FixupBranch f = B(conditions[i]);
+			fixups.push_back(f);
+		}
+	}
+	FixupBranch dont_trap = B();
+
+	for (const FixupBranch& fixup : fixups)
+	{
+		SetJumpTarget(fixup);
+	}
+
+	LDR(INDEX_UNSIGNED, WA, X29, PPCSTATE_OFF(Exceptions));
+	ORR(WA, WA, 24, 0); // Same as WA | EXCEPTION_PROGRAM
+	STR(INDEX_UNSIGNED, WA, X29, PPCSTATE_OFF(Exceptions));
+
+	MOVI2R(WA, js.compilerPC);
+
+	// WA is unlocked in this function
+	WriteExceptionExit(WA);
+
+	SetJumpTarget(dont_trap);
+
+	WriteExit(js.compilerPC + 4);
+}
diff --git a/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp b/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp
index 3695b35480..336bf47640 100644
--- a/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp
+++ b/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp
@@ -42,7 +42,7 @@ static GekkoOPTemplate primarytable[] =
 
 	{1,  &JitArm64::HLEFunction},               //"HLEFunction", OPTYPE_SYSTEM, FL_ENDBLOCK}},
 	{2,  &JitArm64::FallBackToInterpreter},     //"DynaBlock",   OPTYPE_SYSTEM, 0}},
-	{3,  &JitArm64::Break},                     //"twi",         OPTYPE_SYSTEM, FL_ENDBLOCK}},
+	{3,  &JitArm64::twx},                       //"twi",         OPTYPE_SYSTEM, FL_ENDBLOCK}},
 	{17, &JitArm64::sc},                        //"sc",          OPTYPE_SYSTEM, FL_ENDBLOCK, 1}},
 
 	{7,  &JitArm64::FallBackToInterpreter},     //"mulli",    OPTYPE_INTEGER, FL_OUT_D | FL_IN_A | FL_RC_BIT, 2}},
@@ -279,7 +279,7 @@ static GekkoOPTemplate table31[] =
 	{595, &JitArm64::mfsr},                     //"mfsr",   OPTYPE_SYSTEM, FL_OUT_D, 2}},
 	{659, &JitArm64::mfsrin},                   //"mfsrin", OPTYPE_SYSTEM, FL_OUT_D, 2}},
 
-	{4,   &JitArm64::Break},                    //"tw",     OPTYPE_SYSTEM, FL_ENDBLOCK, 1}},
+	{4,   &JitArm64::twx},                      //"tw",     OPTYPE_SYSTEM, FL_ENDBLOCK, 1}},
 	{598, &JitArm64::DoNothing},                //"sync",   OPTYPE_SYSTEM, 0, 2}},
 	{982, &JitArm64::icbi},                     //"icbi",   OPTYPE_SYSTEM, FL_ENDBLOCK, 3}},