Add mesosphere (VERY VERY WIP)

mesosphere/source/core/KCoreContext.cpp

@@ -0,0 +1,11 @@
+#include <mesosphere/core/KCoreContext.hpp>
+#include <mesosphere/threading/KScheduler.hpp>
+
+namespace mesosphere
+{
+
+static KScheduler scheds[4];
+
+std::array<KCoreContext, MAX_CORES> KCoreContext::instances{ &scheds[0], &scheds[1], &scheds[2], &scheds[3] };
+
+}

mesosphere/source/interfaces/IAlarmable.cpp

@@ -0,0 +1,20 @@
+#include <mesosphere/interfaces/IAlarmable.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+#include <mesosphere/interrupts/KAlarm.hpp>
+
+namespace mesosphere
+{
+
+void IAlarmable::SetAlarmTimeImpl(const KSystemClock::time_point &alarmTime)
+{
+    this->alarmTime = alarmTime;
+    KCoreContext::GetCurrentInstance().GetAlarm()->AddAlarmable(*this);
+}
+
+void IAlarmable::ClearAlarm()
+{
+    KCoreContext::GetCurrentInstance().GetAlarm()->RemoveAlarmable(*this);
+    alarmTime = KSystemClock::time_point{};
+}
+
+}

mesosphere/source/interfaces/IInterruptibleWork.cpp

@@ -0,0 +1,12 @@
+#include <mesosphere/interfaces/IInterruptibleWork.hpp>
+
+namespace mesosphere
+{
+
+IWork *IInterruptibleWork::HandleInterrupt(uint interruptId)
+{
+    (void)interruptId;
+    return (IWork *)this;
+}
+
+}

mesosphere/source/interfaces/ILimitedResource.cpp

@@ -0,0 +1,26 @@
+#include <mesosphere/interfaces/ILimitedResource.hpp>
+#include <mesosphere/processes/KProcess.hpp>
+#include <mesosphere/kresources/KResourceLimit.hpp>
+
+namespace mesosphere::detail
+{
+
+void ReleaseResource(const SharedPtr<KResourceLimit> &reslimit, KAutoObject::TypeId typeId, size_t count, size_t realCount)
+{
+    if (reslimit != nullptr) {
+        reslimit->Release(KResourceLimit::GetCategory(typeId), count, realCount);
+    } else {
+        KResourceLimit::GetDefaultInstance().Release(KResourceLimit::GetCategory(typeId), count, realCount);
+    }
+}
+
+void ReleaseResource(const SharedPtr<KProcess> &owner, KAutoObject::TypeId typeId, size_t count, size_t realCount)
+{
+    if (owner != nullptr) {
+        return ReleaseResource(owner->GetResourceLimit(), typeId, count, realCount);
+    }  else {
+        KResourceLimit::GetDefaultInstance().Release(KResourceLimit::GetCategory(typeId), count, realCount);
+    }
+}
+
+}

mesosphere/source/interrupts/KAlarm.cpp

@@ -0,0 +1,58 @@
+#include <mesosphere/interrupts/KAlarm.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+#include <mesosphere/threading/KScheduler.hpp>
+#include <mesosphere/arch/KInterruptMaskGuard.hpp>
+
+namespace mesosphere
+{
+
+void KAlarm::AddAlarmable(IAlarmable &alarmable)
+{
+    std::lock_guard guard{spinlock};
+    alarmables.insert(alarmable);
+
+    KSystemClock::SetAlarm(alarmables.cbegin()->GetAlarmTime());
+}
+
+void KAlarm::RemoveAlarmable(const IAlarmable &alarmable)
+{
+    std::lock_guard guard{spinlock};
+    alarmables.erase(alarmable);
+
+    KSystemClock::SetAlarm(alarmables.cbegin()->GetAlarmTime());
+}
+
+void KAlarm::HandleAlarm()
+{
+    {
+        KCriticalSection &critsec = KScheduler::GetCriticalSection();
+        std::lock_guard criticalSection{critsec};
+        std::lock_guard guard{spinlock};
+
+        KSystemClock::SetInterruptMasked(true); // mask timer interrupt
+        KSystemClock::time_point currentTime = KSystemClock::now(), maxAlarmTime;
+        while (alarmables.begin() != alarmables.end()) {
+            IAlarmable &a = *alarmables.begin();
+            maxAlarmTime = a.alarmTime;
+            if (maxAlarmTime > currentTime) {
+                break;
+            }
+
+            alarmables.erase(a);
+            a.alarmTime = KSystemClock::time_point{};
+
+            a.OnAlarm();
+        }
+
+        if (maxAlarmTime > KSystemClock::time_point{}) {
+            KSystemClock::SetAlarm(maxAlarmTime);
+        }
+    }
+
+    {
+        // TODO Reenable interrupt 30
+        KInterruptMaskGuard guard{};
+    }
+}
+
+}

mesosphere/source/interrupts/KWorkQueue.cpp

