stos/amd64: cpu: Ignore non online capable processors

Signed-off-by: Chloe M. <chloe@mensia.org>

paw/stos/arch/amd64/cpu/lapic.c

@@ -10,6 +10,7 @@
 #include <machine/lapicreg.h>
 #include <machine/msr.h>
 #include <machine/cpuid.h>
+#include <machine/i8254.h>
 #include <hal/mmio.h>
 #include <hal/prim.h>
 #include <drivers/acpi/tables.h>
@@ -136,6 +137,53 @@ LapicEnable(MCB *Mcb)
     DTRACE_BSP("enabled %s %s\n", type, mode);
 }
 
+/*
+ * Stop the Local APIC timer
+ *
+ * @Mcb: Machine core block
+ */
+static VOID
+LapicTimerStop(MCB *Mcb)
+{
+    if (Mcb == NULL) {
+        return;
+    }
+
+    LapicRegWrite(Mcb, LAPIC_LVT_TMR, LAPIC_LVT_MASK);
+    LapicRegWrite(Mcb, LAPIC_INIT_CNT, 0);
+}
+
+/*
+ * Calibrate the Local APIC timer
+ *
+ * @Mcb: Machine core block
+ */
+static VOID
+LapicTimerCalibrate(MCB *Mcb)
+{
+    const USHORT MAX_SAMPLES = 0xFFFF;
+    USHORT TicksStart, TicksEnd;
+    USIZE Freq, TicksTotal;
+
+    if (Mcb == NULL) {
+        return;
+    }
+
+    LapicTimerStop(Mcb);
+    MdPitSetCount(MAX_SAMPLES);
+    TicksStart = MdPitGetCount();
+
+    LapicRegWrite(Mcb, LAPIC_INIT_CNT, MAX_SAMPLES);
+    while (LapicRegRead(Mcb, LAPIC_CUR_CNT) != 0);
+
+    TicksEnd = MdPitGetCount();
+    TicksTotal = TicksStart - TicksEnd;
+
+    Freq = (MAX_SAMPLES / TicksTotal) * I8254_DIVIDEND;
+    LapicTimerStop(Mcb);
+    Mcb->LapicTmrFreq = Freq;
+}
+
 VOID
 MdLapicSendIpi(UCHAR Vector, UCHAR DestId, BOOLEAN LogicalDest,
     IPI_SHORTHAND Xnd, IPI_DELMOD DelMod)
