Kernel: Rewrite the whole scheduler and re-architecture SMP handling

Change Semaphore -> ThreadBlocker
  This was not a semaphore, I just named it one because I didn't know
  what semaphore was. I have meant to change this sooner, but it was in
  no way urgent :D

Implement SMP events. Processors can now be sent SMP events through
IPIs. SMP events can be sent either to a single processor or broadcasted
to every processor.

PageTable::{map_page,map_range,unmap_page,unmap_range}() now send SMP
event to invalidate TLB caches for the changed pages.

Scheduler no longer uses a global run queue. Each processor has its own
scheduler that keeps track of the load on the processor. Once every
second schedulers do load balancing. Schedulers have no access to other
processors' schedulers, they just see approximate loads. If scheduler
decides that it has too much load, it will send a thread to another
processor through a SMP event.

Schedulers are currently run using the timer interrupt on BSB. This
should be not the case, and each processor should use its LAPIC timer
for interrupts. There is no reason to broadcast SMP event to all
processors when BSB gets timer interrupt.

Old scheduler only achieved 20% idle load on qemu. That was probably a
very inefficient implementation. This new scheduler seems to average
around 1% idle load. This is much closer to what I would expect. On my
own laptop idle load seems to be only around 0.5% on each processor.

kernel/include/kernel/ACPI/ACPI.h

@@ -5,6 +5,7 @@
 #include <kernel/ACPI/AML/Namespace.h>
 #include <kernel/ACPI/Headers.h>
 #include <kernel/Memory/Types.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel::ACPI
 {
@@ -63,7 +64,7 @@ namespace Kernel::ACPI
 
 		FADT* m_fadt { nullptr };
 
-		Semaphore m_event_semaphore;
+		ThreadBlocker m_event_thread_blocker;
 		BAN::Array<BAN::RefPtr<AML::Method>, 0xFF> m_gpe_methods;
 
 		bool m_hardware_reduced { false };

kernel/include/kernel/ACPI/AML/Mutex.h

@@ -120,7 +120,7 @@ namespace Kernel::ACPI::AML
 				{
 					if (SystemTimer::get().ms_since_boot() >= wake_time)
 						return ParseResult(Integer::Constants::Ones);
-					SystemTimer::get().sleep(1);
+					Processor::yield();
 				}
 			}
 

kernel/include/kernel/ACPI/AML/Sleep.h

@@ -30,7 +30,7 @@ namespace Kernel::ACPI::AML
 			AML_DEBUG_PRINTLN("Sleeping for {} ms", sleep_time.value());
 #endif
 
-			SystemTimer::get().sleep(sleep_time.value());
+			SystemTimer::get().sleep_ms(sleep_time.value());
 			return ParseResult::Success;
 		}
 	};

kernel/include/kernel/FS/DevFS/FileSystem.h

@@ -4,7 +4,7 @@
 #include <kernel/Device/Device.h>
 #include <kernel/FS/TmpFS/FileSystem.h>
 #include <kernel/Lock/Mutex.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {
@@ -34,8 +34,8 @@ namespace Kernel
 
 		BAN::Vector<BAN::RefPtr<Device>> m_devices;
 
-		Semaphore m_sync_done;
-		Semaphore m_sync_semaphore;
+		ThreadBlocker m_sync_done;
+		ThreadBlocker m_sync_thread_blocker;
 		volatile bool m_should_sync { false };
 	};
 

kernel/include/kernel/FS/Pipe.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <kernel/FS/Inode.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {
@@ -47,7 +47,7 @@ namespace Kernel
 		timespec m_mtime {};
 		timespec m_ctime {};
 		BAN::Vector<uint8_t> m_buffer;
-		Semaphore m_semaphore;
+		ThreadBlocker m_thread_blocker;
 
 		uint32_t m_writing_count { 1 };
 	};

kernel/include/kernel/IDT.h

@@ -8,8 +8,8 @@
 #include <stdint.h>
 
 constexpr uint8_t IRQ_VECTOR_BASE = 0x20;
-constexpr uint8_t IRQ_IPI	= 32;
-constexpr uint8_t IRQ_YIELD	= 33;
+constexpr uint8_t IRQ_YIELD         = 32;
+constexpr uint8_t IRQ_IPI           = 33;
 
 namespace Kernel
 {

kernel/include/kernel/Input/InputDevice.h

@@ -3,6 +3,7 @@
 #include <BAN/ByteSpan.h>
 
 #include <kernel/Device/Device.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {
@@ -42,7 +43,7 @@ namespace Kernel
 		const Type m_type;
 
 		mutable SpinLock m_event_lock;
-		Semaphore m_event_semaphore;
+		ThreadBlocker m_event_thread_blocker;
 
 		static constexpr size_t m_max_event_count { 128 };
 
@@ -63,7 +64,7 @@ namespace Kernel
 	public:
 		static BAN::ErrorOr<BAN::RefPtr<KeyboardDevice>> create(mode_t mode, uid_t uid, gid_t gid);
 
-		void notify() { m_semaphore.unblock(); }
+		void notify() { m_thread_blocker.unblock(); }
 
 	private:
 		KeyboardDevice(mode_t mode, uid_t uid, gid_t gid);
@@ -79,7 +80,7 @@ namespace Kernel
 	private:
 		const dev_t m_rdev;
 		const BAN::StringView m_name;
-		Semaphore m_semaphore;
+		ThreadBlocker m_thread_blocker;
 
 		friend class BAN::RefPtr<KeyboardDevice>;
 	};
