[Kernel] Much better arbiter & thread scheduling impl

include/kernel/kernel.hpp

@@ -23,6 +23,9 @@ class Kernel {
 	std::vector<KernelObject> objects;
 	std::vector<Handle> portHandles;
 
+	// Thread indices, sorted by priority
+	std::vector<int> threadIndices;
+
 	Handle currentProcess;
 	Handle mainThread;
 	int currentThreadIndex;
@@ -62,8 +65,14 @@ class Kernel {
 	Handle makeSession(Handle port);
 	Handle makeThread(u32 entrypoint, u32 initialSP, u32 priority, s32 id, u32 arg,ThreadStatus status = ThreadStatus::Dormant);
 
+	void signalArbiter(u32 waitingAddress, s32 threadCount);
 	void sleepThreadOnArbiter(u32 waitingAddress);
 	void switchThread(int newThreadIndex);
+	void sortThreads();
+	std::optional<int> getNextThread();
+	void switchToNextThread();
+	void rescheduleThreads();
+	bool canThreadRun(const Thread& t);
 
 	std::optional<Handle> getPortHandle(const char* name);
 	void deleteObjectData(KernelObject& object);

src/core/kernel/address_arbiter.cpp

@@ -54,7 +54,7 @@ void Kernel::arbitrateAddress() {
 		regs[0] = SVCResult::InvalidEnumValueAlt;
 		return;
 	}
-	// This needs to put the error code in r0 before we change threats
+	// This needs to put the error code in r0 before we change threads
 	regs[0] = SVCResult::Success;
 
 	switch (static_cast<ArbitrationType>(type)) {
@@ -68,11 +68,32 @@ void Kernel::arbitrateAddress() {
 		}
 
 		case ArbitrationType::Signal:
-			logSVC("Broken ArbitrateAddress (type == SIGNAL)\n");
-			switchThread(0);
+			signalArbiter(address, value);
 			break;
 
 		default:
 			Helpers::panic("ArbitrateAddress: Unimplemented type %s", arbitrationTypeToString(type));
 	}
+}
+
+// Signal up to "threadCount" threads waiting on the arbiter indicated by "waitingAddress"
+void Kernel::signalArbiter(u32 waitingAddress, s32 threadCount) {
+	if (threadCount == 0) [[unlikely]] return;
+	s32 count = 0; // Number of threads we've woken up
+
+	// Wake threads with the highest priority threads being woken up first
+	for (auto index : threadIndices) {
+		Thread& t = threads[index];
+		if (t.status == ThreadStatus::WaitArbiter && t.waitingAddress == waitingAddress) {
+			t.status = ThreadStatus::Ready;
+			count += 1;
+
+			// Check if we've reached the max number of. If count < 0 then all threads are released.
+			if (count == threadCount && threadCount > 0) break;
+		}
+	}
+
+	if (count != 0) {
+		rescheduleThreads();
+	}
 }

src/core/kernel/events.cpp

@@ -72,4 +72,9 @@ void Kernel::waitSynchronizationN() {
 
 	logSVC("WaitSynchronizationN (STUBBED)\n");
 	regs[0] = SVCResult::Success;
+
+	if (currentThreadIndex == 1) {
+		printf("WaitSynchN OoT hack triggered\n");
+		switchThread(0);
+	}
 }

src/core/kernel/kernel.cpp

@@ -7,6 +7,7 @@ Kernel::Kernel(CPU& cpu, Memory& mem, GPU& gpu)
 	: cpu(cpu), regs(cpu.regs()), mem(mem), handleCounter(0), serviceManager(regs, mem, gpu, currentProcess, *this) {
 	objects.reserve(512); // Make room for a few objects to avoid further memory allocs later
 	portHandles.reserve(32);
+	threadIndices.reserve(appResourceLimits.maxThreads);
 
 	for (int i = 0; i < threads.size(); i++) {
 		threads[i].index = i;
@@ -98,6 +99,7 @@ void Kernel::reset() {
 	}
 	objects.clear();
 	portHandles.clear();
+	threadIndices.clear();
 	serviceManager.reset();
 
 	// Allocate handle #0 to a dummy object and make a main process object

src/core/kernel/ports.cpp

@@ -74,7 +74,6 @@ void Kernel::connectToPort() {
 void Kernel::sendSyncRequest() {
 	const auto handle = regs[0];
 	u32 messagePointer = getTLSPointer() + 0x80; // The message is stored starting at TLS+0x80
-
 	logSVC("SendSyncRequest(session handle = %X)\n", handle);
 
 	// The sync request is being sent at a service rather than whatever port, so have the service manager intercept it

src/core/kernel/threads.cpp

@@ -8,13 +8,15 @@
 // 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
+	auto& oldThread = threads[currentThreadIndex];
+	auto& newThread = threads[newThreadIndex];
+	newThread.status = ThreadStatus::Running;
+
+	// Bail early if the new thread is actually the old thread
+	if (currentThreadIndex == newThreadIndex) [[unlikely]] {
 		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
@@ -31,11 +33,69 @@ void Kernel::switchThread(int newThreadIndex) {
 	currentThreadIndex = newThreadIndex;
 }
 
+// Sort the threadIndices vector based on the priority of each thread
+// The threads with higher priority (aka the ones with a lower priority value) should come first in the vector
+void Kernel::sortThreads() {
+	std::vector<int>& v = threadIndices;
+	std::sort(v.begin(), v.end(), [&](int a, int b) {
+		return threads[a].priority < threads[b].priority;
+	});
+}
+
+bool Kernel::canThreadRun(const Thread& t) {
+	if (t.status == ThreadStatus::Ready) {
+		return true;
+	}
+
+	// Handle timeouts and stuff here
+	return false;
+}
+
+// Get the index of the next thread to run by iterating through the thread list and finding the free thread with the highest priority
+// Returns the thread index if a thread is found, or nullopt otherwise
+std::optional<int> Kernel::getNextThread() {
+	for (auto index : threadIndices) {
+		const Thread& t = threads[index];
+
+		// Thread is ready, return it
+		if (canThreadRun(t)) {
+			return index;
+		}
+
+		// TODO: Check timeouts here
+	}
+
+	// No thread was found
+	return std::nullopt;
+}
+
+void Kernel::switchToNextThread() {
+	std::optional<int> newThreadIndex = getNextThread();
+	
+	if (!newThreadIndex.has_value()) {
+		Helpers::panic("Kernel tried to switch to the next thread but none found");
+	} else {
+		switchThread(newThreadIndex.value());
+	}
+}
+
+// See if there;s a higher priority, ready thread and switch to that
+void Kernel::rescheduleThreads() {
+	std::optional<int> newThreadIndex = getNextThread();
+	
+	if (newThreadIndex.has_value()) {
+		threads[currentThreadIndex].status = ThreadStatus::Ready;
+		switchThread(newThreadIndex.value());
+	}
+}
+
 // Internal OS function to spawn a thread
 Handle Kernel::makeThread(u32 entrypoint, u32 initialSP, u32 priority, s32 id, u32 arg, ThreadStatus status) {
 	if (threadCount >= appResourceLimits.maxThreads) {
 		Helpers::panic("Overflowed the number of threads");
 	}
+	threadIndices.push_back(threadCount);
+
 	Thread& t = threads[threadCount++]; // Reference to thread data
 	Handle ret = makeObject(KernelObjectType::Thread);
 	objects[ret].data = &t;
@@ -64,6 +124,7 @@ Handle Kernel::makeThread(u32 entrypoint, u32 initialSP, u32 priority, s32 id, u
 	// Initial TLS base has already been set in Kernel::Kernel()
 	// TODO: Does svcCreateThread zero-set the TLS of the new thread?
 
+	sortThreads();
 	return ret;
 }
 
@@ -90,10 +151,10 @@ void Kernel::createThread() {
 
 void Kernel::sleepThreadOnArbiter(u32 waitingAddress) {
 	Thread& t = threads[currentThreadIndex];
-
 	t.status = ThreadStatus::WaitArbiter;
 	t.waitingAddress = waitingAddress;
-	switchThread(1); // TODO: Properly change threads
+
+	switchToNextThread();
 }
 
 

src/core/services/service_manager.cpp

@@ -103,7 +103,7 @@ void ServiceManager::enableNotification(u32 messagePointer) {
 
 void ServiceManager::receiveNotification(u32 messagePointer) {
 	log("srv::ReceiveNotification() (STUBBED)\n");
-	printf("r15 = %08X", regs[15]);
+
 	mem.write32(messagePointer + 4, Result::Success); // Result code
 	mem.write32(messagePointer + 8, 0); // Notification ID
 }

src/main.cpp

@@ -9,7 +9,7 @@ int main (int argc, char *argv[]) {
 
     emu.initGraphicsContext();
 
-    auto romPath = std::filesystem::current_path() / (argc > 1 ? argv[1] : "OoT.3ds");
+    auto romPath = std::filesystem::current_path() / (argc > 1 ? argv[1] : "SimplerTri.elf");
     if (!emu.loadROM(romPath)) {
         // For some reason just .c_str() doesn't show the proper path
         Helpers::panic("Failed to load ROM file: %s", romPath.string().c_str());