Kernel: Use min heap for scheduler block list

This makes insertion to block list O(log n) instead of O(n) when
blocking with wake time. This is more heavy and currently slower than
the previous implementation, but that is just because we don't really
have too many threads running at any given time. This will be more
scalable in the future and even allows implementing priorities if I ever
wish so

kernel/CMakeLists.txt

@@ -85,6 +85,7 @@ set(KERNEL_SOURCES
 	kernel/Processor.cpp
 	kernel/Random.cpp
 	kernel/Scheduler.cpp
+	kernel/SchedulerThreadNode.cpp
 	kernel/SSP.cpp
 	kernel/Storage/ATA/AHCI/Controller.cpp
 	kernel/Storage/ATA/AHCI/Device.cpp

kernel/include/kernel/Processor.h

@@ -22,7 +22,7 @@ namespace Kernel
 	class GDT;
 	class IDT;
 	class Scheduler;
-	class SchedulerQueueNode;
+	class SchedulerThreadNode;
 	class Thread;
 
 #if ARCH(x86_64) || ARCH(i686)
@@ -54,8 +54,8 @@ namespace Kernel
 			union
 			{
 				TLBEntry flush_tlb;
-				SchedulerQueueNode* new_thread;
+				SchedulerThreadNode* new_thread;
-				SchedulerQueueNode* unblock_thread;
+				SchedulerThreadNode* unblock_thread;
 				bool dummy;
 			};
 		};

kernel/include/kernel/Scheduler.h

@@ -5,6 +5,7 @@
 #include <BAN/NoCopyMove.h>
 #include <kernel/InterruptStack.h>
 #include <kernel/ProcessorID.h>
+#include <kernel/SchedulerThreadNode.h>
 
 #include <sys/types.h>
 
@@ -14,29 +15,6 @@ namespace Kernel
 	class BaseMutex;
 	class Thread;
 	class ThreadBlocker;