@@ -89,7 +90,7 @@ namespace Kernel
 	public:
 		static BAN::ErrorOr<BAN::RefPtr<MouseDevice>> create(mode_t mode, uid_t uid, gid_t gid);
 
-		void notify() { m_semaphore.unblock(); }
+		void notify() { m_thread_blocker.unblock(); }
 
 	private:
 		MouseDevice(mode_t mode, uid_t uid, gid_t gid);
@@ -105,7 +106,7 @@ namespace Kernel
 	private:
 		const dev_t m_rdev;
 		const BAN::StringView m_name;
-		Semaphore m_semaphore;
+		ThreadBlocker m_thread_blocker;
 
 		friend class BAN::RefPtr<MouseDevice>;
 	};

kernel/include/kernel/InterruptStack.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <kernel/Arch.h>
+
 #include <stdint.h>
 
 namespace Kernel

kernel/include/kernel/Lock/Mutex.h

@@ -2,7 +2,7 @@
 
 #include <BAN/Atomic.h>
 #include <BAN/NoCopyMove.h>
-#include <kernel/Scheduler.h>
+#include <kernel/Thread.h>
 
 #include <sys/types.h>
 
@@ -19,7 +19,7 @@ namespace Kernel
 
 		void lock()
 		{
-			auto tid = Scheduler::current_tid();
+			const auto tid = Thread::current_tid();
 			if (tid == m_locker)
 				ASSERT(m_lock_depth > 0);
 			else
@@ -27,11 +27,11 @@ namespace Kernel
 				pid_t expected = -1;
 				while (!m_locker.compare_exchange(expected, tid))
 				{
-					Scheduler::get().yield();
+					Processor::yield();
 					expected = -1;
 				}
 				ASSERT(m_lock_depth == 0);
-				if (Scheduler::current_tid())
+				if (tid)
 					Thread::current().add_mutex();
 			}
 			m_lock_depth++;
@@ -39,7 +39,7 @@ namespace Kernel
 
 		bool try_lock()
 		{
-			auto tid = Scheduler::current_tid();
+			const auto tid = Thread::current_tid();
 			if (tid == m_locker)
 				ASSERT(m_lock_depth > 0);
 			else
@@ -48,7 +48,7 @@ namespace Kernel
 				if (!m_locker.compare_exchange(expected, tid))
 					return false;
 				ASSERT(m_lock_depth == 0);
-				if (Scheduler::current_tid())
+				if (tid)
 					Thread::current().add_mutex();
 			}
 			m_lock_depth++;
@@ -57,12 +57,13 @@ namespace Kernel
 
 		void unlock()
 		{
-			ASSERT(m_locker == Scheduler::current_tid());
+			const auto tid = Thread::current_tid();
+			ASSERT(m_locker == tid);
 			ASSERT(m_lock_depth > 0);
 			if (--m_lock_depth == 0)
 			{
 				m_locker = -1;
-				if (Scheduler::current_tid())
+				if (tid)
 					Thread::current().remove_mutex();
 			}
 		}
@@ -86,7 +87,7 @@ namespace Kernel
 
 		void lock()
 		{
-			auto tid = Scheduler::current_tid();
+			const auto tid = Thread::current_tid();
 			if (tid == m_locker)
 				ASSERT(m_lock_depth > 0);
 			else
@@ -97,11 +98,11 @@ namespace Kernel
 				pid_t expected = -1;
 				while (!(has_priority || m_queue_length == 0) || !m_locker.compare_exchange(expected, tid))
 				{
-					Scheduler::get().yield();
+					Processor::yield();
 					expected = -1;
 				}
 				ASSERT(m_lock_depth == 0);
-				if (Scheduler::current_tid())
+				if (tid)
 					Thread::current().add_mutex();
 			}
 			m_lock_depth++;
@@ -109,7 +110,7 @@ namespace Kernel
 
 		bool try_lock()
 		{
-			auto tid = Scheduler::current_tid();
+			const auto tid = Thread::current_tid();
 			if (tid == m_locker)
 				ASSERT(m_lock_depth > 0);
 			else
@@ -121,7 +122,7 @@ namespace Kernel
 				if (has_priority)
 					m_queue_length++;
 				ASSERT(m_lock_depth == 0);
-				if (Scheduler::current_tid())
+				if (tid)
 					Thread::current().add_mutex();
 			}
 			m_lock_depth++;
@@ -130,7 +131,7 @@ namespace Kernel
 
 		void unlock()
 		{
-			auto tid = Scheduler::current_tid();
+			const auto tid = Thread::current_tid();
 			ASSERT(m_locker == tid);
 			ASSERT(m_lock_depth > 0);
 			if (--m_lock_depth == 0)
@@ -139,7 +140,7 @@ namespace Kernel
 				if (has_priority)
 					m_queue_length--;
 				m_locker = -1;
-				if (Scheduler::current_tid())
+				if (tid)
 					Thread::current().remove_mutex();
 			}
 		}

kernel/include/kernel/Lock/SpinLock.h

@@ -23,34 +23,45 @@ namespace Kernel
 			auto state = Processor::get_interrupt_state();
 			Processor::set_interrupt_state(InterruptState::Disabled);
 
