From 09ba66ae60f8a7f2567bf08fe5c6f855dd6af23d Mon Sep 17 00:00:00 2001
From: wheremyfoodat <gponiris2004@gmail.com>
Date: Sun, 7 May 2023 18:02:51 +0300
Subject: [PATCH] [Kernel] Fully implement semaphores

---
 include/kernel/kernel.hpp   |  2 ++
 src/core/kernel/kernel.cpp  |  2 ++
 src/core/kernel/threads.cpp | 58 +++++++++++++++++++++++++++++++++++++
 3 files changed, 62 insertions(+)

diff --git a/include/kernel/kernel.hpp b/include/kernel/kernel.hpp
index 95a6bf88..d43b88c0 100644
--- a/include/kernel/kernel.hpp
+++ b/include/kernel/kernel.hpp
@@ -133,7 +133,9 @@ private:
 	void svcCloseHandle();
 	void svcCreateEvent();
 	void svcCreateMutex();
+	void svcCreateSemaphore();
 	void svcReleaseMutex();
+	void svcReleaseSemaphore();
 	void svcSignalEvent();
 	void svcSleepThread();
 	void connectToPort();
diff --git a/src/core/kernel/kernel.cpp b/src/core/kernel/kernel.cpp
index 78c858c6..f93dd386 100644
--- a/src/core/kernel/kernel.cpp
+++ b/src/core/kernel/kernel.cpp
@@ -36,6 +36,8 @@ void Kernel::serviceSVC(u32 svc) {
 		case 0x0C: setThreadPriority(); break;
 		case 0x13: svcCreateMutex(); break;
 		case 0x14: svcReleaseMutex(); break;
+		case 0x15: svcCreateSemaphore(); break;
+		case 0x16: svcReleaseSemaphore(); break;
 		case 0x17: svcCreateEvent(); break;
 		case 0x18: svcSignalEvent(); break;
 		case 0x19: svcClearEvent(); break;
diff --git a/src/core/kernel/threads.cpp b/src/core/kernel/threads.cpp
index a2f29cd5..8a87edb7 100644
--- a/src/core/kernel/threads.cpp
+++ b/src/core/kernel/threads.cpp
@@ -231,6 +231,15 @@ void Kernel::acquireSyncObject(KernelObject* object, const Thread& thread) {
 			break;
 		}
 
+		case KernelObjectType::Semaphore: {
+			Semaphore* s = object->getData<Semaphore>();
+			if (s->availableCount <= 0) [[unlikely]] // This should be unreachable but let's check anyways
+				Helpers::panic("Tried to acquire unacquirable semaphore");
+
+			s->availableCount -= 1;
+			break;
+		}
+
 		case KernelObjectType::Thread:
 			break;
 
@@ -493,6 +502,55 @@ void Kernel::svcReleaseMutex() {
 	releaseMutex(moo);
 }
 
+void Kernel::svcCreateSemaphore() {
+	s32 initialCount = static_cast<s32>(regs[1]);
+	s32 maxCount = static_cast<s32>(regs[2]);
+	logSVC("CreateSemaphore (initial count = %d, max count = %d)\n", initialCount, maxCount);
+
+	if (initialCount > maxCount)
+		Helpers::panic("CreateSemaphore: Initial count higher than max count");
+
+	if (initialCount < 0 || maxCount < 0)
+		Helpers::panic("CreateSemaphore: Negative count value");
+
+	regs[0] = SVCResult::Success;
+	regs[1] = makeSemaphore(initialCount, maxCount);
+}
+
+void Kernel::svcReleaseSemaphore() {
+	const Handle handle = regs[1];
+	const s32 releaseCount = static_cast<s32>(regs[2]);
+	logSVC("ReleaseSemaphore (handle = %X, release count = %d)\n", handle, releaseCount);
+
+	const auto object = getObject(handle, KernelObjectType::Semaphore);
+	if (object == nullptr) [[unlikely]] {
+		Helpers::panic("Tried to release non-existent semaphore");
+		regs[0] = SVCResult::BadHandle;
+		return;
+	}
+
+	if (releaseCount < 0)
+		Helpers::panic("ReleaseSemaphore: Negative count");
+
+	Semaphore* s = object->getData<Semaphore>();
+	if (s->maximumCount - s->availableCount < releaseCount)
+		Helpers::panic("ReleaseSemaphore: Release count too high");
+
+	// Write success and old available count to r0 and r1 respectively
+	regs[0] = SVCResult::Success;
+	regs[1] = s->availableCount;
+	// Bump available count
+	s->availableCount += releaseCount;
+
+	// Wake up threads one by one until the available count hits 0 or we run out of threads to wake up
+	while (s->availableCount > 0 && s->waitlist != 0) {
+		int index = wakeupOneThread(s->waitlist, handle); // Wake up highest priority thread
+		s->waitlist ^= (1ull << index); // Remove thread from waitlist
+
+		s->availableCount--; // Decrement available count
+	}
+}
+
 // Returns whether an object is waitable or not
 // The KernelObject type enum is arranged in a specific order in kernel_types.hpp so this
 // can simply compile to a fast sub+cmp+set despite looking slow