1 files changed, 168 insertions, 55 deletions

diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index bf4c8353c..ab5a5559e 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>
@@ -24,10 +25,10 @@ namespace Kernel {
 class Thread : public Kernel::Object {
 public:
 
-    const char* GetName() { return name; }
-    const char* GetTypeName() { return "Thread"; }
+    const char* GetName() const { return name; }
+    const char* GetTypeName() const { return "Thread"; }
 
-    static Kernel::HandleType GetStaticHandleType() {  return Kernel::HandleType::Thread; }
+    static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Thread; }
     Kernel::HandleType GetHandleType() const { return Kernel::HandleType::Thread; }
 
     inline bool IsRunning() const { return (status & THREADSTATUS_RUNNING) != 0; }
@@ -36,6 +37,23 @@ public:
     inline bool IsWaiting() const { return (status & THREADSTATUS_WAIT) != 0; }
     inline bool IsSuspended() const { return (status & THREADSTATUS_SUSPEND) != 0; }
 
+    /**
+     * Wait for kernel object to synchronize
+     * @param wait Boolean wait set if current thread should wait as a result of sync operation
+     * @return Result of operation, 0 on success, otherwise error code
+     */
+    Result WaitSynchronization(bool* wait) {
+        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;
+    }
+
     ThreadContext context;
 
     u32 status;
@@ -49,6 +67,9 @@ public:
     s32 processor_id;
 
     WaitType wait_type;
+    Handle wait_handle;
+
+    std::vector<Handle> waiting_threads;
 
     char name[Kernel::MAX_NAME_LENGTH + 1];
 };
@@ -62,7 +83,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;
@@ -94,15 +114,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.reg_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
@@ -122,6 +142,37 @@ void ChangeReadyState(Thread* t, bool ready) {
     }
 }
 
+/// Verify that a thread has not been released from waiting
+inline bool VerifyWait(const Handle& handle, WaitType type, Handle wait_handle) {
+    Thread* thread = g_object_pool.GetFast<Thread>(handle);
+    _assert_msg_(KERNEL, (thread != nullptr), "called, but thread is nullptr!");
+
+    if (type != thread->wait_type || wait_handle != thread->wait_handle) 
+        return false;
+
+    return true;
+}
+
+/// Stops the current thread
+void StopThread(Handle handle, const char* reason) {
+    Thread* thread = g_object_pool.GetFast<Thread>(handle);
+    _assert_msg_(KERNEL, (thread != nullptr), "called, but thread is nullptr!");
+    
+    ChangeReadyState(thread, false);
+    thread->status = THREADSTATUS_DORMANT;
+    for (size_t i = 0; i < thread->waiting_threads.size(); ++i) {
+        const Handle waiting_thread = thread->waiting_threads[i];
+        if (VerifyWait(waiting_thread, WAITTYPE_THREADEND, handle)) {
+            ResumeThreadFromWait(waiting_thread);
+        }
+    }
+    thread->waiting_threads.clear();
+
+    // Stopped threads are never waiting.
+    thread->wait_type = WAITTYPE_NONE;
+    thread->wait_handle = 0;
+}
+
 /// Changes a threads state
 void ChangeThreadState(Thread* t, ThreadStatus new_status) {
     if (!t || t->status == new_status) {
@@ -132,7 +183,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");
         }
     }
 }
@@ -166,7 +217,7 @@ void SwitchContext(Thread* t) {
         t->wait_type = WAITTYPE_NONE;
         LoadContext(t->context);
     } else {
-        SetCurrentThread(NULL);
+        SetCurrentThread(nullptr);
     }
 }
 
@@ -181,26 +232,43 @@ Thread* NextThread() {
         next = g_thread_ready_queue.pop_first();
     }
     if (next == 0) {
-        return NULL;
+        return nullptr;
     }
     return Kernel::g_object_pool.GetFast<Thread>(next);
 }
 
 /// Puts the current thread in the wait state for the given type
-void WaitCurrentThread(WaitType wait_type) {
-    Thread* t = GetCurrentThread();
-    t->wait_type = wait_type;
-    ChangeThreadState(t, ThreadStatus(THREADSTATUS_WAIT | (t->status & THREADSTATUS_SUSPEND)));
+void WaitCurrentThread(WaitType wait_type, Handle wait_handle) {
+    Thread* thread = GetCurrentThread();
+    thread->wait_type = wait_type;
+    thread->wait_handle = wait_handle;
+    ChangeThreadState(thread, ThreadStatus(THREADSTATUS_WAIT | (thread->status & THREADSTATUS_SUSPEND)));
 }
 
 /// Resumes a thread from waiting by marking it as "ready"
 void ResumeThreadFromWait(Handle handle) {
     u32 error;
-    Thread* t = Kernel::g_object_pool.Get<Thread>(handle, error);
-    if (t) {
-        t->status &= ~THREADSTATUS_WAIT;
-        if (!(t->status & (THREADSTATUS_WAITSUSPEND | THREADSTATUS_DORMANT | THREADSTATUS_DEAD))) {
-            ChangeReadyState(t, true);
+    Thread* thread = Kernel::g_object_pool.Get<Thread>(handle, error);
+    if (thread) {
+        thread->status &= ~THREADSTATUS_WAIT;
+        if (!(thread->status & (THREADSTATUS_WAITSUSPEND | THREADSTATUS_DORMANT | THREADSTATUS_DEAD))) {
+            ChangeReadyState(thread, true);
+        }
+    }
+}
+
+/// 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);
         }
     }
 }