-			auto id = Processor::current_id();
-			ASSERT(m_locker != id);
+			auto id = Processor::current_id().as_u32();
+			ASSERT(m_locker.load(BAN::MemoryOrder::memory_order_relaxed) != id);
 
-			ProcessorID expected = PROCESSOR_NONE;
+			auto expected = PROCESSOR_NONE.as_u32();
 			while (!m_locker.compare_exchange(expected, id, BAN::MemoryOrder::memory_order_acquire))
 			{
-				__builtin_ia32_pause();
-				expected = PROCESSOR_NONE;
+				Processor::pause();
+				expected = PROCESSOR_NONE.as_u32();
 			}
 
 			return state;
 		}
 
+		bool try_lock_interrupts_disabled()
+		{
+			ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
+
+			auto id = Processor::current_id().as_u32();
+			ASSERT(m_locker.load(BAN::MemoryOrder::memory_order_relaxed) != id);
+
+			auto expected = PROCESSOR_NONE.as_u32();
+			return m_locker.compare_exchange(expected, id, BAN::MemoryOrder::memory_order_acquire);
+		}
+
 		void unlock(InterruptState state)
 		{
 			ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-			ASSERT(m_locker == Processor::current_id());
-			m_locker.store(PROCESSOR_NONE, BAN::MemoryOrder::memory_order_release);
+			ASSERT(current_processor_has_lock());
+			m_locker.store(PROCESSOR_NONE.as_u32(), BAN::MemoryOrder::memory_order_release);
 			Processor::set_interrupt_state(state);
 		}
 
 		bool current_processor_has_lock() const
 		{
-			return m_locker == Processor::current_id();
+			return m_locker.load(BAN::MemoryOrder::memory_order_relaxed) == Processor::current_id().as_u32();
 		}
 
 	private:
-		BAN::Atomic<ProcessorID> m_locker { PROCESSOR_NONE };
+		BAN::Atomic<ProcessorID::value_type> m_locker { PROCESSOR_NONE.as_u32() };
 	};
 
 	class RecursiveSpinLock
@@ -66,18 +77,15 @@ namespace Kernel
 			auto state = Processor::get_interrupt_state();
 			Processor::set_interrupt_state(InterruptState::Disabled);
 
-			auto id = Processor::current_id();
-			if (m_locker == id)
-				ASSERT(m_lock_depth > 0);
-			else
+			auto id = Processor::current_id().as_u32();
+
+			ProcessorID::value_type expected = PROCESSOR_NONE.as_u32();
+			while (!m_locker.compare_exchange(expected, id, BAN::MemoryOrder::memory_order_acq_rel))
 			{
-				ProcessorID expected = PROCESSOR_NONE;
-				while (!m_locker.compare_exchange(expected, id, BAN::MemoryOrder::memory_order_acquire))
-				{
-					__builtin_ia32_pause();
-					expected = PROCESSOR_NONE;
-				}
-				ASSERT(m_lock_depth == 0);
+				if (expected == id)
+					break;
+				Processor::pause();
+				expected = PROCESSOR_NONE.as_u32();
 			}
 
 			m_lock_depth++;
@@ -88,21 +96,21 @@ namespace Kernel
 		void unlock(InterruptState state)
 		{
 			ASSERT(Processor::get_interrupt_state() == InterruptState::Disabled);
-			ASSERT(m_locker == Processor::current_id());
+			ASSERT(current_processor_has_lock());
 			ASSERT(m_lock_depth > 0);
 			if (--m_lock_depth == 0)
-				m_locker.store(PROCESSOR_NONE, BAN::MemoryOrder::memory_order_release);
+				m_locker.store(PROCESSOR_NONE.as_u32(), BAN::MemoryOrder::memory_order_release);
 			Processor::set_interrupt_state(state);
 		}
 
 		bool current_processor_has_lock() const
 		{
-			return m_locker == Processor::current_id();
+			return m_locker.load(BAN::MemoryOrder::memory_order_relaxed) == Processor::current_id().as_u32();
 		}
 
 	private:
-		BAN::Atomic<ProcessorID>	m_locker { PROCESSOR_NONE };
-		uint32_t					m_lock_depth { 0 };
+		BAN::Atomic<ProcessorID::value_type> m_locker { PROCESSOR_NONE.as_u32() };
+		uint32_t                             m_lock_depth { 0 };
 	};
 
 	template<typename Lock>

kernel/include/kernel/Memory/PageTable.h

@@ -95,11 +95,11 @@ namespace Kernel
 		static BAN::ErrorOr<PageTable*> create_userspace();
 		~PageTable();
 
-		void unmap_page(vaddr_t);
+		void unmap_page(vaddr_t, bool send_smp_message = true);
 		void unmap_range(vaddr_t, size_t bytes);
 
+		void map_page_at(paddr_t, vaddr_t, flags_t, MemoryType = MemoryType::Normal, bool send_smp_message = true);
 		void map_range_at(paddr_t, vaddr_t, size_t bytes, flags_t, MemoryType = MemoryType::Normal);
-		void map_page_at(paddr_t, vaddr_t, flags_t, MemoryType = MemoryType::Normal);
 
 		paddr_t physical_address_of(vaddr_t) const;
 		flags_t get_page_flags(vaddr_t) const;
