From d262ff92ccefe4299097c00e0d3f0fac8044d7b1 Mon Sep 17 00:00:00 2001 From: Michael Scire Date: Thu, 30 Jan 2020 20:56:24 -0800 Subject: [PATCH] kern: implement KPriorityQueue --- .../mesosphere/kern_k_priority_queue.hpp | 424 ++++++++++++++++++ .../include/mesosphere/kern_k_scheduler.hpp | 7 + .../include/mesosphere/kern_k_thread.hpp | 23 + .../include/vapours/svc/svc_types_common.hpp | 3 + 4 files changed, 457 insertions(+) create mode 100644 libraries/libmesosphere/include/mesosphere/kern_k_priority_queue.hpp diff --git a/libraries/libmesosphere/include/mesosphere/kern_k_priority_queue.hpp b/libraries/libmesosphere/include/mesosphere/kern_k_priority_queue.hpp new file mode 100644 index 000000000..7da585920 --- /dev/null +++ b/libraries/libmesosphere/include/mesosphere/kern_k_priority_queue.hpp @@ -0,0 +1,424 @@ +/* + * Copyright (c) 2018-2020 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once +#include + +namespace ams::kern { + + /* + TODO: C++20 + + template + concept KPriorityQueueAffinityMask = !std::is_reference::value && requires (T &t) { + { t.GetAffinityMask() } -> std::convertible_to; + { t.SetAffinityMask(std::declval()) }; + + { t.GetAffinity(std::declval()) } -> std::same_as; + { t.SetAffinity(std::declval(), std::declval()) }; + { t.SetAll() }; + }; + + template + concept KPriorityQueueMember = !std::is_reference::value && requires (T &t) { + { typename T::QueueEntry() }; + { (typename T::QueueEntry()).Initialize() }; + { (typename T::QueueEntry()).SetPrev(std::addressof(t)) }; + { (typename T::QueueEntry()).SetNext(std::addressof(t)) }; + { (typename T::QueueEntry()).GetNext() } -> std::same_as; + { (typename T::QueueEntry()).GetPrev() } -> std::same_as; + { t.GetPriorityQueueEntry(std::declval()) } -> std::same_as; + + { t.GetAffinityMask() }; + { typename std::remove_cvref::type() } -> KPriorityQueueAffinityMask; + + { t.GetActiveCore() } -> std::convertible_to; + { t.GetPriority() } -> std::convertible_to; + }; + */ + + + template /* TODO C++20: requires KPriorityQueueMember */ + class KPriorityQueue { + public: + using AffinityMaskType = typename std::remove_cv().GetAffinityMask())>::type>::type; + + static_assert(LowestPriority >= 0); + static_assert(HighestPriority >= 0); + static_assert(LowestPriority >= HighestPriority); + static constexpr size_t NumPriority = LowestPriority - HighestPriority + 1; + static constexpr size_t NumCores = _NumCores; + + static constexpr ALWAYS_INLINE bool IsValidCore(s32 core) { + return 0 <= core && core < static_cast(NumCores); + } + + static constexpr ALWAYS_INLINE bool IsValidPriority(s32 priority) { + return HighestPriority <= priority && priority <= LowestPriority + 1; + } + private: + using Entry = typename Member::QueueEntry; + public: + class KPerCoreQueue { + private: + Entry root[NumCores]; + public: + constexpr ALWAYS_INLINE KPerCoreQueue() : root() { + for (size_t i = 0; i < NumCores; i++) { + this->root[i].Initialize(); + } + } + + constexpr ALWAYS_INLINE bool PushBack(s32 core, Member *member) { + /* Get the entry associated with the member. */ + Entry &member_entry = member->GetPriorityQueueEntry(core); + + /* Get the entry associated with the end of the queue. */ + Member *tail = this->root[core].GetPrev(); + Entry &tail_entry = (tail != nullptr) ? tail.GetPriorityQueueEntry(core) : this->root[core]; + + /* Link the entries. */ + member_entry.SetPrev(tail); + member_entry.SetNext(nullptr); + tail_entry.SetNext(member); + this->root[core].SetPrev(member); + + return (tail == nullptr); + } + + constexpr ALWAYS_INLINE bool PushFront(s32 core, Member *member) { + /* Get the entry associated with the member. */ + Entry &member_entry = member->GetPriorityQueueEntry(core); + + /* Get the entry associated with the front of the queue. */ + Member *head = this->root[core].GetNext(); + Entry &head_entry = (head != nullptr) ? head.GetPriorityQueueEntry(core) : this->root[core]; + + /* Link the entries. */ + member_entry.SetPrev(nullptr); + member_entry.SetNext(head); + head.SetPrev(member); + this->root[core].SetNext(member); + + return (head == nullptr); + } + + constexpr ALWAYS_INLINE bool Remove(s32 core, Member *member) { + /* Get the entry associated with the member. */ + Entry &member_entry = member->GetPriorityQueueEntry(core); + + /* Get the entries associated with next and prev. */ + Member *prev = member_entry.GetPrev(); + Member *next = member_entry.GetNext(); + Entry &prev_entry = (prev != nullptr) ? prev.GetPriorityQueueEntry(core) : this->root[core]; + Entry &next_entry = (next != nullptr) ? next.GetPriorityQueueEntry(core) : this->root[core]; + + /* Unlink. */ + prev_entry.SetNext(next); + next_entry.SetPrev(prev); + + return (this->root[core].next == nullptr); + } + + constexpr ALWAYS_INLINE Member *GetFront(s32 core) const { + return this->root[core].GetNext(); + } + }; + + class KPriorityQueueImpl { + private: + KPerCoreQueue queues[NumPriority]; + util::BitSet64 available_priorities[NumCores]; + public: + constexpr ALWAYS_INLINE KPriorityQueueImpl() : queues(), available_priorities() { /* ... */ } + + constexpr ALWAYS_INLINE void PushBack(s32 priority, s32 core, Member *member) { + MESOSPHERE_ASSERT(IsValidCore(core)); + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + if (AMS_LIKELY(priority <= LowestPriority)) { + if (this->queues[priority].PushBack(core, member)) { + this->available_priorities[core].SetBit(priority); + } + } + } + + constexpr ALWAYS_INLINE void PushFront(s32 priority, s32 core, Member *member) { + MESOSPHERE_ASSERT(IsValidCore(core)); + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + if (AMS_LIKELY(priority <= LowestPriority)) { + if (this->queues[priority].PushFront(core, member)) { + this->available_priorities[core].SetBit(priority); + } + } + } + + constexpr ALWAYS_INLINE void Remove(s32 priority, s32 core, Member *member) { + MESOSPHERE_ASSERT(IsValidCore(core)); + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + if (AMS_LIKELY(priority <= LowestPriority)) { + if (this->queues[priority].Remove(core, member)) { + this->available_priorities.ClearBit(priority); + } + } + } + + constexpr ALWAYS_INLINE Member *GetFront(s32 core) const { + MESOSPHERE_ASSERT(IsValidCore(core)); + + const s32 priority = this->available_priorities[core].CountLeadingZero(); + if (AMS_LIKELY(priority <= LowestPriority)) { + return this->queues[priority].GetFront(core); + } else { + return nullptr; + } + } + + constexpr ALWAYS_INLINE Member *GetFront(s32 priority, s32 core) const { + MESOSPHERE_ASSERT(IsValidCore(core)); + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + if (AMS_LIKELY(priority <= LowestPriority)) { + return this->queues[priority].GetFront(core); + } else { + return nullptr; + } + } + + constexpr ALWAYS_INLINE Member *GetNext(s32 core, const Member *member) const { + MESOSPHERE_ASSERT(IsValidCore(core)); + + Member *next = member->GetPriorityQueueEntry(core).GetNext(); + if (next == nullptr) { + const s32 priority = this->available_priorities[core].GetNextSet(member->GetPriority()); + if (AMS_LIKELY(priority <= LowestPriority)) { + next = this->queues[priority].GetFront(core); + } + } + return next; + } + + constexpr ALWAYS_INLINE void MoveToFront(s32 priority, s32 core, Member *member) { + MESOSPHERE_ASSERT(IsValidCore(core)); + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + if (AMS_LIKELY(priority <= LowestPriority)) { + this->queues[priority].Remove(core, member); + this->queues[priority].PushFront(core, member); + } + } + + constexpr ALWAYS_INLINE Member *MoveToBack(s32 priority, s32 core, Member *member) { + MESOSPHERE_ASSERT(IsValidCore(core)); + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + if (AMS_LIKELY(priority <= LowestPriority)) { + this->queues[priority].Remove(core, member); + this->queues[priority].PushBack(core, member); + return this->queues[priority].GetFront(core); + } else { + return nullptr; + } + } + }; + private: + KPriorityQueueImpl scheduled_queue; + KPriorityQueueImpl suggested_queue; + private: + constexpr ALWAYS_INLINE void ClearAffinityBit(u64 &affinity, s32 core) { + affinity &= ~(u64(1ul) << core); + } + + constexpr ALWAYS_INLINE s32 GetNextCore(u64 &affinity) { + const s32 core = __builtin_ctzll(static_cast(affinity)); + ClearAffinityBit(core); + return core; + } + + constexpr ALWAYS_INLINE void PushBack(s32 priority, Member *member) { + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + /* Push onto the scheduled queue for its core, if we can. */ + u64 affinity = member->GetAffinityMask().GetAffinityMask(); + if (const s32 core = member->GetActiveCore(); core >= 0) { + this->scheduled_queue.PushBack(priority, core, member); + ClearAffinityBit(affinity, core); + } + + /* And suggest the thread for all other cores. */ + while (affinity) { + this->suggested_queue.PushBack(priority, GetNextCore(affinity), member); + } + } + + constexpr ALWAYS_INLINE void PushFront(s32 priority, Member *member) { + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + /* Push onto the scheduled queue for its core, if we can. */ + u64 affinity = member->GetAffinityMask().GetAffinityMask(); + if (const s32 core = member->GetActiveCore(); core >= 0) { + this->scheduled_queue.PushFront(priority, core, member); + ClearAffinityBit(affinity, core); + } + + /* And suggest the thread for all other cores. */ + /* Note: Nintendo pushes onto the back of the suggested queue, not the front. */ + while (affinity) { + this->suggested_queue.PushBack(priority, GetNextCore(affinity), member); + } + } + + constexpr ALWAYS_INLINE void Remove(s32 priority, Member *member) { + MESOSPHERE_ASSERT(IsValidPriority(priority)); + + /* Remove from the scheduled queue for its core. */ + u64 affinity = member->GetAffinityMask().GetAffinityMask(); + if (const s32 core = member->GetActiveCore(); core >= 0) { + this->scheduled_queue.Remove(priority, core, member); + ClearAffinityBit(affinity, core); + } + + /* Remove from the suggested queue for all other cores. */ + while (affinity) { + this->suggested_queue.Remove(priority, GetNextCore(affinity), member); + } + } + public: + constexpr ALWAYS_INLINE KPriorityQueue() : scheduled_queue(), suggested_queue() { /* ... */ } + + /* Getters. */ + constexpr ALWAYS_INLINE Member *GetScheduledFront(s32 core) const { + return this->scheduled_queue.GetFront(core); + } + + constexpr ALWAYS_INLINE Member *GetScheduledFront(s32 core, s32 priority) const { + return this->scheduled_queue.GetFront(core, priority); + } + + constexpr ALWAYS_INLINE Member *GetSuggestedFront(s32 core) const { + return this->suggested_queue.GetFront(core); + } + + constexpr ALWAYS_INLINE Member *GetSuggestedFront(s32 core, s32 priority) const { + return this->suggested_queue.GetFront(core, priority); + } + + constexpr ALWAYS_INLINE Member *GetScheduledNext(s32 core, const Member *member) const { + return this->scheduled_queue.GetNext(core, member); + } + + constexpr ALWAYS_INLINE Member *GetSuggestedNext(s32 core, const Member *member) const { + return this->suggested_queue.GetNext(core, member); + } + + constexpr ALWAYS_INLINE Member *GetSamePriorityNext(s32 core, const Member *member) const { + return member->GetPriorityQueueEntry(core).GetNext(); + } + + /* Mutators. */ + constexpr ALWAYS_INLINE void PushBack(Member *member) { + this->PushBack(member, member->GetPriority()); + } + + constexpr ALWAYS_INLINE void Remove(Member *member) { + this->Remove(member, member->GetPriority()); + } + + constexpr ALWAYS_INLINE void MoveToScheduledFront(Member *member) { + this->scheduled_queue.MoveToFront(member->GetPriority(), member->GetActiveCore(), member); + } + + constexpr ALWAYS_INLINE void MoveToScheduledBack(Member *member) { + this->scheduled_queue.MoveToBack(member->GetPriority(), member->GetActiveCore(), member); + } + + /* First class fancy operations. */ + constexpr ALWAYS_INLINE void ChangePriority(s32 prev_priority, bool is_running, Member *member) { + MESOSPHERE_ASSERT(IsValidPriority(prev_priority)); + + /* Remove the member from the queues. */ + const s32 new_priority = member->GetPriority(); + this->Remove(prev_priority, member); + + /* And enqueue. If the member is running, we want to keep it running. */ + if (is_running) { + this->PushFront(new_priority, member); + } else { + this->PushBack(new_priority, member); + } + } + + constexpr ALWAYS_INLINE void ChangeAffinityMask(s32 prev_core, const AffinityMaskType &prev_affinity, Member *member) { + /* Get the new information. */ + const s32 priority = member->GetPriority(); + const AffinityMaskType &new_affinity = member->GetAffinityMask(); + const s32 new_core = member->GetActiveCore(); + + /* Remove the member from all queues it was in before. */ + for (s32 core = 0; core < static_cast(NumCores); core++) { + if (prev_affinity.GetAffinity(core)) { + if (core == prev_core) { + this->scheduled_queue.Remove(priority, core, member); + } else { + this->suggested_queue.Remove(priority, core, member); + } + } + } + + /* And add the member to all queues it should be in now. */ + for (s32 core = 0; core < static_cast(NumCores); core++) { + if (prev_affinity.GetAffinity(core)) { + if (core == new_core) { + this->scheduled_queue.PushBack(priority, core, member); + } else { + this->suggested_queue.PushBack(priority, core, member); + } + } + } + } + + constexpr ALWAYS_INLINE void ChangeCore(s32 prev_core, Member *member, bool to_front = false) { + /* Get the new information. */ + const s32 new_core = member->GetActiveCore(); + const s32 priority = member->GetPriority(); + + /* We don't need to do anything if the core is the same. */ + if (prev_core != new_core) { + /* Remove from the scheduled queue for the previous core. */ + if (prev_core >= 0) { + this->scheduled_queue.Remove(priority, prev_core, member); + } + + /* Remove from the suggested queue and add to the scheduled queue for the new core. */ + if (new_core >= 0) { + this->suggested_queue.Remove(priority, prev_core, member); + if (to_front) { + this->scheduled_queue.PushFront(priority, new_core, member); + } else { + this->scheduled_queue.PushBack(priority, new_core, member); + } + } + + /* Add to the suggested queue for the previous core. */ + if (prev_core >= 0) { + this->suggested_queue.PushBack(priority, prev_core, member); + } + } + } + }; + +} diff --git a/libraries/libmesosphere/include/mesosphere/kern_k_scheduler.hpp b/libraries/libmesosphere/include/mesosphere/kern_k_scheduler.hpp index 7476d1aea..620566b46 100644 --- a/libraries/libmesosphere/include/mesosphere/kern_k_scheduler.hpp +++ b/libraries/libmesosphere/include/mesosphere/kern_k_scheduler.hpp @@ -14,10 +14,17 @@ * along with this program. If not, see . */ #pragma once +#include #include +#include namespace ams::kern { + using KSchedulerPriorityQueue = KPriorityQueue; + static_assert(std::is_same::value); + static_assert(KSchedulerPriorityQueue::NumCores == cpu::NumCores); + static_assert(KSchedulerPriorityQueue::NumPriority == BITSIZEOF(u64)); + class KScheduler { NON_COPYABLE(KScheduler); NON_MOVEABLE(KScheduler); diff --git a/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp b/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp index 64d8c1888..49155b7ca 100644 --- a/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp +++ b/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp @@ -34,6 +34,29 @@ namespace ams::kern { void *context; /* TODO: KThreadContext * */ }; static_assert(alignof(StackParameters) == 0x10); + + struct QueueEntry { + private: + KThread *prev; + KThread *next; + public: + constexpr ALWAYS_INLINE QueueEntry() : prev(nullptr), next(nullptr) { /* ... */ } + + constexpr ALWAYS_INLINE KThread *GetPrev() const { return this->prev; } + constexpr ALWAYS_INLINE KThread *GetNext() const { return this->next; } + constexpr ALWAYS_INLINE void SetPrev(KThread *t) { this->prev = t; } + constexpr ALWAYS_INLINE void SetNext(KThread *t) { this->next = t; } + }; + private: + /* TODO: Other members. These are placeholder to get KScheduler to compile. */ + KAffinityMask affinity_mask; + public: + constexpr KThread() : KAutoObjectWithSlabHeapAndContainer(), affinity_mask() { /* ... */ } + + constexpr ALWAYS_INLINE const KAffinityMask &GetAffinityMask() const { return this->affinity_mask; } + public: + static void PostDestroy(uintptr_t arg); + /* TODO: This is a placeholder definition. */ }; diff --git a/libraries/libvapours/include/vapours/svc/svc_types_common.hpp b/libraries/libvapours/include/vapours/svc/svc_types_common.hpp index 3bd20e6c8..d46f3ae8f 100644 --- a/libraries/libvapours/include/vapours/svc/svc_types_common.hpp +++ b/libraries/libvapours/include/vapours/svc/svc_types_common.hpp @@ -277,6 +277,9 @@ namespace ams::svc { constexpr size_t ThreadLocalRegionSize = 0x200; + constexpr s32 LowestThreadPriority = 63; + constexpr s32 HighestThreadPriority = 0; + /* Process types. */ enum ProcessInfoType : u32 { ProcessInfoType_ProcessState = 0,