pmOS scheduling

void reschedule()
{
    auto *const cpu_str         = get_cpu_struct();
    const auto current_priority = cpu_str->current_task->priority;

    auto const new_task = cpu_str->atomic_pick_highest_priority(current_priority - 1);
    if (new_task) {
        auto const current_task = cpu_str->current_task;

        // It might be fine to lock the locks separately
        Auto_Lock_Scope_Double l(current_task->sched_lock, new_task->sched_lock);

        new_task->switch_to();
        push_ready(current_task);
    }

    while (cpu_str->current_task->status == TaskStatus::TASK_DYING) {
        auto t = cpu_str->current_task;
        t->cleanup();

        find_new_process();
    }
}

klib::shared_ptr<TaskDescriptor> CPU_Info::atomic_pick_highest_priority(priority_t min)
{
    const priority_t max_priority = sched_queues.size() - 1;
    const priority_t to_priority  = min > max_priority ? max_priority : min;

    for (priority_t i = 0; i <= to_priority; ++i) {
        klib::shared_ptr<TaskDescriptor> task;
        {
            auto &queue = sched_queues[i];

            Auto_Lock_Scope l(queue.lock);

            task = queue.pop_front();
        }

        if (task != klib::shared_ptr<TaskDescriptor>(nullptr))
            return task;

        {
            auto &queue = global_sched_queues[i];

            Auto_Lock_Scope l(queue.lock);

            task = queue.pop_front();
        }

        if (task != klib::shared_ptr<TaskDescriptor>(nullptr))
            return task;
    }

    return nullptr;
}

void TaskDescriptor::switch_to()
{
    CPU_Info *c = get_cpu_struct();
    if (c->current_task->page_table != page_table) {
        page_table->atomic_active_sum(1);
        c->current_task->page_table->atomic_active_sum(-1);
        page_table->apply();
        c->paging_rcu_cpu.quiet(paging_rcu, c->cpu_id);
    }

    c->current_task->before_task_switch();

    // Switch task
    if (status != TaskStatus::TASK_DYING)
        // If the task is dying, don't change its status and let the scheduler handle it when
        // returning from the kernel
        status = TaskStatus::TASK_RUNNING;

    c->current_task_priority = priority;
    c->current_task          = weak_self.lock();

    this->after_task_switch();

    start_timer(assign_quantum_on_priority(priority));
}