Kernel: Track processor idle state and wake processors when waking threads

Attempt to wake idle processors to get threads to be scheduled more quickly.
We don't want to wait until the next timer tick if we have processors that
aren't doing anything.

Kernel/Arch/i386/CPU.cpp

@@ -1734,6 +1734,57 @@ ProcessorMessage& Processor::smp_get_from_pool()
     return *msg;
 }
 
+Atomic<u32> Processor::s_idle_cpu_mask{ 0 };
+
+u32 Processor::smp_wake_n_idle_processors(u32 wake_count)
+{
+    ASSERT(Processor::current().in_critical());
+    ASSERT(wake_count > 0);
+    if (!s_smp_enabled)
+        return 0;
+
+    // Wake at most N - 1 processors
+    if (wake_count >= Processor::count()) {
+        wake_count = Processor::count() - 1;
+        ASSERT(wake_count > 0);
+    }
+
+    u32 current_id = Processor::current().id();
+
+    u32 did_wake_count = 0;
+    auto& apic = APIC::the();
+    while (did_wake_count < wake_count) {
+        // Try to get a set of idle CPUs and flip them to busy
+        u32 idle_mask = s_idle_cpu_mask.load(AK::MemoryOrder::memory_order_relaxed) & ~(1u << current_id);
+        u32 idle_count = __builtin_popcountl(idle_mask);
+        if (idle_count == 0)
+            break; // No (more) idle processor available
+
+        u32 found_mask = 0;
+        for (u32 i = 0; i < idle_count; i++) {
+            u32 cpu = __builtin_ffsl(idle_mask) - 1;
+            idle_mask &= ~(1u << cpu);
+            found_mask |= 1u << cpu;
+        }
+
+        idle_mask = s_idle_cpu_mask.fetch_and(~found_mask, AK::MemoryOrder::memory_order_acq_rel) & found_mask;
+        if (idle_mask == 0)
+            continue; // All of them were flipped to busy, try again
+        idle_count = __builtin_popcountl(idle_mask);
+        for (u32 i = 0; i < idle_count; i++) {
+            u32 cpu = __builtin_ffsl(idle_mask) - 1;
+            idle_mask &= ~(1u << cpu);
+
+            // Send an IPI to that CPU to wake it up. There is a possibility
+            // someone else woke it up as well, or that it woke up due to
+            // a timer interrupt. But we tried hard to avoid this...
+            apic.send_ipi(cpu);
+            did_wake_count++;
+        }
+    }
+    return did_wake_count;
+}
+
 void Processor::smp_enable()
 {
     size_t msg_pool_size = Processor::count() * 100u;

Kernel/Arch/i386/CPU.h

@@ -709,6 +709,7 @@ class Processor {
     u32 m_cpu;
     u32 m_in_irq;
     Atomic<u32, AK::MemoryOrder::memory_order_relaxed> m_in_critical;
+    static Atomic<u32> s_idle_cpu_mask;
 
     TSS32 m_tss;
     static FPUState s_clean_fpu_state;
@@ -763,6 +764,16 @@ public:
     void early_initialize(u32 cpu);
     void initialize(u32 cpu);
 
+    void idle_begin()
+    {
+        s_idle_cpu_mask.fetch_or(1u << m_cpu, AK::MemoryOrder::memory_order_relaxed);
+    }
+
+    void idle_end()
+    {
+        s_idle_cpu_mask.fetch_and(~(1u << m_cpu), AK::MemoryOrder::memory_order_relaxed);
+    }
+
     static u32 count()
     {
         // NOTE: because this value never changes once all APs are booted,
@@ -1010,6 +1021,7 @@ public:
     static void smp_unicast(u32 cpu, void (*callback)(), bool async);
     static void smp_unicast(u32 cpu, void (*callback)(void*), void* data, void (*free_data)(void*), bool async);
     static void smp_broadcast_flush_tlb(const PageDirectory*, VirtualAddress, size_t);
+    static u32 smp_wake_n_idle_processors(u32 wake_count);
 
     template<typename Callback>
     static void deferred_call_queue(Callback callback)

Kernel/Scheduler.cpp

@@ -544,13 +544,17 @@ void Scheduler::notify_finalizer()
 
 void Scheduler::idle_loop(void*)
 {
-    dbgln("Scheduler[{}]: idle loop running", Processor::id());
+    auto& proc = Processor::current();
+    dbgln("Scheduler[{}]: idle loop running", proc.get_id());
     ASSERT(are_interrupts_enabled());
 
     for (;;) {
+        proc.idle_begin();
         asm("hlt");
 
-        if (Processor::id() == 0)
+        proc.idle_end();
+        ASSERT_INTERRUPTS_ENABLED();
+        if (Processor::current().id() == 0)
             yield();
     }
 }

Kernel/Thread.cpp

@@ -919,7 +919,9 @@ void Thread::set_state(State new_state, u8 stop_signal)
         }
     }
 
-    if (m_state == Stopped) {
+    if (m_state == Runnable) {
+        Processor::smp_wake_n_idle_processors(1);
+    } else if (m_state == Stopped) {
         // We don't want to restore to Running state, only Runnable!
         m_stop_state = previous_state != Running ? previous_state : Runnable;
         auto& process = this->process();