From ba7592123dd984718567c3615f698c03b948caf1 Mon Sep 17 00:00:00 2001
From: Wendy Liang <jliang@xilinx.com>
Date: Wed, 3 Jan 2018 16:04:01 -0800
Subject: [LINUX PATCH] rpmsg: virtio rpmsg: Add RPMsg char driver support

virtio rpmsg was not implemented to use RPMsg char driver.
Each virtio ns announcement will create a new RPMsg device which
is supposed to bound to a RPMsg driver. It doesn't support
dynamic endpoints with name service per RPMsg device.
With RPMsg char driver, you can have multiple endpoints per
RPMsg device.

Here is the change from this patch:
* Introduce a macro to indicate if want to use RPMsg char driver
  for virtio RPMsg. The RPMsg device can either be bounded to
  a simple RPMsg driver or the RPMsg char driver.
* Create Virtio RPMsg char device when the virtio RPMsg driver is
  probed.
* when there is a remote service announced, keep it in the virtio
  proc remote services list.
* when there is an endpoint created, bind it to a remote service
  from the remote services list. If the service doesn't exist yet,
  create one and mark the service address as ANY.

Signed-off-by: Wendy Liang <jliang@xilinx.com>
---
 drivers/rpmsg/Kconfig            |   8 +
 drivers/rpmsg/virtio_rpmsg_bus.c | 350 ++++++++++++++++++++++++++++++++++++++-
 2 files changed, 351 insertions(+), 7 deletions(-)

diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig
index 0fe6eac..104060c 100644
--- a/drivers/rpmsg/Kconfig
+++ b/drivers/rpmsg/Kconfig
@@ -51,4 +51,12 @@ config RPMSG_VIRTIO
 	select RPMSG
 	select VIRTIO
 
+config RPMSG_VIRTIO_CHAR
+	bool "Enable Virtio RPMSG char device driver support"
+	default y
+	depends on RPMSG_VIRTIO
+	depends on RPMSG_CHAR
+	help
+	  Say y here to enable to use RPMSG char device interface.
+
 endmenu
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 82b8300..f1f9000 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -56,6 +56,7 @@
  * @sendq:	wait queue of sending contexts waiting for a tx buffers
  * @sleepers:	number of senders that are waiting for a tx buffer
  * @ns_ept:	the bus's name service endpoint
