8 files changed, 391 insertions, 55 deletions

diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt
index 43ae8a9e7..850ce8006 100644
--- a/src/common/CMakeLists.txt
+++ b/src/common/CMakeLists.txt
@@ -98,6 +98,7 @@ add_library(common STATIC
     microprofile.h
     microprofileui.h
     misc.cpp
+    multi_level_queue.h
     page_table.cpp
     page_table.h
     param_package.cpp
diff --git a/src/common/bit_util.h b/src/common/bit_util.h
index 1eea17ba1..a4f9ed4aa 100644
--- a/src/common/bit_util.h
+++ b/src/common/bit_util.h
@@ -58,4 +58,43 @@ inline u64 CountLeadingZeroes64(u64 value) {
     return __builtin_clzll(value);
 }
 #endif
+
+#ifdef _MSC_VER
+inline u32 CountTrailingZeroes32(u32 value) {
+    unsigned long trailing_zero = 0;
+
+    if (_BitScanForward(&trailing_zero, value) != 0) {
+        return trailing_zero;
+    }
+
+    return 32;
+}
+
+inline u64 CountTrailingZeroes64(u64 value) {
+    unsigned long trailing_zero = 0;
+
+    if (_BitScanForward64(&trailing_zero, value) != 0) {
+        return trailing_zero;
+    }
+
+    return 64;
+}
+#else
+inline u32 CountTrailingZeroes32(u32 value) {
+    if (value == 0) {
+        return 32;
+    }
+
+    return __builtin_ctz(value);
+}
+
+inline u64 CountTrailingZeroes64(u64 value) {
+    if (value == 0) {
+        return 64;
+    }
+
+    return __builtin_ctzll(value);
+}
+#endif
+
 } // namespace Common
