Merge branch 'for-next/virtio'

3 files changed, 140 insertions, 0 deletions

diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
new file mode 100644
index 0000000000..b42571eca5
--- /dev/null
+++ b/drivers/block/Kconfig
@@ -0,0 +1,6 @@
+config VIRTIO_BLK
+        bool "Virtio block driver"
+        depends on VIRTIO
+        help
+          This is the virtual block driver for virtio.  It can be used with
+          QEMU based VMMs (like KVM or Xen).
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
index 8812c0faec..23d634f006 100644
--- a/drivers/block/Makefile
+++ b/drivers/block/Makefile
@@ -1 +1,2 @@
 obj-$(CONFIG_EFI_BOOTUP) += efi-block-io.o
+obj-$(CONFIG_VIRTIO_BLK) += virtio_blk.o
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
new file mode 100644
index 0000000000..b7a83cf686
--- /dev/null
+++ b/drivers/block/virtio_blk.c
@@ -0,0 +1,133 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2018, Tuomas Tynkkynen <tuomas.tynkkynen@iki.fi>
+ * Copyright (C) 2018, Bin Meng <bmeng.cn@gmail.com>
+ */
+
+#include <common.h>
+#include <driver.h>
+#include <block.h>
+#include <disks.h>
+#include <linux/virtio_types.h>
+#include <linux/virtio.h>
+#include <linux/virtio_ring.h>
+#include <uapi/linux/virtio_blk.h>
+
+struct virtio_blk_priv {
+	struct virtqueue *vq;
+	struct virtio_device *vdev;
+	struct block_device blk;
+};
+
+static int virtio_blk_do_req(struct virtio_blk_priv *priv, void *buffer,
+			     sector_t sector, blkcnt_t blkcnt, u32 type)
+{
+	unsigned int num_out = 0, num_in = 0;
+	struct virtio_sg *sgs[3];
+	u8 status;
+	int ret;
+
+	struct virtio_blk_outhdr out_hdr = {
+		.type = cpu_to_virtio32(priv->vdev, type),
+		.sector = cpu_to_virtio64(priv->vdev, sector),
+	};
+	struct virtio_sg hdr_sg = { &out_hdr, sizeof(out_hdr) };
+	struct virtio_sg data_sg = { buffer, blkcnt * 512 };
+	struct virtio_sg status_sg = { &status, sizeof(status) };
+
+	sgs[num_out++] = &hdr_sg;
+
+	switch(type) {
+	case VIRTIO_BLK_T_OUT:
+		sgs[num_out++] = &data_sg;
+		break;
+	case VIRTIO_BLK_T_IN:
+		sgs[num_out + num_in++] = &data_sg;
+		break;
+	}
+
+	sgs[num_out + num_in++] = &status_sg;
+
+	ret = virtqueue_add(priv->vq, sgs, num_out, num_in);
+	if (ret)
+		return ret;
+
+	virtqueue_kick(priv->vq);
+
+	while (!virtqueue_get_buf(priv->vq, NULL))
+		;
+
+	return status == VIRTIO_BLK_S_OK ? 0 : -EIO;
+}
+
+static int virtio_blk_read(struct block_device *blk, void *buffer,
+			   sector_t start, blkcnt_t blkcnt)
+{
+	struct virtio_blk_priv *priv = container_of(blk, struct virtio_blk_priv, blk);
+	return virtio_blk_do_req(priv, buffer, start, blkcnt,
+				 VIRTIO_BLK_T_IN);
+}
+
+static int virtio_blk_write(struct block_device *blk, const void *buffer,
+			    sector_t start, blkcnt_t blkcnt)
+{
+	struct virtio_blk_priv *priv = container_of(blk, struct virtio_blk_priv, blk);
+	return virtio_blk_do_req(priv, (void *)buffer, start, blkcnt,
+				 VIRTIO_BLK_T_OUT);
+}
+
+static struct block_device_ops virtio_blk_ops = {
+	.read	= virtio_blk_read,
+	.write	= virtio_blk_write,
+};
+
+static int virtio_blk_probe(struct virtio_device *vdev)
+{
+	struct virtio_blk_priv *priv;
+	u64 cap;
+	int devnum;
+	int ret;
+
+	priv = xzalloc(sizeof(*priv));
+
+	ret = virtio_find_vqs(vdev, 1, &priv->vq);
+	if (ret)
+		return ret;
+
+	priv->vdev = vdev;
+
+	devnum = cdev_find_free_index("virtioblk");
+	priv->blk.cdev.name = xasprintf("virtioblk%d", devnum);
+	priv->blk.dev = &vdev->dev;
+	priv->blk.blockbits = SECTOR_SHIFT;
+	virtio_cread(vdev, struct virtio_blk_config, capacity, &cap);
+	priv->blk.num_blocks = cap;
+	priv->blk.ops = &virtio_blk_ops;
+
+	ret = blockdevice_register(&priv->blk);
+	if (ret)
+		return ret;
+
+	parse_partition_table(&priv->blk);
+
+	return 0;
+}
+
+static void virtio_blk_remove(struct virtio_device *vdev)
+{
+	vdev->config->reset(vdev);
+	vdev->config->del_vqs(vdev);
+}
+
+static const struct virtio_device_id id_table[] = {
+        { VIRTIO_ID_BLOCK, VIRTIO_DEV_ANY_ID },
+        { 0 },
+};
+
+static struct virtio_driver virtio_blk = {
+        .driver.name	= "virtio_blk",
+        .id_table	= id_table,
+        .probe		= virtio_blk_probe,
+	.remove		= virtio_blk_remove,
+};
+device_virtio_driver(virtio_blk);