From 2f25bfb2db0f75e7fdc7dc9dbde613418a05492f Mon Sep 17 00:00:00 2001
From: Bananymous <oskari.alaranta@bananymous.com>
Date: Wed, 19 Aug 2026 17:51:29 +0300
Subject: [PATCH] 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                       |   1 +
 kernel/include/kernel/Processor.h           |   6 +-
 kernel/include/kernel/Scheduler.h           |  48 +---
 kernel/include/kernel/SchedulerQueueNode.h  |  36 ---
 kernel/include/kernel/SchedulerThreadNode.h |  96 +++++++
 kernel/include/kernel/Thread.h              |   2 +-
 kernel/include/kernel/ThreadBlocker.h       |   8 +-
 kernel/kernel/Scheduler.cpp                 | 187 +++++---------
 kernel/kernel/SchedulerThreadNode.cpp       | 266 ++++++++++++++++++++
 kernel/kernel/ThreadBlocker.cpp             |   6 +-
 10 files changed, 446 insertions(+), 210 deletions(-)
 delete mode 100644 kernel/include/kernel/SchedulerQueueNode.h
 create mode 100644 kernel/include/kernel/SchedulerThreadNode.h
 create mode 100644 kernel/kernel/SchedulerThreadNode.cpp

diff --git a/kernel/CMakeLists.txt b/kernel/CMakeLists.txt
index d86801f0..6da4d4a8 100644
--- a/kernel/CMakeLists.txt
+++ b/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
diff --git a/kernel/include/kernel/Processor.h b/kernel/include/kernel/Processor.h
index 395da162..3a6e3910 100644
--- a/kernel/include/kernel/Processor.h
+++ b/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;
-				SchedulerQueueNode* unblock_thread;
+				SchedulerThreadNode* new_thread;
+				SchedulerThreadNode* unblock_thread;
 				bool dummy;
 			};
 		};
diff --git a/kernel/include/kernel/Scheduler.h b/kernel/include/kernel/Scheduler.h
index 897b240e..0cf7bc23 100644
--- a/kernel/include/kernel/Scheduler.h
+++ b/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 update_most_loaded_node_queue(SchedulerQueue::Node*, SchedulerQueue* target_queue);
-		void remove_node_from_most_loaded(SchedulerQueue::Node*);
+		void add_current_to_most_loaded(void* target_list);
+		void update_most_loaded_node_list(SchedulerThreadNode*, void* target_list);
+		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 unblock_thread(SchedulerQueue::Node*);
+		void add_thread(SchedulerThreadNode*);
+		void unblock_thread(SchedulerThreadNode*);
 
 	private:
-		SchedulerQueue m_run_queue;
-		SchedulerQueue m_block_queue;
-		SchedulerQueue::Node* m_current { nullptr };
+		SchedulerQueue m_run_list;
+		SchedulerHeap m_block_list;
+		SchedulerThreadNode* m_current { nullptr };
 
 		uint32_t m_thread_count { 0 };
 
@@ -108,8 +86,8 @@ namespace Kernel
 
 		struct ThreadInfo
 		{
-			SchedulerQueue*       queue { nullptr };
-			SchedulerQueue::Node* node  { nullptr };
+			void*                list { nullptr };
+			SchedulerThreadNode* node { nullptr };
 		};
 		BAN::Array<ThreadInfo, 10> m_most_loaded_threads;
 
diff --git a/kernel/include/kernel/SchedulerQueueNode.h b/kernel/include/kernel/SchedulerQueueNode.h
deleted file mode 100644
index a94761f0..00000000
--- a/kernel/include/kernel/SchedulerQueueNode.h
+++ /dev/null
@@ -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 };
-	};
-
-}
diff --git a/kernel/include/kernel/SchedulerThreadNode.h b/kernel/include/kernel/SchedulerThreadNode.h
new file mode 100644
index 00000000..7bd8a833
--- /dev/null
+++ b/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 };
+	};
+
+}
diff --git a/kernel/include/kernel/Thread.h b/kernel/include/kernel/Thread.h
index 0503beab..ea52246c 100644
--- a/kernel/include/kernel/Thread.h
+++ b/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      { };
 
diff --git a/kernel/include/kernel/ThreadBlocker.h b/kernel/include/kernel/ThreadBlocker.h
index dfa14984..672e53bb 100644
--- a/kernel/include/kernel/ThreadBlocker.h
+++ b/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 remove_thread_from_block_queue(SchedulerQueueNode*);
+		void add_thread_to_block_queue(SchedulerThreadNode*);
+		void remove_thread_from_block_queue(SchedulerThreadNode*);
 
 	private:
-		SchedulerQueueNode* m_block_chain { nullptr };
+		SchedulerThreadNode* m_block_chain { nullptr };
 		SpinLock m_lock;
 
 		friend class Scheduler;
diff --git a/kernel/kernel/Scheduler.cpp b/kernel/kernel/Scheduler.cpp
index 7be42f6e..c71b91b8 100644
--- a/kernel/kernel/Scheduler.cpp
+++ b/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].node  = m_current;
+			m_most_loaded_threads[index] = {
+				.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().queue = nullptr;
+		m_most_loaded_threads.back() = {
+			.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)
-			unblock_thread(m_block_queue.front());
+		while (!m_block_list.empty() && current_ns >= m_block_list.front()->wake_time_ns)
+			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())
-			deadline_ns = BAN::Math::min(deadline_ns, m_block_queue.front()->wake_time_ns);
+		if (!m_block_list.empty())
+			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);
-			update_most_loaded_node_queue(node, &m_run_queue);
+			m_run_list.push(node);
+			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);
-		else if (m_block_queue.add_thread_with_wake_time(node))
-			update_wake_up_deadline();
+			m_run_list.push(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(
-				[&](SchedulerQueue::Node* node)
+			m_run_list.walk(
+				[](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(
-				[&](SchedulerQueue::Node* node)
+			m_block_list.walk(
+				[](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;
 
-			{
-				auto& my_queue = (thread_info.queue == &m_run_queue) ? m_run_queue : m_block_queue;
-				my_queue.remove_node(thread_info.node);
-				m_thread_count--;
-			}
+			if (thread_info.list == &m_run_list)
+				m_run_list.pop(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.queue = nullptr;
+			thread_info = {
+				.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_block_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_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);
 
diff --git a/kernel/kernel/SchedulerThreadNode.cpp b/kernel/kernel/SchedulerThreadNode.cpp
new file mode 100644
index 00000000..936efa64
--- /dev/null
+++ b/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;
+	}
+
+}
diff --git a/kernel/kernel/ThreadBlocker.cpp b/kernel/kernel/ThreadBlocker.cpp
index ad32a1f1..f9313269 100644
--- a/kernel/kernel/ThreadBlocker.cpp
+++ b/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);