@@ -212,32 +280,34 @@ Thread* CreateThread(Handle& handle, const char* name, u32 entry_point, s32 prio
     _assert_msg_(KERNEL, (priority >= THREADPRIO_HIGHEST && priority <= THREADPRIO_LOWEST), 
         "CreateThread priority=%d, outside of allowable range!", priority)
 
-    Thread* t = new Thread;
-    
-    handle = Kernel::g_object_pool.Create(t);
-    
+    Thread* thread = new Thread;
+
+    handle = Kernel::g_object_pool.Create(thread);
+
     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;
-    t->stack_size = stack_size;
-    t->initial_priority = t->current_priority = priority;
-    t->processor_id = processor_id;
-    t->wait_type = WAITTYPE_NONE;
-    
-    strncpy(t->name, name, Kernel::MAX_NAME_LENGTH);
-    t->name[Kernel::MAX_NAME_LENGTH] = '\0';
-    
-    return t;
+
+    thread->status = THREADSTATUS_DORMANT;
+    thread->entry_point = entry_point;
+    thread->stack_top = stack_top;
+    thread->stack_size = stack_size;
+    thread->initial_priority = thread->current_priority = priority;
+    thread->processor_id = processor_id;
+    thread->wait_type = WAITTYPE_NONE;
+    thread->wait_handle = 0;
+
+    strncpy(thread->name, name, Kernel::MAX_NAME_LENGTH);
+    thread->name[Kernel::MAX_NAME_LENGTH] = '\0';
+
+    return thread;
 }
 
 /// 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");
+
+    if (name == nullptr) {
+        ERROR_LOG(KERNEL, "CreateThread(): nullptr name");
         return -1;
     }
     if ((u32)stack_size < 0x200) {
@@ -258,20 +328,56 @@ Handle CreateThread(const char* name, u32 entry_point, s32 priority, u32 arg, s3
         return -1;
     }
     Handle handle;
-    Thread* t = CreateThread(handle, name, entry_point, priority, processor_id, stack_top, 
+    Thread* thread = CreateThread(handle, name, entry_point, priority, processor_id, stack_top, 
         stack_size);
 
-    ResetThread(t, arg, 0);
+    ResetThread(thread, arg, 0);
+    CallThread(thread);
+
+    return handle;
+}
 
-    HLE::EatCycles(32000);
+/// Get the priority of the thread specified by handle
+u32 GetThreadPriority(const Handle handle) {
+    Thread* thread = g_object_pool.GetFast<Thread>(handle);
+    _assert_msg_(KERNEL, (thread != nullptr), "called, but thread is nullptr!");
+    return thread->current_priority;
+}
 
-    // 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");
+/// Set the priority of the thread specified by handle
+Result SetThreadPriority(Handle handle, s32 priority) {
+    Thread* thread = nullptr;
+    if (!handle) {
+        thread = GetCurrentThread(); // TODO(bunnei): Is this correct behavior?
+    } else {
+        thread = g_object_pool.GetFast<Thread>(handle);
+    }
+    _assert_msg_(KERNEL, (thread != nullptr), "called, but thread is nullptr!");
 
-    CallThread(t);
-    
-    return handle;
+    // If priority is invalid, clamp to valid range
+    if (priority < THREADPRIO_HIGHEST || priority > THREADPRIO_LOWEST) {
+        s32 new_priority = CLAMP(priority, THREADPRIO_HIGHEST, THREADPRIO_LOWEST);
+        WARN_LOG(KERNEL, "invalid priority=0x%08X, clamping to %08X", priority, new_priority);
+        // TODO(bunnei): Clamping to a valid priority is not necessarily correct behavior... Confirm
+        // validity of this
+        priority = new_priority;
+    }
+
+    // Change thread priority
+    s32 old = thread->current_priority;
+    g_thread_ready_queue.remove(old, handle);
+    thread->current_priority = priority;
+    g_thread_ready_queue.prepare(thread->current_priority);
+
+    // Change thread status to "ready" and push to ready queue
+    if (thread->IsRunning()) {
+        thread->status = (thread->status & ~THREADSTATUS_RUNNING) | THREADSTATUS_READY;
+    }
+    if (thread->IsReady()) {
+        g_thread_ready_queue.push_back(thread->current_priority, handle);
+    }
+
+    return 0;
 }
 
 /// Sets up the primary application thread
@@ -279,10 +385,10 @@ Handle SetupMainThread(s32 priority, int stack_size) {
     Handle handle;
     
     // Initialize new "main" thread
-    Thread* t = CreateThread(handle, "main", Core::g_app_core->GetPC(), priority, 
+    Thread* thread = CreateThread(handle, "main", Core::g_app_core->GetPC(), priority, 
         THREADPROCESSORID_0, Memory::SCRATCHPAD_VADDR_END, stack_size);
     
-    ResetThread(t, 0, 0);
+    ResetThread(thread, 0, 0);
     
     // If running another thread already, set it to "ready" state
     Thread* cur = GetCurrentThread();
@@ -291,24 +397,31 @@ Handle SetupMainThread(s32 priority, int stack_size) {
     }
     
     // Run new "main" thread
-    SetCurrentThread(t);
-    t->status = THREADSTATUS_RUNNING;
-    LoadContext(t->context);
+    SetCurrentThread(thread);
+    thread->status = THREADSTATUS_RUNNING;
+    LoadContext(thread->context);
 
     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);
+        // 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());
+        }
     }
 }