From c211d06ad22fe3a4564367e0d2daaf011aab8bbc Mon Sep 17 00:00:00 2001
From: Ryan Houdek <Sonicadvance1@Gmail.com>
Date: Sat, 14 Sep 2013 05:09:46 +0000
Subject: [PATCH] [ARM] stfd and stfdu implementations.

---
 Source/Core/Core/Src/HW/Memmap.h              |  4 +-
 Source/Core/Core/Src/HW/MemmapFunctions.cpp   | 10 +++
 Source/Core/Core/Src/PowerPC/JitArm32/Jit.h   |  2 +
 .../JitArm32/JitArm_LoadStoreFloating.cpp     | 63 +++++++++++++++++++
 .../Src/PowerPC/JitArm32/JitArm_Tables.cpp    |  4 +-
 5 files changed, 80 insertions(+), 3 deletions(-)

diff --git a/Source/Core/Core/Src/HW/Memmap.h b/Source/Core/Core/Src/HW/Memmap.h
index fe05539ae2..689e5a57dd 100644
--- a/Source/Core/Core/Src/HW/Memmap.h
+++ b/Source/Core/Core/Src/HW/Memmap.h
@@ -140,7 +140,6 @@ u64 Read_U64(const u32 _Address);
 float Read_F32(const u32 _Address);
 double Read_F64(const u32 _Address);
 
-
 // used by JIT. Return zero-extended 32bit values
 u32 Read_U8_ZX(const u32 _Address);
 u32 Read_U16_ZX(const u32 _Address);
@@ -157,6 +156,9 @@ void Write_U16_Swap(const u16 _Data, const u32 _Address);
 void Write_U32_Swap(const u32 _Data, const u32 _Address);
 void Write_U64_Swap(const u64 _Data, const u32 _Address);
 
+// Useful helper functions, used by ARM JIT
+void Write_F64(const double _Data, const u32 _Address);
+
 void WriteHW_U32(const u32 _Data, const u32 _Address);
 void GetString(std::string& _string, const u32 _Address);
 
diff --git a/Source/Core/Core/Src/HW/MemmapFunctions.cpp b/Source/Core/Core/Src/HW/MemmapFunctions.cpp
index a5557d55b1..47872e3cd0 100644
--- a/Source/Core/Core/Src/HW/MemmapFunctions.cpp
+++ b/Source/Core/Core/Src/HW/MemmapFunctions.cpp
@@ -507,6 +507,16 @@ void Write_U64_Swap(const u64 _Data, const u32 _Address) {
 	Write_U64(Common::swap64(_Data), _Address);
 }
 
+void Write_F64(const double _Data, const u32 _Address)
+{
+	union
+	{
+		u64 i;
+		double d;
+	} cvt;
+	cvt.d = _Data;
+	Write_U64(cvt.i, _Address);
+}
 u8 ReadUnchecked_U8(const u32 _Address)
 {    
 	u8 _var = 0;
diff --git a/Source/Core/Core/Src/PowerPC/JitArm32/Jit.h b/Source/Core/Core/Src/PowerPC/JitArm32/Jit.h
index cb2dbbd508..bbaddb650f 100644
--- a/Source/Core/Core/Src/PowerPC/JitArm32/Jit.h
+++ b/Source/Core/Core/Src/PowerPC/JitArm32/Jit.h
@@ -205,6 +205,8 @@ public:
 	void lfdu(UGeckoInstruction _inst);
 	void stfs(UGeckoInstruction _inst);
 	void stfsu(UGeckoInstruction _inst);
+	void stfd(UGeckoInstruction _inst);
+	void stfdu(UGeckoInstruction _inst);
 
 	// Paired Singles
 	void ps_add(UGeckoInstruction _inst);
diff --git a/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_LoadStoreFloating.cpp b/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_LoadStoreFloating.cpp
index 6bc9392978..7b607adeb0 100644
--- a/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_LoadStoreFloating.cpp
+++ b/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_LoadStoreFloating.cpp
@@ -286,3 +286,66 @@ void JitArm::stfsu(UGeckoInstruction inst)
 	gpr.Unlock(rA, rB);
 }
 
+void JitArm::stfd(UGeckoInstruction inst)
+{
+	INSTRUCTION_START
+	JITDISABLE(bJITLoadStoreFloatingOff)
+
+	ARMReg rA = gpr.GetReg();
+	ARMReg rB = gpr.GetReg();
+	ARMReg v0 = fpr.R0(inst.FS);
+	
+	if (inst.RA)
+	{
+		MOVI2R(rB, inst.SIMM_16);
+		ARMReg RA = gpr.R(inst.RA);
+		ADD(rB, rB, RA);
+	}
+	else
+		MOVI2R(rB, (u32)inst.SIMM_16);
+
+	
+	MOVI2R(rA, (u32)&Memory::Write_F64);	
+	PUSH(4, R0, R1, R2, R3);
+	VMOV(D0, v0);
+	MOV(R0, rB);
+
+	BL(rA);
+
+	POP(4, R0, R1, R2, R3);
+	
+	gpr.Unlock(rA, rB);
+}
+
+void JitArm::stfdu(UGeckoInstruction inst)
+{
+	INSTRUCTION_START
+	JITDISABLE(bJITLoadStoreFloatingOff)
+
+	ARMReg RA = gpr.R(inst.RA);
+	ARMReg rA = gpr.GetReg();
+	ARMReg rB = gpr.GetReg();
+	ARMReg v0 = fpr.R0(inst.FS);
+	
+	MOVI2R(rB, inst.SIMM_16);
+	ADD(rB, rB, RA);
+	
+	LDR(rA, R9, PPCSTATE_OFF(Exceptions));
+	CMP(rA, EXCEPTION_DSI);
+	
+	SetCC(CC_NEQ);
+	MOV(RA, rB);
+	SetCC();
+
+	MOVI2R(rA, (u32)&Memory::Write_F64);	
+	PUSH(4, R0, R1, R2, R3);
+	VMOV(D0, v0);
+	MOV(R0, rB);
+
+	BL(rA);
+
+	POP(4, R0, R1, R2, R3);
+	
+	gpr.Unlock(rA, rB);
+}
+
diff --git a/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_Tables.cpp b/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_Tables.cpp
index 9728bc270c..7480e983ce 100644
--- a/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_Tables.cpp
+++ b/Source/Core/Core/Src/PowerPC/JitArm32/JitArm_Tables.cpp
@@ -104,8 +104,8 @@ static GekkoOPTemplate primarytable[] =
 
 	{52, &JitArm::stfs}, //"stfs",  OPTYPE_STOREFP, FL_IN_A}},
 	{53, &JitArm::stfsu}, //"stfsu", OPTYPE_STOREFP, FL_OUT_A | FL_IN_A}},
-	{54, &JitArm::Default}, //"stfd",  OPTYPE_STOREFP, FL_IN_A}},
-	{55, &JitArm::Default}, //"stfdu", OPTYPE_STOREFP, FL_OUT_A | FL_IN_A}},
+	{54, &JitArm::stfd}, //"stfd",  OPTYPE_STOREFP, FL_IN_A}},
+	{55, &JitArm::stfdu}, //"stfdu", OPTYPE_STOREFP, FL_OUT_A | FL_IN_A}},
 
 	{56, &JitArm::psq_l}, //"psq_l",   OPTYPE_PS, FL_IN_A}},
 	{57, &JitArm::psq_l}, //"psq_lu",  OPTYPE_PS, FL_OUT_A | FL_IN_A}},