We can now change threads

src/core/CPU/cpu_dynarmic.cpp

@@ -20,6 +20,7 @@ void CPU::reset() {
     setCPSR(0x00000010);
 
     cp15->reset();
+    cp15->setTLSBase(VirtualAddrs::TLSBase); // Set cp15 TLS pointer to the main thread's thread-local storage
     jit->Reset();
     jit->ClearCache();
     jit->Regs().fill(0);

src/core/kernel/kernel.cpp

@@ -7,6 +7,11 @@ Kernel::Kernel(CPU& cpu, Memory& mem)
 	: cpu(cpu), regs(cpu.regs()), mem(mem), handleCounter(0), serviceManager(regs, mem, currentProcess) {
 	objects.reserve(512); // Make room for a few objects to avoid further memory allocs later
 	portHandles.reserve(32);
+
+	for (int i = 0; i < threads.size(); i++) {
+		threads[i].tlsBase = VirtualAddrs::TLSBase + i * VirtualAddrs::TLSSize;
+		threads[i].status = ThreadStatus::Dead;
+	}
 }
 
 void Kernel::serviceSVC(u32 svc) {
@@ -56,8 +61,8 @@ KernelObject* Kernel::getProcessFromPID(Handle handle) {
 void Kernel::deleteObjectData(KernelObject& object) {
 	using enum KernelObjectType;
 
-	// Resource limit, service and dummy objects do not allocate heap data, so we don't delete anything
-	if (object.data == nullptr || object.type == ResourceLimit || object.type == Dummy) {
+	// Resource limit and thread objects do not allocate heap data, so we don't delete anything
+	if (object.data == nullptr || object.type == ResourceLimit || object.type == Thread) {
 		return;
 	}
 
@@ -82,7 +87,7 @@ void Kernel::reset() {
 	// Make main thread object. We do not have to set the entrypoint and SP for it as the ROM loader does.
 	// Main thread seems to have a priority of 0x30
 	mainThread = makeThread(0, 0, 0x30, 0, ThreadStatus::Running);
-	currentThread = mainThread;
+	currentThreadIndex = 0;
 
 	// Create global service manager port
 	srvHandle = makePort("srv:");

src/core/kernel/threads.cpp

@@ -1,17 +1,63 @@
+#include <cstring>
 #include "kernel.hpp"
 // This header needs to be included because I did stupid forward decl hack so the kernel and CPU can both access each other
 #include "cpu.hpp"
 #include "resource_limits.hpp"
 
+// Switch to another thread
+// newThread: Index of the newThread in the thread array (NOT a handle).
+void Kernel::switchThread(int newThreadIndex) {
+	if (currentThreadIndex == newThreadIndex) { // Bail early if the new thread is actually the old thread
+		return;
+	}
+
+	auto& oldThread = threads[currentThreadIndex];
+	const auto& newThread = threads[newThreadIndex];
+
+	// Backup context
+	std::memcpy(&oldThread.gprs[0], &cpu.regs()[0], 16 * sizeof(u32)); // Backup the 16 GPRs
+	std::memcpy(&oldThread.fprs[0], &cpu.fprs()[0], 32 * sizeof(u32)); // Backup the 32 FPRs
+	oldThread.cpsr = cpu.getCPSR();   // Backup CPSR
+	oldThread.fpscr = cpu.getFPSCR(); // Backup FPSCR
+
+	// Load new context
+	std::memcpy(&cpu.regs()[0], &newThread.gprs[0], 16 * sizeof(u32)); // Load 16 GPRs
+	std::memcpy(&cpu.fprs()[0], &newThread.fprs[0], 32 * sizeof(u32)); // Load 32 FPRs
+	cpu.setCPSR(newThread.cpsr);   // Load CPSR
+	cpu.setFPSCR(newThread.fpscr); // Load FPSCR
+	cpu.setTLSBase(newThread.tlsBase); // Load CP15 thread-local-storage pointer register
+
+	currentThreadIndex = newThreadIndex;
+}
+
 // Internal OS function to spawn a thread
 Handle Kernel::makeThread(u32 entrypoint, u32 initialSP, u32 priority, u32 id, ThreadStatus status) {
 	if (threadCount >= appResourceLimits.maxThreads) {
 		Helpers::panic("Overflowed the number of threads");
 	}
-	threadCount++;
-
+	Thread& t = threads[threadCount++]; // Reference to thread data
 	Handle ret = makeObject(KernelObjectType::Thread);
-	objects[ret].data = new Thread(initialSP, entrypoint, priority, id, status);
+	objects[ret].data = &t;
+
+	const bool isThumb = (entrypoint & 1) != 0; // Whether the thread starts in thumb mode or not
+
+	// Set up initial thread context
+	t.gprs.fill(0);
+	t.fprs.fill(0);
+
+	t.initialSP = initialSP;
+	t.gprs[13] = initialSP;
+	t.entrypoint = entrypoint;
+	t.gprs[15] = entrypoint;
+	t.priority = priority;
+	t.processorID = id;
+	t.status = status;
+	t.handle = ret;
+
+	t.cpsr = CPSR::UserMode | (isThumb ? CPSR::Thumb : 0);
+	t.fpscr = FPSCR::ThreadDefault;
+	// Initial TLS base has already been set in Kernel::Kernel()
+
 	return ret;
 }
 

src/emulator.cpp

@@ -59,5 +59,8 @@ bool Emulator::loadELF(std::ifstream& file) {
         return false;
 
     cpu.setReg(15, entrypoint.value()); // Set initial PC
+    if (entrypoint.value() & 1) {
+        Helpers::panic("Misaligned ELF entrypoint. TODO: Check if ELFs can boot in thumb mode");
+    }
     return true;
 }