9 files changed, 68 insertions, 113 deletions

diff --git a/src/core/hle/function_wrappers.h b/src/core/hle/function_wrappers.h
index 23c86a72d..5949cb470 100644
--- a/src/core/hle/function_wrappers.h
+++ b/src/core/hle/function_wrappers.h
@@ -9,11 +9,15 @@
 #include "core/arm/arm_interface.h"
 #include "core/memory.h"
 #include "core/hle/hle.h"
+#include "core/hle/result.h"
 
 namespace HLE {
 
 #define PARAM(n)    Core::g_app_core->GetReg(n)
 
+/// An invalid result code that is meant to be overwritten when a thread resumes from waiting
+static const ResultCode RESULT_INVALID(0xDEADC0DE);
+
 /**
  * HLE a function return from the current ARM11 userland process
  * @param res Result to return
@@ -57,8 +61,11 @@ template<ResultCode func(s32*, u32*, s32, bool, s64)> void Wrap() {
     s32 param_1 = 0;
     s32 retval = func(&param_1, (Handle*)Memory::GetPointer(PARAM(1)), (s32)PARAM(2),
         (PARAM(3) != 0), (((s64)PARAM(4) << 32) | PARAM(0))).raw;
-    Core::g_app_core->SetReg(1, (u32)param_1);
-    FuncReturn(retval);
+
+    if (retval != RESULT_INVALID.raw) {
+        Core::g_app_core->SetReg(1, (u32)param_1);
+        FuncReturn(retval);
+    }
 }
 
 template<ResultCode func(u32, u32, u32, u32, s64)> void Wrap() {
@@ -73,7 +80,11 @@ template<ResultCode func(u32*)> void Wrap(){
 }
 
 template<ResultCode func(u32, s64)> void Wrap() {
-    FuncReturn(func(PARAM(0), (((s64)PARAM(3) << 32) | PARAM(2))).raw);
+    s32 retval = func(PARAM(0), (((s64)PARAM(3) << 32) | PARAM(2))).raw;
+
+    if (retval != RESULT_INVALID.raw) {
+        FuncReturn(retval);
+    }
 }
 
 template<ResultCode func(void*, void*, u32)> void Wrap(){
diff --git a/src/core/hle/kernel/event.cpp b/src/core/hle/kernel/event.cpp
index e45deb1c6..f338f3266 100644
--- a/src/core/hle/kernel/event.cpp
+++ b/src/core/hle/kernel/event.cpp
@@ -41,10 +41,7 @@ void Event::Acquire() {
 
 void Event::Signal() {
     signaled = true;
-
     WakeupAllWaitingThreads();
-
-    HLE::Reschedule(__func__);
 }
 
 void Event::Clear() {
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 726e4d2ff..20e11da16 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -32,27 +32,13 @@ void WaitObject::RemoveWaitingThread(Thread* thread) {
         waiting_threads.erase(itr);
 }
 
-SharedPtr<Thread> WaitObject::WakeupNextThread() {
-    if (waiting_threads.empty())
-        return nullptr;
-
-    auto next_thread = std::move(waiting_threads.front());
-    waiting_threads.erase(waiting_threads.begin());
-
-    next_thread->ReleaseWaitObject(this);
-
-    return next_thread;
-}
-
 void WaitObject::WakeupAllWaitingThreads() {
-    auto waiting_threads_copy = waiting_threads;
+    for (auto thread : waiting_threads)
+        thread->ResumeFromWait();
 
-    // We use a copy because ReleaseWaitObject will remove the thread from this object's
-    // waiting_threads list
-    for (auto thread : waiting_threads_copy)
-        thread->ReleaseWaitObject(this);
+    waiting_threads.clear();
 
-    ASSERT_MSG(waiting_threads.empty(), "failed to awaken all waiting threads!");
+    HLE::Reschedule(__func__);
 }
 
 HandleTable::HandleTable() {
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index a5a0f4800..64595f758 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -140,12 +140,6 @@ public:
      */
     void RemoveWaitingThread(Thread* thread);
 
-    /**
-     * Wake up the next thread waiting on this object
-     * @return Pointer to the thread that was resumed, nullptr if no threads are waiting
-     */
-    SharedPtr<Thread> WakeupNextThread();
-
     /// Wake up all threads waiting on this object
     void WakeupAllWaitingThreads();
 
diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index 6aa73df86..edb97d324 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -23,12 +23,7 @@ static void ResumeWaitingThread(Mutex* mutex) {
     // Reset mutex lock thread handle, nothing is waiting
     mutex->lock_count = 0;
     mutex->holding_thread = nullptr;
-
-    // Find the next waiting thread for the mutex...
-    auto next_thread = mutex->WakeupNextThread();
-    if (next_thread != nullptr) {
-        mutex->Acquire(next_thread);
-    }
+    mutex->WakeupAllWaitingThreads();
 }
 
 void ReleaseThreadMutexes(Thread* thread) {
@@ -94,8 +89,6 @@ void Mutex::Release() {
             ResumeWaitingThread(this);
         }
     }
-
-    HLE::Reschedule(__func__);
 }
 
 } // namespace
