i2c: add virtual i2c driver

add virtual i2c driver for virtualization platform.

Change-Id: I5aafa3b9db1e06d990b25a393f54f8763e73f9aa
Signed-off-by: xianzhu <xianzhu@codeaurora.org>

drivers/i2c/busses/Kconfig

@@ -1209,5 +1209,11 @@ config I2C_MSM_V2
           This driver can also be built as a module.  If so, the module
           will be called i2c-msm-v2.
 
+config VIRTIO_I2C
+        tristate "VIRTIO_I2C"
+	depends on VIRTIO
+        help
+          If you say yes to this option, the virtio i2c will be
+	  supported.
 
 endmenu

drivers/i2c/busses/Makefile

@@ -97,6 +97,7 @@ obj-$(CONFIG_I2C_XLR)		+= i2c-xlr.o
 obj-$(CONFIG_I2C_XLP9XX)	+= i2c-xlp9xx.o
 obj-$(CONFIG_I2C_RCAR)		+= i2c-rcar.o
 obj-$(CONFIG_I2C_MSM_V2)        += i2c-msm-v2.o
+obj-$(CONFIG_VIRTIO_I2C)	+= virtio-i2c.o
 
 # External I2C/SMBus adapter drivers
 obj-$(CONFIG_I2C_DIOLAN_U2C)	+= i2c-diolan-u2c.o

drivers/i2c/busses/virtio-i2c.c

