[RFC PATCH 06/10] rpmsg: virtio: Add support of the VIRTIO_RPMSG_F_FC feature

[Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>]

Introduce the VIRTIO_RPMSG_F_FC feature in charge of the end
point flow control management.
The virtio feature is negotiated. If the remote side supports it,
the rpmsg_fc device is probed.

Signed-off-by: Arnaud Pouliquen <arnaud.pouliquen@foss.st.com>
---
 drivers/rpmsg/Kconfig            |  1 +
 drivers/rpmsg/virtio_rpmsg_bus.c | 32 +++++++++++++++++++++++++++++++-
 2 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig
index c6659f27c617..e39cf32483de 100644
--- a/drivers/rpmsg/Kconfig
+++ b/drivers/rpmsg/Kconfig
@@ -89,6 +89,7 @@ config RPMSG_VIRTIO
 	depends on HAS_DMA
 	select RPMSG
 	select RPMSG_NS
+	select RPMSG_FC
 	select VIRTIO
 
 endmenu
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 785fda77984e..40d2ab86b395 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -19,6 +19,7 @@
 #include <linux/mutex.h>
 #include <linux/rpmsg.h>
 #include <linux/rpmsg/byteorder.h>
+#include <linux/rpmsg/fc.h>
 #include <linux/rpmsg/ns.h>
 #include <linux/scatterlist.h>
 #include <linux/slab.h>
@@ -70,6 +71,7 @@ struct virtproc_info {
 
 /* The feature bitmap for virtio rpmsg */
 #define VIRTIO_RPMSG_F_NS	0 /* RP supports name service notifications */
+#define VIRTIO_RPMSG_F_FC	1 /* RP supports endpoint flow control notifications */
 
 /**
  * struct rpmsg_hdr - common header for all rpmsg messages
@@ -909,7 +911,7 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	struct virtqueue *vqs[2];
 	struct virtproc_info *vrp;
 	struct virtio_rpmsg_channel *vch = NULL;
-	struct rpmsg_device *rpdev_ns, *rpdev_ctrl;
+	struct rpmsg_device *rpdev_ns = NULL, *rpdev_ctrl, *rpdev_fc;
 	void *bufs_va;
 	int err = 0, i;
 	size_t total_buf_space;
@@ -1013,6 +1015,30 @@ static int rpmsg_probe(struct virtio_device *vdev)
 			goto free_ctrldev;
 	}
 
+	/* If supported by the remote processor, enable the flow control service */
+	if (virtio_has_feature(vdev, VIRTIO_RPMSG_F_FC)) {
+		vch = kzalloc(sizeof(*vch), GFP_KERNEL);
+		if (!vch) {
+			err = -ENOMEM;
+			goto free_ns;
+		}
+
+		/* Link the channel to our vrp */
+		vch->vrp = vrp;
+
+		/* Assign public information to the rpmsg_device */
+		rpdev_fc = &vch->rpdev;
+		rpdev_fc->ops = &virtio_rpmsg_ops;
+		rpdev_fc->little_endian = virtio_is_little_endian(vrp->vdev);
+
+		rpdev_fc->dev.parent = &vrp->vdev->dev;
+		rpdev_fc->dev.release = virtio_rpmsg_release_device;
+
+		err = rpmsg_fc_register_device(rpdev_fc);
+		if (err)
+			goto free_ns;
+	}
+
 	/*
 	 * Prepare to kick but don't notify yet - we can't do this before
 	 * device is ready.
@@ -1034,6 +1060,9 @@ static int rpmsg_probe(struct virtio_device *vdev)
 
 	return 0;
 
+free_ns:
+	if (rpdev_ns)
+		device_unregister(&rpdev_ns->dev);
 free_ctrldev:
 	rpmsg_virtio_del_ctrl_dev(rpdev_ctrl);
 free_coherent:
@@ -1082,6 +1111,7 @@ static struct virtio_device_id id_table[] = {
 
 static unsigned int features[] = {
 	VIRTIO_RPMSG_F_NS,
+	VIRTIO_RPMSG_F_FC,
 };
 
 static struct virtio_driver virtio_ipc_driver = {
-- 
2.25.1