9 files changed, 147 insertions, 41 deletions

diff --git a/src/common/thread_queue_list.h b/src/common/thread_queue_list.h
index 444abf115..4f27fc899 100644
--- a/src/common/thread_queue_list.h
+++ b/src/common/thread_queue_list.h
@@ -40,6 +40,18 @@ struct ThreadQueueList {
         return -1;
     }
 
+    T get_first() {
+        Queue *cur = first;
+        while (cur != nullptr) {
+            if (!cur->data.empty()) {
+                return cur->data.front();
+            }
+            cur = cur->next_nonempty;
+        }
+
+        return T();
+    }
+
     T pop_first() {
         Queue *cur = first;
         while (cur != nullptr) {
@@ -79,6 +91,12 @@ struct ThreadQueueList {
         cur->data.push_back(thread_id);
     }
 
+    void move(const T& thread_id, Priority old_priority, Priority new_priority) {
+        remove(old_priority, thread_id);
+        prepare(new_priority);
+        push_back(new_priority, thread_id);
+    }
+
     void remove(Priority priority, const T& thread_id) {
         Queue *cur = &queues[priority];
         boost::remove_erase(cur->data, thread_id);
diff --git a/src/core/hle/function_wrappers.h b/src/core/hle/function_wrappers.h
index 0b6b6f518..be2626eef 100644
--- a/src/core/hle/function_wrappers.h
+++ b/src/core/hle/function_wrappers.h
@@ -46,6 +46,13 @@ template<ResultCode func(u32*, u32, u32, u32, u32, u32)> void Wrap(){
     FuncReturn(retval);
 }
 
+template<ResultCode func(u32*, s32, u32, u32, u32, s32)> void Wrap() {
+    u32 param_1 = 0;
+    u32 retval = func(&param_1, PARAM(0), PARAM(1), PARAM(2), PARAM(3), PARAM(4)).raw;
+    Core::g_app_core->SetReg(1, param_1);
+    FuncReturn(retval);
+}
+
 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),
diff --git a/src/core/hle/kernel/address_arbiter.cpp b/src/core/hle/kernel/address_arbiter.cpp
index 42f8ce2d9..19135266c 100644
--- a/src/core/hle/kernel/address_arbiter.cpp
+++ b/src/core/hle/kernel/address_arbiter.cpp
@@ -46,14 +46,12 @@ ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address,
     case ArbitrationType::WaitIfLessThan:
         if ((s32)Memory::Read32(address) <= value) {
             Kernel::WaitCurrentThread_ArbitrateAddress(address);
-            HLE::Reschedule(__func__);
         }
         break;
     case ArbitrationType::WaitIfLessThanWithTimeout:
         if ((s32)Memory::Read32(address) <= value) {
             Kernel::WaitCurrentThread_ArbitrateAddress(address);
             GetCurrentThread()->WakeAfterDelay(nanoseconds);
-            HLE::Reschedule(__func__);
         }
         break;
     case ArbitrationType::DecrementAndWaitIfLessThan:
@@ -62,7 +60,6 @@ ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address,
         Memory::Write32(address, memory_value);
         if (memory_value <= value) {
             Kernel::WaitCurrentThread_ArbitrateAddress(address);
-            HLE::Reschedule(__func__);
         }
         break;
     }
@@ -73,7 +70,6 @@ ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address,
         if (memory_value <= value) {
             Kernel::WaitCurrentThread_ArbitrateAddress(address);
             GetCurrentThread()->WakeAfterDelay(nanoseconds);
-            HLE::Reschedule(__func__);
         }
         break;
     }
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 498b2ec98..6261b82b6 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -154,7 +154,7 @@ void Shutdown() {
  */
 bool LoadExec(u32 entry_point) {
     // 0x30 is the typical main thread priority I've seen used so far
-    g_main_thread = Kernel::SetupMainThread(Kernel::DEFAULT_STACK_SIZE, entry_point, 0x30);
+    g_main_thread = Kernel::SetupMainThread(Kernel::DEFAULT_STACK_SIZE, entry_point, THREADPRIO_DEFAULT);
 
     return true;
 }
diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index be2c49706..ebc9e79d7 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -56,7 +56,15 @@ SharedPtr<Mutex> Mutex::Create(bool initial_locked, std::string name) {
 }
 
 bool Mutex::ShouldWait() {
-    return lock_count > 0 && holding_thread != GetCurrentThread();;
+    auto thread = GetCurrentThread();
+    bool wait = lock_count > 0 && holding_thread != thread;
+
+    // If the holding thread of the mutex is lower priority than this thread, that thread should
+    // temporarily inherit this thread's priority
+    if (wait && thread->current_priority < holding_thread->current_priority)
+        holding_thread->BoostPriority(thread->current_priority);
+
+    return wait;
 }
 
 void Mutex::Acquire() {
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index be1aed615..33d66b986 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -140,6 +140,28 @@ void ArbitrateAllThreads(u32 address) {
     }
 }
 
+/// Boost low priority threads (temporarily) that have been starved
+static void PriorityBoostStarvedThreads() {
+    u64 current_ticks = CoreTiming::GetTicks();
+
+    for (auto& thread : thread_list) {
+        // TODO(bunnei): Threads that have been waiting to be scheduled for `boost_ticks` (or
+        // longer) will have their priority temporarily adjusted to 1 higher than the highest
+        // priority thread to prevent thread starvation. This general behavior has been verified
+        // on hardware. However, this is almost certainly not perfect, and the real CTR OS scheduler
+        // should probably be reversed to verify this.
+
+        const u64 boost_timeout = 2000000;  // Boost threads that have been ready for > this long
+
+        u64 delta = current_ticks - thread->last_running_ticks;
+
+        if (thread->status == THREADSTATUS_READY && delta > boost_timeout && !thread->idle) {
+            const s32 priority = std::max(ready_queue.get_first()->current_priority - 1, 0);
+            thread->BoostPriority(priority);
+        }
+    }
+}
+
 /** 
  * Switches the CPU's active thread context to that of the specified thread
  * @param new_thread The thread to switch to
@@ -151,6 +173,7 @@ static void SwitchContext(Thread* new_thread) {
 
     // Save context for previous thread
     if (previous_thread) {
+        previous_thread->last_running_ticks = CoreTiming::GetTicks();
         Core::g_app_core->SaveContext(previous_thread->context);
 
         if (previous_thread->status == THREADSTATUS_RUNNING) {
@@ -168,6 +191,9 @@ static void SwitchContext(Thread* new_thread) {
         ready_queue.remove(new_thread->current_priority, new_thread);
         new_thread->status = THREADSTATUS_RUNNING;
 
+        // Restores thread to its nominal priority if it has been temporarily changed
+        new_thread->current_priority = new_thread->nominal_priority;
+
         Core::g_app_core->LoadContext(new_thread->context);
     } else {
         current_thread = nullptr;
@@ -364,7 +390,8 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     thread->status = THREADSTATUS_DORMANT;
     thread->entry_point = entry_point;
     thread->stack_top = stack_top;
-    thread->initial_priority = thread->current_priority = priority;
+    thread->nominal_priority = thread->current_priority = priority;
+    thread->last_running_ticks = CoreTiming::GetTicks();
     thread->processor_id = processor_id;
     thread->wait_set_output = false;
     thread->wait_all = false;
@@ -400,17 +427,15 @@ static void ClampPriority(const Thread* thread, s32* priority) {
 void Thread::SetPriority(s32 priority) {
     ClampPriority(this, &priority);
 
-    if (current_priority == priority) {
-        return;
-    }
+    // If thread was ready, adjust queues
+    if (status == THREADSTATUS_READY)
+        ready_queue.move(this, current_priority, priority);
 
-    if (status == THREADSTATUS_READY) {
-        // If thread was ready, adjust queues
-        ready_queue.remove(current_priority, this);
-        ready_queue.prepare(priority);
-        ready_queue.push_back(priority, this);
-    }
-    
+    nominal_priority = current_priority = priority;
+}
+
+void Thread::BoostPriority(s32 priority) {
+    ready_queue.move(this, current_priority, priority);
     current_priority = priority;
 }
 
@@ -440,6 +465,9 @@ SharedPtr<Thread> SetupMainThread(u32 stack_size, u32 entry_point, s32 priority)
 
 void Reschedule() {
     Thread* prev = GetCurrentThread();
+
+    PriorityBoostStarvedThreads();
+
     Thread* next = PopNextReadyThread();
     HLE::g_reschedule = false;
 
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index cfd073a70..233bcbdbd 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -17,17 +17,19 @@
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/result.h"
 
-enum ThreadPriority {
-    THREADPRIO_HIGHEST      = 0,    ///< Highest thread priority
-    THREADPRIO_DEFAULT      = 16,   ///< Default thread priority for userland apps
-    THREADPRIO_LOW          = 31,   ///< Low range of thread priority for userland apps
-    THREADPRIO_LOWEST       = 63,   ///< Thread priority max checked by svcCreateThread
+enum ThreadPriority : s32{
+    THREADPRIO_HIGHEST          = 0,  ///< Highest thread priority
+    THREADPRIO_USERLAND_MAX     = 24, ///< Highest thread priority for userland apps
+    THREADPRIO_DEFAULT          = 48, ///< Default thread priority for userland apps
+    THREADPRIO_LOWEST           = 63, ///< Lowest thread priority
 };
 
-enum ThreadProcessorId {
-    THREADPROCESSORID_0     = 0xFFFFFFFE,   ///< Enables core appcode
-    THREADPROCESSORID_1     = 0xFFFFFFFD,   ///< Enables core syscore
-    THREADPROCESSORID_ALL   = 0xFFFFFFFC,   ///< Enables both cores
+enum ThreadProcessorId : s32 {
+    THREADPROCESSORID_DEFAULT   = -2, ///< Run thread on default core specified by exheader
+    THREADPROCESSORID_ALL       = -1, ///< Run thread on either core
+    THREADPROCESSORID_0         =  0, ///< Run thread on core 0 (AppCore)
+    THREADPROCESSORID_1         =  1, ///< Run thread on core 1 (SysCore)
+    THREADPROCESSORID_MAX       =  2, ///< Processor ID must be less than this
 };
 
 enum ThreadStatus {
@@ -88,6 +90,12 @@ public:
     void SetPriority(s32 priority);
 
     /**
+     * Temporarily boosts the thread's priority until the next time it is scheduled
+     * @param priority The new priority
+     */
+    void BoostPriority(s32 priority);
+
+    /**
      * Gets the thread's thread ID
      * @return The thread's ID
      */
@@ -135,8 +143,10 @@ public:
     u32 entry_point;
     u32 stack_top;
 
-    s32 initial_priority;
-    s32 current_priority;
+    s32 nominal_priority;   ///< Nominal thread priority, as set by the emulated application
+    s32 current_priority;   ///< Current thread priority, can be temporarily changed
+
+    u64 last_running_ticks; ///< CPU tick when thread was last running
 
     s32 processor_id;
 
diff --git a/src/core/hle/service/apt/apt.cpp b/src/core/hle/service/apt/apt.cpp
index 4861d9e5f..190c5df7a 100644
--- a/src/core/hle/service/apt/apt.cpp
+++ b/src/core/hle/service/apt/apt.cpp
@@ -32,7 +32,8 @@ static Kernel::SharedPtr<Kernel::SharedMemory> shared_font_mem = nullptr;
 
 static Kernel::SharedPtr<Kernel::Mutex> lock = nullptr;
 static Kernel::SharedPtr<Kernel::Event> notification_event = nullptr; ///< APT notification event
-static Kernel::SharedPtr<Kernel::Event> pause_event = nullptr;        ///< APT pause event
+static Kernel::SharedPtr<Kernel::Event> start_event = nullptr;        ///< APT start event
+
 static std::vector<u8> shared_font;
 
 static u32 cpu_percent = 0; ///< CPU time available to the running application
@@ -44,11 +45,11 @@ void Initialize(Service::Interface* self) {
 
     cmd_buff[2] = 0x04000000; // According to 3dbrew, this value should be 0x04000000
     cmd_buff[3] = Kernel::g_handle_table.Create(notification_event).MoveFrom();
-    cmd_buff[4] = Kernel::g_handle_table.Create(pause_event).MoveFrom();
+    cmd_buff[4] = Kernel::g_handle_table.Create(start_event).MoveFrom();
 
-    // TODO(bunnei): Check if these events are cleared/signaled every time Initialize is called.
+    // TODO(bunnei): Check if these events are cleared every time Initialize is called.
     notification_event->Clear();
-    pause_event->Signal(); // Fire start event
+    start_event->Clear();
 
     ASSERT_MSG((nullptr != lock), "Cannot initialize without lock");
     lock->Release();
@@ -81,7 +82,7 @@ void NotifyToWait(Service::Interface* self) {
     u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 app_id = cmd_buff[1];
     // TODO(Subv): Verify this, it seems to get SWKBD and Home Menu further.
-    pause_event->Signal();
+    start_event->Signal();
 
     cmd_buff[1] = RESULT_SUCCESS.raw; // No error
     LOG_WARNING(Service_APT, "(STUBBED) app_id=%u", app_id);
@@ -312,7 +313,7 @@ void Init() {
 
     // TODO(bunnei): Check if these are created in Initialize or on APT process startup.
     notification_event = Kernel::Event::Create(RESETTYPE_ONESHOT, "APT_U:Notification");
-    pause_event = Kernel::Event::Create(RESETTYPE_ONESHOT, "APT_U:Pause");
+    start_event = Kernel::Event::Create(RESETTYPE_ONESHOT, "APT_U:Start");
 }
 
 void Shutdown() {
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index bbb4eb9cd..76e9b171a 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -283,8 +283,13 @@ static ResultCode ArbitrateAddress(Handle handle, u32 address, u32 type, u32 val
     if (arbiter == nullptr)
         return ERR_INVALID_HANDLE;
 
-    return arbiter->ArbitrateAddress(static_cast<Kernel::ArbitrationType>(type),
-            address, value, nanoseconds);
+    auto res = arbiter->ArbitrateAddress(static_cast<Kernel::ArbitrationType>(type),
+                                         address, value, nanoseconds);
+
+    if (res == RESULT_SUCCESS)
+        HLE::Reschedule(__func__);
+
+    return res;
 }
 
 /// Used to output a message on a debug hardware unit - does nothing on a retail unit
@@ -312,7 +317,7 @@ static ResultCode GetResourceLimitCurrentValues(s64* values, Handle resource_lim
 }
 
 /// Creates a new thread
-static ResultCode CreateThread(u32* out_handle, u32 priority, u32 entry_point, u32 arg, u32 stack_top, u32 processor_id) {
+static ResultCode CreateThread(Handle* out_handle, s32 priority, u32 entry_point, u32 arg, u32 stack_top, s32 processor_id) {
     using Kernel::Thread;
 
     std::string name;
@@ -323,6 +328,27 @@ static ResultCode CreateThread(u32* out_handle, u32 priority, u32 entry_point, u
         name = Common::StringFromFormat("unknown-%08x", entry_point);
     }
 
+    // TODO(bunnei): Implement resource limits to return an error code instead of the below assert.
+    // The error code should be: Description::NotAuthorized, Module::OS, Summary::WrongArgument,
+    // Level::Permanent
+    ASSERT_MSG(priority >= THREADPRIO_USERLAND_MAX, "Unexpected thread priority!");
+
+    if (priority > THREADPRIO_LOWEST) {
+        return ResultCode(ErrorDescription::OutOfRange, ErrorModule::OS,
+                          ErrorSummary::InvalidArgument, ErrorLevel::Usage);
+    }
+
+    switch (processor_id) {
+    case THREADPROCESSORID_DEFAULT:
+    case THREADPROCESSORID_0:
+    case THREADPROCESSORID_1:
+        break;
+    default:
+        // TODO(bunnei): Implement support for other processor IDs
+        ASSERT_MSG(false, "Unsupported thread processor ID: %d", processor_id);
+        break;
+    }
+
     CASCADE_RESULT(SharedPtr<Thread> thread, Kernel::Thread::Create(
             name, entry_point, priority, arg, processor_id, stack_top));
     CASCADE_RESULT(*out_handle, Kernel::g_handle_table.Create(std::move(thread)));
@@ -331,10 +357,7 @@ static ResultCode CreateThread(u32* out_handle, u32 priority, u32 entry_point, u
         "threadpriority=0x%08X, processorid=0x%08X : created handle=0x%08X", entry_point,
         name.c_str(), arg, stack_top, priority, processor_id, *out_handle);
 
-    if (THREADPROCESSORID_1 == processor_id) {
-        LOG_WARNING(Kernel_SVC,
-            "thread designated for system CPU core (UNIMPLEMENTED) will be run with app core scheduling");
-    }
+    HLE::Reschedule(__func__);
 
     return RESULT_SUCCESS;
 }
@@ -374,8 +397,11 @@ static ResultCode CreateMutex(Handle* out_handle, u32 initial_locked) {
     SharedPtr<Mutex> mutex = Mutex::Create(initial_locked != 0);
     CASCADE_RESULT(*out_handle, Kernel::g_handle_table.Create(std::move(mutex)));
 
+    HLE::Reschedule(__func__);
+
     LOG_TRACE(Kernel_SVC, "called initial_locked=%s : created handle=0x%08X",
         initial_locked ? "true" : "false", *out_handle);
+    
     return RESULT_SUCCESS;
 }
 
@@ -390,6 +416,9 @@ static ResultCode ReleaseMutex(Handle handle) {
         return ERR_INVALID_HANDLE;
 
     mutex->Release();
+
+    HLE::Reschedule(__func__);
+
     return RESULT_SUCCESS;
 }
 
@@ -428,6 +457,9 @@ static ResultCode ReleaseSemaphore(s32* count, Handle handle, s32 release_count)
         return ERR_INVALID_HANDLE;
 
     CASCADE_RESULT(*count, semaphore->Release(release_count));
+
+    HLE::Reschedule(__func__);
+
     return RESULT_SUCCESS;
 }
 
@@ -520,6 +552,9 @@ static ResultCode SetTimer(Handle handle, s64 initial, s64 interval) {
         return ERR_INVALID_HANDLE;
 
     timer->Set(initial, interval);
+
+    HLE::Reschedule(__func__);
+
     return RESULT_SUCCESS;
 }
 
@@ -534,6 +569,9 @@ static ResultCode CancelTimer(Handle handle) {
         return ERR_INVALID_HANDLE;
 
     timer->Cancel();
+
+    HLE::Reschedule(__func__);
+
     return RESULT_SUCCESS;
 }