Support Priority & Affinity Mask Changes

app/src/main/cpp/skyline/kernel/scheduler.cpp

@@ -24,6 +24,7 @@ namespace skyline::kernel {
 
     Scheduler::CoreContext &Scheduler::LoadBalance() {
         auto &thread{state.thread};
+        std::lock_guard migrationLock(thread->coreMigrationMutex);
         auto *currentCore{&cores.at(thread->coreId)};
 
         if (!currentCore->queue.empty() && thread->affinityMask.count() != 1) {
@@ -37,7 +38,7 @@ namespace skyline::kernel {
                     u64 timeslice{};
 
                     if (!candidateCore.queue.empty()) {
-                        std::shared_lock lock(candidateCore.mutex);
+                        std::shared_lock coreLock(candidateCore.mutex);
 
                         auto threadIterator{candidateCore.queue.cbegin()};
                         if (threadIterator != candidateCore.queue.cend()) {
@@ -60,21 +61,21 @@ namespace skyline::kernel {
             }
 
             if (optimalCore != currentCore) {
-                std::unique_lock lock(currentCore->mutex);
+                std::unique_lock coreLock(currentCore->mutex);
                 currentCore->queue.erase(std::remove(currentCore->queue.begin(), currentCore->queue.end(), thread), currentCore->queue.end());
                 currentCore->mutateCondition.notify_all();
 
                 thread->coreId = optimalCore->id;
 
-                state.logger->Debug("Load Balancing for #{}: C{} -> C{}", thread->id, currentCore->id, optimalCore->id);
+                state.logger->Debug("Load Balancing: C{} -> C{}", currentCore->id, optimalCore->id);
             } else {
-                state.logger->Debug("Load Balancing for #{}: C{} (Late)", thread->id, currentCore->id);
+                state.logger->Debug("Load Balancing: C{} (Late)", currentCore->id);
             }
 
             return *optimalCore;
         }
 
-        state.logger->Debug("Load Balancing for #{}: C{} (Early)", thread->id, currentCore->id);
+        state.logger->Debug("Load Balancing: C{} (Early)", currentCore->id);
 
         return *currentCore;
     }
@@ -97,12 +98,14 @@ namespace skyline::kernel {
         std::unique_lock lock(core.mutex);
         auto nextThread{std::find_if(core.queue.begin(), core.queue.end(), [&](const std::shared_ptr<type::KThread> &it) { return it->priority > thread->priority; })};
         if (nextThread == core.queue.begin() && nextThread != core.queue.end()) {
+            // If the inserted thread has a higher priority than the currently running thread (and the queue isn't empty)
             core.queue.front()->SendSignal(YieldSignal);
             core.queue.insert(std::next(core.queue.begin()), thread);
         } else {
             core.queue.insert(nextThread, thread);
             core.mutateCondition.notify_all(); // We only want to trigger the conditional variable if the current thread isn't going to be scheduled next
         }
+        thread->needsReorder = true; // We need to reorder the thread from back to align it with other threads of it's priority and ensure strict ordering amongst priorities
     }
 
     void Scheduler::WaitSchedule() {
@@ -147,19 +150,72 @@ namespace skyline::kernel {
             thread->averageTimeslice = (thread->averageTimeslice / 4) + (3 * (util::GetTimeTicks() - thread->timesliceStart / 4)); // 0.25 * old timeslice duration + 0.75 * current timeslice duration
 
             core.queue.pop_front();
+            if (!thread->needsReorder) {
                 core.queue.push_back(thread);
+            } else if (core.queue.size() > 1 && core.queue.back()->priority > thread->priority) {
+                // If 'needsReorder' is set, the core queue isn't empty nor has only one member and the thread at the back of the queue has a lower priority than the current one
+                // We can attempt to reorder this thread, this is done by doing a priority-aware insert with the search starting at the "folding point"
+                // The folding point is where a thread has a lower priority than the one succeeding it in the queue, this is where a new "sequence" starts from highest to lowest priorities
+                u8 lastPriority{core.queue.front()->priority};
+                auto foldingPoint{std::find_if(std::next(core.queue.begin()), core.queue.end(), [&](const std::shared_ptr<type::KThread> &it) {
+                    return lastPriority > it->priority ? true : lastPriority = it->priority, false;
+                })};
+                core.queue.insert(std::find_if(foldingPoint, core.queue.end(), [&](const std::shared_ptr<type::KThread> &it) { return it->priority > thread->priority; }), thread);
+                thread->needsReorder = false;
+            } else {
+                core.queue.push_back(thread);
+                thread->needsReorder = false;
+            }
 
             core.mutateCondition.notify_all();
-        }
 
             if (cooperative && thread->isPreempted) {
                 // If a preemptive thread did a cooperative yield then we need to disarm the preemptive timer
                 struct itimerspec spec{};
                 timer_settime(*thread->preemptionTimer, 0, &spec, nullptr);
             }
-
             thread->isPreempted = false;
         }
+    }
+
+    void Scheduler::UpdatePriority(const std::shared_ptr<type::KThread>& thread) {
+        std::lock_guard migrationLock(thread->coreMigrationMutex);
+        auto *core{&cores.at(thread->coreId)};
+        std::unique_lock coreLock(core->mutex);
+
+        auto currentIt{std::find(core->queue.begin(), core->queue.end(), thread)};
+        if (currentIt == core->queue.end() || currentIt == core->queue.begin())
+            // If the thread isn't in the queue then the new priority will be handled automatically on insertion
+            return;
+        if (currentIt == core->queue.begin()) {
+            // Alternatively, if it's currently running then we'd just want to update after it rotates
+            thread->needsReorder = true;
+            return;
+        }
+
+        auto targetIt{std::find_if(core->queue.begin(), core->queue.end(), [&](const std::shared_ptr<type::KThread> &it) { return it->priority > thread->priority; })};
+        if (currentIt == targetIt)
+            // If this thread's position isn't affected by the priority change then we have nothing to do
+            return;
+
+        core->queue.erase(currentIt);
+
+        if (thread->isPreempted && thread->priority != core->preemptionPriority) {
+            struct itimerspec spec{};
+            timer_settime(*thread->preemptionTimer, 0, &spec, nullptr);
+            thread->isPreempted = false;
+        }
+
+        targetIt = std::find_if(core->queue.begin(), core->queue.end(), [&](const std::shared_ptr<type::KThread> &it) { return it->priority > thread->priority; }); // Iterator invalidation
+        if (targetIt == core->queue.begin() && targetIt != core->queue.end()) {
+            core->queue.front()->SendSignal(YieldSignal);
+            core->queue.insert(std::next(core->queue.begin()), thread);
+        } else {
+            core->queue.insert(targetIt, thread);
+            core->mutateCondition.notify_all();
+        }
+        thread->needsReorder = true;
+    }
 
     void Scheduler::RemoveThread() {
         auto &thread{state.thread};

app/src/main/cpp/skyline/kernel/scheduler.h

@@ -91,6 +91,11 @@ namespace skyline {
              */
             void Rotate(bool cooperative = true);
 
+            /**
+             * @brief Updates the placement of the supplied thread in it's resident core's queue according to it's new priority
+             */
+            void UpdatePriority(const std::shared_ptr<type::KThread>& thread);
+
             /**
              * @brief Removes the calling thread from it's resident core queue
              */

app/src/main/cpp/skyline/kernel/svc.cpp

@@ -309,7 +309,7 @@ namespace skyline::kernel::svc {
         KHandle handle{state.ctx->gpr.w1};
         try {
             auto thread{state.process->GetHandle<type::KThread>(handle)};
-            auto priority{thread->priority};
+            u8 priority{thread->priority};
             state.logger->Debug("svcGetThreadPriority: Retrieving thread #{}'s priority: {}", thread->id, priority);
 
             state.ctx->gpr.w1 = priority;
@@ -331,7 +331,8 @@ namespace skyline::kernel::svc {
         try {
             auto thread{state.process->GetHandle<type::KThread>(handle)};
             state.logger->Debug("svcSetThreadPriority: Setting thread priority to {}", thread->id, priority);
-            thread->UpdatePriority(static_cast<u8>(priority));
+            thread->priority = priority;
+            state.scheduler->UpdatePriority(thread);
             state.ctx->gpr.w0 = Result{};
         } catch (const std::out_of_range &) {
             state.logger->Warn("svcSetThreadPriority: 'handle' invalid: 0x{:X}", handle);
@@ -358,18 +359,18 @@ namespace skyline::kernel::svc {
 
     void SetThreadCoreMask(const DeviceState &state) {
         KHandle handle{state.ctx->gpr.w0};
-        i32 coreId{static_cast<i32>(state.ctx->gpr.w1)};
+        i32 idealCore{static_cast<i32>(state.ctx->gpr.w1)};
         CoreMask affinityMask{state.ctx->gpr.x2};
         try {
             auto thread{state.process->GetHandle<type::KThread>(handle)};
 
-            if (coreId == IdealCoreUseProcessValue) {
+            if (idealCore == IdealCoreUseProcessValue) {
-                coreId = state.process->npdm.meta.idealCore;
+                idealCore = state.process->npdm.meta.idealCore;
-                affinityMask.reset().set(coreId);
+                affinityMask.reset().set(idealCore);
-            } else if (coreId == IdealCoreNoUpdate) {
+            } else if (idealCore == IdealCoreNoUpdate) {
-                coreId = thread->idealCore;
+                idealCore = thread->idealCore;
-            } else if (coreId == IdealCoreDontCare) {
+            } else if (idealCore == IdealCoreDontCare) {
-                coreId = __builtin_ctzll(affinityMask.to_ullong()); // The first enabled core in the affinity mask
+                idealCore = __builtin_ctzll(affinityMask.to_ullong()); // The first enabled core in the affinity mask
             }
 
             auto processMask{state.process->npdm.threadInfo.coreMask};
@@ -379,17 +380,25 @@ namespace skyline::kernel::svc {
                 return;
             }
 
-            if (affinityMask.none() || !affinityMask.test(coreId)) {
+            if (affinityMask.none() || !affinityMask.test(idealCore)) {
-                state.logger->Warn("svcSetThreadCoreMask: 'affinityMask' invalid: {} (Ideal Core: {})", affinityMask, coreId);
+                state.logger->Warn("svcSetThreadCoreMask: 'affinityMask' invalid: {} (Ideal Core: {})", affinityMask, idealCore);
                 state.ctx->gpr.w0 = result::InvalidCombination;
                 return;
             }
 
-            state.logger->Debug("svcSetThreadCoreMask: Setting thread #{}'s Ideal Core ({}) + Affinity Mask ({})", thread->id, coreId, affinityMask);
+            state.logger->Debug("svcSetThreadCoreMask: Setting thread #{}'s Ideal Core ({}) + Affinity Mask ({})", thread->id, idealCore, affinityMask);
 
-            thread->idealCore = coreId;
+            thread->idealCore = idealCore;
             thread->affinityMask = affinityMask;
 
+            if (!affinityMask.test(thread->coreId)) {
+                state.logger->Debug("svcSetThreadCoreMask: Migrating to Ideal Core C{} -> C{}", thread->coreId, idealCore);
+
+                state.scheduler->RemoveThread();
+                thread->coreId = idealCore;
+                state.scheduler->InsertThread(false);
+            }
+
             state.ctx->gpr.w0 = Result{};
         } catch (const std::out_of_range &) {
             state.logger->Warn("svcSetThreadCoreMask: 'handle' invalid: 0x{:X}", handle);

app/src/main/cpp/skyline/kernel/types/KProcess.cpp

@@ -6,9 +6,9 @@
 #include "KProcess.h"
 
 namespace skyline::kernel::type {
-    KProcess::WaitStatus::WaitStatus(i8 priority, KHandle handle) : priority(priority), handle(handle) {}
+    KProcess::WaitStatus::WaitStatus(u8 priority, KHandle handle) : priority(priority), handle(handle) {}
 
-    KProcess::WaitStatus::WaitStatus(i8 priority, KHandle handle, u32 *mutex) : priority(priority), handle(handle), mutex(mutex) {}
+    KProcess::WaitStatus::WaitStatus(u8 priority, KHandle handle, u32 *mutex) : priority(priority), handle(handle), mutex(mutex) {}
 
     KProcess::TlsPage::TlsPage(const std::shared_ptr<KPrivateMemory> &memory) : memory(memory) {}
 
@@ -74,7 +74,7 @@ namespace skyline::kernel::type {
         return tlsPage->ReserveSlot();
     }
 
-    std::shared_ptr<KThread> KProcess::CreateThread(void *entry, u64 argument, void *stackTop, i8 priority, i8 idealCore) {
+    std::shared_ptr<KThread> KProcess::CreateThread(void *entry, u64 argument, void *stackTop, std::optional<u8> priority, std::optional<u8> idealCore) {
         std::lock_guard guard(threadMutex);
         if (disableThreadCreation)
             return nullptr;
@@ -84,7 +84,7 @@ namespace skyline::kernel::type {
                 throw exception("Failed to create guard page for thread stack at 0x{:X}", mainThreadStack->ptr);
             stackTop = mainThreadStack->ptr + mainThreadStack->size;
         }
-        auto thread{NewHandle<KThread>(this, threads.size(), entry, argument, stackTop, (priority == -1) ? state.process->npdm.meta.mainThreadPriority : priority, (idealCore == -1) ? state.process->npdm.meta.idealCore : idealCore).item};
+        auto thread{NewHandle<KThread>(this, threads.size(), entry, argument, stackTop, priority ? *priority : state.process->npdm.meta.mainThreadPriority, idealCore ? *idealCore : state.process->npdm.meta.idealCore).item};
         threads.push_back(thread);
         return thread;
     }

app/src/main/cpp/skyline/kernel/types/KProcess.h

@@ -29,13 +29,13 @@ namespace skyline {
           private:
             struct WaitStatus {
                 std::atomic_bool flag{false};
-                i8 priority;
+                u8 priority;
                 KHandle handle;
                 u32 *mutex{};
 
-                WaitStatus(i8 priority, KHandle handle);
+                WaitStatus(u8 priority, KHandle handle);
 
-                WaitStatus(i8 priority, KHandle handle, u32 *mutex);
+                WaitStatus(u8 priority, KHandle handle, u32 *mutex);
             };
 
             std::unordered_map<u64, std::vector<std::shared_ptr<WaitStatus>>> mutexes; //!< A map from a mutex's address to a vector of Mutex objects for threads waiting on it
@@ -105,7 +105,7 @@ namespace skyline {
              * @return A shared pointer to a KThread initialized with the specified values or nullptr, if thread creation has been disabled
              * @note The default values are for the main thread and will use values from the NPDM
              */
-            std::shared_ptr<KThread> CreateThread(void *entry, u64 argument = 0, void *stackTop = nullptr, i8 priority = -1, i8 idealCore = -1);
+            std::shared_ptr<KThread> CreateThread(void *entry, u64 argument = 0, void *stackTop = nullptr, std::optional<u8> priority = std::nullopt, std::optional<u8> idealCore = std::nullopt);
 
             /**
             * @brief The output for functions that return created kernel objects

app/src/main/cpp/skyline/kernel/types/KThread.cpp

@@ -10,9 +10,8 @@
 #include "KThread.h"
 
 namespace skyline::kernel::type {
-    KThread::KThread(const DeviceState &state, KHandle handle, KProcess *parent, size_t id, void *entry, u64 argument, void *stackTop, i8 priority, i8 idealCore) : handle(handle), parent(parent), id(id), entry(entry), entryArgument(argument), stackTop(stackTop), idealCore(idealCore), coreId(idealCore), KSyncObject(state, KType::KThread) {
+    KThread::KThread(const DeviceState &state, KHandle handle, KProcess *parent, size_t id, void *entry, u64 argument, void *stackTop, u8 priority, i8 idealCore) : handle(handle), parent(parent), id(id), entry(entry), entryArgument(argument), stackTop(stackTop), priority(priority), idealCore(idealCore), coreId(idealCore), KSyncObject(state, KType::KThread) {
         affinityMask.set(coreId);
-        UpdatePriority(priority);
     }
 
     KThread::~KThread() {
@@ -197,8 +196,4 @@ namespace skyline::kernel::type {
         if (running)
             pthread_kill(pthread, signal);
     }
-
-    void KThread::UpdatePriority(i8 priority) {
-        this->priority = priority;
-    }
 }

app/src/main/cpp/skyline/kernel/types/KThread.h

@@ -38,7 +38,7 @@ namespace skyline {
             u64 entryArgument;
             void *stackTop;
 
-            i8 priority;
+            std::atomic<u8> priority; //!< The priority of the thread for the scheduler
             i8 idealCore; //!< The ideal CPU core for this thread to run on
             i8 coreId; //!< The CPU core on which this thread is running
             CoreMask affinityMask{}; //!< A mask of CPU cores this thread is allowed to run on
@@ -46,8 +46,10 @@ namespace skyline {
             u64 averageTimeslice{}; //!< A weighted average of the timeslice duration for this thread
             std::optional<timer_t> preemptionTimer{}; //!< A kernel timer used for preemption interrupts
             bool isPreempted{}; //!< If the preemption timer has been armed and will fire
+            bool needsReorder{}; //!< If the thread needs to reorder itself during scheduler rotation
+            std::mutex coreMigrationMutex; //!< Synchronizes operations which depend on which core the thread is running on
 
-            KThread(const DeviceState &state, KHandle handle, KProcess *parent, size_t id, void *entry, u64 argument, void *stackTop, i8 priority, i8 idealCore);
+            KThread(const DeviceState &state, KHandle handle, KProcess *parent, size_t id, void *entry, u64 argument, void *stackTop, u8 priority, i8 idealCore);
 
             ~KThread();
 
@@ -67,8 +69,6 @@ namespace skyline {
              * @brief Sends a host OS signal to the thread which is running this KThread
              */
             void SendSignal(int signal);
-
-            void UpdatePriority(i8 priority);
         };
     }
 }