@@ -127,7 +127,8 @@ namespace Kernel
 		void initialize_kernel();
 		void map_kernel_memory();
 		void prepare_fast_page();
-		static void invalidate(vaddr_t);
+
+		static void invalidate(vaddr_t, bool send_smp_message);
 
 		static void map_fast_page(paddr_t);
 		static void unmap_fast_page();

kernel/include/kernel/Networking/ARPTable.h

@@ -5,7 +5,7 @@
 #include <BAN/UniqPtr.h>
 #include <kernel/Networking/NetworkInterface.h>
 #include <kernel/Process.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {
@@ -58,7 +58,7 @@ namespace Kernel
 
 		Process*									m_process = nullptr;
 		BAN::CircularQueue<PendingArpPacket, 128>	m_pending_packets;
-		Semaphore									m_pending_semaphore;
+		ThreadBlocker									m_pending_thread_blocker;
 
 		friend class BAN::UniqPtr<ARPTable>;
 	};

kernel/include/kernel/Networking/IPv4Layer.h

@@ -77,7 +77,7 @@ namespace Kernel
 		static constexpr size_t pending_packet_buffer_size = 128 * PAGE_SIZE;
 		BAN::UniqPtr<VirtualRange>					m_pending_packet_buffer;
 		BAN::CircularQueue<PendingIPv4Packet, 128>	m_pending_packets;
-		Semaphore									m_pending_semaphore;
+		ThreadBlocker									m_pending_thread_blocker;
 		SpinLock									m_pending_lock;
 		size_t										m_pending_total_size { 0 };
 

kernel/include/kernel/Networking/TCPSocket.h

@@ -7,7 +7,7 @@
 #include <kernel/Networking/NetworkInterface.h>
 #include <kernel/Networking/NetworkSocket.h>
 #include <kernel/Process.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {
@@ -161,7 +161,7 @@ namespace Kernel
 
 		uint64_t m_time_wait_start_ms { 0 };
 
-		Semaphore m_semaphore;
+		ThreadBlocker m_thread_blocker;
 
 		RecvWindowInfo m_recv_window;
 		SendWindowInfo m_send_window;

kernel/include/kernel/Networking/UDPSocket.h

@@ -6,7 +6,7 @@
 #include <kernel/Memory/VirtualRange.h>
 #include <kernel/Networking/NetworkInterface.h>
 #include <kernel/Networking/NetworkSocket.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {
@@ -57,7 +57,7 @@ namespace Kernel
 		BAN::CircularQueue<PacketInfo, 32>	m_packets;
 		size_t								m_packet_total_size { 0 };
 		SpinLock							m_packet_lock;
-		Semaphore							m_packet_semaphore;
+		ThreadBlocker							m_packet_thread_blocker;
 
 		friend class BAN::RefPtr<UDPSocket>;
 	};

kernel/include/kernel/Networking/UNIX/Socket.h

@@ -48,7 +48,7 @@ namespace Kernel
 			mutable BAN::Atomic<bool>					target_closed { false };
 			BAN::WeakPtr<UnixDomainSocket>				connection;
 			BAN::Queue<BAN::RefPtr<UnixDomainSocket>>	pending_connections;
-			Semaphore									pending_semaphore;
+			ThreadBlocker									pending_thread_blocker;
 			SpinLock									pending_lock;
 		};
 
@@ -67,7 +67,7 @@ namespace Kernel
 		size_t							m_packet_size_total { 0 };
 		BAN::UniqPtr<VirtualRange>		m_packet_buffer;
 		SpinLock						m_packet_lock;
-		Semaphore						m_packet_semaphore;
+		ThreadBlocker						m_packet_thread_blocker;
 
 		friend class BAN::RefPtr<UnixDomainSocket>;
 	};

kernel/include/kernel/Process.h

