[Lkw] [PATCH 5/6] part-7

[Emmanuel Arias]

Signed-off-by: Emmanuel Arias <eamanu at riseup.net>
---
 drivers/block/nvme-cmb.c | 69 +++++++++++++++++++++++++++++++++++++++-
 1 file changed, 68 insertions(+), 1 deletion(-)

diff --git a/drivers/block/nvme-cmb.c b/drivers/block/nvme-cmb.c
index 04053ef74..324e23327 100644
--- a/drivers/block/nvme-cmb.c
+++ b/drivers/block/nvme-cmb.c
@@ -1,5 +1,6 @@
-#include <linux/module.h>
+#include <linux/delay.h>
 #include <linux/init.h>
+#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
 #include <linux/nvme.h>
@@ -9,11 +10,16 @@ MODULE_LICENSE("GPL");
 
 struct nvme_cmb_dev {
 	void __iomem *bar;
+	void __iomem *buffer_bar;
+	size_t buffer_offset;
+	size_t buffer_size;
 
 	struct nvme_command *admin_sqes;
 	dma_addr_t admin_sq_dma_addr;
 	struct nvme_completion *admin_cqes;
 	dma_addr_t admin_cq_dma_addr;
+
+	u32 cc;
 };
 
 #define QUEUE_SIZE 2
@@ -55,6 +61,54 @@ static void nvme_cmb_free_queues(struct nvme_cmb_dev *dev, struct pci_dev *pdev)
 			  dev->admin_sqes, dev->admin_sq_dma_addr);
 }
 
+static void nvme_cmb_enable(struct nvme_cmb_dev *dev)
+{
+	dev->cc = NVME_CC_CSS_NVM | NVME_CC_AMS_RR |
+	    NVME_CC_IOSQES | NVME_CC_IOCQES;
+	writel(dev->cc, dev->bar + NVME_REG_CC);
+
+	dev->cc |= NVME_CC_ENABLE;
+	writel(dev->cc, dev->bar + NVME_REG_CC);
+	while (true) {
+		u32 csts = readl(dev->bar + NVME_REG_CSTS);
+
+		if (csts & NVME_CSTS_RDY)
+			break;
+		usleep_range(1000, 2000);
+	}
+}
+
+static void nvme_cmb_shutdown(struct nvme_cmb_dev *dev)
+{
+	dev->cc &= ~NVME_CC_SHN_MASK;
+	dev->cc |= NVME_CC_SHN_NORMAL;
+	writel(dev->cc, dev->bar + NVME_REG_CC);
+	while (true) {
+		u32 csts = readl(dev->bar + NVME_REG_CSTS);
+
+		if ((csts & NVME_CSTS_SHST_MASK) == NVME_CSTS_SHST_CMPLT)
+			break;
+		usleep_range(1000, 2000);
+	}
+}
+
+static int nvme_cmb_map_buffer(struct nvme_cmb_dev *dev, struct pci_dev *pdev)
+{
+	u32 cmbloc = readl(dev->bar + NVME_REG_CMBLOC);
+	u32 cmbsz = readl(dev->bar + NVME_REG_CMBSZ);
+	u64 unit = 1ULL << (12 + 4 *
+			    ((cmbsz >> NVME_CMBSZ_SZU_SHIFT) &
+			     NVME_CMBSZ_SZU_MASK));
+
+	dev->buffer_bar = pci_ioremap_bar(pdev, NVME_CMB_BIR(cmbloc));
+	if (!dev->buffer_bar)
+		return -ENOMEM;
+	dev->buffer_offset = unit * NVME_CMB_OFST(cmbloc);
+	dev->buffer_size =
+	    unit * ((cmbsz >> NVME_CMBSZ_SZ_SHIFT) & NVME_CMBSZ_SZ_MASK);
+	return 0;
+}
+
 static int nvme_cmb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct nvme_cmb_dev *dev;
@@ -89,9 +143,20 @@ static int nvme_cmb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (error)
 		goto out_iounmap;
 
+	nvme_cmb_enable(dev);
+
+	error = nvme_cmb_map_buffer(dev, pdev);
+	if (error)
+		goto out_shutdown;
+
 	dev_set_drvdata(&pdev->dev, dev);
+	dev_info(&pdev->dev, "added %zu MiB ramdisk\n",
+		 dev->buffer_size / SZ_1M);
 	return 0;
 
+out_shutdown:
+	nvme_cmb_shutdown(dev);
+	nvme_cmb_free_queues(dev, pdev);
 out_iounmap:
 	iounmap(dev->bar);
 out_release_regions:
@@ -109,6 +174,8 @@ static void nvme_cmb_remove(struct pci_dev *pdev)
 
 	dev_info(&pdev->dev, "unbinding NVMe device\n");
 
+	iounmap(dev->buffer_bar);
+	nvme_cmb_shutdown(dev);
 	nvme_cmb_free_queues(dev, pdev);
 	iounmap(dev->bar);
 	pci_release_mem_regions(pdev);
-- 
2.39.2