+ * @rsvcs:	remote services
  *
  * This structure stores the rpmsg state of a given virtio remote processor
  * device (there might be several virtio proc devices for each physical
@@ -75,6 +76,9 @@ struct virtproc_info {
 	wait_queue_head_t sendq;
 	atomic_t sleepers;
 	struct rpmsg_endpoint *ns_ept;
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct list_head rsvcs;
+#endif
 };
 
 /* The feature bitmap for virtio rpmsg */
@@ -141,6 +145,36 @@ struct virtio_rpmsg_channel {
 #define to_virtio_rpmsg_channel(_rpdev) \
 	container_of(_rpdev, struct virtio_rpmsg_channel, rpdev)
 
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+/**
+ * struct virtio_rpmsg_rsvc - virtio RPMsg remote service
+ * @name: name of the RPMsg remote service
+ * @addr: RPMsg address of the remote service
+ * @ept:  local endpoint bound to the remote service
+ * @node: list node
+ */
+struct virtio_rpmsg_rsvc {
+	char name[RPMSG_NAME_SIZE];
+	u32 addr;
+	struct rpmsg_endpoint *ept;
+	struct list_head node;
+};
+
+/**
+ * struct virtio_rpmsg_ept - virtio RPMsg endpoint
+ * @rsvc: RPMsg service
+ * @ept:  RPMsg endpoint
+ *
+ */
+struct virtio_rpmsg_ept {
+	struct virtio_rpmsg_rsvc *rsvc;
+	struct rpmsg_endpoint ept;
+};
+
+#define to_virtio_rpmsg_ept(_ept) \
+	container_of(_ept, struct virtio_rpmsg_ept, ept)
+#endif
+
 /*
  * We're allocating buffers of 512 bytes each for communications. The
  * number of buffers will be computed from the number of buffers supported
@@ -216,6 +250,180 @@ rpmsg_sg_init(struct scatterlist *sg, void *cpu_addr, unsigned int len)
 	}
 }
 
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+/**
+ * virtio_rpmsg_find_rsv - find the announced remote service
+ * @vrp: virtio remote proc information
+ * @name: remote service name
+ * @addr: remote service address
+ *
+ * The caller is supposed to have mutex lock before calling this function
+ *
+ * return NULL if no remote service has been found, otherwise, return
+ * the remote service pointer.
+ */
+static struct virtio_rpmsg_rsvc *
+virtio_rpmsg_find_rsvc(struct virtproc_info *vrp, char *name, u32 addr)
+{
+	struct virtio_rpmsg_rsvc *rsvc;
+
+	list_for_each_entry(rsvc, &vrp->rsvcs, node) {
+		if (!strncmp(rsvc->name, name, RPMSG_NAME_SIZE)) {
+			if (addr == RPMSG_ADDR_ANY || addr == rsvc->addr)
+				/* remote service has found */
+				return rsvc;
+		}
+	}
+
+	return NULL;
+}
+
+/**
+ * virtio_rpmsg_create_rsvc - create remote service
+ * @vrp: virtio remote proc information
+ * @name: remote service name
+ * @addr: remote service address
+ *
+ * The caller is supposed to have mutex lock before calling this function
+ *
+ * return NULL if remote service creation failed. otherwise, return
+ * the remote service pointer.
+ */
+static struct virtio_rpmsg_rsvc *
+virtio_rpmsg_create_rsvc(struct virtproc_info *vrp, char *name, u32 addr)
+{
+	struct virtio_rpmsg_rsvc *rsvc;
+	struct virtio_device *vdev = vrp->vdev;
+
+	rsvc = virtio_rpmsg_find_rsvc(vrp, name, addr);
+	if (rsvc)
+		return rsvc;
+	rsvc = kzalloc(sizeof(*rsvc), GFP_KERNEL);
+	if (!rsvc)
+		return NULL;
+	rsvc->addr = addr;
+	strncpy(rsvc->name, name, RPMSG_NAME_SIZE);
+	list_add_tail(&rsvc->node, &vrp->rsvcs);
+	dev_info(&vdev->dev, "service %s, %d is created.\n",
+		 name, rsvc->addr);
+	return rsvc;
+}
+
+/**
+ * virtio_rpmsg_remove_rsvc - remove remote service
+ * @vrp: virtio remote proc information
+ * @name: remote service name
+ * @addr: service address
+ *
+ */
+static void
+virtio_rpmsg_remove_rsvc(struct virtproc_info *vrp, char *name, u32 addr)
+{
+	struct virtio_rpmsg_rsvc *rsvc;
+	struct rpmsg_endpoint *ept;
+	struct virtio_rpmsg_ept *vept;
+
+	list_for_each_entry(rsvc, &vrp->rsvcs, node) {
+		if (!strncmp(rsvc->name, name, RPMSG_NAME_SIZE) &&
+		    addr == rsvc->addr) {
+			/* remote service has found, no need to
+			 * create
+			 */
+			ept = rsvc->ept;
+			if (ept) {
+				vept = to_virtio_rpmsg_ept(ept);
+				vept->rsvc = NULL;
+			}
+			list_del(&rsvc->node);
+			kfree(rsvc);
+			break;
+		}
+	}
+}
+
+/**
+ * virtio_rpmsg_announce_ept_create - announce endpoint has been created
+ * @ept: RPMsg endpoint
+ *
+ * return 0 if succeeded, otherwise, return error code.
+ */
+static int virtio_rpmsg_announce_ept_create(struct rpmsg_endpoint *ept)
+{
+	struct virtio_rpmsg_ept *vept = to_virtio_rpmsg_ept(ept);
+	struct virtio_rpmsg_rsvc *rsvc = vept->rsvc;
+	struct rpmsg_device *rpdev = ept->rpdev;
+	struct virtio_rpmsg_channel *vch;
+	struct virtproc_info *vrp;
+	struct device *dev;
+	int err = 0;
+
+	if (!rpdev)
+		/* If the endpoint is not connected to a RPMsg device,
+		 * no need to send the announcement.
+		 */
+		return 0;
+	vch = to_virtio_rpmsg_channel(rpdev);
+	vrp = vch->vrp;
+	dev = &ept->rpdev->dev;
+	/* need to tell remote processor's name service about this channel ? */
+	if (virtio_has_feature(vrp->vdev, VIRTIO_RPMSG_F_NS)) {
+		struct rpmsg_ns_msg nsm;
+
+		strncpy(nsm.name, rsvc->name, RPMSG_NAME_SIZE);
+		nsm.addr = ept->addr;
+		nsm.flags = RPMSG_NS_CREATE;
+
+		err = rpmsg_sendto(ept, &nsm, sizeof(nsm),
+				   RPMSG_NS_ADDR);
+		if (err)
+			dev_err(dev, "failed to announce service %d\n", err);
+	}
+
+	return err;
+}
+
+/**
+ * virtio_rpmsg_announce_ept_destroy - announce endpoint has been destroyed
+ * @ept: RPMsg endpoint
+ *
+ * return 0 if succeeded, otherwise, return error code.
+ */
+static int virtio_rpmsg_announce_ept_destroy(struct rpmsg_endpoint *ept)
+{
+	struct virtio_rpmsg_ept *vept = to_virtio_rpmsg_ept(ept);
+	struct virtio_rpmsg_rsvc *rsvc = vept->rsvc;
+	struct rpmsg_device *rpdev = ept->rpdev;
+	struct virtio_rpmsg_channel *vch;
+	struct virtproc_info *vrp;
+	struct device *dev;
+	int err = 0;
+
+	if (!rpdev)
+		/* If the endpoint is not connected to a RPMsg device,
+		 * no need to send the announcement.
+		 */
+		return 0;
+	vch = to_virtio_rpmsg_channel(rpdev);
+	vrp = vch->vrp;
+	dev = &ept->rpdev->dev;
+	/* tell remote processor's name service we're removing this channel */
+	if (virtio_has_feature(vrp->vdev, VIRTIO_RPMSG_F_NS)) {
+		struct rpmsg_ns_msg nsm;
+
+		strncpy(nsm.name, rsvc->name, RPMSG_NAME_SIZE);
+		nsm.addr = ept->addr;
+		nsm.flags = RPMSG_NS_DESTROY;
+
+		err = rpmsg_sendto(ept, &nsm, sizeof(nsm),
+				   RPMSG_NS_ADDR);
+		if (err)
+			dev_err(dev, "failed to announce service %d\n", err);
+	}
+
+	return err;
+}
+#endif
+
 /**
  * __ept_release() - deallocate an rpmsg endpoint
  * @kref: the ept's reference count
@@ -229,26 +437,53 @@ static void __ept_release(struct kref *kref)
 {
 	struct rpmsg_endpoint *ept = container_of(kref, struct rpmsg_endpoint,
 						  refcount);
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct virtio_rpmsg_ept *vept = to_virtio_rpmsg_ept(ept);
+	struct virtio_rpmsg_channel *vch;
+	struct virtproc_info *vrp;
+
+	if (ept->rpdev) {
+		vch = to_virtio_rpmsg_channel(ept->rpdev);
+		vrp = vch->vrp;
+		(void)virtio_rpmsg_announce_ept_destroy(ept);
+		mutex_lock(&vrp->endpoints_lock);
+		vept->rsvc->ept = NULL;
+		mutex_unlock(&vrp->endpoints_lock);
+	}
+	kfree(vept);
+#else
 	/*
 	 * At this point no one holds a reference to ept anymore,
 	 * so we can directly free it
 	 */
 	kfree(ept);
+#endif
 }
 
 /* for more info, see below documentation of rpmsg_create_ept() */
 static struct rpmsg_endpoint *__rpmsg_create_ept(struct virtproc_info *vrp,
 						 struct rpmsg_device *rpdev,
 						 rpmsg_rx_cb_t cb,
-						 void *priv, u32 addr)
+						 void *priv,
+						 struct rpmsg_channel_info *ch)
 {
 	int id_min, id_max, id;
 	struct rpmsg_endpoint *ept;
+	u32 addr = ch->src;
 	struct device *dev = rpdev ? &rpdev->dev : &vrp->vdev->dev;
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct virtio_rpmsg_ept *vept;
+	struct virtio_rpmsg_rsvc *rsvc;
 
+	vept = kzalloc(sizeof(*vept), GFP_KERNEL);
+	if (!vept)
+		return NULL;
+	ept = &vept->ept;
+#else
 	ept = kzalloc(sizeof(*ept), GFP_KERNEL);
 	if (!ept)
 		return NULL;
+#endif
 
 	kref_init(&ept->refcount);
 	mutex_init(&ept->cb_lock);
@@ -259,7 +494,7 @@ static struct rpmsg_endpoint *__rpmsg_create_ept(struct virtproc_info *vrp,
 	ept->ops = &virtio_endpoint_ops;
 
 	/* do we need to allocate a local address ? */
-	if (addr == RPMSG_ADDR_ANY) {
+	if (ch->src == RPMSG_ADDR_ANY) {
 		id_min = RPMSG_RESERVED_ADDRESSES;
 		id_max = 0;
 	} else {
@@ -277,6 +512,20 @@ static struct rpmsg_endpoint *__rpmsg_create_ept(struct virtproc_info *vrp,
 	}
 	ept->addr = id;
 
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	if (ept->addr != RPMSG_NS_ADDR) {
+		rsvc = virtio_rpmsg_create_rsvc(vrp, ch->name, ch->dst);
+		if (!rsvc)
+			goto free_ept;
+		vept->rsvc = rsvc;
+		rsvc->ept = ept;
+		if (ch->dst == RPMSG_ADDR_ANY)
+			(void)virtio_rpmsg_announce_ept_create(ept);
+		dev_info(&vrp->vdev->dev, "RPMsg ept created, %s:%d,%d.\n",
+			 ch->name, ept->addr, rsvc->addr);
+	}
+#endif
+
 	mutex_unlock(&vrp->endpoints_lock);
 
 	return ept;
@@ -294,7 +543,7 @@ static struct rpmsg_endpoint *virtio_rpmsg_create_ept(struct rpmsg_device *rpdev
 {
 	struct virtio_rpmsg_channel *vch = to_virtio_rpmsg_channel(rpdev);
 
-	return __rpmsg_create_ept(vch->vrp, rpdev, cb, priv, chinfo.src);
+	return __rpmsg_create_ept(vch->vrp, rpdev, cb, priv, &chinfo);
 }
 
 /**
@@ -392,6 +641,7 @@ static void virtio_rpmsg_release_device(struct device *dev)
 	kfree(vch);
 }
 
+#ifndef CONFIG_RPMSG_VIRTIO_CHAR
 /*
  * create an rpmsg channel using its name and address info.
  * this function will be used to create both static and dynamic
@@ -444,6 +694,7 @@ static struct rpmsg_device *rpmsg_create_channel(struct virtproc_info *vrp,
 
 	return rpdev;
 }
+#endif
 
 /* super simple buffer "allocator" that is just enough for now */
 static void *get_a_tx_buf(struct virtproc_info *vrp)
@@ -660,8 +911,21 @@ static int rpmsg_send_offchannel_raw(struct rpmsg_device *rpdev,
 static int virtio_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len)
 {
 	struct rpmsg_device *rpdev = ept->rpdev;
-	u32 src = ept->addr, dst = rpdev->dst;
+	u32 src = ept->addr;
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct virtio_rpmsg_ept *vept = to_virtio_rpmsg_ept(ept);
+	u32 dst = vept->rsvc->addr;
+
+	if (dst == RPMSG_ADDR_ANY)
+		return -EPIPE;
+#else
+	u32 dst = rpdev->dst;
+#endif
 
+	if (!rpdev) {
+		pr_err("%s, ept %p rpdev is NULL\n", __func__, ept);
+		return -EINVAL;
+	}
 	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
 }
 
@@ -685,7 +949,16 @@ static int virtio_rpmsg_send_offchannel(struct rpmsg_endpoint *ept, u32 src,
 static int virtio_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len)
 {
 	struct rpmsg_device *rpdev = ept->rpdev;
-	u32 src = ept->addr, dst = rpdev->dst;
+	u32 src = ept->addr;
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct virtio_rpmsg_ept *vept = to_virtio_rpmsg_ept(ept);
+	u32 dst = vept->rsvc->addr;
+
+	if (dst == RPMSG_ADDR_ANY)
+		return -EPIPE;
+#else
+	u32 dst = rpdev->dst;
+#endif
 
 	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
 }
@@ -824,11 +1097,13 @@ static int rpmsg_ns_cb(struct rpmsg_device *rpdev, void *data, int len,
 		       void *priv, u32 src)
 {
 	struct rpmsg_ns_msg *msg = data;
-	struct rpmsg_device *newch;
 	struct rpmsg_channel_info chinfo;
 	struct virtproc_info *vrp = priv;
 	struct device *dev = &vrp->vdev->dev;
+#ifndef CONFIG_RPMSG_VIRTIO_CHAR
+	struct rpmsg_device *newch;
 	int ret;
+#endif
 
 #if defined(CONFIG_DYNAMIC_DEBUG)
 	dynamic_hex_dump("NS announcement: ", DUMP_PREFIX_NONE, 16, 1,
@@ -863,13 +1138,29 @@ static int rpmsg_ns_cb(struct rpmsg_device *rpdev, void *data, int len,
 	chinfo.dst = msg->addr;
 
 	if (msg->flags & RPMSG_NS_DESTROY) {
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+		mutex_lock(&vrp->endpoints_lock);
+		virtio_rpmsg_remove_rsvc(vrp, chinfo.name, chinfo.dst);
+		mutex_unlock(&vrp->endpoints_lock);
+#else
 		ret = rpmsg_unregister_device(&vrp->vdev->dev, &chinfo);
 		if (ret)
 			dev_err(dev, "rpmsg_destroy_channel failed: %d\n", ret);
+#endif
 	} else {
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+		struct virtio_rpmsg_rsvc *rsvc;
+
+		mutex_lock(&vrp->endpoints_lock);
+		rsvc = virtio_rpmsg_create_rsvc(vrp, chinfo.name, chinfo.dst);
+		mutex_unlock(&vrp->endpoints_lock);
+		if (!rsvc)
+			dev_err(dev, "virtio_rpmsg_create_rsvc failed\n");
+#else
 		newch = rpmsg_create_channel(vrp, &chinfo);
 		if (!newch)
 			dev_err(dev, "rpmsg_create_channel failed\n");
+#endif
 	}
 
 	return 0;
@@ -885,6 +1176,10 @@ static int rpmsg_probe(struct virtio_device *vdev)
 	int err = 0, i;
 	size_t total_buf_space;
 	bool notify;
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct virtio_rpmsg_channel *vch;
+	struct rpmsg_device *rp_char_dev;
+#endif
 
 	vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
 	if (!vrp)
@@ -956,9 +1251,14 @@ static int rpmsg_probe(struct virtio_device *vdev)
 
 	/* if supported by the remote processor, enable the name service */
 	if (virtio_has_feature(vdev, VIRTIO_RPMSG_F_NS)) {
+		struct rpmsg_channel_info ns_chinfo;
+
+		ns_chinfo.src = RPMSG_NS_ADDR;
+		ns_chinfo.dst = RPMSG_NS_ADDR;
+		strcpy(ns_chinfo.name, "name_service");
 		/* a dedicated endpoint handles the name service msgs */
 		vrp->ns_ept = __rpmsg_create_ept(vrp, NULL, rpmsg_ns_cb,
-						vrp, RPMSG_NS_ADDR);
+						vrp, &ns_chinfo);
 		if (!vrp->ns_ept) {
 			dev_err(&vdev->dev, "failed to create the ns ept\n");
 			err = -ENOMEM;
@@ -966,6 +1266,32 @@ static int rpmsg_probe(struct virtio_device *vdev)
 		}
 	}
 
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	vch = kzalloc(sizeof(*vch), GFP_KERNEL);
+	if (!vch) {
+		err = -ENOMEM;
+		goto free_coherent;
+	}
+
+	/* Link the channel to our vrp */
+	vch->vrp = vrp;
+	/* Initialize remote services list */
+	INIT_LIST_HEAD(&vrp->rsvcs);
+
+	/* Assign public information to the rpmsg_device */
+	rp_char_dev = &vch->rpdev;
+	rp_char_dev->ops = &virtio_rpmsg_ops;
+
+	rp_char_dev->dev.parent = &vrp->vdev->dev;
+	rp_char_dev->dev.release = virtio_rpmsg_release_device;
+	err = rpmsg_chrdev_register_device(rp_char_dev);
+	if (err) {
+		kfree(vch);
+		goto free_coherent;
+	}
+	dev_info(&vdev->dev, "Registered as RPMsg char device.\n");
+#endif
+
 	/*
 	 * Prepare to kick but don't notify yet - we can't do this before
 	 * device is ready.
@@ -1009,6 +1335,9 @@ static void rpmsg_remove(struct virtio_device *vdev)
 	struct virtproc_info *vrp = vdev->priv;
 	size_t total_buf_space = vrp->num_bufs * vrp->buf_size;
 	int ret;
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	struct virtio_rpmsg_rsvc *rsvc, *tmp;
+#endif
 
 	vdev->config->reset(vdev);
 
@@ -1021,6 +1350,13 @@ static void rpmsg_remove(struct virtio_device *vdev)
 
 	idr_destroy(&vrp->endpoints);
 
+#ifdef CONFIG_RPMSG_VIRTIO_CHAR
+	list_for_each_entry_safe(rsvc, tmp, &vrp->rsvcs, node) {
+		list_del(&rsvc->node);
+		kfree(rsvc);
+	}
+#endif
+
 	vdev->config->del_vqs(vrp->vdev);
 
 	dma_free_coherent(vdev->dev.parent->parent, total_buf_space,
-- 
2.7.4