@@ -251,7 +251,7 @@ namespace Kernel
 	private:
 		struct ExitStatus
 		{
-			Semaphore			semaphore;
+			ThreadBlocker		thread_blocker;
 			int					exit_code	{ 0 };
 			BAN::Atomic<bool>	exited		{ false };
 			BAN::Atomic<int>	waiting		{ 0 };

kernel/include/kernel/Processor.h

@@ -1,12 +1,14 @@
 #pragma once
 
+#include <BAN/Atomic.h>
+#include <BAN/Formatter.h>
 #include <BAN/ForwardList.h>
 
 #include <kernel/Arch.h>
 #include <kernel/GDT.h>
 #include <kernel/IDT.h>
 #include <kernel/InterruptStack.h>
-#include <kernel/SchedulerQueue.h>
+#include <kernel/Scheduler.h>
 
 namespace Kernel
 {
@@ -17,8 +19,28 @@ namespace Kernel
 		Enabled,
 	};
 
-	using ProcessorID = uint32_t;
-	constexpr ProcessorID PROCESSOR_NONE = 0xFFFFFFFF;
+	class ProcessorID
+	{
+	public:
+		using value_type = uint32_t;
+
+	public:
+		ProcessorID() = default;
+
+		uint32_t as_u32() const { return m_id; }
+		bool operator==(ProcessorID other) const { return m_id == other.m_id; }
+
+	private:
+		explicit ProcessorID(uint32_t id) : m_id(id) {}
+
+	private:
+		uint32_t m_id = static_cast<uint32_t>(-1);
+
+		friend class Processor;
+		friend class APIC;
+	};
+
+	constexpr ProcessorID PROCESSOR_NONE { };
 
 #if ARCH(x86_64) || ARCH(i686)
 	class Processor
@@ -26,12 +48,44 @@ namespace Kernel
 		BAN_NON_COPYABLE(Processor);
 		BAN_NON_MOVABLE(Processor);
 
+	public:
+		struct SMPMessage
+		{
+			enum class Type
+			{
+				FlushTLB,
+				NewThread,
+				UnblockThread,
+				// FIXME: all processors should LAPIC for their preemption
+				SchedulerPreemption,
+			};
+			SMPMessage* next { nullptr };
+			Type type;
+			union
+			{
+				struct
+				{
+					uintptr_t vaddr;
+					size_t page_count;
+				} flush_tlb;
+				Scheduler::NewThreadRequest new_thread;
+				Scheduler::UnblockRequest unblock_thread;
+				uintptr_t scheduler_preemption;
+			};
+		};
+
 	public:
 		static Processor& create(ProcessorID id);
 		static Processor& initialize();
-		static void allocate_idle_thread();
 
 		static ProcessorID current_id() { return read_gs_sized<ProcessorID>(offsetof(Processor, m_id)); }
+		static ProcessorID id_from_index(size_t index);
+
+		static uint8_t count()       { return s_processor_count; }
+		static bool is_smp_enabled() { return s_is_smp_enabled; }
+		static void wait_until_processors_ready();
+
+		static void toggle_should_print_cpu_load() { s_should_print_cpu_load = !s_should_print_cpu_load; }
 
 		static ProcessorID bsb_id() { return s_bsb_id; }
 		static bool current_is_bsb() { return current_id() == bsb_id(); }
@@ -53,31 +107,40 @@ namespace Kernel
 			return InterruptState::Disabled;
 		};
 
-		static uintptr_t current_stack_bottom() { return reinterpret_cast<uintptr_t>(read_gs_ptr(offsetof(Processor, m_stack))); }
+		static void pause()
+		{
+			__builtin_ia32_pause();
+			if (is_smp_enabled())
+				handle_smp_messages();
+		}
+
+		static uintptr_t current_stack_bottom() { return read_gs_sized<uintptr_t>(offsetof(Processor, m_stack)); }
 		static uintptr_t current_stack_top()	{ return current_stack_bottom() + s_stack_size; }
 
 		uintptr_t stack_bottom() const	{ return reinterpret_cast<uintptr_t>(m_stack); }
 		uintptr_t stack_top() const		{ return stack_bottom() + s_stack_size; }
 
-		static GDT& gdt() { return *reinterpret_cast<GDT*>(read_gs_ptr(offsetof(Processor, m_gdt))); }
-		static IDT& idt() { return *reinterpret_cast<IDT*>(read_gs_ptr(offsetof(Processor, m_idt))); }
+		static GDT& gdt() { return *read_gs_sized<GDT*>(offsetof(Processor, m_gdt)); }
+		static IDT& idt() { return *read_gs_sized<IDT*>(offsetof(Processor, m_idt)); }
 
-		static void* get_current_page_table()					{ return read_gs_ptr(offsetof(Processor, m_current_page_table)); }
-		static void set_current_page_table(void* page_table)	{ write_gs_ptr(offsetof(Processor, m_current_page_table), page_table); }
+		static void* get_current_page_table()					{ return read_gs_sized<void*>(offsetof(Processor, m_current_page_table)); }
+		static void set_current_page_table(void* page_table)	{ write_gs_sized<void*>(offsetof(Processor, m_current_page_table), page_table); }
 
-		static Thread* idle_thread()									{ return reinterpret_cast<Thread*>(read_gs_ptr(offsetof(Processor, m_idle_thread))); }
-		static SchedulerQueue::Node* get_current_thread()				{ return reinterpret_cast<SchedulerQueue::Node*>(read_gs_ptr(offsetof(Processor, m_current_thread))); }
-		static void set_current_thread(SchedulerQueue::Node* thread)	{ write_gs_ptr(offsetof(Processor, m_current_thread), thread); }
+		static void yield();
+		static Scheduler& scheduler() { return *read_gs_sized<Scheduler*>(offsetof(Processor, m_scheduler)); }
 
-		static void enter_interrupt(InterruptStack*, InterruptRegisters*);
-		static void leave_interrupt();
-		static InterruptStack& get_interrupt_stack();
-		static InterruptRegisters& get_interrupt_registers();
+		static void handle_ipi();
+
+		static void handle_smp_messages();
+		static void send_smp_message(ProcessorID, const SMPMessage&, bool send_ipi = true);
+		static void broadcast_smp_message(const SMPMessage&);
 
 	private:
 		Processor() = default;
 		~Processor() { ASSERT_NOT_REACHED(); }
 
+		static ProcessorID read_processor_id();
+
 		template<typename T>
 		static T read_gs_sized(uintptr_t offset) requires(sizeof(T) <= 8)
 		{
@@ -110,11 +173,10 @@ namespace Kernel
 #undef __ASM_INPUT
 		}
 
-		static void* read_gs_ptr(uintptr_t offset) { return read_gs_sized<void*>(offset); }
-		static void write_gs_ptr(uintptr_t offset, void* value) { write_gs_sized<void*>(offset, value); }
-
 	private:
 		static ProcessorID s_bsb_id;
