Add x2apic mode to apic test

index b712ef8..72dd963 100644
@@ -102,6 +102,12 @@ static idt_entry_t idt[256];
 static int g_fail;
 static int g_tests;
 
+struct apic_ops {
+    u32 (*reg_read)(unsigned reg);
+    void (*reg_write)(unsigned reg, u32 val);
+    void (*icr_write)(u32 val, u32 dest);
+};
+
 static void outb(unsigned char data, unsigned short port)
 {
     asm volatile ("out %0, %1" : : "a"(data), "d"(port));

@@ -115,16 +121,47 @@ static void report(const char *msg, int pass)
         ++g_fail;
 }
 
-static u32 apic_read(unsigned reg)
+static u32 xapic_read(unsigned reg)
 {
     return *(volatile u32 *)(g_apic + reg);
 }
 
-static void apic_write(unsigned reg, u32 val)
+static void xapic_write(unsigned reg, u32 val)
 {
     *(volatile u32 *)(g_apic + reg) = val;
 }
 
+static void xapic_icr_write(u32 val, u32 dest)
+{
+    while (xapic_read(APIC_ICR) & APIC_ICR_BUSY)
+        ;
+    xapic_write(APIC_ICR2, dest << 24);
+    xapic_write(APIC_ICR, val);
+}
+
+static const struct apic_ops xapic_ops = {
+    .reg_read = xapic_read,
+    .reg_write = xapic_write,
+    .icr_write = xapic_icr_write,
+};
+
+static const struct apic_ops *apic_ops = &xapic_ops;
+
+static u32 apic_read(unsigned reg)
+{
+    return apic_ops->reg_read(reg);
+}
+
+static void apic_write(unsigned reg, u32 val)
+{
+    apic_ops->reg_write(reg, val);
+}
+
+static void apic_icr_write(u32 val, u32 dest)
+{
+    apic_ops->icr_write(val, dest);
+}
+
 static void test_lapic_existence(void)
 {
     u32 lvr;

index 72dd963..fdeec4c 100644
@@ -260,8 +260,8 @@ static void test_self_ipi(void)
 
     set_idt_entry(vec, self_ipi_isr);
     irq_enable();
-    apic_write(APIC_ICR,
-               APIC_DEST_SELF | APIC_DEST_PHYSICAL | APIC_DM_FIXED | vec);
+    apic_icr_write(APIC_DEST_SELF | APIC_DEST_PHYSICAL | APIC_DM_FIXED | vec,
+                   0);
     asm volatile ("nop");
     report("self ipi", ipi_count == 1);
 }

index fdeec4c..504def2 100644
@@ -9,6 +9,7 @@ typedef unsigned char u8;
 typedef unsigned short u16;
 typedef unsigned u32;
 typedef unsigned long ulong;
+typedef unsigned long long u64;
 
 typedef struct {
     unsigned short offset0;

@@ -147,6 +148,31 @@ static const struct apic_ops xapic_ops = {
 
 static const struct apic_ops *apic_ops = &xapic_ops;
 
+static u32 x2apic_read(unsigned reg)
+{
+    unsigned a, d;
+
+    asm volatile ("rdmsr" : "=a"(a), "=d"(d) : "c"(APIC_BASE_MSR + reg/16));
+    return a | (u64)d << 32;
+}
+
+static void x2apic_write(unsigned reg, u32 val)
+{
+    asm volatile ("wrmsr" : : "a"(val), "d"(0), "c"(APIC_BASE_MSR + reg/16));
+}
+
+static void x2apic_icr_write(u32 val, u32 dest)
+{
+    asm volatile ("wrmsr" : : "a"(val), "d"(dest),
+                  "c"(APIC_BASE_MSR + APIC_ICR/16));
+}
+
+static const struct apic_ops x2apic_ops = {
+    .reg_read = x2apic_read,
+    .reg_write = x2apic_write,
+    .icr_write = x2apic_icr_write,
+};
+
 static u32 apic_read(unsigned reg)
 {
     return apic_ops->reg_read(reg);

@@ -171,6 +197,25 @@ static void test_lapic_existence(void)
     report("apic existence", (u16)lvr == 0x14);
 }
 
+#define MSR_APIC_BASE 0x0000001b
+
+static void enable_x2apic(void)
+{
+    unsigned a, b, c, d;
+
+    asm ("cpuid" : "=a"(a), "=b"(b), "=c"(c), "=d"(d) : "0"(1));
+
+    if (c & (1 << 21)) {
+        asm ("rdmsr" : "=a"(a), "=d"(d) : "c"(MSR_APIC_BASE));
+        a |= 1 << 10;
+        asm ("wrmsr" : : "a"(a), "d"(d), "c"(MSR_APIC_BASE));
+        apic_ops = &x2apic_ops;
+        printf("x2apic enabled\n");
+    } else {
+        printf("x2apic not detected\n");
+    }
+}
+
 static u16 read_cs(void)
 {
     u16 v;

@@ -388,6 +433,7 @@ int main()
 
     mask_pic_interrupts();
     enable_apic();
+    enable_x2apic();
     init_idt();
 
     test_self_ipi();