-	struct SchedulerQueueNode;
-
-	class SchedulerQueue
-	{
-	public:
-		using Node = SchedulerQueueNode;
-
-	public:
-		void add_thread_to_back(Node*);
-		bool add_thread_with_wake_time(Node*); // return true if node was inserted as the first element
-		template<typename F>
-		Node* remove_with_condition(F callback);
-		void remove_node(Node*);
-		Node* front();
-		Node* pop_front();
-
-		bool empty() const { return m_head == nullptr; }
-
-	private:
-		Node* m_head { nullptr };
-		Node* m_tail { nullptr };
-	};
-
 	class Scheduler
 	{
 		BAN_NON_COPYABLE(Scheduler);
@@ -45,12 +23,12 @@ namespace Kernel
 	public:
 		struct NewThreadRequest
 		{
-			SchedulerQueue::Node* node;
+			SchedulerThreadNode* node;
 		};
 
 		struct UnblockRequest
 		{
-			SchedulerQueue::Node* node;
+			SchedulerThreadNode* node;
 		};
 
 	public:
@@ -79,9 +57,9 @@ namespace Kernel
 	private:
 		Scheduler() = default;
 
-		void add_current_to_most_loaded(SchedulerQueue* target_queue);
+		void add_current_to_most_loaded(void* target_list);
-		void update_most_loaded_node_queue(SchedulerQueue::Node*, SchedulerQueue* target_queue);
+		void update_most_loaded_node_list(SchedulerThreadNode*, void* target_list);
-		void remove_node_from_most_loaded(SchedulerQueue::Node*);
+		void remove_node_from_most_loaded(SchedulerThreadNode*);
 
 		void update_wake_up_deadline();
 		void wake_up_sleeping_threads();
@@ -90,13 +68,13 @@ namespace Kernel
 
 		class ProcessorID find_least_loaded_processor() const;
 
-		void add_thread(SchedulerQueue::Node*);
+		void add_thread(SchedulerThreadNode*);
-		void unblock_thread(SchedulerQueue::Node*);
+		void unblock_thread(SchedulerThreadNode*);
 
 	private:
-		SchedulerQueue m_run_queue;
+		SchedulerQueue m_run_list;
-		SchedulerQueue m_block_queue;
+		SchedulerHeap m_block_list;
-		SchedulerQueue::Node* m_current { nullptr };
+		SchedulerThreadNode* m_current { nullptr };
 
 		uint32_t m_thread_count { 0 };
 
@@ -108,8 +86,8 @@ namespace Kernel
 
 		struct ThreadInfo
 		{
-			SchedulerQueue*       queue { nullptr };
+			void*                list { nullptr };
-			SchedulerQueue::Node* node  { nullptr };
+			SchedulerThreadNode* node { nullptr };
 		};
 		BAN::Array<ThreadInfo, 10> m_most_loaded_threads;
 

kernel/include/kernel/SchedulerQueueNode.h

@@ -1,36 +0,0 @@
-#pragma once
-
-#include <BAN/Atomic.h>
-#include <kernel/ProcessorID.h>
-
-namespace Kernel
-{
-
-	class Thread;
-	class ThreadBlocker;
-
-	struct SchedulerQueueNode
-	{
-		SchedulerQueueNode(Thread* thread)
-			: thread(thread)
-		{}
-
-		Thread* const thread;
-
-		SchedulerQueueNode* next { nullptr };
-		SchedulerQueueNode* prev { nullptr };
-
-		uint64_t wake_time_ns { static_cast<uint64_t>(-1) };
-
-		BAN::Atomic<ThreadBlocker*> blocker { nullptr };
-		SchedulerQueueNode* block_chain_prev { nullptr };
-		SchedulerQueueNode* block_chain_next { nullptr };
-
-		ProcessorID processor_id { PROCESSOR_NONE };
-		bool blocked { false };
-
-		uint64_t last_start_ns { 0 };
-		uint64_t time_used_ns  { 0 };
-	};
-
-}

kernel/include/kernel/SchedulerThreadNode.h

@@ -0,0 +1,96 @@
+#pragma once
+
+#include <BAN/Atomic.h>
+#include <BAN/NoCopyMove.h>
+#include <kernel/ProcessorID.h>
+
+namespace Kernel
+{
+
+	class Thread;
+	class ThreadBlocker;
+
+	struct SchedulerThreadNode
+	{
+		SchedulerThreadNode(Thread* thread)
+			: thread(thread)
+			, heap({ nullptr, nullptr, nullptr })
+		{}
+
+		Thread* const thread;
+
+		union
+		{
+			struct
+			{
+				SchedulerThreadNode* next;
+				SchedulerThreadNode* prev;
+			} queue;
+			struct
+			{
+				SchedulerThreadNode* parent;
+				SchedulerThreadNode* lchild;
+				SchedulerThreadNode* rchild;
+			} heap;
+		};
+
+		uint64_t wake_time_ns { static_cast<uint64_t>(-1) };
+
+		BAN::Atomic<ThreadBlocker*> blocker { nullptr };
+		SchedulerThreadNode* block_chain_prev { nullptr };
+		SchedulerThreadNode* block_chain_next { nullptr };
+
+		ProcessorID processor_id { PROCESSOR_NONE };
+		bool blocked { false };
+
+		uint64_t last_start_ns { 0 };
+		uint64_t time_used_ns  { 0 };
+	};
+
+	class SchedulerQueue
+	{
+		BAN_NON_COPYABLE(SchedulerQueue);
+		BAN_NON_MOVABLE(SchedulerQueue);
+	public:
+		SchedulerQueue() = default;
+
+		SchedulerThreadNode* front();
+		SchedulerThreadNode* pop_front();
+
+		void push(SchedulerThreadNode*);
+		void pop(SchedulerThreadNode*);
+
+		void walk(void (*)(const SchedulerThreadNode*, void*), void*) const;
+		bool empty() const { return m_head == nullptr; }
+
+	private:
+		SchedulerThreadNode* m_head { nullptr };
+		SchedulerThreadNode* m_tail { nullptr };
+	};
+
+	class SchedulerHeap
+	{
+		BAN_NON_COPYABLE(SchedulerHeap);
+		BAN_NON_MOVABLE(SchedulerHeap);
+	public:
+		SchedulerHeap() = default;
+
+		SchedulerThreadNode* front();
+		SchedulerThreadNode* pop_front();
+
+		void push(SchedulerThreadNode*);
+		void pop(SchedulerThreadNode*);
+
+		void walk(void (*)(const SchedulerThreadNode*, void*), void*) const;
+		bool empty() const { return m_root == nullptr; }
+
+	private:
+		void walk_impl(void (*)(const SchedulerThreadNode*, void*), void*, const SchedulerThreadNode*) const;
+		void swap_nodes(SchedulerThreadNode*, SchedulerThreadNode*);
+
+	private:
+		SchedulerThreadNode* m_root { nullptr };
+		SchedulerThreadNode* m_last { nullptr };
+	};
+
+}

kernel/include/kernel/Thread.h

@@ -192,7 +192,7 @@ namespace Kernel
 		vaddr_t                    m_fsbase               { 0 };
 		vaddr_t                    m_gsbase               { 0 };
 
-		SchedulerQueueNode*        m_scheduler_node       { nullptr };
+		SchedulerThreadNode*       m_scheduler_node       { nullptr };
 
 		YieldRegisters             m_yield_registers      { };
 

kernel/include/kernel/ThreadBlocker.h

@@ -7,7 +7,7 @@
 namespace Kernel
 {
 
-	class SchedulerQueueNode;
+	class SchedulerThreadNode;
 
 	class ThreadBlocker
 	{
@@ -29,11 +29,11 @@ namespace Kernel
 		}
 
 	private:
-		void add_thread_to_block_queue(SchedulerQueueNode*);
+		void add_thread_to_block_queue(SchedulerThreadNode*);
-		void remove_thread_from_block_queue(SchedulerQueueNode*);
+		void remove_thread_from_block_queue(SchedulerThreadNode*);
 
 	private:
-		SchedulerQueueNode* m_block_chain { nullptr };
+		SchedulerThreadNode* m_block_chain { nullptr };
 		SpinLock m_lock;
 
 		friend class Scheduler;

kernel/kernel/Scheduler.cpp

@@ -6,7 +6,6 @@
 #include <kernel/Lock/Mutex.h>
 #include <kernel/Process.h>
 #include <kernel/Scheduler.h>
-#include <kernel/SchedulerQueueNode.h>
 #include <kernel/Thread.h>
 #include <kernel/Timer/Timer.h>
 
@@ -37,85 +36,6 @@ namespace Kernel
 
 	static BAN::Atomic<size_t> s_next_processor_index { 0 };
 
-
-	void SchedulerQueue::add_thread_to_back(Node* node)
-	{
-		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-		node->next = nullptr;
-		node->prev = m_tail;
-		(m_tail ? m_tail->next : m_head) = node;
-		m_tail = node;
-	}
-
-	bool SchedulerQueue::add_thread_with_wake_time(Node* node)
-	{
-		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-
-		if (m_tail == nullptr || node->wake_time_ns >= m_tail->wake_time_ns)
-		{
-			add_thread_to_back(node);
-			return node == m_head;
-		}
-
-		Node* next = m_head;
-		Node* prev = nullptr;
-		while (next && node->wake_time_ns > next->wake_time_ns)
-		{
-			prev = next;
-			next = next->next;
-		}
-
-		node->next = next;
-		node->prev = prev;
-		(next ? next->prev : m_tail) = node;
-		(prev ? prev->next : m_head) = node;
-
-		return node == m_head;
-	}
-
-	template<typename F>
-	SchedulerQueue::Node* SchedulerQueue::remove_with_condition(F callback)
-	{
-		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-
-		for (Node* node = m_head; node; node = node->next)
-		{
-			if (!callback(node))
-				continue;
-			remove_node(node);
-			return node;
-		}
-
-		return nullptr;
-	}
-
-	void SchedulerQueue::remove_node(Node* node)
-	{
-		(node->prev ? node->prev->next : m_head) = node->next;
-		(node->next ? node->next->prev : m_tail) = node->prev;
-		node->prev = nullptr;
-		node->next = nullptr;
-	}
-
-	SchedulerQueue::Node* SchedulerQueue::front()
-	{
-		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-		ASSERT(!empty());
-		return m_head;
-	}
-
-	SchedulerQueue::Node* SchedulerQueue::pop_front()
-	{
-		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-		if (empty())
-			return nullptr;
-		Node* result = m_head;
-		m_head = m_head->next;
-		(m_head ? m_head->prev : m_tail) = nullptr;
-		result->next = nullptr;
-		return result;
-	}
-
 	BAN::ErrorOr<Scheduler*> Scheduler::create()
 	{
 		auto* scheduler = new Scheduler();
@@ -143,7 +63,7 @@ namespace Kernel
 		return {};
 	}
 
-	void Scheduler::add_current_to_most_loaded(SchedulerQueue* target_queue)
+	void Scheduler::add_current_to_most_loaded(void* target_list)
 	{
 		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
 
@@ -152,7 +72,7 @@ namespace Kernel
 		{
 			if (info.node == m_current)
 			{
-				info.queue = target_queue;
+				info.list = target_list;
 				has_current = true;
 				break;
 			}
@@ -164,8 +84,10 @@ namespace Kernel
 			for (; index < m_most_loaded_threads.size() - 1; index++)
 				if (m_most_loaded_threads[index].node == nullptr)
 					break;
-			m_most_loaded_threads[index].queue = target_queue;
+			m_most_loaded_threads[index] = {
-			m_most_loaded_threads[index].node  = m_current;
+				.list = target_list,
+				.node = m_current,
+			};
 		}
 
 		BAN::sort::sort(m_most_loaded_threads.begin(), m_most_loaded_threads.end(),
@@ -178,7 +100,7 @@ namespace Kernel
 		);
 	}
 
-	void Scheduler::update_most_loaded_node_queue(SchedulerQueue::Node* node, SchedulerQueue* target_queue)
+	void Scheduler::update_most_loaded_node_list(SchedulerThreadNode* node, void* target_list)
 	{
 		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
 
@@ -186,13 +108,13 @@ namespace Kernel
 		{
 			if (info.node == node)
 			{
-				info.queue = target_queue;
+				info.list = target_list;
 				break;
 			}
 		}
 	}
 
-	void Scheduler::remove_node_from_most_loaded(SchedulerQueue::Node* node)
+	void Scheduler::remove_node_from_most_loaded(SchedulerThreadNode* node)
 	{
 		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
 
@@ -204,8 +126,10 @@ namespace Kernel
 		for (; i < m_most_loaded_threads.size() - 1; i++)
 			m_most_loaded_threads[i] = m_most_loaded_threads[i + 1];
 
-		m_most_loaded_threads.back().node = nullptr;
+		m_most_loaded_threads.back() = {
-		m_most_loaded_threads.back().queue = nullptr;
+			.list = nullptr,
+			.node = nullptr,
+		};
 	}
 
 	void Scheduler::reschedule(YieldRegisters* yield_registers)
@@ -213,7 +137,7 @@ namespace Kernel
 		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
 
 		// If there are no other threads in run queue, reschedule can be no-op :)
-		if (m_run_queue.empty() && (!m_current || !m_current->blocked) && current_thread().state() == Thread::State::Executing)
+		if (m_run_list.empty() && (!m_current || !m_current->blocked) && current_thread().state() == Thread::State::Executing)
 			return;
 
 		if (m_current == nullptr)
@@ -233,12 +157,13 @@ namespace Kernel
 				case Thread::State::Executing:
 					m_current->thread->yield_registers() = *yield_registers;
 					m_current->time_used_ns += SystemTimer::get().ns_since_boot() - m_current->last_start_ns;
-					add_current_to_most_loaded(m_current->blocked ? &m_block_queue : &m_run_queue);
+					add_current_to_most_loaded(m_current->blocked ? static_cast<void*>(&m_block_list) : &m_run_list);
 					if (!m_current->blocked)
-						m_run_queue.add_thread_to_back(m_current);
+						m_run_list.push(m_current);
 					else
 					{
-						if (m_block_queue.add_thread_with_wake_time(m_current))
+						m_block_list.push(m_current);
+						if (m_block_list.front() == m_current)
 							update_wake_up_deadline();
 						Processor::set_disable_smp_messages(false);
 					}
@@ -247,12 +172,12 @@ namespace Kernel
 					ASSERT(!m_current->blocked);
 					m_current->time_used_ns = 0;
 					remove_node_from_most_loaded(m_current);
-					m_run_queue.add_thread_to_back(m_current);
+					m_run_list.push(m_current);
 					break;
 			}
 		}
 