diff --git a/src/common/detached_tasks.cpp b/src/common/detached_tasks.cpp
index a347d9e02..f268d6021 100644
--- a/src/common/detached_tasks.cpp
+++ b/src/common/detached_tasks.cpp
@@ -16,22 +16,22 @@ DetachedTasks::DetachedTasks() {
 }
 
 void DetachedTasks::WaitForAllTasks() {
-    std::unique_lock<std::mutex> lock(mutex);
+    std::unique_lock lock{mutex};
     cv.wait(lock, [this]() { return count == 0; });
 }
 
 DetachedTasks::~DetachedTasks() {
-    std::unique_lock<std::mutex> lock(mutex);
+    std::unique_lock lock{mutex};
     ASSERT(count == 0);
     instance = nullptr;
 }
 
 void DetachedTasks::AddTask(std::function<void()> task) {
-    std::unique_lock<std::mutex> lock(instance->mutex);
+    std::unique_lock lock{instance->mutex};
     ++instance->count;
     std::thread([task{std::move(task)}]() {
         task();
-        std::unique_lock<std::mutex> lock(instance->mutex);
+        std::unique_lock lock{instance->mutex};
         --instance->count;
         std::notify_all_at_thread_exit(instance->cv, std::move(lock));
     })
diff --git a/src/common/logging/backend.cpp b/src/common/logging/backend.cpp
index 4462ff3fb..a03179520 100644
--- a/src/common/logging/backend.cpp
+++ b/src/common/logging/backend.cpp
@@ -46,12 +46,12 @@ public:
     }
 
     void AddBackend(std::unique_ptr<Backend> backend) {
-        std::lock_guard<std::mutex> lock(writing_mutex);
+        std::lock_guard lock{writing_mutex};
         backends.push_back(std::move(backend));
     }
 
     void RemoveBackend(std::string_view backend_name) {
-        std::lock_guard<std::mutex> lock(writing_mutex);
+        std::lock_guard lock{writing_mutex};
         const auto it =
             std::remove_if(backends.begin(), backends.end(),
                            [&backend_name](const auto& i) { return backend_name == i->GetName(); });
@@ -80,7 +80,7 @@ private:
         backend_thread = std::thread([&] {
             Entry entry;
             auto write_logs = [&](Entry& e) {
-                std::lock_guard<std::mutex> lock(writing_mutex);
+                std::lock_guard lock{writing_mutex};
                 for (const auto& backend : backends) {
                     backend->Write(e);
                 }
diff --git a/src/common/multi_level_queue.h b/src/common/multi_level_queue.h
new file mode 100644
index 000000000..2b61b91e0
--- /dev/null
+++ b/src/common/multi_level_queue.h
@@ -0,0 +1,337 @@
+// Copyright 2019 TuxSH
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <array>
+#include <iterator>
+#include <list>
+#include <utility>
+
+#include "common/bit_util.h"
+#include "common/common_types.h"
+
+namespace Common {
+
+/**
+ * A MultiLevelQueue is a type of priority queue which has the following characteristics:
+ * - iteratable through each of its elements.
+ * - back can be obtained.
+ * - O(1) add, lookup (both front and back)
+ * - discrete priorities and a max of 64 priorities (limited domain)
+ * This type of priority queue is normaly used for managing threads within an scheduler
+ */
+template <typename T, std::size_t Depth>
+class MultiLevelQueue {
+public:
+    using value_type = T;
+    using reference = value_type&;
+    using const_reference = const value_type&;
+    using pointer = value_type*;
+    using const_pointer = const value_type*;
+
+    using difference_type = typename std::pointer_traits<pointer>::difference_type;
+    using size_type = std::size_t;
+
+    template <bool is_constant>
+    class iterator_impl {
+    public:
+        using iterator_category = std::bidirectional_iterator_tag;
+        using value_type = T;
+        using pointer = std::conditional_t<is_constant, T*, const T*>;
+        using reference = std::conditional_t<is_constant, const T&, T&>;
+        using difference_type = typename std::pointer_traits<pointer>::difference_type;
+
+        friend bool operator==(const iterator_impl& lhs, const iterator_impl& rhs) {
+            if (lhs.IsEnd() && rhs.IsEnd())
+                return true;
+            return std::tie(lhs.current_priority, lhs.it) == std::tie(rhs.current_priority, rhs.it);
+        }
+
+        friend bool operator!=(const iterator_impl& lhs, const iterator_impl& rhs) {
+            return !operator==(lhs, rhs);
+        }
+
+        reference operator*() const {
+            return *it;
+        }
+
+        pointer operator->() const {
+            return it.operator->();
+        }
+
+        iterator_impl& operator++() {
+            if (IsEnd()) {
+                return *this;
+            }
+
+            ++it;
+
+            if (it == GetEndItForPrio()) {
+                u64 prios = mlq.used_priorities;
+                prios &= ~((1ULL << (current_priority + 1)) - 1);
+                if (prios == 0) {
+                    current_priority = mlq.depth();
+                } else {
+                    current_priority = CountTrailingZeroes64(prios);
+                    it = GetBeginItForPrio();
+                }
+            }
+            return *this;
+        }
+
+        iterator_impl& operator--() {
+            if (IsEnd()) {
+                if (mlq.used_priorities != 0) {
+                    current_priority = 63 - CountLeadingZeroes64(mlq.used_priorities);
+                    it = GetEndItForPrio();
+                    --it;
+                }
+            } else if (it == GetBeginItForPrio()) {
+                u64 prios = mlq.used_priorities;
+                prios &= (1ULL << current_priority) - 1;
+                if (prios != 0) {
+                    current_priority = CountTrailingZeroes64(prios);
+                    it = GetEndItForPrio();
+                    --it;
+                }
+            } else {
+                --it;
+            }
+            return *this;
+        }
+
+        iterator_impl operator++(int) {
+            const iterator_impl v{*this};
+            ++(*this);
+            return v;
+        }
+
+        iterator_impl operator--(int) {
+            const iterator_impl v{*this};
+            --(*this);
+            return v;
+        }
+
+        // allow implicit const->non-const
+        iterator_impl(const iterator_impl<false>& other)
+            : mlq(other.mlq), it(other.it), current_priority(other.current_priority) {}
+
+        iterator_impl(const iterator_impl<true>& other)
+            : mlq(other.mlq), it(other.it), current_priority(other.current_priority) {}
+
+        iterator_impl& operator=(const iterator_impl<false>& other) {
+            mlq = other.mlq;
+            it = other.it;
+            current_priority = other.current_priority;
+            return *this;
+        }
+
+        friend class iterator_impl<true>;
+        iterator_impl() = default;
+
+    private:
+        friend class MultiLevelQueue;
+        using container_ref =
+            std::conditional_t<is_constant, const MultiLevelQueue&, MultiLevelQueue&>;
+        using list_iterator = std::conditional_t<is_constant, typename std::list<T>::const_iterator,
+                                                 typename std::list<T>::iterator>;
+
+        explicit iterator_impl(container_ref mlq, list_iterator it, u32 current_priority)
+            : mlq(mlq), it(it), current_priority(current_priority) {}
+        explicit iterator_impl(container_ref mlq, u32 current_priority)
+            : mlq(mlq), it(), current_priority(current_priority) {}
+
+        bool IsEnd() const {
+            return current_priority == mlq.depth();
+        }
+
+        list_iterator GetBeginItForPrio() const {
+            return mlq.levels[current_priority].begin();
+        }
+
+        list_iterator GetEndItForPrio() const {
+            return mlq.levels[current_priority].end();
+        }
+
+        container_ref mlq;
+        list_iterator it;
+        u32 current_priority;
+    };
+
+    using iterator = iterator_impl<false>;
+    using const_iterator = iterator_impl<true>;
+
+    void add(const T& element, u32 priority, bool send_back = true) {
+        if (send_back)
+            levels[priority].push_back(element);
+        else
+            levels[priority].push_front(element);
+        used_priorities |= 1ULL << priority;
+    }
+
+    void remove(const T& element, u32 priority) {
+        auto it = ListIterateTo(levels[priority], element);
+        if (it == levels[priority].end())
+            return;
+        levels[priority].erase(it);
+        if (levels[priority].empty()) {
+            used_priorities &= ~(1ULL << priority);
+        }
+    }
+
+    void adjust(const T& element, u32 old_priority, u32 new_priority, bool adjust_front = false) {
+        remove(element, old_priority);
+        add(element, new_priority, !adjust_front);
+    }
+    void adjust(const_iterator it, u32 old_priority, u32 new_priority, bool adjust_front = false) {
+        adjust(*it, old_priority, new_priority, adjust_front);
+    }
+
+    void transfer_to_front(const T& element, u32 priority, MultiLevelQueue& other) {
+        ListSplice(other.levels[priority], other.levels[priority].begin(), levels[priority],
+                   ListIterateTo(levels[priority], element));
+
+        other.used_priorities |= 1ULL << priority;
+
+        if (levels[priority].empty()) {
+            used_priorities &= ~(1ULL << priority);
+        }
+    }
+
+    void transfer_to_front(const_iterator it, u32 priority, MultiLevelQueue& other) {
+        transfer_to_front(*it, priority, other);
+    }
+
+    void transfer_to_back(const T& element, u32 priority, MultiLevelQueue& other) {
+        ListSplice(other.levels[priority], other.levels[priority].end(), levels[priority],
+                   ListIterateTo(levels[priority], element));
+
+        other.used_priorities |= 1ULL << priority;
+
+        if (levels[priority].empty()) {
+            used_priorities &= ~(1ULL << priority);
+        }
+    }
+
+    void transfer_to_back(const_iterator it, u32 priority, MultiLevelQueue& other) {
+        transfer_to_back(*it, priority, other);
+    }
+
+    void yield(u32 priority, std::size_t n = 1) {
+        ListShiftForward(levels[priority], n);
+    }
+
+    std::size_t depth() const {
+        return Depth;
+    }
+
+    std::size_t size(u32 priority) const {
+        return levels[priority].size();
+    }
+
+    std::size_t size() const {
+        u64 priorities = used_priorities;
+        std::size_t size = 0;
+        while (priorities != 0) {
+            const u64 current_priority = CountTrailingZeroes64(priorities);
+            size += levels[current_priority].size();
+            priorities &= ~(1ULL << current_priority);
+        }
+        return size;
+    }
+
+    bool empty() const {
+        return used_priorities == 0;
+    }
+
+    bool empty(u32 priority) const {
+        return (used_priorities & (1ULL << priority)) == 0;
+    }
+
+    u32 highest_priority_set(u32 max_priority = 0) const {
+        const u64 priorities =
+            max_priority == 0 ? used_priorities : (used_priorities & ~((1ULL << max_priority) - 1));
+        return priorities == 0 ? Depth : static_cast<u32>(CountTrailingZeroes64(priorities));
+    }
+
+    u32 lowest_priority_set(u32 min_priority = Depth - 1) const {
+        const u64 priorities = min_priority >= Depth - 1
+                                   ? used_priorities
+                                   : (used_priorities & ((1ULL << (min_priority + 1)) - 1));
+        return priorities == 0 ? Depth : 63 - CountLeadingZeroes64(priorities);
+    }
+
+    const_iterator cbegin(u32 max_prio = 0) const {
+        const u32 priority = highest_priority_set(max_prio);
+        return priority == Depth ? cend()
+                                 : const_iterator{*this, levels[priority].cbegin(), priority};
+    }
+    const_iterator begin(u32 max_prio = 0) const {
+        return cbegin(max_prio);
+    }
+    iterator begin(u32 max_prio = 0) {
+        const u32 priority = highest_priority_set(max_prio);
+        return priority == Depth ? end() : iterator{*this, levels[priority].begin(), priority};
+    }
+
+    const_iterator cend(u32 min_prio = Depth - 1) const {
+        return min_prio == Depth - 1 ? const_iterator{*this, Depth} : cbegin(min_prio + 1);
+    }
+    const_iterator end(u32 min_prio = Depth - 1) const {
+        return cend(min_prio);
+    }
+    iterator end(u32 min_prio = Depth - 1) {
+        return min_prio == Depth - 1 ? iterator{*this, Depth} : begin(min_prio + 1);
+    }
+
+    T& front(u32 max_priority = 0) {
+        const u32 priority = highest_priority_set(max_priority);
+        return levels[priority == Depth ? 0 : priority].front();
+    }
+    const T& front(u32 max_priority = 0) const {
+        const u32 priority = highest_priority_set(max_priority);
+        return levels[priority == Depth ? 0 : priority].front();
+    }
+
+    T back(u32 min_priority = Depth - 1) {
+        const u32 priority = lowest_priority_set(min_priority); // intended
+        return levels[priority == Depth ? 63 : priority].back();
+    }
+    const T& back(u32 min_priority = Depth - 1) const {
+        const u32 priority = lowest_priority_set(min_priority); // intended
+        return levels[priority == Depth ? 63 : priority].back();
+    }
+
+private:
+    using const_list_iterator = typename std::list<T>::const_iterator;
+
+    static void ListShiftForward(std::list<T>& list, const std::size_t shift = 1) {
+        if (shift >= list.size()) {
+            return;
+        }
+
+        const auto begin_range = list.begin();
+        const auto end_range = std::next(begin_range, shift);
+        list.splice(list.end(), list, begin_range, end_range);
+    }
+
+    static void ListSplice(std::list<T>& in_list, const_list_iterator position,
+                           std::list<T>& out_list, const_list_iterator element) {
+        in_list.splice(position, out_list, element);
+    }
+
+    static const_list_iterator ListIterateTo(const std::list<T>& list, const T& element) {
+        auto it = list.cbegin();
+        while (it != list.cend() && *it != element) {
+            ++it;
+        }
+        return it;
+    }
+
+    std::array<std::list<T>, Depth> levels;
+    u64 used_priorities = 0;
+};
+
+} // namespace Common
diff --git a/src/common/thread.cpp b/src/common/thread.cpp
index 5144c0d9f..fe7a420cc 100644
--- a/src/common/thread.cpp
+++ b/src/common/thread.cpp
@@ -27,18 +27,6 @@ namespace Common {
 
 #ifdef _MSC_VER
 
-void SetThreadAffinity(std::thread::native_handle_type thread, u32 mask) {
-    SetThreadAffinityMask(thread, mask);
-}
-
-void SetCurrentThreadAffinity(u32 mask) {
-    SetThreadAffinityMask(GetCurrentThread(), mask);
-}
-
-void SwitchCurrentThread() {
-    SwitchToThread();
-}
-
 // Sets the debugger-visible name of the current thread.
 // Uses undocumented (actually, it is now documented) trick.
 // http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vsdebug/html/vxtsksettingthreadname.asp
@@ -70,31 +58,6 @@ void SetCurrentThreadName(const char* name) {
 
 #else // !MSVC_VER, so must be POSIX threads
 
-void SetThreadAffinity(std::thread::native_handle_type thread, u32 mask) {
-#ifdef __APPLE__
-    thread_policy_set(pthread_mach_thread_np(thread), THREAD_AFFINITY_POLICY, (integer_t*)&mask, 1);
-#elif (defined __linux__ || defined __FreeBSD__) && !(defined ANDROID)
-    cpu_set_t cpu_set;
-    CPU_ZERO(&cpu_set);
-
-    for (int i = 0; i != sizeof(mask) * 8; ++i)
-        if ((mask >> i) & 1)
-            CPU_SET(i, &cpu_set);
-
-    pthread_setaffinity_np(thread, sizeof(cpu_set), &cpu_set);
-#endif
-}
-
-void SetCurrentThreadAffinity(u32 mask) {
-    SetThreadAffinity(pthread_self(), mask);
-}
-
-#ifndef _WIN32
-void SwitchCurrentThread() {
-    usleep(1000 * 1);
-}
-#endif
-
 // MinGW with the POSIX threading model does not support pthread_setname_np
 #if !defined(_WIN32) || defined(_MSC_VER)
 void SetCurrentThreadName(const char* name) {
diff --git a/src/common/thread.h b/src/common/thread.h
index 2cf74452d..0cfd98be6 100644
--- a/src/common/thread.h
+++ b/src/common/thread.h
@@ -9,14 +9,13 @@
 #include <cstddef>
 #include <mutex>
 #include <thread>
-#include "common/common_types.h"
 
 namespace Common {
 
 class Event {
 public:
     void Set() {
-        std::lock_guard<std::mutex> lk(mutex);
+        std::lock_guard lk{mutex};
         if (!is_set) {
             is_set = true;
             condvar.notify_one();
@@ -24,14 +23,14 @@ public:
     }
 
     void Wait() {
-        std::unique_lock<std::mutex> lk(mutex);
+        std::unique_lock lk{mutex};
         condvar.wait(lk, [&] { return is_set; });
         is_set = false;
     }
 
     template <class Clock, class Duration>
     bool WaitUntil(const std::chrono::time_point<Clock, Duration>& time) {
-        std::unique_lock<std::mutex> lk(mutex);
+        std::unique_lock lk{mutex};
         if (!condvar.wait_until(lk, time, [this] { return is_set; }))
             return false;
         is_set = false;
@@ -39,7 +38,7 @@ public:
     }
 
     void Reset() {
-        std::unique_lock<std::mutex> lk(mutex);
+        std::unique_lock lk{mutex};
         // no other action required, since wait loops on the predicate and any lingering signal will
         // get cleared on the first iteration
         is_set = false;
@@ -57,7 +56,7 @@ public:
 
     /// Blocks until all "count" threads have called Sync()
     void Sync() {
-        std::unique_lock<std::mutex> lk(mutex);
+        std::unique_lock lk{mutex};
         const std::size_t current_generation = generation;
 
         if (++waiting == count) {
@@ -78,9 +77,6 @@ private:
     std::size_t generation = 0; // Incremented once each time the barrier is used
 };
 
-void SetThreadAffinity(std::thread::native_handle_type thread, u32 mask);
-void SetCurrentThreadAffinity(u32 mask);
-void SwitchCurrentThread(); // On Linux, this is equal to sleep 1ms
 void SetCurrentThreadName(const char* name);
 
 } // namespace Common
diff --git a/src/common/threadsafe_queue.h b/src/common/threadsafe_queue.h
index 821e8536a..e714ba5b3 100644
--- a/src/common/threadsafe_queue.h
+++ b/src/common/threadsafe_queue.h
@@ -78,7 +78,7 @@ public:
 
     T PopWait() {
         if (Empty()) {
-            std::unique_lock<std::mutex> lock(cv_mutex);
+            std::unique_lock lock{cv_mutex};
             cv.wait(lock, [this]() { return !Empty(); });
         }
         T t;
@@ -137,7 +137,7 @@ public:
 
     template <typename Arg>
     void Push(Arg&& t) {
-        std::lock_guard<std::mutex> lock(write_lock);
+        std::lock_guard lock{write_lock};
         spsc_queue.Push(t);
     }