Support both X2 and XAPIC modes

arch/x86_64/src/acpi/local_apic.rs

@@ -1,5 +1,10 @@
+use core::intrinsics::{volatile_load, volatile_store};
+use x86::cpuid::CpuId;
 use x86::msr::*;
 
+use memory::Frame;
+use paging::{entry, ActivePageTable, PhysicalAddress};
+
 bitflags! {
     pub flags LocalApicIcr: u64 {
         const ICR_VECTOR = 0xFF,
@@ -25,27 +30,81 @@ bitflags! {
 
 /// Local APIC
 #[repr(packed)]
-pub struct LocalApic;
+pub struct LocalApic {
+    address: u32,
+    pub x2: bool
+}
 
 impl LocalApic {
-    pub fn new() -> Self {
-        unsafe { wrmsr(IA32_APIC_BASE, rdmsr(IA32_APIC_BASE) | 1 << 11 | 1 << 10) }
-        LocalApic
+    pub fn new(active_table: &mut ActivePageTable) -> Self {
+        let mut apic = LocalApic {
+            address: (unsafe { rdmsr(IA32_APIC_BASE) as u32 } & 0xFFFF0000),
+            x2: false
+        };
+
+        println!("APIC BASE: {:>08X}", apic.address);
+
+        unsafe { wrmsr(IA32_APIC_BASE, rdmsr(IA32_APIC_BASE) & !(1 << 11 | 1 << 10)) };
+
+        unsafe { wrmsr(IA32_APIC_BASE, rdmsr(IA32_APIC_BASE) | 1 << 11) };
+
+        if CpuId::new().get_feature_info().unwrap().has_x2apic() {
+            unsafe { wrmsr(IA32_APIC_BASE, rdmsr(IA32_APIC_BASE) | 1 << 10) };
+            apic.x2 = true;
+            println!("X2APIC {:X}", unsafe { rdmsr(IA32_APIC_BASE) });
+        } else {
+            active_table.identity_map(Frame::containing_address(PhysicalAddress::new(apic.address as usize)), entry::PRESENT | entry::WRITABLE | entry::NO_EXECUTE);
+
+            println!("XAPIC {:X}", unsafe { rdmsr(IA32_APIC_BASE) });
+        }
+
+        apic
+    }
+
+    unsafe fn read(&self, reg: u32) -> u32 {
+        volatile_load((self.address + reg) as *const u32)
+    }
+
+    unsafe fn write(&self, reg: u32, value: u32) {
+        volatile_store((self.address + reg) as *mut u32, value);
     }
 
     pub fn id(&self) -> u32 {
+        if self.x2 {
             unsafe { rdmsr(IA32_X2APIC_APICID) as u32 }
+        } else {
+            unsafe { self.read(0x20) }
+        }
     }
 
     pub fn version(&self) -> u32 {
+        if self.x2 {
             unsafe { rdmsr(IA32_X2APIC_VERSION) as u32 }
+        } else {
+            unsafe { self.read(0x30) }
+        }
     }
 
     pub fn icr(&self) -> u64 {
+        if self.x2 {
             unsafe { rdmsr(IA32_X2APIC_ICR) }
+        } else {
+            unsafe {
+                (self.read(0x310) as u64) << 32 | self.read(0x300) as u64
+            }
+        }
     }
 
     pub fn set_icr(&mut self, value: u64) {
-        unsafe { wrmsr(IA32_X2APIC_ICR, value) }
+        if self.x2 {
+            unsafe { wrmsr(IA32_X2APIC_ICR, value); }
+        } else {
+            unsafe {
+                while self.read(0x300) & 1 << 12 == 1 << 12 {}
+                self.write(0x310, (value >> 32) as u32);
+                self.write(0x300, value as u32);
+                while self.read(0x300) & 1 << 12 == 1 << 12 {}
+            }
+        }
     }
 }

arch/x86_64/src/acpi/mod.rs

@@ -34,7 +34,7 @@ pub fn init_sdt(sdt: &'static Sdt, active_table: &mut ActivePageTable) {
     if let Some(madt) = Madt::new(sdt) {
         println!("    {:>016X}: {}", madt.local_address, madt.flags);
 
-        let mut local_apic = LocalApic::new();
+        let mut local_apic = LocalApic::new(active_table);
 
         let me = local_apic.id() as u8;
 
@@ -74,15 +74,28 @@ pub fn init_sdt(sdt: &'static Sdt, active_table: &mut ActivePageTable) {
 
                         // Send INIT IPI
                         {
-                            let icr = 0x00004500 | (ap_local_apic.id as u64) << 32;
+                            let mut icr = 0x4500;
+                            if local_apic.x2 {
+                                icr |= (ap_local_apic.id as u64) << 32;
+                            } else {
+                                icr |= (ap_local_apic.id as u64) << 56;
+                            }
                             println!("        Sending IPI to {}: {:>016X} {:?}", ap_local_apic.id, icr, LocalApicIcr::from_bits(icr));
                             local_apic.set_icr(icr);
                         }
 
                         // Send START IPI
                         {
+                            //Start at 0x0800:0000 => 0x8000. Hopefully the bootloader code is still there
                             let ap_segment = (AP_STARTUP >> 12) & 0xFF;
-                            let icr = 0x00004600 | ((ap_local_apic.id as u64) << 32) | ap_segment as u64; //Start at 0x0800:0000 => 0x8000. Hopefully the bootloader code is still there
+                            let mut icr = 0x4600 | ap_segment as u64;
+
+                            if local_apic.x2 {
+                                icr |= (ap_local_apic.id as u64) << 32;
+                            } else {
+                                icr |= (ap_local_apic.id as u64) << 56;
+                            }
+                            
                             println!("        Sending SIPI to {}: {:>016X} {:?}", ap_local_apic.id, icr, LocalApicIcr::from_bits(icr));
                             local_apic.set_icr(icr);
                         }