-		while ((m_current = m_run_queue.pop_front()))
+		while ((m_current = m_run_list.pop_front()))
 		{
 			if (m_current->thread->state() != Thread::State::Terminated)
 				break;
@@ -274,7 +199,7 @@ namespace Kernel
 			return;
 		}
 
-		update_most_loaded_node_queue(m_current, nullptr);
+		update_most_loaded_node_list(m_current, nullptr);
 
 		auto* thread = m_current->thread;
 
@@ -310,8 +235,8 @@ namespace Kernel
 		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
 
 		const uint64_t current_ns = SystemTimer::get().ns_since_boot();
-		while (!m_block_queue.empty() && current_ns >= m_block_queue.front()->wake_time_ns)
+		while (!m_block_list.empty() && current_ns >= m_block_list.front()->wake_time_ns)
-			unblock_thread(m_block_queue.front());
+			unblock_thread(m_block_list.front());
 	}
 
 	void Scheduler::update_wake_up_deadline()
@@ -325,8 +250,8 @@ namespace Kernel
 			return;
 
 		uint64_t deadline_ns = m_next_reschedule_ns;
-		if (!m_block_queue.empty())
+		if (!m_block_list.empty())
-			deadline_ns = BAN::Math::min(deadline_ns, m_block_queue.front()->wake_time_ns);
+			deadline_ns = BAN::Math::min(deadline_ns, m_block_list.front()->wake_time_ns);
 		if (Processor::is_smp_enabled())
 			deadline_ns = BAN::Math::min(deadline_ns, m_last_load_balance_ns + s_load_balance_interval_ns);
 