+		static BAN::Atomic<uint8_t> s_processor_count;
+		static BAN::Atomic<bool>    s_is_smp_enabled;
 
 		ProcessorID m_id { PROCESSOR_NONE };
 
@@ -124,11 +186,15 @@ namespace Kernel
 		GDT* m_gdt { nullptr };
 		IDT* m_idt { nullptr };
 
-		Thread* m_idle_thread { nullptr };
-		SchedulerQueue::Node* m_current_thread { nullptr };
+		Scheduler* m_scheduler { nullptr };
 
-		InterruptStack* m_interrupt_stack { nullptr };
-		InterruptRegisters* m_interrupt_registers { nullptr };
+		BAN::Atomic<bool> m_smp_pending_lock { false };
+		SMPMessage* m_smp_pending { nullptr };
+
+		BAN::Atomic<bool> m_smp_free_lock { false };
+		SMPMessage* m_smp_free    { nullptr };
+
+		SMPMessage* m_smp_message_storage;
 
 		void* m_current_page_table { nullptr };
 
@@ -139,3 +205,14 @@ namespace Kernel
 #endif
 
 }
+
+namespace BAN::Formatter
+{
+
+	template<typename F>
+	void print_argument(F putc, Kernel::ProcessorID processor_id, const ValueFormat& format)
+	{
+		print_argument(putc, processor_id.as_u32(), format);
+	}
+
+}

kernel/include/kernel/Scheduler.h

@@ -1,55 +1,149 @@
 #pragma once
 
-#include <kernel/SchedulerQueue.h>
-#include <kernel/Semaphore.h>
-#include <kernel/Thread.h>
+#include <BAN/Array.h>
+#include <BAN/ForwardList.h>
+#include <BAN/NoCopyMove.h>
+#include <kernel/InterruptStack.h>
+
+#include <sys/types.h>
 
 namespace Kernel
 {
 
-	class Scheduler
+	class Thread;
+	class ThreadBlocker;
+
+	class SchedulerQueue
 	{
 	public:
-		static BAN::ErrorOr<void> initialize();
-		static Scheduler& get();
+		struct Node
+		{
+			Node(Thread* thread)
+				: thread(thread)
+			{}
 
-		[[noreturn]] void start();
+			Node* next { nullptr };
+			Node* prev { nullptr };
 
-		void yield();
+			Thread* thread;
+			ThreadBlocker* blocker { nullptr };
+			uint64_t wake_time_ns { static_cast<uint64_t>(-1) };
 
-		void timer_reschedule();
-		void irq_reschedule();
-		void reschedule_if_idling();
+			uint64_t last_start_ns { 0 };
+			uint64_t time_used_ns  { 0 };
+		};
 
-		void set_current_thread_sleeping(uint64_t wake_time);
+	public:
+		void add_thread_to_back(Node*);
+		void add_thread_with_wake_time(Node*);
+		template<typename F>
+		Node* remove_with_condition(F callback);
+		void remove_node(Node*);
+		Node* front();
+		Node* pop_front();
 
-		void block_current_thread(Semaphore*, uint64_t wake_time);
-		void unblock_threads(Semaphore*);
-		// Makes sleeping or blocked thread with tid active.
+		bool empty() const { return m_head == nullptr; }
+
+	private:
+		Node* m_head { nullptr };
+		Node* m_tail { nullptr };
+	};
+
+	class Scheduler
+	{
+		BAN_NON_COPYABLE(Scheduler);
+		BAN_NON_MOVABLE(Scheduler);
+
+	public:
+		struct NewThreadRequest
+		{
+			SchedulerQueue::Node* node;
+			bool blocked;
+		};
+
+		struct UnblockRequest
+		{
+			enum class Type
+			{
+				ThreadBlocker,
+				ThreadID,
+			};
+			Type type;
+			union
+			{
+				ThreadBlocker* blocker;
+				pid_t tid;
+			};
+		};
+
+	public:
+		static BAN::ErrorOr<Scheduler*> create();
+		BAN::ErrorOr<void> initialize();
+
+		void reschedule(InterruptStack*, InterruptRegisters*);
+		void reschedule_if_idle();
+
+		void timer_interrupt();
+
+		BAN::ErrorOr<void> add_thread(Thread*);
+
+		void block_current_thread(ThreadBlocker* thread_blocker, uint64_t wake_time_ns);
+		void unblock_threads(ThreadBlocker*);
 		void unblock_thread(pid_t tid);
 
 		Thread& current_thread();
-		static pid_t current_tid();
+		Thread& idle_thread();
 
-		// This is no return if called on current thread
-		void terminate_thread(Thread*);
+		pid_t current_tid() const;
+		bool is_idle() const;
 
 	private:
 		Scheduler() = default;
 
-		void set_current_thread_sleeping_impl(Semaphore* semaphore, uint64_t wake_time);
+		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 setup_next_thread();
+		bool do_unblock(ThreadBlocker*);
+		bool do_unblock(pid_t);
+		void do_load_balancing();
 
-		BAN::ErrorOr<void> add_thread(Thread*);
+		class ProcessorID find_least_loaded_processor() const;
+
+		void preempt();
+
+		void handle_unblock_request(const UnblockRequest&);
+		void handle_new_thread_request(const NewThreadRequest&);
 
 	private:
-		SpinLock m_lock;
+		SchedulerQueue m_run_queue;
+		SchedulerQueue m_block_queue;
+		SchedulerQueue::Node* m_current { nullptr };
+		bool m_current_will_block { false };
 
-		SchedulerQueue m_active_threads;
-		SchedulerQueue m_blocking_threads;
+		uint32_t m_thread_count { 0 };
 
-		friend class Process;
+		InterruptStack* m_interrupt_stack { nullptr };
+		InterruptRegisters* m_interrupt_registers { nullptr };
+
+		uint64_t m_last_reschedule_ns { 0 };
+		uint64_t m_last_load_balance_ns { 0 };
+
+		struct ThreadInfo
+		{
+			SchedulerQueue*       queue { nullptr };
+			SchedulerQueue::Node* node  { nullptr };
+		};
+		BAN::Array<ThreadInfo, 10> m_most_loaded_threads;
+
+		uint64_t m_idle_start_ns { 0 };
+		uint64_t m_idle_ns { 0 };
+
+		bool m_should_calculate_max_load_threads { true };
+
+		Thread* m_idle_thread { nullptr };
+
+		friend class Processor;
 	};
 
 }

