Add simple NVMe module and switch to NVMe emulation

2026-07-23 20:52:10 +03:00
parent ba9beebf8e
commit 065aa261f5
10 changed files with 345 additions and 21 deletions
@@ -0,0 +1,281 @@
+#include "src/kernel/nvme.h"
+#include "src/kernel/log.h"
+#include "src/kernel/memory.h"
+#include "src/kernel/panic.h"
+#include "src/kernel/pci.h"
+#include "src/lib/layout.h"
+#include "src/lib/memory.h"
+#include <stdint.h>
+
+#define SECTOR_SIZE 512
+
+typedef volatile struct __attribute__((packed)) {
+  uint64_t cap;
+  uint32_t vs;
+  uint32_t intms;
+  uint32_t intmc;
+  uint32_t cc;
+  uint32_t rsvd;
+  uint32_t csts;
+  uint32_t nssr;
+  uint32_t aqa;
+  uint64_t asq;
+  uint64_t acq;
+} nvme_regs_t;
+
+static nvme_regs_t *regs = (nvme_regs_t *)(KERNEL_VIRTUAL_BASE + MEMORY_SIZE);
+
+#define QUEUE_DEPTH 2
+
+typedef volatile struct __attribute__((packed)) {
+  uint8_t opc;
+  uint8_t flags;
+  uint16_t cid;
+  uint32_t nsid;
+  uint64_t reserved;
+  uint64_t mptr;
+  uint64_t prp1;
+  uint64_t prp2;
+  uint32_t cdw10;
+  uint32_t cdw11;
+  uint32_t cdw12;
+  uint32_t cdw13;
+  uint32_t cdw14;
+  uint32_t cdw15;
+} nvme_sqe_t;
+
+typedef volatile struct __attribute__((packed)) {
+  uint32_t dw0;
+  uint32_t reserved;
+  uint16_t sqhd;
+  uint16_t sqid;
+  uint16_t cid;
+  uint16_t status;
+} nvme_cqe_t;
+
+uint8_t admin_cq_phase = 1;
+static nvme_sqe_t admin_sq[QUEUE_DEPTH] __attribute__((aligned(PAGE_SIZE)));
+static uint16_t admin_sq_tail = 0;
+static nvme_cqe_t admin_cq[QUEUE_DEPTH] __attribute__((aligned(PAGE_SIZE)));
+static uint16_t admin_cq_head = 0;
+uint8_t io_cq_phase = 1;
+static nvme_sqe_t io_sq[QUEUE_DEPTH] __attribute__((aligned(PAGE_SIZE)));
+static uint16_t io_sq_tail = 0;
+static nvme_cqe_t io_cq[QUEUE_DEPTH] __attribute__((aligned(PAGE_SIZE)));
+static uint16_t io_cq_head = 0;
+
+static void admin_exec_sync(nvme_sqe_t *cmd) {
+  cmd->cid = admin_sq_tail;
+
+  TRACE("admin_exec_sync: Queueing submission...\n");
+  TRACE("admin_exec_sync: opc=%x\n", cmd->opc);
+  TRACE("admin_exec_sync: nsid=%x\n", cmd->nsid);
+  TRACE("admin_exec_sync: cdw10=%x\n", cmd->cdw10);
+  TRACE("admin_exec_sync: cdw11=%x\n", cmd->cdw11);
+  TRACE("admin_exec_sync: cdw12=%x\n", cmd->cdw12);
+  TRACE("admin_exec_sync: prp1=%x\n", cmd->prp1);
+  TRACE("admin_exec_sync: prp2=%x\n", cmd->prp2);
+
+  memory_copy(cmd, sizeof(nvme_sqe_t), &admin_sq[admin_sq_tail]);
+
+  TRACE("admin_exec_sync: Advancing submission doorbell...\n");
+
+  *(volatile uint32_t *)((uint8_t *)regs + 0x1000) = admin_sq_tail = (admin_sq_tail + 1) % QUEUE_DEPTH;
+
+  TRACE("admin_exec_sync: Waiting for completion...\n");
+
+  while ((admin_cq[admin_cq_head].status & 1) != admin_cq_phase)
+    ;
+
+  TRACE("admin_exec_sync: Received completion...\n");
+  TRACE("admin_exec_sync: dw0=%x\n", io_cq[io_cq_head].dw0);
+  TRACE("admin_exec_sync: reserved=%x\n", io_cq[io_cq_head].reserved);
+  TRACE("admin_exec_sync: sqhd=%x\n", io_cq[io_cq_head].sqhd);
+  TRACE("admin_exec_sync: sqid=%x\n", io_cq[io_cq_head].sqid);
+  TRACE("admin_exec_sync: cid=%x\n", io_cq[io_cq_head].cid);
+  TRACE("admin_exec_sync: status=%x\n", io_cq[io_cq_head].status);
+
+  ASSERT((admin_cq[admin_cq_head].status >> 1) == 0, "admin_exec_sync: NVMe command failed.");
+
+  TRACE("admin_exec_sync: Advancing completion doorbell...\n");
+
+  *(volatile uint32_t *)((uint8_t *)regs + 0x1004) = admin_cq_head = (admin_cq_head + 1) % QUEUE_DEPTH; // TODO Get offset from caps.
+
+  admin_cq_phase ^= (admin_cq_head == 0);
+
+  TRACE("admin_exec_sync: Done.\n");
+}
+
+static void io_exec_sync(nvme_sqe_t *cmd) {
+  cmd->cid = io_sq_tail;
+
+  TRACE("io_exec_sync: Queueing submission...\n");
+  TRACE("io_exec_sync: opc=%x\n", cmd->opc);
+  TRACE("io_exec_sync: nsid=%x\n", cmd->nsid);
+  TRACE("io_exec_sync: cdw10=%x\n", cmd->cdw10);
+  TRACE("io_exec_sync: cdw11=%x\n", cmd->cdw11);
+  TRACE("io_exec_sync: cdw12=%x\n", cmd->cdw12);
+  TRACE("io_exec_sync: prp1=%x\n", cmd->prp1);
+  TRACE("io_exec_sync: prp2=%x\n", cmd->prp2);
+
+  memory_copy(cmd, sizeof(nvme_sqe_t), &io_sq[io_sq_tail]);
+
+  TRACE("io_exec_sync: Advancing submission doorbell...\n");
+
+  *(volatile uint32_t *)((uint8_t *)regs + 0x1008) = io_sq_tail = (io_sq_tail + 1) % QUEUE_DEPTH; // TODO Get offset from caps.
+
+  TRACE("io_exec_sync: Waiting for completion...\n");
+
+  while ((io_cq[io_cq_head].status & 1) != io_cq_phase)
+    ;
+
+  TRACE("io_exec_sync: Received completion...\n");
+  TRACE("io_exec_sync: dw0=%x\n", io_cq[io_cq_head].dw0);
+  TRACE("io_exec_sync: reserved=%x\n", io_cq[io_cq_head].reserved);
+  TRACE("io_exec_sync: sqhd=%x\n", io_cq[io_cq_head].sqhd);
+  TRACE("io_exec_sync: sqid=%x\n", io_cq[io_cq_head].sqid);
+  TRACE("io_exec_sync: cid=%x\n", io_cq[io_cq_head].cid);
+  TRACE("io_exec_sync: status=%x\n", io_cq[io_cq_head].status);
+
+  ASSERT((io_cq[io_cq_head].status >> 1) == 0, "io_exec_sync: NVMe command failed.");
+
+  TRACE("io_exec_sync: Advancing completion doorbell...\n");
+
+  *(volatile uint32_t *)((uint8_t *)regs + 0x100C) = io_cq_head = (io_cq_head + 1) % QUEUE_DEPTH; // TODO Get offset from caps.
+
+  io_cq_phase ^= (io_cq_head == 0);
+
+  TRACE("io_exec_sync: Done.\n");
+}
+
+void nvme_init() {
+  LOG("nvme_init: Searching for suitable device...\n");
+  uint8_t found = 0;
+  for (uint16_t bus = 0; bus < 256 && !found; bus++) {
+    for (uint8_t device = 0; device < 32 && !found; device++) {
+      for (uint8_t function = 0; function < 8 && !found; function++) {
+        uint32_t id = pci_read((uint8_t)bus, device, function, 0);
+        if ((id & 0xFFFF) == 0xFFFF) {
+          continue;
+        }
+        uint32_t class = pci_read((uint8_t)bus, device, function, 8);
+        if ((class & 0xFF000000) >> 24 != 0x01 || (class & 0x00FF0000) >> 16 != 0x08) {
+          continue;
+        }
+
+        pci_write(0, 4, 0, 0x04, pci_read(0, 4, 0, 0x04) | 0x06);
+
+        uint32_t bar0 = pci_read((uint8_t)bus, device, function, 0x10) & 0xFFFFFFF0;
+        uint32_t bar1 = pci_read((uint8_t)bus, device, function, 0x14);
+        uint64_t phys = ((uint64_t)bar1 << 32) | bar0;
+
+        LOG("nvme_init: Mapping device to memory, phys %x <-> virt %x\n", phys, regs);
+        for (uint8_t i = 0; i < 4; i++) {
+          memory_page_map((void *)KERNEL_VIRTUAL_PML4, (void *)((uint8_t *)regs + i * PAGE_SIZE), (void *)((uint8_t *)phys + i * PAGE_SIZE),
+                          PAGE_WRITABLE | PAGE_PCD | PAGE_USER);
+        }
+        found = 1;
+      }
+    }
+  }
+
+  ASSERT(found, "nvme_init: No suitable devices found.");
+  ASSERT(regs->cap & (1ULL << 37), "nvme_init: NVM command set not supported.");
+
+  LOG("nvme_init: Disabling device...\n");
+
+  regs->cc = 0;
+  while (regs->csts & 1)
+    ;
+
+  LOG("nvme_init: Configuring device...\n");
+  regs->asq = (uint64_t)(VIRT_TO_PHYS(admin_sq));
+  TRACE("nvme_init: Setting ASQ, expected=%x, set=%x...\n", (uint64_t)(VIRT_TO_PHYS(admin_sq)), regs->asq);
+  regs->acq = (uint64_t)(VIRT_TO_PHYS(admin_cq));
+  TRACE("nvme_init: Setting ACQ, expected=%x, set=%x...\n", (uint64_t)(VIRT_TO_PHYS(admin_cq)), regs->acq);
+  regs->aqa = (QUEUE_DEPTH - 1) << 16 | (QUEUE_DEPTH - 1);
+  TRACE("nvme_init: Setting AQA, expected=%x, set=%x...\n", (QUEUE_DEPTH - 1) << 16 | (QUEUE_DEPTH - 1), regs->aqa);
+
+  regs->cc = (4 << 20) | (6 << 16) | (0 << 4) | 1;
+  LOG("nvme_init: Enabling device, expected=%x, set=%x...\n", (4 << 20) | (6 << 16) | (0 << 4) | 1, regs->cc);
+  while (!(regs->csts & 1))
+    ;
+
+  LOG("nvme_init: Creating I/O completion queue...\n");
+
+  nvme_sqe_t create_io_cq = {
+      .opc = 0x05,
+      .prp1 = (uint64_t)VIRT_TO_PHYS(io_cq),
+      .cdw10 = ((QUEUE_DEPTH - 1) << 16) | 1,
+      .cdw11 = 1,
+  };
+  admin_exec_sync(&create_io_cq);
+
+  LOG("nvme_init: Creating I/O submission queue...\n");
+
+  nvme_sqe_t create_io_sq = {
+      .opc = 0x01,
+      .prp1 = (uint64_t)VIRT_TO_PHYS(io_sq),
+      .cdw10 = ((QUEUE_DEPTH - 1) << 16) | 1,
+      .cdw11 = (1 << 16) | 1,
+  };
+  admin_exec_sync(&create_io_sq);
+
+  LOG("nvme_init: Done.\n");
+}
+
+void nvme_read_sectors(uint32_t index, uint8_t count, void *to) {
+  TRACE("nvme_read_sectors: Reading %u sectors at %u...\n", count, index);
+
+  while (count) {
+    uint64_t remainder = PAGE_SIZE - (uint64_t)to % PAGE_SIZE;
+
+    uint8_t i_count = (uint8_t)((remainder + PAGE_SIZE) / SECTOR_SIZE);
+    i_count = i_count > count ? count : i_count;
+
+    nvme_sqe_t read_sq = {
+        .opc = 0x02,
+        .nsid = 1,
+        .cdw10 = index,
+        .cdw11 = 0,
+        .cdw12 = i_count - 1,
+        .prp1 = (uint64_t)VIRT_TO_PHYS(to),
+        .prp2 = i_count * SECTOR_SIZE > remainder ? (uint64_t)VIRT_TO_PHYS(to + remainder) : 0,
+    };
+    io_exec_sync(&read_sq);
+
+    index += i_count;
+    count -= i_count;
+    to = (void *)((uint8_t *)to + i_count * SECTOR_SIZE);
+  }
+
+  TRACE("nvme_read_sectors: Done.\n", count, index);
+}
+
+void nvme_write_sectors(uint32_t index, uint8_t count, const void *from) {
+  TRACE("nvme_write_sectors: Reading %u sectors at %u...\n", count, index);
+
+  while (count) {
+    uint64_t remainder = PAGE_SIZE - (uint64_t)from % PAGE_SIZE;
+
+    uint8_t i_count = (uint8_t)((remainder + PAGE_SIZE) / SECTOR_SIZE);
+    i_count = i_count > count ? count : i_count;
+
+    nvme_sqe_t write_sq = {
+        .opc = 0x01,
+        .nsid = 1,
+        .cdw10 = index,
+        .cdw11 = 0,
+        .cdw12 = i_count - 1,
+        .prp1 = (uint64_t)VIRT_TO_PHYS(from),
+        .prp2 = i_count * SECTOR_SIZE > remainder ? (uint64_t)VIRT_TO_PHYS(from + remainder) : 0,
+    };
+    io_exec_sync(&write_sq);
+
+    index += i_count;
+    count -= i_count;
+    from = (void *)((uint8_t *)from + i_count * SECTOR_SIZE);
+  }
+
+  TRACE("nvme_write_sectors: Done.\n", count, index);
+}