2 files changed, 64 insertions, 16 deletions

diff --git a/src/core/hle/kernel/semaphore.cpp b/src/core/hle/kernel/semaphore.cpp
index 73ffbe3cf..674b727d5 100644
--- a/src/core/hle/kernel/semaphore.cpp
+++ b/src/core/hle/kernel/semaphore.cpp
@@ -2,8 +2,7 @@
 // Licensed under GPLv2+
 // Refer to the license.txt file included.
 
-#include <map>
-#include <vector>
+#include <queue>
 
 #include "common/common.h"
 
@@ -21,12 +20,20 @@ public:
     static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Semaphore; }
     Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::Semaphore; }
 
-    u32 initial_count;                          ///< Number of reserved entries
+    u32 initial_count;                          ///< Number of reserved entries TODO(Subv): Make use of this
     u32 max_count;                              ///< Maximum number of simultaneous holders the semaphore can have
     u32 current_usage;                          ///< Number of currently used entries in the semaphore
-    std::vector<Handle> waiting_threads;        ///< Threads that are waiting for the semaphore
+    std::queue<Handle> waiting_threads;         ///< Threads that are waiting for the semaphore
     std::string name;                           ///< Name of semaphore (optional)
 
+    /**
+     * Tests whether a semaphore is at its peak capacity
+     * @return Whether the semaphore is full
+     */
+    bool IsFull() const {
+        return current_usage == max_count;
+    }
+
     ResultVal<bool> SyncRequest() override {
         // TODO(Subv): ImplementMe
         return MakeResult<bool>(false);
@@ -37,7 +44,7 @@ public:
 
         if (wait) {
             Kernel::WaitCurrentThread(WAITTYPE_SEMA, GetHandle());
-            waiting_threads.push_back(GetCurrentThreadHandle());
+            waiting_threads.push(GetCurrentThreadHandle());
         } else {
             ++current_usage;
         }
@@ -56,21 +63,53 @@ public:
  * @param name Optional name of semaphore
  * @return Pointer to new Semaphore object
  */
-Semaphore* CreateSemaphore(Handle& handle, u32 initial_count, u32 max_count, const std::string& name) {
+Semaphore* CreateSemaphore(Handle& handle, u32 initial_count, 
+    u32 max_count, const std::string& name) {
+
     Semaphore* semaphore = new Semaphore;
-    handle = Kernel::g_object_pool.Create(semaphore);
+    handle = g_object_pool.Create(semaphore);
 
-    semaphore->initial_count = semaphore->current_usage = initial_count;
-    semaphore->max_count = max_count;
+    semaphore->initial_count = initial_count;
+    // When the semaphore is created, all slots are used by the creator thread
+    semaphore->max_count = semaphore->current_usage = max_count;
     semaphore->name = name;
 
     return semaphore;
 }
 
-Handle CreateSemaphore(u32 initial_count, u32 max_count, const std::string& name) {
-    Handle handle;
-    Semaphore* semaphore = CreateSemaphore(handle, initial_count, max_count, name);
-    return handle;
+ResultCode CreateSemaphore(Handle* handle, u32 initial_count, 
+    u32 max_count, const std::string& name) {
+
+    if (initial_count > max_count)
+        return ResultCode(ErrorDescription::InvalidCombination, ErrorModule::Kernel,
+                          ErrorSummary::WrongArgument, ErrorLevel::Permanent);
+    Semaphore* semaphore = CreateSemaphore(*handle, initial_count, max_count, name);
+
+    return RESULT_SUCCESS;
+}
+
+ResultCode ReleaseSemaphore(s32* count, Handle handle, s32 release_count) {
+
+    Semaphore* semaphore = g_object_pool.Get<Semaphore>(handle);
+    if (semaphore == nullptr)
+        return InvalidHandle(ErrorModule::Kernel);
+
+    if (semaphore->current_usage < release_count)
+        return ResultCode(ErrorDescription::OutOfRange, ErrorModule::Kernel, 
+                          ErrorSummary::InvalidArgument, ErrorLevel::Permanent);
+
+    *count = semaphore->max_count - semaphore->current_usage;
+    semaphore->current_usage = semaphore->current_usage - release_count;
+
+    // Notify some of the threads that the semaphore has been released
+    // stop once the semaphore is full again or there are no more waiting threads
+    while (!semaphore->waiting_threads.empty() && !semaphore->IsFull()) {
+        Kernel::ResumeThreadFromWait(semaphore->waiting_threads.front());
+        semaphore->waiting_threads.pop();
+        semaphore->current_usage++;
+    }
+
+    return RESULT_SUCCESS;
 }
 
 } // namespace
diff --git a/src/core/hle/kernel/semaphore.h b/src/core/hle/kernel/semaphore.h
index 6a686db2e..854831ecf 100644
--- a/src/core/hle/kernel/semaphore.h
+++ b/src/core/hle/kernel/semaphore.h
@@ -12,11 +12,20 @@ namespace Kernel {
 
 /**
  * Creates a semaphore
+ * @param handle Pointer to the handle of the newly created object
  * @param initial_count number of reserved entries in the semaphore
  * @param max_count maximum number of holders the semaphore can have
-  * @param name Optional name of semaphore
- * @return Handle to newly created object
+ * @param name Optional name of semaphore
+ * @return ResultCode of the error
  */
-Handle CreateSemaphore(u32 initial_count, u32 max_count, const std::string& name = "Unknown");
+ResultCode CreateSemaphore(Handle* handle, u32 initial_count, u32 max_count, const std::string& name = "Unknown");
 
+/**
+ * Releases a certain number of slots from a semaphore
+ * @param count The number of free slots the semaphore had before this call
+ * @param handle The handle of the semaphore to release
+ * @param release_count The number of slots to release
+ * @return ResultCode of the error
+ */
+ResultCode ReleaseSemaphore(s32* count, Handle handle, s32 release_count);
 } // namespace