kernel/include/kernel/SchedulerQueue.h

@@ -1,127 +0,0 @@
-#pragma once
-
-#include <BAN/Assert.h>
-#include <BAN/NoCopyMove.h>
-
-#include <stdint.h>
-
-namespace Kernel
-{
-
-	class Thread;
-	class Semaphore;
-
-	class SchedulerQueue
-	{
-		BAN_NON_COPYABLE(SchedulerQueue);
-		BAN_NON_MOVABLE(SchedulerQueue);
-
-	public:
-		struct Node
-		{
-			Node(Thread* thread)
-				: thread(thread)
-			{}
-
-			Thread*		thread;
-			uint64_t	wake_time { 0 };
-			Semaphore*	semaphore { nullptr };
-			bool		should_block { false };
-
-		private:
-			Node* next { nullptr };
-			friend class SchedulerQueue;
-			friend class Scheduler;
-		};
-
-	public:
-		SchedulerQueue() = default;
-		~SchedulerQueue() { ASSERT_NOT_REACHED(); }
-
-		bool empty() const { return m_front == nullptr; }
-
-		Node* pop_front()
-		{
-			ASSERT(!empty());
-
-			Node* node = m_front;
-
-			m_front = m_front->next;
-			if (m_front == nullptr)
-				m_back = nullptr;
-
-			node->next = nullptr;
-
-			return node;
-		}
-
-		void push_back(Node* node)
-		{
-			ASSERT(node);
-			node->next = nullptr;
-
-			(empty() ? m_front : m_back->next) = node;
-			m_back = node;
-		}
-
-		void add_with_wake_time(Node* node)
-		{
-			ASSERT(node);
-			node->next = nullptr;
-
-			if (empty() || node->wake_time >= m_back->wake_time)
-			{
-				push_back(node);
-				return;
-			}
-
-			if (node->wake_time < m_front->wake_time)
-			{
-				node->next = m_front;
-				m_front = node;
-				return;
-			}
-
-			Node* prev = m_front;
-			for (; node->wake_time >= prev->next->wake_time; prev = prev->next)
-				continue;
-			node->next = prev->next;
-			prev->next = node;
-		}
-
-		void remove_with_wake_time(SchedulerQueue& out, uint64_t current_time)
-		{
-			while (!empty() && m_front->wake_time <= current_time)
-				out.push_back(pop_front());
-		}
-
-		template<typename F>
-		void remove_with_condition(SchedulerQueue& out, F comp)
-		{
-			while (!empty() && comp(m_front))
-				out.push_back(pop_front());
-
-			if (empty())
-				return;
-
-			for (Node* prev = m_front; prev->next;)
-			{
-				Node* node = prev->next;
-				if (!comp(node))
-					prev = prev->next;
-				else
-				{
-					prev->next = node->next;
-					if (node == m_back)
-						m_back = prev;
-					out.push_back(node);
-				}
-			}
-		}
-
-	private:
-		Node* m_front { nullptr };
-		Node* m_back { nullptr };
-	};
-
-}

kernel/include/kernel/Semaphore.h

@@ -1,15 +0,0 @@
-#pragma once
-
-namespace Kernel
-{
-
-	class Semaphore
-	{
-	public:
-		void block_indefinite();
-		void block_with_timeout(uint64_t timeout_ms);
-		void block_with_wake_time(uint64_t wake_time_ms);
-		void unblock();
-	};
-
-}

kernel/include/kernel/Storage/ATA/AHCI/Device.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 #include <kernel/Storage/ATA/AHCI/Definitions.h>
 #include <kernel/Storage/ATA/ATADevice.h>
 

kernel/include/kernel/Storage/NVMe/Queue.h

@@ -4,7 +4,7 @@
 #include <BAN/Vector.h>
 #include <kernel/Interruptable.h>
 #include <kernel/Memory/DMARegion.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 #include <kernel/Storage/NVMe/Definitions.h>
 
 namespace Kernel
@@ -31,7 +31,7 @@ namespace Kernel
 		uint32_t m_cq_head { 0 };
 		uint16_t m_cq_valid_phase { 1 };
 