diff --git a/src/core/hle/kernel/semaphore.cpp b/src/core/hle/kernel/semaphore.cpp
index 96d61ed3a..4b359ed07 100644
--- a/src/core/hle/kernel/semaphore.cpp
+++ b/src/core/hle/kernel/semaphore.cpp
@@ -48,13 +48,7 @@ ResultVal<s32> Semaphore::Release(s32 release_count) {
     s32 previous_count = available_count;
     available_count += 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 (!ShouldWait() && WakeupNextThread() != nullptr) {
-        Acquire();
-    }
-
-    HLE::Reschedule(__func__);
+    WakeupAllWaitingThreads();
 
     return MakeResult<s32>(previous_count);
 }
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 22c795ad4..4729a7fe0 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -13,6 +13,7 @@
 #include "common/thread_queue_list.h"
 
 #include "core/arm/arm_interface.h"
+#include "core/arm/skyeye_common/armdefs.h"
 #include "core/core.h"
 #include "core/core_timing.h"
 #include "core/hle/hle.h"
@@ -193,8 +194,22 @@ static void SwitchContext(Thread* new_thread) {
     if (new_thread) {
         DEBUG_ASSERT_MSG(new_thread->status == THREADSTATUS_READY, "Thread must be ready to become running.");
 
+        // Cancel any outstanding wakeup events for this thread
+        CoreTiming::UnscheduleEvent(ThreadWakeupEventType, new_thread->callback_handle);
+
         current_thread = new_thread;
 
+        // If the thread was waited by a svcWaitSynch call, step back PC by one instruction to rerun
+        // the SVC when the thread wakes up. This is necessary to ensure that the thread can acquire
+        // the requested wait object(s) before continuing.
+        if (new_thread->waitsynch_waited) {
+            // CPSR flag indicates CPU mode
+            bool thumb_mode = (new_thread->context.cpsr & TBIT) != 0;
+
+            // SVC instruction is 2 bytes for THUMB, 4 bytes for ARM
+            new_thread->context.pc -= thumb_mode ? 2 : 4;
+        }
+
         ready_queue.remove(new_thread->current_priority, new_thread);
         new_thread->status = THREADSTATUS_RUNNING;
 
@@ -243,6 +258,7 @@ void WaitCurrentThread_WaitSynchronization(std::vector<SharedPtr<WaitObject>> wa
     thread->wait_set_output = wait_set_output;
     thread->wait_all = wait_all;
     thread->wait_objects = std::move(wait_objects);
+    thread->waitsynch_waited = true;
     thread->status = THREADSTATUS_WAIT_SYNCH;
 }
 
@@ -268,6 +284,8 @@ static void ThreadWakeupCallback(u64 thread_handle, int cycles_late) {
         return;
     }
 
+    thread->waitsynch_waited = false;
+
     if (thread->status == THREADSTATUS_WAIT_SYNCH) {
         thread->SetWaitSynchronizationResult(ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
                                                         ErrorSummary::StatusChanged, ErrorLevel::Info));
@@ -288,63 +306,20 @@ void Thread::WakeAfterDelay(s64 nanoseconds) {
     CoreTiming::ScheduleEvent(usToCycles(microseconds), ThreadWakeupEventType, callback_handle);
 }
 
-void Thread::ReleaseWaitObject(WaitObject* wait_object) {
-    if (status != THREADSTATUS_WAIT_SYNCH || wait_objects.empty()) {
-        LOG_CRITICAL(Kernel, "thread is not waiting on any objects!");
-        return;
-    }
-
-    // Remove this thread from the waiting object's thread list
-    wait_object->RemoveWaitingThread(this);
-
-    unsigned index = 0;
-    bool wait_all_failed = false; // Will be set to true if any object is unavailable
-
-    // Iterate through all waiting objects to check availability...
-    for (auto itr = wait_objects.begin(); itr != wait_objects.end(); ++itr) {
-        if ((*itr)->ShouldWait())
-            wait_all_failed = true;
-
-        // The output should be the last index of wait_object
-        if (*itr == wait_object)
-            index = itr - wait_objects.begin();
-    }
-
-    // If we are waiting on all objects...
-    if (wait_all) {
-        // Resume the thread only if all are available...
-        if (!wait_all_failed) {
-            SetWaitSynchronizationResult(RESULT_SUCCESS);
-            SetWaitSynchronizationOutput(-1);
-
-            ResumeFromWait();
-        }
-    } else {
-        // Otherwise, resume
-        SetWaitSynchronizationResult(RESULT_SUCCESS);
-
-        if (wait_set_output)
-            SetWaitSynchronizationOutput(index);
-
-        ResumeFromWait();
-    }
-}
-
 void Thread::ResumeFromWait() {
-    // Cancel any outstanding wakeup events for this thread
-    CoreTiming::UnscheduleEvent(ThreadWakeupEventType, callback_handle);
-
     switch (status) {
         case THREADSTATUS_WAIT_SYNCH:
-            // Remove this thread from all other WaitObjects
-            for (auto wait_object : wait_objects)
-                wait_object->RemoveWaitingThread(this);
-            break;
         case THREADSTATUS_WAIT_ARB:
         case THREADSTATUS_WAIT_SLEEP:
             break;
-        case THREADSTATUS_RUNNING:
+
         case THREADSTATUS_READY:
+            // If the thread is waiting on multiple wait objects, it might be awoken more than once
+            // before actually resuming. We can ignore subsequent wakeups if the thread status has
+            // already been set to THREADSTATUS_READY.
+            return;
+
+        case THREADSTATUS_RUNNING:
             DEBUG_ASSERT_MSG(false, "Thread with object id %u has already resumed.", GetObjectId());
             return;
         case THREADSTATUS_DEAD:
@@ -415,6 +390,7 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     thread->callback_handle = wakeup_callback_handle_table.Create(thread).MoveFrom();
     thread->owner_process = g_current_process;
     thread->tls_index = -1;
+    thread->waitsynch_waited = false;
 
     // Find the next available TLS index, and mark it as used
     auto& used_tls_slots = Kernel::g_current_process->used_tls_slots;
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 2c65419c3..b8160bb2c 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -96,12 +96,6 @@ public:
     u32 GetThreadId() const { return thread_id; }
 
     /**
-     * Release an acquired wait object
-     * @param wait_object WaitObject to release
-     */
-    void ReleaseWaitObject(WaitObject* wait_object);
-
-    /**
      * Resumes a thread from waiting
      */
     void ResumeFromWait();
@@ -152,6 +146,8 @@ public:
 
     s32 tls_index; ///< Index of the Thread Local Storage of the thread
 
+    bool waitsynch_waited; ///< Set to true if the last svcWaitSynch call caused the thread to wait
+
     /// Mutexes currently held by this thread, which will be released when it exits.
     boost::container::flat_set<SharedPtr<Mutex>> held_mutexes;
 
@@ -163,12 +159,12 @@ public:
 
     std::string name;
 
+    /// Handle used as userdata to reference this object when inserting into the CoreTiming queue.
+    Handle callback_handle;
+
 private:
     Thread();
     ~Thread() override;
-
-    /// Handle used as userdata to reference this object when inserting into the CoreTiming queue.
-    Handle callback_handle;
 };
 
 /**
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index d1555c753..6cde4fc87 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -40,9 +40,6 @@ const ResultCode ERR_NOT_FOUND(ErrorDescription::NotFound, ErrorModule::Kernel,
 const ResultCode ERR_PORT_NAME_TOO_LONG(ErrorDescription(30), ErrorModule::OS,
         ErrorSummary::InvalidArgument, ErrorLevel::Usage); // 0xE0E0181E
 
-/// An invalid result code that is meant to be overwritten when a thread resumes from waiting
-const ResultCode RESULT_INVALID(0xDEADC0DE);
-
 enum ControlMemoryOperation {
     MEMORY_OPERATION_HEAP       = 0x00000003,
     MEMORY_OPERATION_GSP_HEAP   = 0x00010003,
@@ -143,6 +140,10 @@ static ResultCode CloseHandle(Handle handle) {
 /// Wait for a handle to synchronize, timeout after the specified nanoseconds
 static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
     auto object = Kernel::g_handle_table.GetWaitObject(handle);
+    Kernel::Thread* thread = Kernel::GetCurrentThread();
+
+    thread->waitsynch_waited = false;
+
     if (object == nullptr)
         return ERR_INVALID_HANDLE;
 
@@ -154,14 +155,14 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
     // Check for next thread to schedule
     if (object->ShouldWait()) {
 
-        object->AddWaitingThread(Kernel::GetCurrentThread());
+        object->AddWaitingThread(thread);
         Kernel::WaitCurrentThread_WaitSynchronization({ object }, false, false);
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
-        Kernel::GetCurrentThread()->WakeAfterDelay(nano_seconds);
+        thread->WakeAfterDelay(nano_seconds);
 
         // NOTE: output of this SVC will be set later depending on how the thread resumes
-        return RESULT_INVALID;
+        return HLE::RESULT_INVALID;
     }
 
     object->Acquire();
@@ -173,6 +174,9 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
 static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_count, bool wait_all, s64 nano_seconds) {
     bool wait_thread = !wait_all;
     int handle_index = 0;
+    Kernel::Thread* thread = Kernel::GetCurrentThread();
+    bool was_waiting = thread->waitsynch_waited;
+    thread->waitsynch_waited = false;
 
     // Check if 'handles' is invalid
     if (handles == nullptr)
@@ -190,6 +194,9 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
     // necessary
     if (handle_count != 0) {
         bool selected = false; // True once an object has been selected
+
+        Kernel::SharedPtr<Kernel::WaitObject> wait_object;
+
         for (int i = 0; i < handle_count; ++i) {
             auto object = Kernel::g_handle_table.GetWaitObject(handles[i]);
             if (object == nullptr)
@@ -204,10 +211,11 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
                     wait_thread = true;
             } else {
                 // Do not wait on this object, check if this object should be selected...
-                if (!wait_all && !selected) {
+                if (!wait_all && (!selected || (wait_object == object && was_waiting))) {
                     // Do not wait the thread
                     wait_thread = false;
                     handle_index = i;
+                    wait_object = object;
                     selected = true;
                 }
             }
@@ -241,7 +249,7 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         Kernel::GetCurrentThread()->WakeAfterDelay(nano_seconds);
 
         // NOTE: output of this SVC will be set later depending on how the thread resumes
-        return RESULT_INVALID;
+        return HLE::RESULT_INVALID;
     }
 
     // Acquire objects if we did not wait...
@@ -261,7 +269,7 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
 
     // TODO(bunnei): If 'wait_all' is true, this is probably wrong. However, real hardware does
     // not seem to set it to any meaningful value.
-    *out = wait_all ? 0 : handle_index;
+    *out = handle_count != 0 ? (wait_all ? -1 : handle_index) : 0;
 
     return RESULT_SUCCESS;
 }