From d8cf0e2de2cca6fb4c9ba6ad111a1f7f7dbdd92c Mon Sep 17 00:00:00 2001
From: wheremyfoodat <gponiris2004@gmail.com>
Date: Thu, 15 Sep 2022 22:42:36 +0300
Subject: [PATCH] cp15 but better

---
 include/cpu_dynarmic.hpp      |  1 +
 include/dynarmic_cp15.hpp     | 11 +++++++++--
 include/memory.hpp            |  5 ++++-
 src/core/CPU/cpu_dynarmic.cpp |  7 ++++---
 src/core/memory.cpp           | 19 ++++++++++++++++++-
 5 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/include/cpu_dynarmic.hpp b/include/cpu_dynarmic.hpp
index f95fbed4..643b8809 100644
--- a/include/cpu_dynarmic.hpp
+++ b/include/cpu_dynarmic.hpp
@@ -76,6 +76,7 @@ public:
 
 class CPU {
     std::unique_ptr<Dynarmic::A32::Jit> jit;
+    std::shared_ptr<CP15> cp15;
     MyEnvironment env;
     Memory& mem;
 
diff --git a/include/dynarmic_cp15.hpp b/include/dynarmic_cp15.hpp
index 39b7cdd9..6aa5ffc6 100644
--- a/include/dynarmic_cp15.hpp
+++ b/include/dynarmic_cp15.hpp
@@ -4,6 +4,7 @@
 #include "dynarmic/interface/A32/config.h"
 #include "dynarmic/interface/A32/coprocessor.h"
 #include "helpers.hpp"
+#include "memory.hpp"
 
 class CP15 final : public Dynarmic::A32::Coprocessor {
     using Callback = Dynarmic::A32::Coprocessor::Callback;
@@ -11,7 +12,7 @@ class CP15 final : public Dynarmic::A32::Coprocessor {
     using CallbackOrAccessOneWord = Dynarmic::A32::Coprocessor::CallbackOrAccessOneWord;
     using CallbackOrAccessTwoWords = Dynarmic::A32::Coprocessor::CallbackOrAccessTwoWords;
 
-    u32 threadStoragePointer = 0x696966969; // Pointer to thread-local storage
+    u32 threadStoragePointer; // Pointer to thread-local storage
 
     std::optional<Callback> CompileInternalOperation(bool two, unsigned opc1,
         CoprocReg CRd, CoprocReg CRn,
@@ -30,6 +31,11 @@ class CP15 final : public Dynarmic::A32::Coprocessor {
 
     CallbackOrAccessOneWord CompileGetOneWord(bool two, unsigned opc1, CoprocReg CRn,
         CoprocReg CRm, unsigned opc2) override {
+        // Some sort of pointer to TLS, accessed via mrc p15, 0, rd, c13, c0, 3
+        if (!two && CRn == CoprocReg::C13 && opc1 == 0 && CRm == CoprocReg::C0 && opc2 == 3) {
+            return &threadStoragePointer;
+        }
+
         Helpers::panic("CP15: CompileGetOneWord");
     }
 
@@ -47,7 +53,8 @@ class CP15 final : public Dynarmic::A32::Coprocessor {
         return std::nullopt;
     }
 
+public:
     void reset() {
-
+        threadStoragePointer = VirtualAddrs::TLSBase;
     }
 };
\ No newline at end of file
diff --git a/include/memory.hpp b/include/memory.hpp
index 04be0286..2f186afc 100644
--- a/include/memory.hpp
+++ b/include/memory.hpp
@@ -14,7 +14,10 @@ namespace VirtualAddrs {
 		// Typically 0x4000 bytes, determined by exheader
 		StackTop = 0x10000000,
 		StackBottom = 0x0FFFC000,
-		StackSize = 0x4000
+		StackSize = 0x4000,
+
+		// Start of TLS for first thread. Next thread's storage will be at TLSBase + 0x1000, and so on
+		TLSBase = 0xFF400000
 	};
 }
 
diff --git a/src/core/CPU/cpu_dynarmic.cpp b/src/core/CPU/cpu_dynarmic.cpp
index 67c9ef0f..279b928b 100644
--- a/src/core/CPU/cpu_dynarmic.cpp
+++ b/src/core/CPU/cpu_dynarmic.cpp
@@ -2,18 +2,19 @@
 #include "cpu_dynarmic.hpp"
 
 CPU::CPU(Memory& mem, Kernel& kernel) : mem(mem), env(mem, kernel) {
-    Dynarmic::A32::UserConfig config;
+    cp15 = std::make_shared<CP15>();
 
+    Dynarmic::A32::UserConfig config;
     config.callbacks = &env;
-    config.coprocessors[15] = std::make_shared<CP15>();
+    config.coprocessors[15] = cp15;
     jit = std::make_unique<Dynarmic::A32::Jit>(config);
 }
 
 void CPU::reset() {
     // ARM mode, all flags disabled, interrupts and aborts all enabled, user mode
-
     setCPSR(0x00000010);
 
+    cp15->reset();
     jit->Reset();
     jit->ClearCache();
     jit->Regs().fill(0);
diff --git a/src/core/memory.cpp b/src/core/memory.cpp
index 000450da..8775fe4c 100644
--- a/src/core/memory.cpp
+++ b/src/core/memory.cpp
@@ -30,6 +30,14 @@ Memory::Memory() {
 		writeTable[page++] = pointer;
 		fcramStackPage++;
 	}
+
+	// Map 1 page of FCRAM for thread-local storage
+	u32 fcramTLSPage = fcramPageCount - 5;
+	page = VirtualAddrs::TLSBase / pageSize;
+
+	auto pointer = (uintptr_t)&fcram[fcramTLSPage * pageSize];
+	readTable[page] = pointer;
+	writeTable[page++] = pointer;
 }
 
 u8 Memory::read8(u32 vaddr) {
@@ -53,7 +61,16 @@ u32 Memory::read32(u32 vaddr) {
 }
 
 void Memory::write8(u32 vaddr, u8 value) {
-	Helpers::panic("Unimplemented 8-bit write, addr: %08X, val: %02X", vaddr, value);
+	const u32 page = vaddr >> pageShift;
+	const u32 offset = vaddr & pageMask;
+
+	uintptr_t pointer = writeTable[page];
+	if (pointer != 0) [[likely]] {
+		*(u8*)(pointer + offset) = value;
+	}
+	else {
+		Helpers::panic("Unimplemented 8-bit write, addr: %08X, val: %02X", vaddr, value);
+	}
 }
 
 void Memory::write16(u32 vaddr, u16 value) {