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 #include #include +#include #include @@ -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 - 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 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 -#include - -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(-1) }; - - BAN::Atomic 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 +#include +#include + +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(-1) }; + + BAN::Atomic 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 #include #include -#include #include #include @@ -37,85 +36,6 @@ namespace Kernel static BAN::Atomic 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 - 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::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(&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(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(arg); const uint64_t load_percent_x1000 = BAN::Math::div_round_up(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(&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(arg); const uint64_t load_percent_x1000 = BAN::Math::div_round_up(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(&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(node)->time_used_ns = 0; }, nullptr); + m_block_list.walk([](const SchedulerThreadNode* node, void*) { const_cast(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 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 +#include +#include + +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 #include -#include +#include #include #include @@ -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);