@@ -337,7 +262,7 @@ namespace Kernel
 	{
 		ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
 
-		if ((is_idle() && !m_run_queue.empty()) || m_has_pending_reschedule)
+		if ((is_idle() && !m_run_list.empty()) || m_has_pending_reschedule)
 		{
 			m_has_pending_reschedule = false;
 			Processor::yield();
@@ -375,7 +300,7 @@ namespace Kernel
 			update_wake_up_deadline();
 	}
 
-	void Scheduler::unblock_thread(SchedulerQueue::Node* node)
+	void Scheduler::unblock_thread(SchedulerThreadNode* node)
 	{
 		auto state = Processor::get_interrupt_state();
 		Processor::set_interrupt_state(InterruptState::Disabled);
@@ -386,12 +311,12 @@ namespace Kernel
 				return;
 			ASSERT(node != m_current);
 			Processor::set_disable_smp_messages(true);
-			m_block_queue.remove_node(node);
+			m_block_list.pop(node);
 			if (auto* blocker = node->blocker.load())
 				blocker->remove_thread_from_block_queue(node);
 			node->blocked = false;
-			m_run_queue.add_thread_to_back(node);
+			m_run_list.push(node);
-			update_most_loaded_node_queue(node, &m_run_queue);
+			update_most_loaded_node_list(node, &m_run_list);
 			Processor::set_disable_smp_messages(false);
 		}
 		else
@@ -405,7 +330,7 @@ namespace Kernel
 		Processor::set_interrupt_state(state);
 	}
 