@@ -143,7 +191,6 @@ MdLapicSendIpi(UCHAR Vector, UCHAR DestId, BOOLEAN LogicalDest,
     KPCR *ThisCore;
     MCB *Mcb;
     ULONG IcrLow, IcrHigh;
-    ULONG Icr = 0;
 
     /*
      * If the shorthand is refering to ourselves and we are x2APIC we
@@ -156,30 +203,29 @@ MdLapicSendIpi(UCHAR Vector, UCHAR DestId, BOOLEAN LogicalDest,
     }
 
     Mcb = &ThisCore->Mcb;
+    IcrLow = 0;
+    IcrHigh = 0;
 
     /* Clamp these fields */
     DelMod &= 7;
     Xnd &= 3;
 
     /* Encode the ICR */
-    Icr |= Vector & 0xFF;
+    IcrLow |= Vector & 0xFF;
-    Icr |= Xnd << 18;
+    IcrLow |= Xnd << 18;
-    Icr |= DelMod << 8;
+    IcrLow |= DelMod << 8;
-    Icr |= LogicalDest << 11;
+    IcrLow |= LogicalDest << 11;
 
     /* For xAPIC only */
     if (!Mcb->HasX2Apic) {
-        IcrHigh = ((UQUAD)Icr >> 32) & (ULONG)-1;
-        IcrLow = Icr & (ULONG)-1;
         IcrHigh |= (UQUAD)DestId << 24;
-
         LapicRegWrite(Mcb, LAPIC_ICRHI, IcrHigh);
         LapicRegWrite(Mcb, LAPIC_ICRLO, IcrLow);
 
         /* Only on legacy xAPICs do we poll */
         for (;;) {
-            Icr = LapicRegRead(Mcb, LAPIC_ICRLO);
+            IcrLow = LapicRegRead(Mcb, LAPIC_ICRLO);
-            if (ISSET(Icr, IPI_DELSTAT_PENDING))
+            if (ISSET(IcrLow, IPI_DELSTAT_PENDING))
                 HalCpuSpinWait();
         }
 
@@ -187,8 +233,24 @@ MdLapicSendIpi(UCHAR Vector, UCHAR DestId, BOOLEAN LogicalDest,
     }
 
     /* On x2APICs only as it queues */
-    Icr |= (UQUAD)DestId << 32;
+    LapicRegWrite(Mcb, LAPIC_ICRLO, ((UQUAD)DestId << 32) | IcrLow);
-    LapicRegWrite(Mcb, LAPIC_ICRLO, Icr);
+}
+
+ULONG
+MdLapicId(VOID)
+{
+    KPCR *ThisCore;
+    MCB *Mcb;
+
+    ThisCore = HalKpcrCurrent();
+    Mcb = &ThisCore->Mcb;
+
+    /* 32-bit when in x2APIC mode */
+    if (Mcb->HasX2Apic) {
+        return LapicRegRead(Mcb, LAPIC_ID) & 0xFFFFFFFF;
+    }
+
+    return (LapicRegRead(Mcb, LAPIC_ID) >> 24) & 0xF;
 }
 
 VOID
@@ -225,4 +287,7 @@ MdLapicInit(KPCR *Kpcr)
 
     /* Enable the Local APIC */
     LapicEnable(Mcb);
+
+    /* Calibrate the timer */
+    LapicTimerCalibrate(Mcb);
 }