-		Semaphore			m_semaphore;
+		ThreadBlocker			m_thread_blocker;
 		SpinLock			m_lock;
 		BAN::Atomic<size_t>	m_used_mask			{ 0 };
 		BAN::Atomic<size_t>	m_done_mask			{ 0 };

kernel/include/kernel/Terminal/TTY.h

@@ -5,7 +5,7 @@
 #include <kernel/Lock/SpinLock.h>
 #include <kernel/Terminal/TerminalDriver.h>
 #include <kernel/Terminal/termios.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 #include <LibInput/KeyEvent.h>
 
 namespace Kernel
@@ -74,7 +74,7 @@ namespace Kernel
 		{
 			bool draw_graphics { true };
 			bool receive_input { true };
-			Semaphore semaphore;
+			ThreadBlocker thread_blocker;
 		};
 		tty_ctrl_t m_tty_ctrl;
 
@@ -83,7 +83,7 @@ namespace Kernel
 			BAN::Array<uint8_t, 1024> buffer;
 			size_t bytes { 0 };
 			bool flush { false };
-			Semaphore semaphore;
+			ThreadBlocker thread_blocker;
 		};
 		Buffer m_output;
 

kernel/include/kernel/Terminal/VirtualTTY.h

@@ -5,7 +5,7 @@
 #include <kernel/Terminal/TerminalDriver.h>
 #include <kernel/Terminal/termios.h>
 #include <kernel/Terminal/TTY.h>
-#include <kernel/Semaphore.h>
+#include <kernel/ThreadBlocker.h>
 
 namespace Kernel
 {

kernel/include/kernel/Thread.h

@@ -47,10 +47,12 @@ namespace Kernel
 		void handle_signal(int signal = 0);
 		bool add_signal(int signal);
 
-		// blocks semaphore and returns either on unblock, eintr, spuriously or after timeout
-		BAN::ErrorOr<void> block_or_eintr_indefinite(Semaphore& semaphore);
-		BAN::ErrorOr<void> block_or_eintr_or_timeout(Semaphore& semaphore, uint64_t timeout_ms, bool etimedout);
-		BAN::ErrorOr<void> block_or_eintr_or_waketime(Semaphore& semaphore, uint64_t wake_time_ms, bool etimedout);
+		// blocks current thread and returns either on unblock, eintr, spuriously or after timeout
+		BAN::ErrorOr<void> block_or_eintr_indefinite(ThreadBlocker& thread_blocker);
+		BAN::ErrorOr<void> block_or_eintr_or_timeout_ms(ThreadBlocker& thread_blocker, uint64_t timeout_ms, bool etimedout) { return block_or_eintr_or_timeout_ns(thread_blocker, timeout_ms * 1'000'000, etimedout); }
+		BAN::ErrorOr<void> block_or_eintr_or_waketime_ms(ThreadBlocker& thread_blocker, uint64_t wake_time_ms, bool etimedout) { return block_or_eintr_or_waketime_ns(thread_blocker, wake_time_ms * 1'000'000, etimedout); }
+		BAN::ErrorOr<void> block_or_eintr_or_timeout_ns(ThreadBlocker& thread_blocker, uint64_t timeout_ns, bool etimedout);
+		BAN::ErrorOr<void> block_or_eintr_or_waketime_ns(ThreadBlocker& thread_blocker, uint64_t wake_time_ns, bool etimedout);
 
 		pid_t tid() const { return m_tid; }
 

kernel/include/kernel/ThreadBlocker.h

@@ -0,0 +1,17 @@
+#pragma once
+
+namespace Kernel
+{
+
+	class ThreadBlocker
+	{
+	public:
+		void block_indefinite();
+		void block_with_timeout_ms(uint64_t timeout_ms) { return block_with_timeout_ns(timeout_ms * 1'000'000); }
+		void block_with_wake_time_ms(uint64_t wake_time_ms) { return block_with_wake_time_ns(wake_time_ms * 1'000'000); }
+		void block_with_timeout_ns(uint64_t timeout_ns);
+		void block_with_wake_time_ns(uint64_t wake_time_ns);
+		void unblock();
+	};
+
+}

kernel/include/kernel/Timer/Timer.h

@@ -29,7 +29,8 @@ namespace Kernel
 		virtual uint64_t ns_since_boot() const override;
 		virtual timespec time_since_boot() const override;
 
-		void sleep(uint64_t ms) const;
+		void sleep_ms(uint64_t ms) const { return sleep_ns(ms * 1'000'000); }
+		void sleep_ns(uint64_t ns) const;
 
 		timespec real_time() const;
 

kernel/include/kernel/USB/XHCI/Controller.h

@@ -4,6 +4,7 @@
 
 #include <kernel/Lock/Mutex.h>
 #include <kernel/Memory/DMARegion.h>
+#include <kernel/ThreadBlocker.h>
 #include <kernel/USB/USBManager.h>
 #include <kernel/USB/XHCI/Definitions.h>
 
@@ -79,7 +80,7 @@ namespace Kernel
 		Mutex m_mutex;
 
 		Process* m_port_updater { nullptr };
-		Semaphore m_port_semaphore;
+		ThreadBlocker m_port_thread_blocker;
 		BAN::Atomic<bool> m_port_changed { false };
 
 		PCI::Device& m_pci_device;