@@ -0,0 +1,356 @@
+/* Copyright (c) 2019, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/idr.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/spinlock.h>
+#include <linux/virtio.h>
+#include <uapi/linux/virtio_ids.h>
+#include <linux/virtio_config.h>
+#include <linux/i2c.h>
+
+#define I2C_ADAPTER_NR	0x00
+
+#define I2C_VIRTIO_RD		0x01
+#define I2C_VIRTIO_WR		0x02
+#define I2C_VIRTIO_RDWR		0x03
+
+/**
+ * struct virtio_i2c - virtio i2c device
+ * @adapter: i2c adapter
+ * @vdev: the virtio device
+ * @vq: i2c virtqueue
+ */
+struct virtio_i2c {
+	struct i2c_adapter adapter;
+	struct virtio_device *vdev;
+	struct virtqueue *vq;
+	wait_queue_head_t inq;
+};
+
+struct i2c_transfer_head {
+	u32 type; /* read or write from or to slave */
+	u32 addr; /* slave addr */
+	u32 length; /* buffer length */
+	u32 total_length; /* merge write and read will use this segment */
+};
+
+struct i2c_transfer_end {
+	u32 result; /* return value from backend */
+};
+
+struct virtio_i2c_req {
+	struct i2c_transfer_head head;
+	char   *buf;
+	struct i2c_transfer_end  end;
+};
+
+static int virti2c_transfer(struct virtio_i2c *vi2c,
+	struct virtio_i2c_req *i2c_req)
+{
+	struct virtqueue *vq = vi2c->vq;
+	struct scatterlist outhdr, bufhdr, inhdr, *sgs[3];
+	unsigned int num_out = 0, num_in = 0, err, len;
+	struct virtio_i2c_req *req_handled = NULL;
+
+	/* send the head queue to the backend */
+	sg_init_one(&outhdr, &i2c_req->head, sizeof(i2c_req->head));
+	sgs[num_out++] = &outhdr;
+
+	/* send the buffer queue to the backend */
+	sg_init_one(&bufhdr, i2c_req->buf,
+		(i2c_req->head.type == I2C_VIRTIO_RDWR) ?
+			i2c_req->head.total_length : i2c_req->head.length);
+	if (i2c_req->head.type & I2C_VIRTIO_WR)
+		sgs[num_out++] = &bufhdr;
+	else
+		sgs[num_out + num_in++] = &bufhdr;
+
+	/* send the result queue to the backend */
+	sg_init_one(&inhdr, &i2c_req->end, sizeof(i2c_req->end));
+	sgs[num_out + num_in++] = &inhdr;
+
+	/* call the virtqueue function */
+	err = virtqueue_add_sgs(vq, sgs, num_out, num_in, i2c_req, GFP_KERNEL);
+	if (err)
+		goto req_exit;
+
+	/* Tell Host to go! */
+	err = virtqueue_kick(vq);
+
+	wait_event(vi2c->inq,
+		(req_handled = virtqueue_get_buf(vq, &len)));
+
+	if (i2c_req->head.type == I2C_VIRTIO_RDWR) {
+		if (i2c_req->end.result ==
+			i2c_req->head.total_length - i2c_req->head.length)
+			err = 0;
+		else
+			err = -EINVAL;
+	} else {
+		if (i2c_req->end.result == i2c_req->head.length)
+			err = 0;
+		else
+			err = -EINVAL;
+	}
+req_exit:
+	return err;
+}
+
+/* prepare the transfer req */
+static struct virtio_i2c_req *virti2c_transfer_prepare(struct i2c_msg *msg_1,
+						struct i2c_msg *msg_2)
+{
+	char *ptr  = NULL;
+	int merge  = 0;
+	struct virtio_i2c_req *i2c_req;
+
+	if (msg_1 == NULL)
+		return NULL;
+
+	if (msg_2)
+		merge = 1;
+
+	i2c_req = kzalloc(sizeof(struct virtio_i2c_req), GFP_KERNEL);
+	if (i2c_req == NULL)
+		return NULL;
+
+	if (merge)
+		ptr = kzalloc((msg_1->len + msg_2->len), GFP_KERNEL);
+	else
+		ptr = msg_1->buf;
+	if (ptr == NULL)
+		goto err_mem;
+
+	/* prepare the head */
+	i2c_req->head.type = merge ?
+		I2C_VIRTIO_RDWR : ((msg_1->flags & I2C_M_RD) ?
+					I2C_VIRTIO_RD : I2C_VIRTIO_WR);
+	i2c_req->head.addr = msg_1->addr;
+	i2c_req->head.length = msg_1->len;
+	if (merge)
+		i2c_req->head.total_length = msg_1->len + msg_2->len;
+
+	/* prepare the buf */
+	if (merge)
+		memcpy(ptr, msg_1->buf, msg_1->len);
+	i2c_req->buf = ptr;
+
+	return i2c_req;
+err_mem:
+	kfree(i2c_req);
+	i2c_req = NULL;
+	return NULL;
+}
+
+static void virti2c_transfer_end(struct virtio_i2c_req *req,
+						struct i2c_msg *msg)
+{
+	if (req->head.type == I2C_VIRTIO_RDWR) {
+		memcpy(msg->buf, req->buf + req->head.length, msg->len);
+		kfree(req->buf);
+		req->buf = NULL;
+	}
+
+	kfree(req);
+	req = NULL;
+}
+
+static int virtio_i2c_master_xfer(struct i2c_adapter *adap,
+				struct i2c_msg *msgs, int num)
+{
+	int i, ret;
+	struct virtio_i2c_req *i2c_req;
+	struct virtio_i2c *vi2c = i2c_get_adapdata(adap);
+
+	if (num < 1) {
+		dev_err(&vi2c->vdev->dev,
+		"error on number of msgs(%d) received\n", num);
+		return -EINVAL;
+	}
+
+	if (IS_ERR_OR_NULL(msgs)) {
+		dev_err(&vi2c->vdev->dev, " error no msgs Accessing invalid pointer location\n");
+		return PTR_ERR(msgs);
+	}
+
+	for (i = 0; i < num; i++) {
+
+		if (msgs[i].flags & I2C_M_RD) {
+			/* read the data from slave to master*/
+			i2c_req = virti2c_transfer_prepare(&msgs[i], NULL);
+
+		} else if ((i + 1 < num) && (msgs[i + 1].flags & I2C_M_RD) &&
+				(msgs[i].addr == msgs[i + 1].addr)) {
+			/* write then read from same address*/
+			i2c_req = virti2c_transfer_prepare(&msgs[i],
+								&msgs[i+1]);
+			i += 1;
+
+		} else {
+			/* write the data to slave */
+			i2c_req = virti2c_transfer_prepare(&msgs[i], NULL);
+		}
+
+		if (i2c_req == NULL) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		ret = virti2c_transfer(vi2c, i2c_req);
+		virti2c_transfer_end(i2c_req, &msgs[i]);
+		if (ret)
+			goto err;
+	}
+	return num;
+err:
+	return ret;
+}
+
+static u32 virtio_i2c_functionality(struct i2c_adapter *adapter)
+{
+	return  I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static struct i2c_algorithm virtio_i2c_algorithm = {
+	.master_xfer = virtio_i2c_master_xfer,
+	.functionality = virtio_i2c_functionality,
+};
+
+/* virtqueue incoming data interrupt IRQ */
+static void virti2c_vq_isr(struct virtqueue *vq)
+{
+	struct virtio_i2c *vi2c = vq->vdev->priv;
+
+	wake_up(&vi2c->inq);
+}
+
+static int virti2c_init_vqs(struct virtio_i2c *vi2c)
+{
+	struct virtqueue *vqs[1];
+	vq_callback_t *cbs[] = { virti2c_vq_isr };
+	static const char * const names[] = { "virti2c_vq_isr" };
+	int err;
+
+	err = vi2c->vdev->config->find_vqs(vi2c->vdev, 1, vqs, cbs, names);
+	if (err)
+		return err;
+	vi2c->vq = vqs[0];
+
+	return 0;
+}
+
+static void virti2c_del_vqs(struct virtio_i2c *vi2c)
+{
+	vi2c->vdev->config->del_vqs(vi2c->vdev);
+}
+
+static int virti2c_init_hw(struct virtio_device *vdev,
+				struct virtio_i2c *vi2c)
+{
+	int err;
+
+	i2c_set_adapdata(&vi2c->adapter, vi2c);
+	vi2c->adapter.algo = &virtio_i2c_algorithm;
+
+	vi2c->adapter.owner = THIS_MODULE;
+	vi2c->adapter.dev.parent = &vdev->dev;
+	vi2c->adapter.dev.of_node = vdev->dev.parent->of_node;
+
+	/* read virtio i2c config info */
+	vi2c->adapter.nr = virtio_cread32(vdev, I2C_ADAPTER_NR);
+	snprintf(vi2c->adapter.name, sizeof(vi2c->adapter.name),
+				"virtio_i2c_%d", vi2c->adapter.nr);
+
+	err = i2c_add_numbered_adapter(&vi2c->adapter);
+	if (err)
+		return err;
+	return 0;
+}
+
+static int virti2c_probe(struct virtio_device *vdev)
+{
+	struct virtio_i2c *vi2c;
+	int err = 0;
+
+	if (!virtio_has_feature(vdev, VIRTIO_F_VERSION_1))
+		return -ENODEV;
+
+	vi2c = kzalloc(sizeof(*vi2c), GFP_KERNEL);
+	if (!vi2c)
+		return -ENOMEM;
+
+	vi2c->vdev = vdev;
+	vdev->priv = vi2c;
+	init_waitqueue_head(&vi2c->inq);
+
+	err = virti2c_init_vqs(vi2c);
+	if (err)
+		goto err_init_vq;
+
+	err = virti2c_init_hw(vdev, vi2c);
+	if (err)
+		goto err_init_hw;
+
+	virtio_device_ready(vdev);
+
+	virtqueue_enable_cb(vi2c->vq);
+	return 0;
+
+err_init_hw:
+	virti2c_del_vqs(vi2c);
+err_init_vq:
+	kfree(vi2c);
+	return err;
+}
+static void virti2c_remove(struct virtio_device *vdev)
+{
+	struct virtio_i2c *vi2c = vdev->priv;
+
+	i2c_del_adapter(&vi2c->adapter);
+	vdev->config->reset(vdev);
+	virti2c_del_vqs(vi2c);
+	kfree(vi2c);
+}
+
+static unsigned int features[] = {
+	/* none */
+};
+static struct virtio_device_id id_table[] = {
+	{ VIRTIO_ID_I2C, VIRTIO_DEV_ANY_ID },
+	{ 0 },
+};
+
+static struct virtio_driver virtio_i2c_driver = {
+	.driver.name		= KBUILD_MODNAME,
+	.driver.owner		= THIS_MODULE,
+	.feature_table		= features,
+	.feature_table_size	= ARRAY_SIZE(features),
+	.id_table		= id_table,
+	.probe			= virti2c_probe,
+	.remove			= virti2c_remove,
+};
+
+module_virtio_driver(virtio_i2c_driver);
+MODULE_DEVICE_TABLE(virtio, id_table);
+
+MODULE_DESCRIPTION("Virtio i2c frontend driver");
+MODULE_LICENSE("GPL v2");

include/uapi/linux/virtio_ids.h

@@ -43,4 +43,6 @@
 #define VIRTIO_ID_INPUT        18 /* virtio input */
 #define VIRTIO_ID_VSOCK        19 /* virtio vsock transport */
 
+#define VIRTIO_ID_I2C		32 /* virtio i2c */
+
 #endif /* _LINUX_VIRTIO_IDS_H */