Kernel/PCI + CPU: Allow to access unaligned data

3 files changed, 62 insertions, 8 deletions

diff --git a/Kernel/Arch/x86/CPU.h b/Kernel/Arch/x86/CPU.h
index 34c5b43fa3..a747b8f307 100644
--- a/Kernel/Arch/x86/CPU.h
+++ b/Kernel/Arch/x86/CPU.h
@@ -195,6 +195,50 @@ public:
 class GenericInterruptHandler;
 struct RegisterState;
 
+template<typename T>
+void read_possibly_unaligned_data(u8* where, T& data)
+{
+    // FIXME: Implement 64 bit unaligned data access
+    VERIFY(sizeof(T) == 8 || sizeof(T) == 4 || sizeof(T) == 2);
+    if (((FlatPtr)where % sizeof(T)) == 0) {
+        data = *(T*)where;
+        return;
+    }
+    if (sizeof(T) == 2) {
+        data = *(u8*)where | ((u16)(*(where + 1)) << 8);
+        return;
+    }
+    if (sizeof(T) == 4) {
+        data = *(u8*)where | (((u32) * (where + 1)) << 8) | (((u32) * (where + 2)) << 16) | (((u32) * (u8*)(where + 3)) << 24);
+        return;
+    }
+    VERIFY_NOT_REACHED();
+}
+
+template<typename T>
+void write_possibly_unaligned_data(u8* where, T data)
+{
+    // FIXME: Implement 64 bit unaligned data access
+    VERIFY(sizeof(T) == 8 || sizeof(T) == 4 || sizeof(T) == 2);
+    if (((FlatPtr)where % sizeof(T)) == 0) {
+        *(T*)where = data;
+        return;
+    }
+    if (sizeof(T) == 2) {
+        where[0] = (u8)(data & 0xFF);
+        where[1] = (u8)((data >> 8) & 0xFF);
+        return;
+    }
+    if (sizeof(T) == 4) {
+        where[0] = (u8)(data & 0xFF);
+        where[1] = (u8)((data >> 8) & 0xFF);
+        where[2] = (u8)((data >> 16) & 0xFF);
+        where[3] = (u8)((data >> 24) & 0xFF);
+        return;
+    }
+    VERIFY_NOT_REACHED();
+}
+
 const DescriptorTablePointer& get_gdtr();
 const DescriptorTablePointer& get_idtr();
 void register_interrupt_handler(u8 number, void (*handler)());
diff --git a/Kernel/PCI/MMIOAccess.cpp b/Kernel/PCI/MMIOAccess.cpp
index 32ad3a8dbd..47f96469af 100644
--- a/Kernel/PCI/MMIOAccess.cpp
+++ b/Kernel/PCI/MMIOAccess.cpp
@@ -26,6 +26,7 @@
 
 #include <AK/Optional.h>
 #include <AK/StringView.h>
+#include <Kernel/Arch/x86/CPU.h>
 #include <Kernel/Debug.h>
 #include <Kernel/PCI/MMIOAccess.h>
 #include <Kernel/VM/MemoryManager.h>
@@ -142,7 +143,9 @@ u16 MMIOAccess::read16_field(Address address, u32 field)
     ScopedSpinLock lock(m_access_lock);
     VERIFY(field < 0xfff);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Reading 16-bit field {:#08x} for {}", field, address);
-    return *((volatile u16*)(get_device_configuration_space(address).get() + (field & 0xfff)));
+    u16 data = 0;
+    read_possibly_unaligned_data<u16>(get_device_configuration_space(address).offset(field & 0xfff).as_ptr(), data);
+    return data;
 }
 
 u32 MMIOAccess::read32_field(Address address, u32 field)
@@ -150,7 +153,9 @@ u32 MMIOAccess::read32_field(Address address, u32 field)
     ScopedSpinLock lock(m_access_lock);
     VERIFY(field <= 0xffc);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Reading 32-bit field {:#08x} for {}", field, address);
-    return *((volatile u32*)(get_device_configuration_space(address).get() + (field & 0xfff)));
+    u32 data = 0;
+    read_possibly_unaligned_data<u32>(get_device_configuration_space(address).offset(field & 0xfff).as_ptr(), data);
+    return data;
 }
 
 void MMIOAccess::write8_field(Address address, u32 field, u8 value)
