kernel: nvme: Setup I/O queues and namespaces

Signed-off-by: Ian Moffett <ian@osmora.org>

2 files changed, 211 insertions, 0 deletions

diff --git a/sys/dev/ic/nvme.c b/sys/dev/ic/nvme.c
index 444cc3e..951f3ac 100644
--- a/sys/dev/ic/nvme.c
+++ b/sys/dev/ic/nvme.c
@@ -46,9 +46,12 @@
 #define pr_trace(fmt, ...) kprintf("nvme: " fmt, ##__VA_ARGS__)
 #define pr_error(...) pr_trace(__VA_ARGS__)
 
+static TAILQ_HEAD(,nvme_ns) namespaces;
 static struct pci_device *nvme_dev;
 static struct timer tmr;
 
+static int nvme_poll_submit_cmd(struct nvme_queue *q, struct nvme_cmd cmd);
+
 static inline int
 is_4k_aligned(void *ptr)
 {
@@ -135,6 +138,40 @@ nvme_create_queue(struct nvme_bar *bar, struct nvme_queue *queue, size_t id)
     return 0;
 }
 
+static int
+nvme_create_ioq(struct nvme_ns *ns, size_t id)
+{
+    struct nvme_queue *ioq = &ns->ioq;
+    struct nvme_ctrl *ctrl = ns->ctrl;
+    struct nvme_bar *bar = ctrl->bar;
+    struct nvme_create_iocq_cmd *create_iocq;
+    struct nvme_create_iosq_cmd *create_iosq;
+    struct nvme_cmd cmd = {0};
+    int error;
+
+    if ((error = nvme_create_queue(bar, ioq, id)) != 0)
+        return error;
+
+    create_iocq = &cmd.create_iocq;
+    create_iocq->opcode = NVME_OP_CREATE_IOCQ;
+    create_iocq->qflags = BIT(0);   /* Physically contiguous */
+    create_iocq->qsize = ctrl->cqes;
+    create_iocq->qid = id;
+    create_iocq->prp1 = VIRT_TO_PHYS(ns->ioq.cq);
+
+    if ((error = nvme_poll_submit_cmd(&ctrl->adminq, cmd)) != 0)
+        return error;
+
+    create_iosq = &cmd.create_iosq;
+    create_iosq->opcode = NVME_OP_CREATE_IOSQ;
+    create_iosq->qflags = BIT(0);    /* Physically contiguous */
+    create_iosq->qsize = ctrl->sqes;
+    create_iosq->cqid = id;
+    create_iosq->sqid = id;
+    create_iosq->prp1 = VIRT_TO_PHYS(ns->ioq.sq);
+    return nvme_poll_submit_cmd(&ctrl->adminq, cmd);
+}
+
 /*
  * Stop and reset the NVMe controller.
  */
@@ -310,12 +347,71 @@ nvme_init_pci(void)
     pci_writel(nvme_dev, PCIREG_CMDSTATUS, tmp);
 }
 
+/*
+ * Initializes an NVMe namespace.
+ *
+ * @ctrl: Controller.
+ * @nsid: Namespace ID.
+ */
+static int
+nvme_init_ns(struct nvme_ctrl *ctrl, uint8_t nsid)
+{
+    struct nvme_ns *ns = NULL;
+    struct nvme_id_ns *idns = NULL;
+    uint8_t lba_format;
+    int status = 0;
+
+    idns = dynalloc_memalign(sizeof(*idns), 0x1000);
+    ns = dynalloc(sizeof(*ns));
+
+    if (idns == NULL) {
+        status = -ENOMEM;
+        goto done;
+    }
+
+    if (ns == NULL) {
+        status = -ENOMEM;
+        goto done;
+    }
+
+    if ((status = nvme_identify(ctrl, idns, nsid, 0)) != 0)  {
+        goto done;
+    }
+
+    /* Setup the namespace structure */
+    lba_format = idns->flbas & 0xF;
+    ns->lba_fmt = idns->lbaf[lba_format];
+    ns->nsid = nsid;
+    ns->lba_bsize = 1 << ns->lba_fmt.ds;
+    ns->size = idns->size;
+    ns->ctrl = ctrl;
+
+    if ((status = nvme_create_ioq(ns, ns->nsid)) != 0) {
+        goto done;
+    }
+
+    if (status != 0) {
+        goto done;
+    }
+
+    TAILQ_INSERT_TAIL(&namespaces, ns, link);
+done:
+    if (ns != NULL && status != 0)
+        dynfree(ns);
+    if (idns != NULL && status != 0)
+        dynfree(idns);
+
+    return status;
+}
+
 static int
 nvme_init_ctrl(struct nvme_bar *bar)
 {
     int error;
     uint64_t caps;
+    uint32_t config;
     uint16_t mqes;
+    uint8_t *nsids;
     struct nvme_ctrl ctrl = {0};
     struct nvme_queue *adminq;
     struct nvme_id *id;
@@ -345,10 +441,42 @@ nvme_init_ctrl(struct nvme_bar *bar)
         return -ENOMEM;
     }
 