-	void Scheduler::add_thread(SchedulerQueue::Node* node)
+	void Scheduler::add_thread(SchedulerThreadNode* node)
 	{
 		auto state = Processor::get_interrupt_state();
 		Processor::set_interrupt_state(InterruptState::Disabled);
@@ -413,9 +338,13 @@ namespace Kernel
 		ASSERT(node->processor_id == Processor::current_id());
 
 		if (!node->blocked)
-			m_run_queue.add_thread_to_back(node);
+			m_run_list.push(node);
-		else if (m_block_queue.add_thread_with_wake_time(node))
+		else
+		{
+			m_block_list.push(node);
+			if (m_block_list.front() == node)
 				update_wake_up_deadline();
+		}
 
 		if (auto* thread = node->thread; thread->is_userspace() && thread->has_process())
 			thread->update_processor_index_address();
@@ -483,21 +412,21 @@ namespace Kernel
 				const uint64_t load_percent_x1000 = BAN::Math::div_round_up<uint64_t>(m_current->time_used_ns * 100'000, processing_ns);
 				dprintln("  tid { 2}: { 3}.{3}% <{}> current", m_current->thread->tid(), load_percent_x1000 / 1000, load_percent_x1000 % 1000, name);
 			}
-			m_run_queue.remove_with_condition(
+			m_run_list.walk(
-				[&](SchedulerQueue::Node* node)
+				[](const SchedulerThreadNode* node, void* arg)
 				{
+					const uint64_t processing_ns = *static_cast<const uint64_t*>(arg);
 					const uint64_t load_percent_x1000 = BAN::Math::div_round_up<uint64_t>(node->time_used_ns * 100'000, processing_ns);
 					dprintln("  tid { 2}: { 3}.{3}% active", node->thread->tid(), load_percent_x1000 / 1000, load_percent_x1000 % 1000);
-					return false;
+				}, const_cast<uint64_t*>(&processing_ns)
-				}
 			);
-			m_block_queue.remove_with_condition(
+			m_block_list.walk(
-				[&](SchedulerQueue::Node* node)
+				[](const SchedulerThreadNode* node, void* arg)
 				{
+					const uint64_t processing_ns = *static_cast<const uint64_t*>(arg);
 					const uint64_t load_percent_x1000 = BAN::Math::div_round_up<uint64_t>(node->time_used_ns * 100'000, processing_ns);
 					dprintln("  tid { 2}: { 3}.{3}% blocked", node->thread->tid(), load_percent_x1000 / 1000, load_percent_x1000 % 1000);
-					return false;
+				}, const_cast<uint64_t*>(&processing_ns)
-				}
 			);
 		}
 
@@ -533,7 +462,7 @@ namespace Kernel
 			auto& thread_info = m_most_loaded_threads[i];
 			if (thread_info.node == nullptr)
 				break;
-			if (thread_info.node == m_current || thread_info.queue == nullptr)
+			if (thread_info.node == m_current || thread_info.list == nullptr)
 				continue;
 
 			auto least_loaded_id = find_least_loaded_processor();
@@ -597,11 +526,11 @@ namespace Kernel
 
 			thread_info.node->time_used_ns = 0;
 
-			{
+			if (thread_info.list == &m_run_list)
-				auto& my_queue = (thread_info.queue == &m_run_queue) ? m_run_queue : m_block_queue;
+				m_run_list.pop(thread_info.node);
-				my_queue.remove_node(thread_info.node);
+			else
+				m_block_list.pop(thread_info.node);
 			m_thread_count--;
-			}
 
 			thread_info.node->processor_id = least_loaded_id;
 			Processor::send_smp_message(least_loaded_id, {
@@ -609,8 +538,10 @@ namespace Kernel
 				.new_thread = thread_info.node
 			});
 
-			thread_info.node = nullptr;
+			thread_info = {
-			thread_info.queue = nullptr;
+				.list = nullptr,
+				.node = nullptr,
+			};
 
 			if (m_idle_ns == 0)
 				break;
@@ -623,8 +554,8 @@ namespace Kernel
 			m_current->time_used_ns = 0;
 		for (auto& thread_info : m_most_loaded_threads)
 			thread_info = {};
-		m_run_queue  .remove_with_condition([&](SchedulerQueue::Node* node) { node->time_used_ns = 0; return false; });
+		m_run_list  .walk([](const SchedulerThreadNode* node, void*) { const_cast<SchedulerThreadNode*>(node)->time_used_ns = 0; }, nullptr);
-		m_block_queue.remove_with_condition([&](SchedulerQueue::Node* node) { node->time_used_ns = 0; return false; });
+		m_block_list.walk([](const SchedulerThreadNode* node, void*) { const_cast<SchedulerThreadNode*>(node)->time_used_ns = 0; }, nullptr);
 		m_idle_ns = 0;
 
 		m_should_calculate_max_load_threads = true;
@@ -635,7 +566,7 @@ namespace Kernel
 	BAN::ErrorOr<void> Scheduler::bind_thread_to_processor(Thread* thread, ProcessorID processor_id)
 	{
 		ASSERT(thread->m_scheduler_node == nullptr);
-		auto* new_node = new SchedulerQueue::Node(thread);
+		auto* new_node = new SchedulerThreadNode(thread);
 		if (new_node == nullptr)
 			return BAN::Error::from_errno(ENOMEM);
 

kernel/kernel/SchedulerThreadNode.cpp

@@ -0,0 +1,266 @@
+#include <BAN/Assert.h>
+#include <BAN/Swap.h>
+#include <kernel/SchedulerThreadNode.h>
+
+namespace Kernel
+{
+
+	SchedulerThreadNode* SchedulerQueue::front()
+	{
+		return m_head;
+	}
+
+	SchedulerThreadNode* SchedulerQueue::pop_front()
+	{
+		if (empty())
+			return nullptr;
+		auto* const result = m_head;
+		m_head = m_head->queue.next;
+		(m_head ? m_head->queue.prev : m_tail) = nullptr;
+		result->queue.prev = nullptr;
+		result->queue.next = nullptr;
+		return result;
+	}
+
+	void SchedulerQueue::push(SchedulerThreadNode* node)
+	{
+		ASSERT(node->queue.prev == nullptr);
+		ASSERT(node->queue.next == nullptr);
+
+		node->queue.prev = m_tail;
+		node->queue.next = nullptr;
+		(m_tail ? m_tail->queue.next : m_head) = node;
+		m_tail = node;
+	}
+
+	void SchedulerQueue::pop(SchedulerThreadNode* node)
+	{
+		(node->queue.prev ? node->queue.prev->queue.next : m_head) = node->queue.next;
+		(node->queue.next ? node->queue.next->queue.prev : m_tail) = node->queue.prev;
+		node->queue.prev = nullptr;
+		node->queue.next = nullptr;
+	}
+
+	void SchedulerQueue::walk(void (*callback)(const SchedulerThreadNode*, void*), void* arg) const
+	{
+		for (auto* node = m_head; node; node = node->queue.next)
+			callback(node, arg);
+	}
+
+
+
+	SchedulerThreadNode* SchedulerHeap::front()
+	{
+		return m_root;
+	}
+
+	SchedulerThreadNode* SchedulerHeap::pop_front()
+	{
+		if (empty())
+			return nullptr;
+		auto* const result = m_root;
+		pop(result);
+		return result;
+	}
+
+	void SchedulerHeap::push(SchedulerThreadNode* node)
+	{
+		ASSERT(node->heap.parent == nullptr);
+		ASSERT(node->heap.lchild == nullptr);
+		ASSERT(node->heap.rchild == nullptr);
+
+		if (m_root == nullptr)
+		{
+			// push to empty heap
+			node->heap.parent = nullptr;
+			node->heap.lchild = nullptr;
+			node->heap.rchild = nullptr;
+			m_root = node;
+			m_last = node;
+			return;
+		}
+
+		auto* parent = m_last;
+
+		{
+			// find parent of the new node
+			SchedulerThreadNode* temp;
+			while ((temp = parent->heap.parent) && parent == temp->heap.rchild)
+				parent = temp;
+			if (temp && temp->heap.rchild == nullptr)
+				parent = temp;
+			else
+			{
+				if (temp != nullptr)
+					parent = temp->heap.rchild;
+				while ((temp = parent->heap.lchild))
+					parent = temp;
+			}
+		}
+
+		// insert node as the last node
+		(parent->heap.lchild ? parent->heap.rchild : parent->heap.lchild) = node;
+		node->heap.parent = parent;
+		node->heap.lchild = nullptr;
+		node->heap.rchild = nullptr;
+		m_last = node;
+
+		// fix heap properties
+		while ((parent = node->heap.parent) && node->wake_time_ns < parent->wake_time_ns)
+			swap_nodes(node, parent);
+	}
+
+	void SchedulerHeap::pop(SchedulerThreadNode* old_node)
+	{
+		if (m_root == m_last)
+		{
+			// remove the only node
+			old_node->heap.parent = nullptr;
+			old_node->heap.lchild = nullptr;
+			old_node->heap.rchild = nullptr;
+			m_root = nullptr;
+			m_last = nullptr;
+			return;
+		}
+
+		auto* fix_node = m_last;
+		swap_nodes(old_node, m_last);
+
+		{
+			// update last to point to the previous node
+			SchedulerThreadNode* temp;
+			while ((temp = m_last->heap.parent) && m_last == temp->heap.lchild)
+				m_last = temp;
+			if (temp != nullptr)
+				m_last = temp->heap.lchild;
+			ASSERT(m_last);
+			while ((temp = m_last->heap.rchild))
+				m_last = temp;
+		}
+
+		{
+			// delete links to/from the deleted node
+			if (auto* parent = old_node->heap.parent)
+				(old_node == parent->heap.rchild ? parent->heap.rchild : parent->heap.lchild) = nullptr;
+			old_node->heap.parent = nullptr;
+			old_node->heap.lchild = nullptr;
+			old_node->heap.rchild = nullptr;
+		}
+
+		// fix heap properties
+		if (fix_node->wake_time_ns == old_node->wake_time_ns)
+			;
+		else if (fix_node->wake_time_ns < old_node->wake_time_ns)
+		{
+			SchedulerThreadNode* parent;
+			while ((parent = fix_node->heap.parent) && fix_node->wake_time_ns < parent->wake_time_ns)
+				swap_nodes(fix_node, parent);
+		}
+		else for (;;)
+		{
+			const bool l_ok = !fix_node->heap.lchild || fix_node->wake_time_ns <= fix_node->heap.lchild->wake_time_ns;
+			const bool r_ok = !fix_node->heap.rchild || fix_node->wake_time_ns <= fix_node->heap.rchild->wake_time_ns;
+			if (l_ok && r_ok)
+				break;
+			auto* child = (!l_ok && !r_ok)
+				? (fix_node->heap.lchild->wake_time_ns < fix_node->heap.rchild->wake_time_ns ? fix_node->heap.lchild : fix_node->heap.rchild)
+				: (r_ok ? fix_node->heap.lchild : fix_node->heap.rchild);
+			swap_nodes(fix_node, child);
+		}
+	}
+
+	void SchedulerHeap::walk(void (*callback)(const SchedulerThreadNode*, void*), void* arg) const
+	{
+		walk_impl(callback, arg, m_root);
+	}
+
+	void SchedulerHeap::walk_impl(void (*callback)(const SchedulerThreadNode*, void*), void* arg, const SchedulerThreadNode* node) const
+	{
+		if (node == nullptr)
+			return;
+		callback(node, arg);
+		walk_impl(callback, arg, node->heap.lchild);
+		walk_impl(callback, arg, node->heap.rchild);
+	}
+
+	void SchedulerHeap::swap_nodes(SchedulerThreadNode* node1, SchedulerThreadNode* node2)
+	{
+		if (node1 == node2)
+			return;
+
+		if (node2 == node1->heap.parent)
+			BAN::swap(node1, node2);
+
+		auto* const p1 = node1->heap.parent;
+		auto* const l1 = node1->heap.lchild;
+		auto* const r1 = node1->heap.rchild;
+
+		auto* const p2 = node2->heap.parent;
+		auto* const l2 = node2->heap.lchild;
+		auto* const r2 = node2->heap.rchild;
+
+		if (node1 == node2->heap.parent)
+		{
+			node1->heap.parent = node2;
+			node1->heap.lchild = l2;
+			node1->heap.rchild = r2;
+
+			node2->heap.parent = p1;
+
+			if (l1 == node2)
+			{
+				node2->heap.lchild = node1;
+				node2->heap.rchild = r1;
+				if (r1) r1->heap.parent = node2;
+			}
+			else
+			{
+				node2->heap.lchild = l1;
+				node2->heap.rchild = node1;
+				if (l1) l1->heap.parent = node2;
+			}
+
+			if (p1) (node1 == p1->heap.lchild ? p1->heap.lchild : p1->heap.rchild) = node2;
+
+			if (l2) l2->heap.parent = node1;
+			if (r2) r2->heap.parent = node1;
+		}
+		else
+		{
+			node1->heap.parent = p2;
+			node1->heap.lchild = l2;
+			node1->heap.rchild = r2;
+
+			node2->heap.parent = p1;
+			node2->heap.lchild = l1;
+			node2->heap.rchild = r1;
+
+			if (l1) l1->heap.parent = node2;
+			if (r1) r1->heap.parent = node2;
+
+			if (l2) l2->heap.parent = node1;
+			if (r2) r2->heap.parent = node1;
+
+			if (p1 || p2)
+			{
+				if (p1 == p2)
+					BAN::swap(p1->heap.lchild, p1->heap.rchild);
+				else
+				{
+					if (p1) (p1->heap.lchild == node1 ? p1->heap.lchild : p1->heap.rchild) = node2;
+					if (p2) (p2->heap.lchild == node2 ? p2->heap.lchild : p2->heap.rchild) = node1;
+				}
+			}
+		}
+
+		auto* const root = m_root;
+		auto* const last = m_last;
+
+		if      (node1 == root) m_root = node2;
+		else if (node1 == last) m_last = node2;
+
+		if      (node2 == root) m_root = node1;
+		else if (node2 == last) m_last = node1;
+	}
+
+}

kernel/kernel/ThreadBlocker.cpp

@@ -1,6 +1,6 @@
 #include <kernel/Processor.h>
 #include <kernel/Scheduler.h>
-#include <kernel/SchedulerQueueNode.h>
+#include <kernel/SchedulerThreadNode.h>
 #include <kernel/ThreadBlocker.h>
 #include <kernel/Timer/Timer.h>
 
@@ -45,7 +45,7 @@ namespace Kernel
 		m_block_chain = nullptr;
 	}
 
-	void ThreadBlocker::add_thread_to_block_queue(SchedulerQueueNode* node)
+	void ThreadBlocker::add_thread_to_block_queue(SchedulerThreadNode* node)
 	{
 		SpinLockGuard _(m_lock);
 
@@ -61,7 +61,7 @@ namespace Kernel
 		m_block_chain = node;
 	}
 
-	void ThreadBlocker::remove_thread_from_block_queue(SchedulerQueueNode* node)
+	void ThreadBlocker::remove_thread_from_block_queue(SchedulerThreadNode* node)
 	{
 		SpinLockGuard _(m_lock);