paw/stos/arch/amd64/cpu/mp.c

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2026, Chloe M.
+ * Provided under the BSD-3 clause.
+ *
+ * Description:  MP bringup
+ * Author:       Chloe M.
+ */
+
+#include <stdef.h>
+#include <hal/mp.h>
+#include <hal/prim.h>
+#include <ex/trace.h>
+#include <machine/irqchip.h>
+#include <machine/hpet.h>
+#include <machine/lapic.h>
+#include <units.h>
+
+/* Limit used to prevent log spam */
+#define MP_LOG_CAP 4
+#define MP_DEBUG 1
+
+/* Local APIC flags */
+#define LAPIC_ONLINE_CAPABLE BIT(1)
+
+#define DTRACE(Fmt, ...) \
+    TRACE("[ MP ]: " Fmt, ##__VA_ARGS__)
+
+#if MP_DEBUG
+#define MP_DTRACE(...)                  \
+    if (ProcessorsUp < MP_LOG_CAP) {    \
+        DTRACE(__VA_ARGS__);            \
+    }
+#else
+#define MP_DTRACE(...) (VOID)0
+#endif  /* MP_DEBUG */
+
+static USIZE ProcessorsUp = 0;
+
+/*
+ * Bring up a single application processor
+ *
+ * @Lapic: Local APIC of processor to bring up
+ */
+static VOID
+ApKick(ACPI_LOCAL_APIC *Lapic)
+{
+    UQUAD UsecCur, UsecGoal;
+    ULONG BspId;
+
+    if (Lapic == NULL) {
+        return;
+    }
+
+    /* Don't nuke ourselves */
+    BspId = MdLapicId();
+    if (Lapic->ApicId == BspId) {
+        return;
+    }
+
+    /*
+     * Some processor entries may not be "online capable", if we
+     * encounter such entries, ignore it.
+     */
+    if (!ISSET(Lapic->Flags, LAPIC_ONLINE_CAPABLE)) {
+        return;
+    }
+
+    /* Send an INIT IPI to the processor */
+    MdLapicSendIpi(
+        0,
+        Lapic->ApicId,
+        0,
+        IPI_XND_NONE,
+        IPI_DELMOD_INIT
+    );
+
+    /* MP spec states to wait 10 usec */
+    UsecCur = MdHpetTimeUsec();
+    UsecGoal = UsecCur + (USEC_PER_MSEC * 10);
+    while (MdHpetTimeUsec() < UsecGoal) {
+        HalCpuSpinWait();
+    }
+
+    DTRACE("sent init to apic%d\n", Lapic->ApicId);
+    ++ProcessorsUp;
+}
+
+VOID
+HalMpBringUp(VOID)
+{
+    ACPI_LOCAL_APIC *LocalApic;
+    USIZE Idx;
+
+    /* Bring up each AP */
+    for (Idx = 0;; ++Idx) {
+        LocalApic = MdLapicByIndex(Idx);
+        if (LocalApic == NULL) {
+            break;
+        }
+
+        ApKick(LocalApic);
+    }
+}

paw/stos/arch/amd64/platform/hpet.c

@@ -14,6 +14,7 @@
 #include <mm/vmm.h>
 #include <ke/knot.h>
 #include <ex/trace.h>
+#include <units.h>
 
 #define DTRACE(Fmt, ...) \
     TRACE("[ HPET ]: " Fmt, ##__VA_ARGS__)
@@ -57,6 +58,20 @@ HpetReadq(UCHAR Register)
     return MMIORead64(RegBase);
 }
 
+USIZE
+MdHpetTimeUsec(VOID)
+{
+    UQUAD Period, Freq, Caps;
+    UQUAD Counter;
+
+    Caps = HpetReadq(HPET_GENERAL_CAP);
+    Period = (Caps >> HPET_PERIOD_SHIFT) & HPET_PERIOD_MASK;
+    Freq = FSEC_PER_SECOND / Period;
+    Counter = HpetReadq(HPET_MAIN_COUNTER);
+
+    return (Counter * USEC_PER_SECOND) / Freq;
+}
+
 VOID
 MdHpetInit(VOID)
 {

paw/stos/arch/amd64/platform/i8254.c

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2026, Chloe M.
+ * Provided under the BSD-3 clause.
+ *
+ * Description:  i8254 timer driver
+ * Author:       Chloe M.
+ */
+
+#include <machine/i8254.h>
+#include <machine/pio.h>
+
+USHORT
+MdPitGetCount(VOID)
+{
+    UCHAR Low, High;
+
+    MdOutb(I8254_COMMAND, 0x00);
+    Low = MdInb(0x40);
+    High = MdInb(0x40);
+    return (High << 8) | Low;
+}
+
+VOID
+MdPitSetCount(USHORT Value)
+{
+    MdOutb(I8254_COMMAND, 0x34);
+    MdOutb(0x40, Value & 0xFF);
+    MdOutb(0x40, (Value >> 8) & 0xFF);
+}
+
+VOID
+MdPitSetFrequency(UQUAD Frequency)
+{
+    UQUAD Divisor;
+
+    Divisor = I8254_DIVIDEND / Frequency;
+    if ((I8254_DIVIDEND % Frequency) > (Frequency / 2)) {
+        ++Divisor;
+    }
+
+    MdPitSetCount(Divisor);
+}

paw/stos/head/arch/amd64/hpet.h

@@ -16,4 +16,9 @@
  */
 VOID MdHpetInit(VOID);
 
+/*
+ * Obtain the elapsed time in microseconds
+ */
+USIZE MdHpetTimeUsec(VOID);
+
 #endif  /* !_MACHINE_HPET_H_ */

paw/stos/head/arch/amd64/i8254.h

@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2026, Chloe M.
+ * Provided under the BSD-3 clause.
+ *
+ * Description:  i8254 timer driver
+ * Author:       Chloe M.
+ */
+
+#ifndef _MACHINE_I8254_H_
+#define _MACHINE_I8254_H_ 1
+
+#include <stdef.h>
+
+#define I8254_COMMAND     0x43
+#define I8254_CHANNEL_0   0x40
+#define I8254_CHANNEL_2   0x42
+#define I8254_DIVIDEND    1193182ULL
+
+/*
+ * Obtain the current counter value
+ */
+USHORT MdPitGetCount(VOID);
+
+/*
+ * Set a new counter value
+ */
+VOID MdPitSetCount(USHORT Value);
+
+/*
+ * Set the PIT counter to match a frequency in Hz
+ *
+ * @Frequency: Frequency in HZ to set
+ */
+VOID MdPitSetFrequency(UQUAD Frequency);
+
+#endif  /* !_MACHINE_I8254_H_ */

paw/stos/head/arch/amd64/lapic.h

@@ -46,7 +46,7 @@ typedef enum {
     IPI_DELMOD_SMI,
     IPI_DELMOD_RESERVED,
     IPI_DELMOD_NMI,
-    IPI_DEMOD_INIT,
+    IPI_DELMOD_INIT,
     IPI_DELMOD_STARTUP
 } IPI_DELMOD;
 
@@ -64,6 +64,11 @@ typedef enum {
  */
 VOID MdLapicInit(KPCR *Kpcr);
 
+/*
+ * Obtain the APIC ID of the current processor
+ */
+ULONG MdLapicId(VOID);
+
 /*
  * Send an inter-processor interrupt
  *

paw/stos/head/arch/amd64/mcb.h

@@ -19,12 +19,14 @@
  * @FamilyId:       Processor family ID
  * @LapicBase:      Local APIC MMIO base
  * @HasX2Apic:      If set, x2APIC is supported
+ * @LapicTmrFreq:   Local APIC timer frequency
  */
 typedef struct {
     UCHAR ModelId;
     USHORT FamilyId : 12;
     VOID *LapicBase;
     UCHAR HasX2Apic : 1;
+    USIZE LapicTmrFreq;
 } MCB;
 
 #endif  /* !_MACHINE_MCB_H_ */

paw/stos/head/hal/mp.h

@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2026, Chloe M.
+ * Provided under the BSD-3 clause.
+ *
+ * Description:  MP bringup HAL layer
+ * Author:       Chloe M.
+ */
+
+#ifndef _HAL_MP_H_
+#define _HAL_MP_H_ 1
+
+#include <stdef.h>
+
+/*
+ * Bring the application processors up
+ */
+VOID HalMpBringUp(VOID);
+
+#endif  /* !_HAL_MP_H_ */

paw/stos/init/init.c

@@ -14,6 +14,7 @@
 #include <hal/serial.h>
 #include <hal/kpcr.h>
 #include <hal/board.h>
+#include <hal/mp.h>
 #include <ob/object.h>
 #include <drivers/bootvid/fbio.h>
 #include <drivers/acpi/acpi.h>
@@ -89,4 +90,7 @@ KiKernelEntry(VOID)
 
     /* Phase 3 initialization of the bootstrap core */
     HalKpcrP3Init(&BootstrapCore);
+
+    /* Bring up all other cores */
+    HalMpBringUp();
 }

sdk/head/units.h

@@ -18,5 +18,9 @@
 #define FSEC_PER_SECOND 1000000000000000ULL
 #define NSEC_PER_SECOND 1000000000ULL
 #define USEC_PER_SECOND 1000000ULL
+#define USEC_PER_MSEC   1000ULL
+
+/* Frequency units */
+#define HZ_PER_MHZ 1000000
 
 #endif  /* !_SDK_UNITS_H_ */

tools/qemu-x64.sh

@@ -14,4 +14,5 @@ qemu-system-x86_64            \
     -serial stdio             \
     -device virtio-vga-gl     \
     -display sdl,gl=on        \
+    -smp 4                    \
     -cpu host