@@ -165,14 +170,14 @@ void MMIOAccess::write16_field(Address address, u32 field, u16 value)
     ScopedSpinLock lock(m_access_lock);
     VERIFY(field < 0xfff);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Writing 16-bit field {:#08x}, value={:#02x} for {}", field, value, address);
-    *((volatile u16*)(get_device_configuration_space(address).get() + (field & 0xfff))) = value;
+    write_possibly_unaligned_data<u16>(get_device_configuration_space(address).offset(field & 0xfff).as_ptr(), value);
 }
 void MMIOAccess::write32_field(Address address, u32 field, u32 value)
 {
     ScopedSpinLock lock(m_access_lock);
     VERIFY(field <= 0xffc);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Writing 32-bit field {:#08x}, value={:#02x} for {}", field, value, address);
-    *((volatile u32*)(get_device_configuration_space(address).get() + (field & 0xfff))) = value;
+    write_possibly_unaligned_data<u32>(get_device_configuration_space(address).offset(field & 0xfff).as_ptr(), value);
 }
 
 void MMIOAccess::enumerate_hardware(Function<void(Address, ID)> callback)
diff --git a/Kernel/PCI/WindowedMMIOAccess.cpp b/Kernel/PCI/WindowedMMIOAccess.cpp
index 67f975a371..b1f2ff708e 100644
--- a/Kernel/PCI/WindowedMMIOAccess.cpp
+++ b/Kernel/PCI/WindowedMMIOAccess.cpp
@@ -26,6 +26,7 @@
 
 #include <AK/Optional.h>
 #include <AK/StringView.h>
+#include <Kernel/Arch/x86/CPU.h>
 #include <Kernel/Debug.h>
 #include <Kernel/PCI/WindowedMMIOAccess.h>
 #include <Kernel/VM/MemoryManager.h>
@@ -96,7 +97,9 @@ u16 WindowedMMIOAccess::read16_field(Address address, u32 field)
     InterruptDisabler disabler;
     VERIFY(field < 0xfff);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Reading 16-bit field {:#08x} for {}", field, address);
-    return *((u16*)(get_device_configuration_space(address).value().get() + (field & 0xfff)));
+    u16 data = 0;
+    read_possibly_unaligned_data<u16>(get_device_configuration_space(address).value().offset(field & 0xfff).as_ptr(), data);
+    return data;
 }
 
 u32 WindowedMMIOAccess::read32_field(Address address, u32 field)
@@ -104,7 +107,9 @@ u32 WindowedMMIOAccess::read32_field(Address address, u32 field)
     InterruptDisabler disabler;
     VERIFY(field <= 0xffc);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Reading 32-bit field {:#08x} for {}", field, address);
-    return *((u32*)(get_device_configuration_space(address).value().get() + (field & 0xfff)));
+    u32 data = 0;
+    read_possibly_unaligned_data<u32>(get_device_configuration_space(address).value().offset(field & 0xfff).as_ptr(), data);
+    return data;
 }
 
 void WindowedMMIOAccess::write8_field(Address address, u32 field, u8 value)
@@ -119,14 +124,14 @@ void WindowedMMIOAccess::write16_field(Address address, u32 field, u16 value)
     InterruptDisabler disabler;
     VERIFY(field < 0xfff);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Writing 16-bit field {:#08x}, value={:#02x} for {}", field, value, address);
-    *((u16*)(get_device_configuration_space(address).value().get() + (field & 0xfff))) = value;
+    write_possibly_unaligned_data<u16>(get_device_configuration_space(address).value().offset(field & 0xfff).as_ptr(), value);
 }
 void WindowedMMIOAccess::write32_field(Address address, u32 field, u32 value)
 {
     InterruptDisabler disabler;
     VERIFY(field <= 0xffc);
     dbgln_if(PCI_DEBUG, "PCI: MMIO Writing 32-bit field {:#08x}, value={:#02x} for {}", field, value, address);
-    *((u32*)(get_device_configuration_space(address).value().get() + (field & 0xfff))) = value;
+    write_possibly_unaligned_data<u32>(get_device_configuration_space(address).value().offset(field & 0xfff).as_ptr(), value);
 }
 
 }