@@ -0,0 +1,45 @@
+#include <mesosphere/interrupts/KWorkQueue.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+#include <mesosphere/threading/KScheduler.hpp>
+#include <mesosphere/arch/KInterruptMaskGuard.hpp>
+
+namespace mesosphere
+{
+
+void KWorkQueue::AddWork(IWork &work)
+{
+    workQueue.push_back(work);
+    KCoreContext::GetCurrentInstance().GetScheduler()->SetContextSwitchNeededForWorkQueue();
+}
+
+void KWorkQueue::Initialize()
+{
+    //handlerThread.reset(new KThread); //TODO!
+    kassert(handlerThread == nullptr);
+}
+
+void KWorkQueue::HandleWorkQueue()
+{
+    KCoreContext &cctx = KCoreContext::GetCurrentInstance();
+    while (true) {
+        IWork *work = nullptr;
+        do {
+            KInterruptMaskGuard imguard{};
+            auto it = workQueue.begin();
+            if (it != workQueue.end()) {
+                work = &*it;
+                workQueue.erase(it);
+            } else {
+                {
+                    //TODO: thread usercontext scheduler/bottom hard guard
+                    cctx.GetCurrentThread()->Reschedule(KThread::SchedulingStatus::Paused);
+                }
+                cctx.GetScheduler()->ForceContextSwitch();
+            } 
+        } while (work == nullptr);
+
+        work->DoWork();
+    }
+}
+
+}

mesosphere/source/kresources/KAutoObject.cpp

@@ -0,0 +1,10 @@
+#include <mesosphere/kresources/KAutoObject.hpp>
+
+namespace mesosphere
+{
+
+KAutoObject::~KAutoObject()
+{
+}
+
+}

mesosphere/source/kresources/KResourceLimit.cpp

@@ -0,0 +1,83 @@
+#include <mesosphere/kresources/KResourceLimit.hpp>
+
+namespace mesosphere
+{
+
+KResourceLimit KResourceLimit::defaultInstance{};
+
+size_t KResourceLimit::GetCurrentValue(KResourceLimit::Category category) const
+{
+    // Caller should check category
+    std::lock_guard guard{condvar.mutex()};
+    return currentValues[(uint)category];
+}
+
+size_t KResourceLimit::GetLimitValue(KResourceLimit::Category category) const
+{
+    // Caller should check category
+    std::lock_guard guard{condvar.mutex()};
+    return limitValues[(uint)category];
+}
+
+size_t KResourceLimit::GetRemainingValue(KResourceLimit::Category category) const
+{
+    // Caller should check category
+    std::lock_guard guard{condvar.mutex()};
+    return limitValues[(uint)category] - currentValues[(uint)category];
+}
+
+bool KResourceLimit::SetLimitValue(KResourceLimit::Category category, size_t value)
+{
+    std::lock_guard guard{condvar.mutex()};
+    if ((long)value < 0 || currentValues[(uint)category] > value) {
+        return false;
+    } else {
+        limitValues[(uint)category] = value;
+        return true;
+    }
+}
+
+void KResourceLimit::Release(KResourceLimit::Category category, size_t count, size_t realCount)
+{
+    // Caller should ensure parameters are correct
+    std::lock_guard guard{condvar.mutex()};
+    currentValues[(uint)category] -= count;
+    realValues[(uint)category] -= realCount;
+    condvar.notify_all();
+}
+
+bool KResourceLimit::ReserveDetail(KResourceLimit::Category category, size_t count, const KSystemClock::time_point &timeoutTime)
+{
+    std::lock_guard guard{condvar.mutex()};
+    if ((long)count <= 0 || realValues[(uint)category] >= limitValues[(uint)category]) {
+        return false;
+    }
+
+    size_t newCur = currentValues[(uint)category] + count;
+    bool ok = false;
+
+    auto condition =
+    [=, &newCur] {
+            newCur = this->currentValues[(uint)category] + count;
+            size_t lval = this->limitValues[(uint)category];
+            return this->realValues[(uint)category] <= lval && newCur <= lval; // need to check here
+    };
+
+    if (timeoutTime <= KSystemClock::never) {
+        // TODO, check is actually < 0
+        // TODO timeout
+        ok = true;
+        condvar.wait(condition);
+    } else {
+        ok = condvar.wait_until(timeoutTime, condition);
+    }
+
+    if (ok) {
+        currentValues[(uint)category] += count;
+        realValues[(uint)category] += count;
+    }
+
+    return ok;
+}
+
+}

mesosphere/source/my_libstdc++.cpp

@@ -0,0 +1,6 @@
+#include <cstddef>
+
+void *operator new(std::size_t) { for(;;); }
+void *operator new[](std::size_t) { for(;;); }
+void operator delete(void *) { }
+void operator delete[](void *) { }

mesosphere/source/processes/KHandleTable.cpp

