stos/amd64: lapic: Fully enable Local APIC

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 <hal/mmio.h>
 #include <drivers/acpi/tables.h>
 #include <drivers/acpi/acpi.h>
 #include <mm/vmm.h>
@@ -57,17 +58,81 @@ LapicHasX2Apic(VOID)
 }
 
 /*
- * Enable the Local APIC unit
+ * Read a Local APIC register
+ *
+ * @Mcb:        Machine core block
+ * @Register:   Register to read
+ */
+static UQUAD
+LapicRegRead(MCB *Mcb, ULONG Register)
+{
+    ULONG *Base;
+    UQUAD Value;
+
+    if (!Mcb->HasX2Apic) {
+        Base = PTR_OFFSET(Mcb->LapicBase, Register);
+        Value = MMIORead32(Base);
+    } else {
+        Register >>= 4;
+        Value = MdRdmsr(x2APIC_MSR_BASE + Register);
+    }
+
+    return Value;
+}
+
+/*
+ * Write a value to a Local APIC register space
+ *
+ * @Mcb:        Machine core block
+ * @Register:   Register to read
+ * @Value:      Value to write
  */
 static VOID
-LapicEnable(VOID)
+LapicRegWrite(MCB *Mcb, ULONG Register, UQUAD Value)
+{
+    ULONG *Base;
+
+    if (!Mcb->HasX2Apic) {
+        Base = PTR_OFFSET(Mcb->LapicBase, Register);
+        MMIOWrite32(Base, (ULONG)Value);
+    } else {
+        Register >>= 4;
+        MdWrmsr(x2APIC_MSR_BASE + Register, Value);
+    }
+}
+
+/*
+ * Enable the Local APIC unit
+ *
+ * @Mcb:   Machine core block
+ */
+static VOID
+LapicEnable(MCB *Mcb)
 {
     UQUAD ApicBase;
+    ULONG Svr, Version;
+    const CHAR *mode;
+    const CHAR *type = "discrete 82489DX";
 
     /* Hardware enable the Local APIC unit */
     ApicBase = MdRdmsr(IA32_APIC_BASE_MSR);
     ApicBase |= LAPIC_HW_ENABLE;
+    ApicBase |= Mcb->HasX2Apic << x2APIC_ENABLE_SHIFT;
     MdWrmsr(IA32_APIC_BASE_MSR, ApicBase);
+
+    /* Software enable the Local APIC unit */
+    Svr = LapicRegRead(Mcb, LAPIC_SVR);
+    Svr |= LAPIC_SW_ENABLE;
+    LapicRegWrite(Mcb, LAPIC_SVR, Svr);
+
+    /* Check the type */
+    Version = LapicRegRead(Mcb, LAPIC_VERSION);
+    if ((Version & 0xFF) > 0) {
+        type = "integrated";
+    }
+
+    mode = Mcb->HasX2Apic ? "x2apic" : "xapic";
+    DTRACE_BSP("enabled %s %s\n", type, mode);
 }
 
 VOID
@@ -101,5 +166,5 @@ MdLapicInit(KPCR *Kpcr)
     Mcb->HasX2Apic = LapicHasX2Apic();
 
     /* Enable the Local APIC */
-    LapicEnable();
+    LapicEnable(Mcb);
 }