PCI check for MSIs and enable if possible

kernel/device/pci.c

@@ -5,6 +5,7 @@
 #include <libk/lengthof.h>
 #include <libk/std.h>
 #include <libk/string.h>
+#include <limine/requests.h>
 #include <sync/spin_lock.h>
 #include <sys/debug.h>
 #include <sys/smp.h>
@@ -105,6 +106,61 @@ void pci_write8 (uint8_t bus, uint8_t slot, uint8_t func, uint8_t offset, uint8_
   spin_unlock (&pci_lock, fpci);
 }
 
+uint8_t pci_find_cap (uint8_t bus, uint8_t slot, uint8_t func, uint8_t cap_id) {
+  uint16_t status = pci_read16 (bus, slot, func, PCI_STATUS);
+
+  if (!(status & (1 << 4)))
+    return 0;
+
+  uint8_t cap = pci_read8 (bus, slot, func, PCI_CAPABILITY);
+
+  while (cap != 0) {
+    uint8_t id = pci_read8 (bus, slot, func, cap);
+
+    if (id == cap_id)
+      return cap;
+
+    cap = pci_read8 (bus, slot, func, cap + 1);
+  }
+
+  return 0;
+}
+
+bool pci_msi_init (uint8_t bus, uint8_t slot, uint8_t func, uint8_t vec, uint32_t lapic_id) {
+  struct limine_hhdm_response* hhdm = limine_hhdm_request.response;
+
+  uint8_t cap = pci_find_cap (bus, slot, func, PCI_CAP_MSI);
+
+  if (cap == 0) {
+    DEBUG ("Device does not support MSI\n");
+    return false;
+  }
+
+  uint16_t msg_ctrl = pci_read16 (bus, slot, func, cap + 2);
+
+  uintptr_t lapic_phys = thiscpu->lapic_mmio_base - (uintptr_t)hhdm->offset;
+
+  uint32_t msi_addr = (uint32_t)lapic_phys | (lapic_id << 12);
+
+  uint32_t msi_data = (uint32_t)vec & 0xFF;
+
+  pci_write32 (bus, slot, func, cap + 4, msi_addr);
+
+  /* 32 or 64 bit */
+  if (msg_ctrl & (1 << 7)) {
+    /* 64 */
+    pci_write32 (bus, slot, func, cap + 12, msi_data);
+  } else {
+    pci_write32 (bus, slot, func, cap + 8, msi_data);
+  }
+
+  pci_write16 (bus, slot, func, cap + 2, msg_ctrl | 0x0001);
+
+  DEBUG ("MSI enabled (lapic %u)\n", lapic_id);
+
+  return true;
+}
+
 static void pci_check_bus (struct proc* proc, struct reschedule_ctx* rctx, uint8_t bus,
                            pci_cb_func_t cb);
 
@@ -214,6 +270,12 @@ static void pci_discovery_cb (struct proc* proc, struct reschedule_ctx* rctx,
          pci_info.device, pci_info.class, pci_info.subclass, pci_info.bus, pci_info.slot,
          pci_info.func, vname, dname, cname);
 
+  uint8_t cap = pci_find_cap (pci_info.bus, pci_info.slot, pci_info.func, PCI_CAP_MSI);
+
+  if (cap) {
+    DEBUG ("Device supports MSI!\n");
+  }
+
   for (size_t driver = 0; driver < lengthof (pci_driver_infos); driver++) {
     if (pci_driver_infos[driver].class == pci_info.class &&
         pci_driver_infos[driver].subclass == pci_info.subclass) {