@@ -0,0 +1,133 @@
+#include <mutex>
+#include <algorithm>
+#include <mesosphere/processes/KHandleTable.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+#include <mesosphere/threading/KThread.hpp>
+
+namespace mesosphere
+{
+
+bool KHandleTable::IsValid(Handle handle) const
+{
+    // Official kernel checks for nullptr, however this makes the deferred-init logic more difficult.
+    // We use our own, more secure, logic instead, that is, free entries are <= 0.
+    return handle.index < capacity && handle.id > 0 && entries[handle.index].id == handle.id;
+}
+
+SharedPtr<KAutoObject> KHandleTable::GetAutoObject(Handle handle) const
+{
+    if (!handle.IsAliasOrFree()) {
+        // Note: official kernel locks the spinlock here, but we don't need to.
+        return nullptr;
+    } else {
+        std::lock_guard guard{spinlock};
+        return IsValid(handle) ? entries[handle.index].object : nullptr;
+    }
+}
+
+SharedPtr<KThread> KHandleTable::GetThread(Handle handle, bool allowAlias) const
+{
+    if (allowAlias && handle == selfThreadAlias) {
+        return KCoreContext::GetCurrentInstance().GetCurrentThread();
+    } else {
+        return DynamicObjectCast<KThread>(GetAutoObject(handle));
+    }
+}
+
+SharedPtr<KProcess> KHandleTable::GetProcess(Handle handle, bool allowAlias) const
+{
+    if (allowAlias && handle == selfProcessAlias) {
+        return KCoreContext::GetCurrentInstance().GetCurrentProcess();
+    } else {
+        return DynamicObjectCast<KProcess>(GetAutoObject(handle));
+    }
+}
+
+bool KHandleTable::Close(Handle handle)
+{
+    SharedPtr<KAutoObject> tmp{nullptr}; // ensure any potential dtor is called w/o the spinlock being held
+
+    if (handle.IsAliasOrFree()) {
+        return false;
+    } else {
+        std::lock_guard guard{spinlock};
+        if (IsValid(handle)) {
+            entries[-firstFreeIndex].id = firstFreeIndex;
+            firstFreeIndex = -(s16)handle.index;
+            --numActive;
+            tmp = std::move(entries[handle.index].object);
+            return true;
+        } else {
+            return false;
+        }
+    }
+}
+
+bool KHandleTable::Generate(Handle &out, SharedPtr<KAutoObject> obj)
+{
+    // Note: nullptr is accepted, for deferred-init.
+
+    std::lock_guard guard{spinlock};
+    if (numActive >= capacity) {
+        return false; // caller should return 0xD201
+    }
+
+    // Get/allocate the entry
+    u16 index = (u16)-firstFreeIndex;
+    Entry *e = &entries[-firstFreeIndex];
+    firstFreeIndex = e->id;
+
+    e->id = idCounter;
+    e->object = std::move(obj);
+
+    out.index = index;
+    out.id = e->id;
+    out.isAlias = false;
+
+    size = ++numActive > size ? numActive : size;
+    idCounter = idCounter == 0x7FFF ? 1 : idCounter + 1;
+
+    return true;
+}
+
+bool KHandleTable::Set(SharedPtr<KAutoObject> obj, Handle handle)
+{
+    if (!handle.IsAliasOrFree() && IsValid(handle)) {
+        std::lock_guard guard{spinlock};
+        entries[handle.index].object = std::move(obj);
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void KHandleTable::Destroy()
+{
+    spinlock.lock();
+    u16 capa = capacity;
+    capacity = 0;
+    firstFreeIndex = 0;
+    spinlock.unlock();
+
+    for (u16 i = 0; i < capa; i++) {
+        entries[i].object = nullptr;
+        entries[i].id = -(i + 1);
+    }
+}
+
+KHandleTable::KHandleTable(size_t capacity_) : capacity((u16)capacity_)
+{
+    // Note: caller should check the > case, and return an error in that case!
+    capacity = capacity > capacityLimit || capacity == 0 ? (u16)capacityLimit : capacity;
+
+    u16 capa = capacity;
+    Destroy();
+    capacity = capa;
+}
+
+KHandleTable::~KHandleTable()
+{
+    Destroy();
+}
+
+}

mesosphere/source/processes/KProcess.cpp

@@ -0,0 +1,14 @@
+#include <mesosphere/processes/KProcess.hpp>
+#include <mesosphere/threading/KThread.hpp>
+#include <mesosphere/kresources/KResourceLimit.hpp>
+
+namespace mesosphere
+{
+
+void KProcess::SetLastThreadAndIdleSelectionCount(KThread *thread, ulong idleSelectionCount)
+{
+    lastThreads[thread->GetCurrentCoreId()] = thread;
+    lastIdleSelectionCount[thread->GetCurrentCoreId()] = idleSelectionCount;
+}
+
+}

mesosphere/source/test.cpp

@@ -0,0 +1,10 @@
+int main(void) {
+    for(;;);
+    return 0;
+}
+
+extern "C" {
+    void _start(void) {
+        main();
+    }
+}

mesosphere/source/threading/KConditionVariable.cpp

@@ -0,0 +1,39 @@
+#include <mesosphere/threading/KConditionVariable.hpp>
+#include <mesosphere/threading/KScheduler.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+
+namespace mesosphere
+{
+
+void KConditionVariable::wait_until_impl(const KSystemClock::time_point &timeoutPoint) noexcept
+{
+    // Official kernel counts number of waiters, but that isn't necessary
+    {
+        KThread *currentThread = KCoreContext::GetCurrentInstance().GetCurrentThread();
+        std::lock_guard guard{KScheduler::GetCriticalSection()};
+        mutex_.unlock();
+        if (currentThread->WaitForKernelSync(waiterList)) {
+            (void)timeoutPoint; //TODO!
+        } else {
+            // Termination
+        }
+    }
+    mutex_.lock();
+}
+
+void KConditionVariable::notify_one() noexcept
+{
+    std::lock_guard guard{KScheduler::GetCriticalSection()};
+    auto t = waiterList.begin();
+    if (t != waiterList.end()) {
+        t->ResumeFromKernelSync();
+    }
+}
+
+void KConditionVariable::notify_all() noexcept
+{
+    std::lock_guard guard{KScheduler::GetCriticalSection()};
+    KThread::ResumeAllFromKernelSync(waiterList);
+}
+
+}

mesosphere/source/threading/KMutex.cpp

@@ -0,0 +1,62 @@
+#include <mesosphere/threading/KMutex.hpp>
+#include <mesosphere/threading/KThread.hpp>
+#include <mesosphere/threading/KScheduler.hpp>
+
+namespace mesosphere
+{
+
+void KMutex::lock_slow_path(KThread &owner, KThread &requester)
+{
+    // Requester is currentThread most of (all ?) the time
+    KCriticalSection &critsec = KScheduler::GetCriticalSection();
+    std::lock_guard criticalSection{critsec};
+    if (KCoreContext::GetCurrentInstance().GetScheduler()->IsActive()) {
+        requester.SetWantedMutex((uiptr)this);
+        owner.AddMutexWaiter(requester);
+
+        // If the requester is/was running, pause it (sets status even if force-paused).
+        requester.RescheduleIfStatusEquals(KThread::SchedulingStatus::Running, KThread::SchedulingStatus::Paused);
+
+        // If the owner is force-paused, temporarily wake it.
+        if (owner.IsForcePaused()) {
+            owner.AdjustScheduling(owner.RevertForcePauseToField());
+        }
+
+        // Commit scheduler changes NOW.
+        critsec.unlock();
+        critsec.lock();
+
+        /*
+            At this point, mutex ownership has been transferred to requester or another thread (false wake).
+            Make sure the requester, now resumed, isn't in any mutex wait list.
+        */
+       owner.RemoveMutexWaiter(requester);
+    }
+}
+
+void KMutex::unlock_slow_path(KThread &owner)
+{
+    std::lock_guard criticalSection{KScheduler::GetCriticalSection()};
+    size_t count;
+    KThread *newOwner = owner.RelinquishMutex(&count, (uiptr)this);
+    native_handle_type newTag;
+
+    if (newOwner != nullptr) {
+        // Wake up new owner
+        newTag = (native_handle_type)newOwner | (count > 1 ? 1 : 0);
+        // Sets status even if force-paused.
+        newOwner->RescheduleIfStatusEquals(KThread::SchedulingStatus::Paused, KThread::SchedulingStatus::Running);
+    } else {
+        // Free the mutex.
+        newTag = 0;
+    }
+
+    // Allow previous owner to get back to forced-sleep, if no other thread wants the kmutexes it is holding.
+    if (!owner.IsDying() && owner.GetNumberOfKMutexWaiters() == 0) {
+        owner.AdjustScheduling(owner.CommitForcePauseToField());
+    }
+
+    tag = newTag;
+}
+
+}

mesosphere/source/threading/KScheduler.cpp

@@ -0,0 +1,344 @@
+#include <algorithm>
+#include <atomic>
+
+#include <mesosphere/threading/KScheduler.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+
+namespace mesosphere
+{
+
+namespace {
+    struct MlqTraitsFactory {
+        constexpr KThread::SchedulerValueTraits operator()(size_t i) const
+        {
+            return KThread::SchedulerValueTraits{(uint)i};
+        }
+    };
+}
+
+using MlqT = KScheduler::Global::MlqType;
+
+bool KScheduler::Global::reselectionRequired = false;
+
+std::array<MlqT, MAX_CORES> KScheduler::Global::scheduledMlqs =
+    detail::MakeArrayWithFactorySequenceOf<MlqT, MlqTraitsFactory, MAX_CORES>(
+        &KThread::GetPriorityOf
+    );
+
+std::array<MlqT, MAX_CORES> KScheduler::Global::suggestedMlqs =
+    detail::MakeArrayWithFactorySequenceOf<MlqT, MlqTraitsFactory, MAX_CORES>(
+        &KThread::GetPriorityOf
+    );
+
+
+void KScheduler::Global::SetThreadRunning(KThread &thread)
+{
+    ApplyReschedulingOperation([](MlqT &mlq, KThread &t){ mlq.add(t); }, thread);
+}
+
+void KScheduler::Global::SetThreadPaused(KThread &thread)
+{
+    ApplyReschedulingOperation([](MlqT &mlq, KThread &t){ mlq.remove(t); }, thread);
+}
+
+void KScheduler::Global::AdjustThreadPriorityChanged(KThread &thread, uint oldPrio, bool isCurrentThread)
+{
+    ApplyReschedulingOperation(
+    [oldPrio, isCurrentThread](MlqT &mlq, KThread &t){
+        mlq.adjust(t, oldPrio, isCurrentThread);
+    }, thread);
+}
+
+void KScheduler::Global::AdjustThreadAffinityChanged(KThread &thread, int oldCoreId, u64 oldAffinityMask)
+{
+    int newCoreId = thread.GetCurrentCoreId();
+    u64 newAffinityMask = thread.GetAffinityMask();
+
+    ApplyReschedulingOperationImpl([](MlqT &mlq, KThread &t){ mlq.remove(t); }, thread, oldCoreId, oldAffinityMask);
+    ApplyReschedulingOperationImpl([](MlqT &mlq, KThread &t){ mlq.add(t); }, thread, newCoreId, newAffinityMask);
+
+    thread.IncrementSchedulerOperationCount();
+    reselectionRequired = true;
+}
+
+void KScheduler::Global::TransferThreadToCore(KThread &thread, int coreId)
+{
+    int currentCoreId = thread.GetCurrentCoreId();
+
+    if (currentCoreId != coreId) {
+        if (currentCoreId != -1) {
+            scheduledMlqs[currentCoreId].transferToBack(thread, suggestedMlqs[currentCoreId]);
+        }
+
+        if (coreId != -1) {
+            suggestedMlqs[coreId].transferToFront(thread, scheduledMlqs[coreId]);
+        }
+    }
+
+    thread.SetCurrentCoreId(coreId);
+}
+
+void KScheduler::Global::AskForReselectionOrMarkRedundant(KThread *currentThread, KThread *winner)
+{
+    if (currentThread == winner) {
+        // Nintendo (not us) has a nullderef bug on currentThread->owner, but which is never triggered.
+        currentThread->SetRedundantSchedulerOperation();
+    } else {
+        reselectionRequired = true;
+    }
+}
+
+KThread *KScheduler::Global::PickOneSuggestedThread(const std::array<KThread *, MAX_CORES> &curThreads,
+uint coreId, bool compareTime, bool allowSecondPass, uint maxPrio, uint minPrio) {
+    if (minPrio < maxPrio) {
+        return nullptr;
+    }
+
+    auto hasWorseTime = [coreId, minPrio, compareTime](const KThread &t) {
+        if (!compareTime || scheduledMlqs[coreId].size(minPrio) <= 1 || t.GetPriority() < minPrio) {
+            return false;
+        } else {
+            // Condition means the thread *it would have been scheduled again after the thread
+            return t.GetLastScheduledTime() > scheduledMlqs[coreId].front(minPrio).GetLastScheduledTime();
+        }
+    };
+
+    std::array<uint, MAX_CORES> secondPassCores;
+    size_t numSecondPassCores = 0;
+
+    auto it = std::find_if(
+        suggestedMlqs[coreId].begin(maxPrio),
+        suggestedMlqs[coreId].end(minPrio),
+        [&hasWorseTime, &secondPassCores, &numSecondPassCores, &curThreads](const KThread &t) {
+            int srcCoreId = t.GetCurrentCoreId();
+            //bool worseTime = compareTime && hasWorseTime(t);
+            // break if hasWorse time too
+            if (srcCoreId >= 0) {
+                bool srcHasEphemeralKernThread = scheduledMlqs[srcCoreId].highestPrioritySet() < minRegularPriority;
+                bool isSrcCurT = &t == curThreads[srcCoreId];
+                if (isSrcCurT) {
+                    secondPassCores[numSecondPassCores++] = (uint)srcCoreId;
+                }
+
+                // Note, if checkTime official kernel breaks if srcHasEphemeralKernThread
+                // I believe this is a bug
+                if(srcHasEphemeralKernThread || isSrcCurT) {
+                    return false;
+                }
+            }
+
+            return true;
+        }
+    );
+
+    if (it != suggestedMlqs[coreId].end(minPrio) && (!compareTime || !hasWorseTime(*it))) {
+        return &*it;
+    } else if (allowSecondPass) {
+        // Allow to re-pick a selected thread about to be current, if it doesn't make the core idle
+        auto srcCoreIdPtr = std::find_if(
+            secondPassCores.cbegin(),
+            secondPassCores.cbegin() + numSecondPassCores,
+            [](uint id) {
+                return scheduledMlqs[id].highestPrioritySet() >= minRegularPriority && scheduledMlqs[id].size() > 1;
+            }
+        );
+
+        return srcCoreIdPtr == secondPassCores.cbegin() + numSecondPassCores ? nullptr : &scheduledMlqs[*srcCoreIdPtr].front();
+    } else {
+        return nullptr;
+    }
+}
+
+void KScheduler::Global::YieldThread(KThread &currentThread)
+{
+    // Note: caller should use critical section, etc.
+    kassert(currentThread.GetCurrentCoreId() >= 0);
+    uint coreId = (uint)currentThread.GetCurrentCoreId();
+    uint priority = currentThread.GetPriority();
+
+    // Yield the thread
+    scheduledMlqs[coreId].yield(currentThread);
+    currentThread.IncrementSchedulerOperationCount();
+
+    KThread *winner = &scheduledMlqs[coreId].front(priority);
+    AskForReselectionOrMarkRedundant(&currentThread, winner);
+}
+
+void KScheduler::Global::YieldThreadAndBalanceLoad(KThread &currentThread)
+{
+    // Note: caller should check if !currentThread.IsSchedulerOperationRedundant and use critical section, etc.
+    kassert(currentThread.GetCurrentCoreId() >= 0);
+    uint coreId = (uint)currentThread.GetCurrentCoreId();
+    uint priority = currentThread.GetPriority();
+
+    std::array<KThread *, MAX_CORES> curThreads;
+    for (uint i = 0; i < MAX_CORES; i++) {
+        curThreads[i] = scheduledMlqs[i].empty() ? nullptr : &scheduledMlqs[i].front();
+    }
+
+    // Yield the thread
+    scheduledMlqs[coreId].yield(currentThread);
+    currentThread.IncrementSchedulerOperationCount();
+
+    KThread *winner = PickOneSuggestedThread(curThreads, coreId, true, false, 0, priority);
+
+    if (winner != nullptr) {
+        TransferThreadToCore(*winner, coreId);
+        winner->IncrementSchedulerOperationCount();
+        currentThread.SetRedundantSchedulerOperation();
+    } else {
+        winner = &scheduledMlqs[coreId].front(priority);
+    }
+
+    AskForReselectionOrMarkRedundant(&currentThread, winner);
+}
+
+void KScheduler::Global::YieldThreadAndWaitForLoadBalancing(KThread &currentThread)
+{
+    // Note: caller should check if !currentThread.IsSchedulerOperationRedundant and use critical section, etc.
+    KThread *winner = nullptr;
+    kassert(currentThread.GetCurrentCoreId() >= 0);
+    uint coreId = (uint)currentThread.GetCurrentCoreId();
+
+    // Remove the thread from its scheduled mlq, put it on the corresponding "suggested" one instead
+    TransferThreadToCore(currentThread, -1);
+    currentThread.IncrementSchedulerOperationCount();
+
+    // If the core is idle, perform load balancing, excluding the threads that have just used this function...
+    if (scheduledMlqs[coreId].empty()) {
+        // Here, "curThreads" is calculated after the ""yield"", unlike yield -1
+        std::array<KThread *, MAX_CORES> curThreads;
+        for (uint i = 0; i < MAX_CORES; i++) {
+            curThreads[i] = scheduledMlqs[i].empty() ? nullptr : &scheduledMlqs[i].front();
+        }
+
+        KThread *winner = PickOneSuggestedThread(curThreads, coreId, false);
+
+        if (winner != nullptr) {
+            TransferThreadToCore(*winner, coreId);
+            winner->IncrementSchedulerOperationCount();
+        } else {
+            winner = &currentThread;
+        }
+    }
+
+    AskForReselectionOrMarkRedundant(&currentThread, winner);
+}
+
+void KScheduler::Global::YieldPreemptThread(KThread &currentKernelHandlerThread, uint coreId, uint maxPrio)
+{
+    if (!scheduledMlqs[coreId].empty(maxPrio)) {
+        // Yield the first thread in the level queue
+        scheduledMlqs[coreId].front(maxPrio).IncrementSchedulerOperationCount();
+        scheduledMlqs[coreId].yield(maxPrio);
+        if (scheduledMlqs[coreId].size() > 1) {
+            scheduledMlqs[coreId].front(maxPrio).IncrementSchedulerOperationCount();
+        }
+    }
+
+    // Here, "curThreads" is calculated after the forced yield, unlike yield -1
+    std::array<KThread *, MAX_CORES> curThreads;
+    for (uint i = 0; i < MAX_CORES; i++) {
+        curThreads[i] = scheduledMlqs[i].empty() ? nullptr : &scheduledMlqs[i].front();
+    }
+
+    KThread *winner = PickOneSuggestedThread(curThreads, coreId, true, false, maxPrio, maxPrio);
+    if (winner != nullptr) {
+        TransferThreadToCore(*winner, coreId);
+        winner->IncrementSchedulerOperationCount();
+    }
+
+    for (uint i = 0; i < MAX_CORES; i++) {
+        curThreads[i] = scheduledMlqs[i].empty() ? nullptr : &scheduledMlqs[i].front();
+    }
+
+    // Find first thread which is not the kernel handler thread.
+    auto itFirst = std::find_if(
+        scheduledMlqs[coreId].begin(),
+        scheduledMlqs[coreId].end(),
+        [&currentKernelHandlerThread, coreId](const KThread &t) {
+            return &t != &currentKernelHandlerThread;
+        }
+    );
+
+    if (itFirst != scheduledMlqs[coreId].end()) {
+        // If under the threshold, do load balancing again
+        winner = PickOneSuggestedThread(curThreads, coreId, true, false, maxPrio, itFirst->GetPriority() - 1);
+        if (winner != nullptr) {
+            TransferThreadToCore(*winner, coreId);
+            winner->IncrementSchedulerOperationCount();
+        }
+    }
+
+    reselectionRequired = true;
+}
+
+void KScheduler::Global::SelectThreads()
+{
+    auto updateThread = [](KThread *thread, KScheduler &sched) {
+        if (thread != sched.selectedThread) {
+            if (thread != nullptr) {
+                thread->IncrementSchedulerOperationCount();
+                thread->UpdateLastScheduledTime();
+                thread->SetProcessLastThreadAndIdleSelectionCount(sched.idleSelectionCount);
+            } else {
+                ++sched.idleSelectionCount;
+            }
+            sched.selectedThread = thread;
+            sched.isContextSwitchNeeded = true;
+        }
+        std::atomic_thread_fence(std::memory_order_seq_cst);
+    };
+
+    // This maintain the "current thread is on front of queue" invariant
+    std::array<KThread *, MAX_CORES> curThreads;
+    for (uint i = 0; i < MAX_CORES; i++) {
+        KScheduler &sched = *KCoreContext::GetInstance(i).GetScheduler();
+        curThreads[i] = scheduledMlqs[i].empty() ? nullptr : &scheduledMlqs[i].front();
+        updateThread(curThreads[i], sched);
+    }
+
+    // Do some load-balancing. Allow second pass.
+    std::array<KThread *, MAX_CORES> curThreads2 = curThreads;
+    for (uint i = 0; i < MAX_CORES; i++) {
+        if (scheduledMlqs[i].empty()) {
+            KThread *winner = PickOneSuggestedThread(curThreads2, i, false, true);
+            if (winner != nullptr) {
+                curThreads2[i] = winner;
+                TransferThreadToCore(*winner, i);
+                winner->IncrementSchedulerOperationCount();
+            }
+        }
+    }
+
+    // See which to-be-current threads have changed & update accordingly
+    for (uint i = 0; i < MAX_CORES; i++) {
+        KScheduler &sched = *KCoreContext::GetInstance(i).GetScheduler();
+        if (curThreads2[i] != curThreads[i]) {
+            updateThread(curThreads2[i], sched);
+        }
+    }
+    reselectionRequired = false;
+}
+
+KCriticalSection KScheduler::criticalSection{};
+
+void KScheduler::YieldCurrentThread()
+{
+    KCoreContext &cctx = KCoreContext::GetCurrentInstance();
+    cctx.GetScheduler()->DoYieldOperation(Global::YieldThread, *cctx.GetCurrentThread());
+}
+
+void KScheduler::YieldCurrentThreadAndBalanceLoad()
+{
+    KCoreContext &cctx = KCoreContext::GetCurrentInstance();
+    cctx.GetScheduler()->DoYieldOperation(Global::YieldThreadAndBalanceLoad, *cctx.GetCurrentThread());
+}
+
+void KScheduler::YieldCurrentThreadAndWaitForLoadBalancing()
+{
+    KCoreContext &cctx = KCoreContext::GetCurrentInstance();
+    cctx.GetScheduler()->DoYieldOperation(Global::YieldThreadAndWaitForLoadBalancing, *cctx.GetCurrentThread());
+}
+
+}

mesosphere/source/threading/KThread.cpp

@@ -0,0 +1,237 @@
+#include <mutex>
+#include <atomic>
+#include <algorithm>
+
+#include <mesosphere/threading/KThread.hpp>
+#include <mesosphere/threading/KScheduler.hpp>
+#include <mesosphere/core/KCoreContext.hpp>
+
+namespace mesosphere
+{
+
+bool KThread::IsAlive() const
+{
+    return true;
+}
+
+void KThread::OnAlarm()
+{
+    CancelKernelSync();
+}
+
+void KThread::AdjustScheduling(ushort oldMaskFull)
+{
+    if (currentSchedMaskFull == oldMaskFull) {
+        return;
+    } else if (CompareSchedulingStatusFull(oldMaskFull, SchedulingStatus::Running)) {
+        KScheduler::Global::SetThreadPaused(*this);
+    } else if (CompareSchedulingStatusFull(SchedulingStatus::Running)) {
+        KScheduler::Global::SetThreadRunning(*this);
+    }
+}
+
+void KThread::Reschedule(KThread::SchedulingStatus newStatus)
+{
+    std::lock_guard criticalSection{KScheduler::GetCriticalSection()};
+    AdjustScheduling(SetSchedulingStatusField(newStatus));
+}
+
+void KThread::RescheduleIfStatusEquals(SchedulingStatus expectedStatus, SchedulingStatus newStatus)
+{
+    if(GetSchedulingStatus() == expectedStatus) {
+        Reschedule(newStatus);
+    }
+}
+
+void KThread::AddForcePauseReason(KThread::ForcePauseReason reason)
+{
+    std::lock_guard criticalSection{KScheduler::GetCriticalSection()};
+
+    if (!IsDying()) {
+        AddForcePauseReasonToField(reason);
+        if (numKernelMutexWaiters == 0) {
+            AdjustScheduling(CommitForcePauseToField());
+        }
+    }
+}
+
+void KThread::RemoveForcePauseReason(KThread::ForcePauseReason reason)
+{
+    std::lock_guard criticalSection{KScheduler::GetCriticalSection()};
+
+    if (!IsDying()) {
+        RemoveForcePauseReasonToField(reason);
+        if (!IsForcePaused() && numKernelMutexWaiters == 0) {
+            AdjustScheduling(CommitForcePauseToField());
+        }
+    }
+}
+
+bool KThread::WaitForKernelSync(KThread::WaitList &waitList)
+{
+    // Has to be called from critical section
+    currentWaitList = &waitList;
+    Reschedule(SchedulingStatus::Paused);
+    waitList.push_back(*this);
+    if (IsDying()) {
+        // Whoops
+        ResumeFromKernelSync();
+        return false;
+    }
+
+    return true;
+}
+
+void KThread::ResumeFromKernelSync()
+{
+    // Has to be called from critical section
+    currentWaitList->erase(currentWaitList->iterator_to(*this));
+    currentWaitList = nullptr;
+    Reschedule(SchedulingStatus::Running);
+}
+
+void KThread::ResumeAllFromKernelSync(KThread::WaitList &waitList)
+{
+    // Has to be called from critical section
+    waitList.clear_and_dispose(
+        [](KThread *t) {
+            t->currentWaitList = nullptr;
+            t->Reschedule(SchedulingStatus::Running);
+        }
+    );
+}
+
+void KThread::CancelKernelSync()
+{
+    std::lock_guard criticalSection{KScheduler::GetCriticalSection()};
+    if (GetSchedulingStatus() == SchedulingStatus::Paused) {
+        // Note: transparent to force-pause
+        if (currentWaitList != nullptr) {
+            ResumeFromKernelSync();
+        } else {
+            Reschedule(SchedulingStatus::Running);
+        }
+    }
+}
+
+void KThread::CancelKernelSync(Result res)
+{
+    syncResult = res;
+    CancelKernelSync();
+}
+
+void KThread::AddToMutexWaitList(KThread &thread)
+{
+    // TODO: check&increment numKernelMutexWaiters
+    // Ordered list insertion
+    auto it = std::find_if(
+        mutexWaitList.begin(),
+        mutexWaitList.end(),
+        [&thread](const KThread &t) {
+            return t.GetPriority() > thread.GetPriority();
+        }
+    );
+
+    if (it != mutexWaitList.end()) {
+        mutexWaitList.insert(it, thread);
+    } else {
+        mutexWaitList.push_back(thread);
+    }
+}
+
+KThread::MutexWaitList::iterator KThread::RemoveFromMutexWaitList(KThread::MutexWaitList::const_iterator it)
+{
+    // TODO: check&decrement numKernelMutexWaiters
+    return mutexWaitList.erase(it);
+}
+
+void KThread::RemoveFromMutexWaitList(const KThread &t)
+{
+    RemoveFromMutexWaitList(mutexWaitList.iterator_to(t));
+}
+
+void KThread::InheritDynamicPriority()
+{
+    /*
+        Do priority inheritance
+        Since we're maybe changing the priority of the thread,
+        we must go through the entire mutex owner chain.
+        The invariant must be preserved:
+            A thread holding a mutex must have a higher-or-same priority than
+            all threads waiting for it to release the mutex.
+    */
+
+    for (KThread *t = this; t != nullptr; t = t->wantedMutexOwner) {
+        uint newPrio, oldPrio = priority;
+        if (!mutexWaitList.empty() && mutexWaitList.front().priority < basePriority) {
+            newPrio =  mutexWaitList.front().priority;
+        } else {
+            newPrio = basePriority;
+        }
+
+        if (newPrio == oldPrio) {
+            break;
+        } else {
+            // Update everything that depends on dynamic priority:
+
+            // TODO update condvar
+            // TODO update ctr arbiter
+            priority = newPrio;
+            // TODO update condvar
+            // TODO update ctr arbiter
+            if (CompareSchedulingStatusFull(SchedulingStatus::Running)) {
+                KScheduler::Global::AdjustThreadPriorityChanged(*this, oldPrio, this == KCoreContext::GetCurrentInstance().GetCurrentThread());
+            }
+
+            if (wantedMutexOwner != nullptr) {
+                wantedMutexOwner->RemoveFromMutexWaitList(*this);
+                wantedMutexOwner->AddToMutexWaitList(*this);
+            }
+        }
+    }
+}
+
+void KThread::AddMutexWaiter(KThread &waiter)
+{
+    AddToMutexWaitList(waiter);
+    InheritDynamicPriority();
+}
+
+void KThread::RemoveMutexWaiter(KThread &waiter)
+{
+    RemoveFromMutexWaitList(waiter);
+    InheritDynamicPriority();
+}
+
+KThread *KThread::RelinquishMutex(size_t *count, uiptr mutexAddr)
+{
+    KThread *newOwner = nullptr;
+    *count = 0;
+
+    // First in list wanting mutexAddr becomes owner, the rest is transferred
+    for (auto it = mutexWaitList.begin(); it != mutexWaitList.end(); ) {
+        if (it->wantedMutex != mutexAddr) {
+            ++it;
+            continue;
+        } else {
+            KThread &t = *it;
+            ++(*count);
+            it = RemoveFromMutexWaitList(it);
+            if (newOwner == nullptr) {
+                newOwner = &t;
+            } else {
+                newOwner->AddToMutexWaitList(t);
+            }
+        }
+    }
+
+    // Mutex waiters list have changed
+    InheritDynamicPriority();
+    if (newOwner != nullptr) {
+        newOwner->InheritDynamicPriority();
+    }
+
+    return newOwner;
+}
+
+}