stos/amd64: lapic: Calibrate the Local APIC timer

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)
@@ -225,4 +273,7 @@ MdLapicInit(KPCR *Kpcr)
 
     /* Enable the Local APIC */
     LapicEnable(Mcb);
+
+    /* Calibrate the timer */
+    LapicTimerCalibrate(Mcb);
 }

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

@@ -15,16 +15,18 @@
  * The machine-core block contains machine specific
  * processor information
  *
- * @ModelId:    Processor model ID
- * @FamilyId:   Processor family ID
- * @LapicBase:  Local APIC MMIO base
- * @HasX2Apic:  If set, x2APIC is supported
+ * @ModelId:        Processor model ID
+ * @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_ */