+    nsids = dynalloc_memalign(0x1000, 0x1000);
+    if (nsids == NULL) {
+        dynfree(id);
+        return -ENOMEM;
+    }
+
     nvme_identify(&ctrl, id, 0, ID_CNS_CTRL);
     nvme_log_ctrl_id(id);
+    nvme_identify(&ctrl, nsids, 0, ID_CNS_NSID_LIST);
+
+    ctrl.sqes = id->sqes >> 4;
+    ctrl.cqes = id->cqes >> 4;
+
+    /*
+     * Before creating any I/O queues we need to set CC.IOCQES
+     * and CC.IOSQES... Bits 3:0 is the minimum and bits 7:4
+     * is the maximum.
+     */
+    config = mmio_read32(&bar->config);
+    config |= (ctrl.sqes << CONFIG_IOSQES_SHIFT);
+    config |= (ctrl.cqes << CONFIG_IOCQES_SHIFT);
+    mmio_write32(&bar->config, config);
+
+    /* Init all active namespaces */
+    for (size_t i = 0; i < id->nn; ++i) {
+        if (nsids[i] == 0) {
+            continue;
+        }
+
+        if (nvme_init_ns(&ctrl, nsids[i]) != 0) {
+            pr_error("Failed to initialize NSID %d\n", nsids[i]);
+        }
+    }
 
     dynfree(id);
+    dynfree(nsids);
     return 0;
 }
 
diff --git a/sys/include/dev/ic/nvmevar.h b/sys/include/dev/ic/nvmevar.h
index 355abd8..b16f51d 100644
--- a/sys/include/dev/ic/nvmevar.h
+++ b/sys/include/dev/ic/nvmevar.h
@@ -64,9 +64,41 @@ struct nvme_cq_entry {
     uint16_t status;
 };
 
+/* Create I/O completion queue */
+struct nvme_create_iocq_cmd {
+    uint8_t opcode;
+    uint8_t flags;
+    uint16_t cid;
+    uint32_t unused1[5];
+    uint64_t prp1;
+    uint64_t unused2;
+    uint16_t qid;
+    uint16_t qsize;
+    uint16_t qflags;
+    uint16_t irqvec;
+    uint64_t unused3[2];
+};
+
+/* Create I/O submission queue */
+struct nvme_create_iosq_cmd {
+    uint8_t opcode;
+    uint8_t flags;
+    uint16_t cid;
+    uint32_t unused1[5];
+    uint64_t prp1;
+    uint64_t unused2;
+    uint16_t sqid;
+    uint16_t qsize;
+    uint16_t qflags;
+    uint16_t cqid;
+    uint64_t unused3[2];
+};
+
 struct nvme_cmd {
     union {
         struct nvme_identify_cmd identify;
+        struct nvme_create_iocq_cmd create_iocq;
+        struct nvme_create_iosq_cmd create_iosq;
     };
 };
 
@@ -126,8 +158,59 @@ struct nvme_id {
     uint8_t vs[1024];
 };
 
+struct nvme_lbaf {
+    uint16_t ms;    /* Number of metadata bytes per LBA */
+    uint8_t ds;     /* Data size */
+    uint8_t rp;
+};
+
+/* Identify namespace data */
+struct nvme_id_ns {
+    uint64_t size;
+    uint64_t capabilities;
+    uint64_t nuse;
+    uint8_t features;
+    uint8_t nlbaf;
+    uint8_t flbas;
+    uint8_t mc;
+    uint8_t dpc;
+    uint8_t dps;
+    uint8_t nmic;
+    uint8_t rescap;
+    uint8_t fpi;
+    uint8_t unused1;
+    uint16_t nawun;
+    uint16_t nawupf;
+    uint16_t nacwu;
+    uint16_t nabsn;
+    uint16_t nabo;
+    uint16_t nabspf;
+    uint16_t unused2;
+    uint64_t nvmcap[2];
+    uint64_t unusued3[5];
+    uint8_t nguid[16];
+    uint8_t eui64[8];
+    struct nvme_lbaf lbaf[16];
+    uint64_t unused3[24];
+    uint8_t vs[3712];
+};
+
+/* NVMe namespace */
+struct nvme_ns {
+    size_t nsid;                /* Namespace ID */
+    size_t lba_bsize;           /* LBA block size */
+    size_t size;                /* Size in logical blocks */
+    struct nvme_queue ioq;      /* I/O queue */
+    struct nvme_lbaf lba_fmt;   /* LBA format */
+    struct nvme_ctrl *ctrl;     /* NVMe controller */
+    TAILQ_ENTRY(nvme_ns) link;
+};
+
 struct nvme_ctrl {
     struct nvme_queue adminq;
+    struct nvme_bar *bar;
+    uint8_t sqes;
+    uint8_t cqes;
 };
 
 #endif  /* !_IC_NVMEVAR_H_ */