Kernel: Added real support for thread and event blocking

- SVC: Added ExitThread support
- SVC: Added SignalEvent support
- Thread: Added WAITTYPE_EVENT for waiting threads for event signals
- Thread: Added support for blocking on other threads to finish (e.g. Thread::Join)
- Thread: Added debug function for printing current threads ready for execution
- Thread: Removed hack/broken thread ready state code from Kernel::Reschedule
- Mutex: Moved WaitCurrentThread from SVC to Mutex::WaitSynchronization
- Event: Added support for blocking threads on event signalling

Kernel: Added missing algorithm #include for use of std::find on non-Windows platforms.

6 files changed, 196 insertions, 76 deletions

diff --git a/src/core/hle/kernel/event.cpp b/src/core/hle/kernel/event.cpp
index 70e50115d..787e9f5fd 100644
--- a/src/core/hle/kernel/event.cpp
+++ b/src/core/hle/kernel/event.cpp
@@ -3,12 +3,14 @@
 // Refer to the license.txt file included.  
 
 #include <map>
+#include <algorithm>
 #include <vector>
 
 #include "common/common.h"
 
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/event.h"
+#include "core/hle/kernel/thread.h"
 
 namespace Kernel {
 
@@ -20,12 +22,13 @@ public:
     static Kernel::HandleType GetStaticHandleType() {  return Kernel::HandleType::Event; }
     Kernel::HandleType GetHandleType() const { return Kernel::HandleType::Event; }
 
-    ResetType intitial_reset_type;  ///< ResetType specified at Event initialization
-    ResetType reset_type;           ///< Current ResetType
+    ResetType intitial_reset_type;          ///< ResetType specified at Event initialization
+    ResetType reset_type;                   ///< Current ResetType
 
-    bool locked;                    ///< Current locked state
-    bool permanent_locked;          ///< Hack - to set event permanent state (for easy passthrough)
-    std::string name;               ///< Name of event (optional)
+    bool locked;                            ///< Event signal wait
+    bool permanent_locked;                  ///< Hack - to set event permanent state (for easy passthrough)
+    std::vector<Handle> waiting_threads;    ///< Threads that are waiting for the event
+    std::string name;                       ///< Name of event (optional)
 
     /**
      * Synchronize kernel object 
@@ -44,8 +47,14 @@ public:
      * @return Result of operation, 0 on success, otherwise error code
      */
     Result WaitSynchronization(bool* wait) {
-        // TODO(bunnei): ImplementMe
         *wait = locked;
+        if (locked) {
+            Handle thread = GetCurrentThreadHandle();
+            if (std::find(waiting_threads.begin(), waiting_threads.end(), thread) == waiting_threads.end()) {
+                waiting_threads.push_back(thread);
+            }
+            Kernel::WaitCurrentThread(WAITTYPE_EVENT);
+        }
         if (reset_type != RESETTYPE_STICKY && !permanent_locked) {
             locked = true;
         }
@@ -54,6 +63,22 @@ public:
 };
 
 /**
+ * Hackish function to set an events permanent lock state, used to pass through synch blocks
+ * @param handle Handle to event to change
+ * @param permanent_locked Boolean permanent locked value to set event
+ * @return Result of operation, 0 on success, otherwise error code
+ */
+Result SetPermanentLock(Handle handle, const bool permanent_locked) {
+    Event* evt = g_object_pool.GetFast<Event>(handle);
+    if (!evt) {
+        ERROR_LOG(KERNEL, "called with unknown handle=0x%08X", handle);
+        return -1;
+    }
+    evt->permanent_locked = permanent_locked;
+    return 0;
+}
+
+/**
  * Changes whether an event is locked or not
  * @param handle Handle to event to change
  * @param locked Boolean locked value to set event
@@ -72,18 +97,32 @@ Result SetEventLocked(const Handle handle, const bool locked) {
 }
 
 /**
- * Hackish function to set an events permanent lock state, used to pass through synch blocks
- * @param handle Handle to event to change
- * @param permanent_locked Boolean permanent locked value to set event
+ * Signals an event
+ * @param handle Handle to event to signal
  * @return Result of operation, 0 on success, otherwise error code
  */
-Result SetPermanentLock(Handle handle, const bool permanent_locked) {
+Result SignalEvent(const Handle handle) {
     Event* evt = g_object_pool.GetFast<Event>(handle);
     if (!evt) {
         ERROR_LOG(KERNEL, "called with unknown handle=0x%08X", handle);
         return -1;
     }
-    evt->permanent_locked = permanent_locked;
+    // Resume threads waiting for event to signal
+    bool event_caught = false;
+    for (size_t i = 0; i < evt->waiting_threads.size(); ++i) {
+        ResumeThreadFromWait( evt->waiting_threads[i]);
+
+        // If any thread is signalled awake by this event, assume the event was "caught" and reset 
+        // the event. This will result in the next thread waiting on the event to block. Otherwise,
+        // the event will not be reset, and the next thread to call WaitSynchronization on it will
+        // not block. Not sure if this is correct behavior, but it seems to work.
+        event_caught = true;
+    }
+    evt->waiting_threads.clear();
+
+    if (!evt->permanent_locked) {
+        evt->locked = event_caught;
+    }
     return 0;
 }
 
@@ -93,7 +132,15 @@ Result SetPermanentLock(Handle handle, const bool permanent_locked) {
  * @return Result of operation, 0 on success, otherwise error code
  */
 Result ClearEvent(Handle handle) {
-    return SetEventLocked(handle, true);
+    Event* evt = g_object_pool.GetFast<Event>(handle);
+    if (!evt) {
+        ERROR_LOG(KERNEL, "called with unknown handle=0x%08X", handle);
+        return -1;
+    }
+    if (!evt->permanent_locked) {
+        evt->locked = true;
+    }
+    return 0;
 }
 
 /**
diff --git a/src/core/hle/kernel/event.h b/src/core/hle/kernel/event.h
index eed09f0e3..3527b01fd 100644
--- a/src/core/hle/kernel/event.h
+++ b/src/core/hle/kernel/event.h
@@ -28,6 +28,13 @@ Result SetEventLocked(const Handle handle, const bool locked);
 Result SetPermanentLock(Handle handle, const bool permanent_locked);
 
 /**
+ * Signals an event
+ * @param handle Handle to event to signal
+ * @return Result of operation, 0 on success, otherwise error code
+ */
+Result SignalEvent(const Handle handle);
+
+/**
  * Clears an event
  * @param handle Handle to event to clear
  * @return Result of operation, 0 on success, otherwise error code
diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index 7e60fbfe0..133c43079 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -46,6 +46,11 @@ public:
     Result WaitSynchronization(bool* wait) {
         // TODO(bunnei): ImplementMe
         *wait = locked;
+
+        if (locked) {
+            Kernel::WaitCurrentThread(WAITTYPE_MUTEX);
+        }
+
         return 0;
     }
 };
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index c84fdf91d..d372df709 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -5,6 +5,7 @@
 #include <stdio.h>
 
 #include <list>
+#include <algorithm>
 #include <vector>
 #include <map>
 #include <string>
@@ -52,7 +53,14 @@ public:
      * @return Result of operation, 0 on success, otherwise error code
      */
     Result WaitSynchronization(bool* wait) {
-        // TODO(bunnei): ImplementMe
+        if (status != THREADSTATUS_DORMANT) {
+            Handle thread = GetCurrentThreadHandle();
+            if (std::find(waiting_threads.begin(), waiting_threads.end(), thread) == waiting_threads.end()) {
+                waiting_threads.push_back(thread);
+            }
+            WaitCurrentThread(WAITTYPE_THREADEND, this->GetHandle());
+            *wait = true;
+        }
         return 0;
     }
 
@@ -69,6 +77,9 @@ public:
     s32 processor_id;
 
     WaitType wait_type;
+    Handle wait_handle;
+
+    std::vector<Handle> waiting_threads;
 
     char name[Kernel::MAX_NAME_LENGTH + 1];
 };
@@ -82,7 +93,6 @@ Common::ThreadQueueList<Handle> g_thread_ready_queue;
 Handle g_current_thread_handle;
 Thread* g_current_thread;
 
-
 /// Gets the current thread
 inline Thread* GetCurrentThread() {
     return g_current_thread;
@@ -114,15 +124,15 @@ void ResetThread(Thread* t, u32 arg, s32 lowest_priority) {
     memset(&t->context, 0, sizeof(ThreadContext));
 
     t->context.cpu_registers[0] = arg;
-    t->context.pc = t->entry_point;
+    t->context.pc = t->context.cpu_registers[15] = t->entry_point;
     t->context.sp = t->stack_top;
     t->context.cpsr = 0x1F; // Usermode
     
     if (t->current_priority < lowest_priority) {
         t->current_priority = t->initial_priority;
     }
-        
     t->wait_type = WAITTYPE_NONE;
+    t->wait_handle = 0;
 }
 
 /// Change a thread to "ready" state
@@ -142,6 +152,43 @@ void ChangeReadyState(Thread* t, bool ready) {
     }
 }
 
+/// Verify that a thread has not been released from waiting
+inline bool VerifyWait(const Handle& thread, WaitType type, Handle handle) {
+    Handle wait_id = 0;
+    Thread *t = g_object_pool.GetFast<Thread>(thread);
+    if (t) {
+        if (type == t->wait_type && handle == t->wait_handle) {
+            return true;
+        }
+    } else {
+        ERROR_LOG(KERNEL, "thread 0x%08X does not exist", thread);
+    }
+    return false;
+}
+
+/// Stops the current thread
+void StopThread(Handle thread, const char* reason) {
+    u32 error;
+    Thread *t = g_object_pool.Get<Thread>(thread, error);
+    if (t) {
+        ChangeReadyState(t, false);
+        t->status = THREADSTATUS_DORMANT;
+        for (size_t i = 0; i < t->waiting_threads.size(); ++i) {
+            const Handle waiting_thread = t->waiting_threads[i];
+            if (VerifyWait(waiting_thread, WAITTYPE_THREADEND, thread)) {
+                ResumeThreadFromWait(waiting_thread);
+            }
+        }
+        t->waiting_threads.clear();
+
+        // Stopped threads are never waiting.
+        t->wait_type = WAITTYPE_NONE;
+        t->wait_handle = 0;
+    } else {
+        ERROR_LOG(KERNEL, "thread 0x%08X does not exist", thread);
+    }
+}
+
 /// Changes a threads state
 void ChangeThreadState(Thread* t, ThreadStatus new_status) {
     if (!t || t->status == new_status) {
@@ -152,7 +199,7 @@ void ChangeThreadState(Thread* t, ThreadStatus new_status) {
     
     if (new_status == THREADSTATUS_WAIT) {
         if (t->wait_type == WAITTYPE_NONE) {
-            printf("ERROR: Waittype none not allowed here\n");
+            ERROR_LOG(KERNEL, "Waittype none not allowed");
         }
     }
 }
@@ -207,9 +254,10 @@ Thread* NextThread() {
 }
 
 /// Puts the current thread in the wait state for the given type
-void WaitCurrentThread(WaitType wait_type) {
+void WaitCurrentThread(WaitType wait_type, Handle wait_handle) {
     Thread* t = GetCurrentThread();
     t->wait_type = wait_type;
+    t->wait_handle = wait_handle;
     ChangeThreadState(t, ThreadStatus(THREADSTATUS_WAIT | (t->status & THREADSTATUS_SUSPEND)));
 }
 
@@ -225,6 +273,22 @@ void ResumeThreadFromWait(Handle handle) {
     }
 }
 
+/// Prints the thread queue for debugging purposes
+void DebugThreadQueue() {
+    Thread* thread = GetCurrentThread();
+    if (!thread) {
+        return;
+    }
+    INFO_LOG(KERNEL, "0x%02X 0x%08X (current)", thread->current_priority, GetCurrentThreadHandle());
+    for (u32 i = 0; i < g_thread_queue.size(); i++) {
+        Handle handle = g_thread_queue[i];
+        s32 priority = g_thread_ready_queue.contains(handle);
+        if (priority != -1) {
+            INFO_LOG(KERNEL, "0x%02X 0x%08X", priority, handle);
+        }
+    }
+}
+
 /// Creates a new thread
 Thread* CreateThread(Handle& handle, const char* name, u32 entry_point, s32 priority,
     s32 processor_id, u32 stack_top, int stack_size) {
@@ -233,12 +297,12 @@ Thread* CreateThread(Handle& handle, const char* name, u32 entry_point, s32 prio
         "CreateThread priority=%d, outside of allowable range!", priority)
 
     Thread* t = new Thread;
-    
+
     handle = Kernel::g_object_pool.Create(t);
-    
+
     g_thread_queue.push_back(handle);
     g_thread_ready_queue.prepare(priority);
-    
+
     t->status = THREADSTATUS_DORMANT;
     t->entry_point = entry_point;
     t->stack_top = stack_top;
@@ -246,16 +310,18 @@ Thread* CreateThread(Handle& handle, const char* name, u32 entry_point, s32 prio
     t->initial_priority = t->current_priority = priority;
     t->processor_id = processor_id;
     t->wait_type = WAITTYPE_NONE;
-    
+    t->wait_handle = 0;
+
     strncpy(t->name, name, Kernel::MAX_NAME_LENGTH);
     t->name[Kernel::MAX_NAME_LENGTH] = '\0';
-    
+
     return t;
 }
 
 /// Creates a new thread - wrapper for external user
 Handle CreateThread(const char* name, u32 entry_point, s32 priority, u32 arg, s32 processor_id,
     u32 stack_top, int stack_size) {
+
     if (name == NULL) {
         ERROR_LOG(KERNEL, "CreateThread(): NULL name");
         return -1;
@@ -289,7 +355,7 @@ Handle CreateThread(const char* name, u32 entry_point, s32 priority, u32 arg, s3
 
     // This won't schedule to the new thread, but it may to one woken from eating cycles.
     // Technically, this should not eat all at once, and reschedule in the middle, but that's hard.
-    //HLE::Reschedule("thread created");
+    //HLE::Reschedule(__func__);
     
     return handle;
 }
@@ -363,35 +429,24 @@ Handle SetupMainThread(s32 priority, int stack_size) {
     return handle;
 }
 
+
 /// Reschedules to the next available thread (call after current thread is suspended)
 void Reschedule() {
     Thread* prev = GetCurrentThread();
     Thread* next = NextThread();
+    HLE::g_reschedule = false;
     if (next > 0) {
         INFO_LOG(KERNEL, "context switch 0x%08X -> 0x%08X", prev->GetHandle(), next->GetHandle());
-
+        
         SwitchContext(next);
 
-        // Hack - automatically change previous thread (which would have been in "wait" state) to
-        // "ready" state, so that we can immediately resume to it when new thread yields. FixMe to
-        // actually wait for whatever event it is supposed to be waiting on.
-
-        ChangeReadyState(prev, true);
-    } else {
-        INFO_LOG(KERNEL, "no ready threads, staying on 0x%08X", prev->GetHandle());
-
-        // Hack - no other threads are available, so decrement current PC to the last instruction, 
-        // and then resume current thread. This should always be called on a blocking instruction 
-        // (e.g. svcWaitSynchronization), and the result should be that the instruction is repeated
-        // until it no longer blocks. 
-
-        // TODO(bunnei): A better solution: Have the CPU switch to an idle thread
-
-        ThreadContext ctx;
-        SaveContext(ctx);
-        ctx.pc -= 4;
-        LoadContext(ctx);
-        ChangeReadyState(prev, true);
+        // Hack - There is no mechanism yet to waken the primary thread if it has been put to sleep
+        // by a simulated VBLANK thread switch. So, we'll just immediately set it to "ready" again.
+        // This results in the current thread yielding on a VBLANK once, and then it will be 
+        // immediately placed back in the queue for execution.
+        if (prev->wait_type == WAITTYPE_VBLANK) {
+            ResumeThreadFromWait(prev->GetHandle());
+        }
     }
 }
 
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 094c8d43e..04914ba90 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -34,7 +34,7 @@ enum WaitType {
     WAITTYPE_NONE,
     WAITTYPE_SLEEP,
     WAITTYPE_SEMA,
-    WAITTYPE_EVENTFLAG,
+    WAITTYPE_EVENT,
     WAITTYPE_THREADEND,
     WAITTYPE_VBLANK,
     WAITTYPE_MUTEX,
@@ -53,8 +53,8 @@ Handle SetupMainThread(s32 priority, int stack_size=Kernel::DEFAULT_STACK_SIZE);
 /// Reschedules to the next available thread (call after current thread is suspended)
 void Reschedule();
 
-/// Puts the current thread in the wait state for the given type
-void WaitCurrentThread(WaitType wait_type);
+/// Stops the current thread
+void StopThread(Handle thread, const char* reason);
 
 /// Resumes a thread from waiting by marking it as "ready"
 void ResumeThreadFromWait(Handle handle);
@@ -62,6 +62,9 @@ void ResumeThreadFromWait(Handle handle);
 /// Gets the current thread handle
 Handle GetCurrentThreadHandle();
 
+/// Puts the current thread in the wait state for the given type
+void WaitCurrentThread(WaitType wait_type, Handle wait_handle=GetCurrentThreadHandle());
+
 /// Put current thread in a wait state - on WaitSynchronization
 void WaitThread_Synchronization();
 
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index c8eb8ea80..0ce831103 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -93,8 +93,8 @@ Result SendSyncRequest(Handle handle) {
     bool wait = false;
     Kernel::Object* object = Kernel::g_object_pool.GetFast<Kernel::Object>(handle);
 
-    DEBUG_LOG(SVC, "called handle=0x%08X", handle);
     _assert_msg_(KERNEL, object, "called, but kernel object is NULL!");
+    DEBUG_LOG(SVC, "called handle=0x%08X(%s)", handle, object->GetTypeName());
 
     Result res = object->SyncRequest(&wait);
     if (wait) {
@@ -115,29 +115,21 @@ Result CloseHandle(Handle handle) {
 Result WaitSynchronization1(Handle handle, s64 nano_seconds) {
     // TODO(bunnei): Do something with nano_seconds, currently ignoring this
     bool wait = false;
+    bool wait_infinite = (nano_seconds == -1); // Used to wait until a thread has terminated
 
     Kernel::Object* object = Kernel::g_object_pool.GetFast<Kernel::Object>(handle);
 
-    DEBUG_LOG(SVC, "called handle=0x%08X, nanoseconds=%d", handle, 
-        nano_seconds);
+    DEBUG_LOG(SVC, "called handle=0x%08X(%s:%s), nanoseconds=%d", handle, object->GetTypeName(), 
+            object->GetName(), nano_seconds);
+
     _assert_msg_(KERNEL, object, "called, but kernel object is NULL!");
 
     Result res = object->WaitSynchronization(&wait);
 
+    // Check for next thread to schedule
     if (wait) {
-        // Set current thread to wait state if handle was not unlocked
-        Kernel::WaitCurrentThread(WAITTYPE_SYNCH); // TODO(bunnei): Is this correct?
-
-        // Check for next thread to schedule
         HLE::Reschedule(__func__);
-
-        // Context switch - Function blocked, is not actually returning (will be "called" again)
-
-        // TODO(bunnei): This saves handle to R0 so that it's correctly reloaded on context switch
-        // (otherwise R0 will be set to whatever is returned, and handle will be invalid when this
-        // thread is resumed). There is probably a better way of keeping track of state so that we
-        // don't necessarily have to do this.
-        return (Result)PARAM(0);
+        return 0;
     }
 
     return res;
@@ -150,6 +142,7 @@ Result WaitSynchronizationN(void* _out, void* _handles, u32 handle_count, u32 wa
     s32* out = (s32*)_out;
     Handle* handles = (Handle*)_handles;
     bool unlock_all = true;
+    bool wait_infinite = (nano_seconds == -1); // Used to wait until a thread has terminated
 
     DEBUG_LOG(SVC, "called handle_count=%d, wait_all=%s, nanoseconds=%d", 
         handle_count, (wait_all ? "true" : "false"), nano_seconds);
@@ -162,7 +155,8 @@ Result WaitSynchronizationN(void* _out, void* _handles, u32 handle_count, u32 wa
         _assert_msg_(KERNEL, object, "called handle=0x%08X, but kernel object "
             "is NULL!", handles[i]);
 
-        DEBUG_LOG(SVC, "\thandle[%d] = 0x%08X", i, handles[i]);
+        DEBUG_LOG(SVC, "\thandle[%d] = 0x%08X(%s:%s)", i, handles[i], object->GetTypeName(), 
+            object->GetName());
 
         Result res = object->WaitSynchronization(&wait);
 
@@ -179,19 +173,10 @@ Result WaitSynchronizationN(void* _out, void* _handles, u32 handle_count, u32 wa
         return 0;
     }
 
-    // Set current thread to wait state if not all handles were unlocked
-    Kernel::WaitCurrentThread(WAITTYPE_SYNCH); // TODO(bunnei): Is this correct?
-    
     // Check for next thread to schedule
     HLE::Reschedule(__func__);
 
-    // Context switch - Function blocked, is not actually returning (will be "called" again)
-
-    // TODO(bunnei): This saves handle to R0 so that it's correctly reloaded on context switch
-    // (otherwise R0 will be set to whatever is returned, and handle will be invalid when this
-    // thread is resumed). There is probably a better way of keeping track of state so that we
-    // don't necessarily have to do this.
-    return (Result)PARAM(0);
+    return 0;
 }
 
 /// Create an address arbiter (to allocate access to shared resources)
@@ -258,6 +243,17 @@ Result CreateThread(u32 priority, u32 entry_point, u32 arg, u32 stack_top, u32 p
     return 0;
 }
 
+/// Called when a thread exits
+u32 ExitThread() {
+    Handle thread = Kernel::GetCurrentThreadHandle();
+
+    DEBUG_LOG(SVC, "called, pc=0x%08X", Core::g_app_core->GetPC()); // PC = 0x0010545C
+
+    Kernel::StopThread(thread, __func__);
+    HLE::Reschedule(__func__);
+    return 0;
+}
+
 /// Gets the priority for the specified thread
 Result GetThreadPriority(void* _priority, Handle handle) {
     s32* priority = (s32*)_priority;
@@ -326,6 +322,13 @@ Result DuplicateHandle(void* _out, Handle handle) {
     return 0;
 }
 
+/// Signals an event
+Result SignalEvent(Handle evt) {
+    Result res = Kernel::SignalEvent(evt);
+    DEBUG_LOG(SVC, "called event=0x%08X", evt);
+    return res;
+}
+
 /// Clears an event
 Result ClearEvent(Handle evt) {
     Result res = Kernel::ClearEvent(evt);
@@ -348,7 +351,7 @@ const HLE::FunctionDef SVC_Table[] = {
     {0x06,  NULL,                                       "GetProcessIdealProcessor"},
     {0x07,  NULL,                                       "SetProcessIdealProcessor"},
     {0x08,  WrapI_UUUUU<CreateThread>,                  "CreateThread"},
-    {0x09,  NULL,                                       "ExitThread"},
+    {0x09,  WrapU_V<ExitThread>,                        "ExitThread"},
     {0x0A,  WrapV_S64<SleepThread>,                     "SleepThread"},
     {0x0B,  WrapI_VU<GetThreadPriority>,                "GetThreadPriority"},
     {0x0C,  WrapI_UI<SetThreadPriority>,                "SetThreadPriority"},
@@ -363,7 +366,7 @@ const HLE::FunctionDef SVC_Table[] = {
     {0x15,  NULL,                                       "CreateSemaphore"},
     {0x16,  NULL,                                       "ReleaseSemaphore"},
     {0x17,  WrapI_VU<CreateEvent>,                      "CreateEvent"},
-    {0x18,  NULL,                                       "SignalEvent"},
+    {0x18,  WrapI_U<SignalEvent>,                       "SignalEvent"},
     {0x19,  WrapI_U<ClearEvent>,                        "ClearEvent"},
     {0x1A,  NULL,                                       "CreateTimer"},
     {0x1B,  NULL,                                       "SetTimer"},