We can now change threads

include/cpu.hpp

@@ -6,4 +6,61 @@
 #error KVM CPU is not implemented yet
 #else
 #error No CPU core implemented :(
-#endif
+#endif
+
+// Status register definitions
+namespace CPSR {
+    enum : u32 {
+        // Privilege modes
+        UserMode = 16,
+        FIQMode = 17,
+        IRQMode = 18,
+        SVCMode = 19,
+        AbortMode = 23,
+        UndefMode = 27,
+        SystemMode = 31,
+
+        // If (CPSR & Thumb) the we're in thumb mode
+        Thumb = 1 << 5
+	};
+}
+
+namespace FPSCR {
+    // FPSCR Flags
+    enum : u32 {
+        NFlag = (1U << 31U), // Negative condition flag
+        ZFlag = (1 << 30),   // Zero condition flag
+        CFlag = (1 << 29),   // Carry condition flag
+        VFlag = (1 << 28),   // Overflow condition flag
+
+        QC = (1 << 27),   // Cumulative saturation bit
+        AHP = (1 << 26),   // Alternative half-precision control bit
+        DefaultNan = (1 << 25),   // Default NaN mode control bit
+        FlushToZero = (1 << 24),   // Flush abnormals to 0 control bit
+        RmodeMask = (3 << 22),   // Rounding Mode bit mask
+        StrideMask = (3 << 20),   // Vector stride bit mask
+        LengthMask = (7 << 16),   // Vector length bit mask
+
+        IDE = (1 << 15), // Input Denormal exception trap enable.
+        IXE = (1 << 12), // Inexact exception trap enable
+        UFE = (1 << 11), // Undeflow exception trap enable
+        OFE = (1 << 10), // Overflow exception trap enable
+        DZE = (1 << 9),  // Division by Zero exception trap enable
+        IOE = (1 << 8),  // Invalid Operation exception trap enable
+
+        IDC = (1 << 7), // Input Denormal cumulative exception bit
+        IXC = (1 << 4), // Inexact cumulative exception bit
+        UFC = (1 << 3), // Undeflow cumulative exception bit
+        OFC = (1 << 2), // Overflow cumulative exception bit
+        DZC = (1 << 1), // Division by Zero cumulative exception bit
+        IOC = (1 << 0), // Invalid Operation cumulative exception bit
+
+        RoundNearest = (0 << 22),
+        RoundPlusInf = (1 << 22),
+        RoundMinusInf = (2 << 22),
+        RoundToZero = (3 << 22),
+
+        // Default FPSCR value for threads
+        ThreadDefault = DefaultNan | FlushToZero | RoundToZero | IXC
+    };
+}

include/cpu_dynarmic.hpp

@@ -119,6 +119,14 @@ public:
         return jit->Regs();
     }
 
+    // Get reference to array of FPRs. This array consists of the FPRs as single precision values
+    // Hence why its base type is u32
+    // Note: Dynarmic keeps 64 VFP registers as VFPv3 extends the VFP register set to 64 registers.
+    // However the 3DS ARM11 is an ARMv6k processor with VFPv2, so only the first 32 registers are actually used
+    std::array<u32, 64>& fprs() {
+        return jit->ExtRegs();
+    }
+
     void setCPSR(u32 value) {
         jit->SetCpsr(value);
     }
@@ -127,6 +135,19 @@ public:
         return jit->Cpsr();
     }
 
+    void setFPSCR(u32 value) {
+        jit->SetFpscr(value);
+    }
+
+    u32 getFPSCR() {
+        return jit->Fpscr();
+    }
+
+    // Set the base pointer to thread-local storage, stored in a CP15 register on the 3DS
+    void setTLSBase(u32 value) {
+        cp15->setTLSBase(value);
+    }
+
     void runFrame() {
         env.ticksLeft = 268111856 / 60;
         const auto exitReason = jit->Run();

include/dynarmic_cp15.hpp

@@ -62,7 +62,10 @@ class CP15 final : public Dynarmic::A32::Coprocessor {
     }
 
 public:
-    void reset() {
+    void setTLSBase(u32 value) {
-        threadStoragePointer = VirtualAddrs::TLSBase;
+        threadStoragePointer = value;
     }
+
+    // Currently does nothing but may be needed in the future
+    void reset() {}
 };

include/kernel/kernel.hpp

@@ -6,6 +6,7 @@
 #include "kernel_types.hpp"
 #include "helpers.hpp"
 #include "memory.hpp"
+#include "resource_limits.hpp"
 #include "services/service_manager.hpp"
 
 class CPU;
@@ -17,12 +18,13 @@ class Kernel {
 
 	// The handle number for the next kernel object to be created
 	u32 handleCounter;
+	std::array<Thread, appResourceLimits.maxThreads> threads;
 	std::vector<KernelObject> objects;
 	std::vector<Handle> portHandles;
 
 	Handle currentProcess;
-	Handle currentThread;
 	Handle mainThread;
+	int currentThreadIndex;
 	Handle srvHandle; // Handle for the special service manager port "srv:"
 	u32 arbiterCount;
 	u32 threadCount;
@@ -64,6 +66,8 @@ class Kernel {
 	Handle makeSession(Handle port);
 	Handle makeThread(u32 entrypoint, u32 initialSP, u32 priority, u32 id, ThreadStatus status = ThreadStatus::Dormant);
 
+	void switchThread(int newThreadIndex);
+
 	std::optional<Handle> getPortHandle(const char* name);
 	void deleteObjectData(KernelObject& object);
 

include/kernel/kernel_types.hpp

@@ -1,4 +1,5 @@
 #pragma once
+#include <array>
 #include <cstring>
 #include "handles.hpp"
 #include "helpers.hpp"
@@ -101,11 +102,15 @@ struct Thread {
     u32 entrypoint; // Initial r15 value
     u32 priority;
     u32 processorID;
-
     ThreadStatus status;
+    Handle handle;  // OS handle for this thread
 
-    Thread(u32 initialSP, u32 entrypoint, u32 priority, u32 processorID, ThreadStatus status = ThreadStatus::Dormant)
+    // Thread context used for switching between threads
-        : initialSP(initialSP), entrypoint(entrypoint), priority(priority), processorID(processorID), status(status) {}
+    std::array<u32, 16> gprs;
+    std::array<u32, 32> fprs; // Stored as u32 because dynarmic does it
+    u32 cpsr;
+    u32 fpscr;
+    u32 tlsBase; // Base pointer for thread-local storage
 };
 
 static const char* kernelObjectTypeToString(KernelObjectType t) {

include/kernel/resource_limits.hpp

@@ -1,5 +1,5 @@
 #pragma once
-#include "kernel_types.hpp"
+#include "helpers.hpp"
 
 // Values and resource limit structure taken from Citra
 

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
+	// 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 == Dummy) {
+	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;
 }