[RFC PATCH 14/22] rpmsg: Add VHOST based remote processor messaging bus

[Kishon Vijay Abraham I]

Add a VHOST-based inter-processor communication bus, which enables
kernel drivers to communicate with VIRTIO-based messaging bus,
running on remote processors, over shared memory using a simple
messaging protocol.

Signed-off-by: Kishon Vijay Abraham I <kishon@xxxxxx>
---
 drivers/rpmsg/Kconfig           |   10 +
 drivers/rpmsg/Makefile          |    1 +
 drivers/rpmsg/vhost_rpmsg_bus.c | 1151 +++++++++++++++++++++++++++++++
 include/linux/rpmsg.h           |    1 +
 4 files changed, 1163 insertions(+)
 create mode 100644 drivers/rpmsg/vhost_rpmsg_bus.c

diff --git a/drivers/rpmsg/Kconfig b/drivers/rpmsg/Kconfig
index a9108ff563dc..881712f424d3 100644
--- a/drivers/rpmsg/Kconfig
+++ b/drivers/rpmsg/Kconfig
@@ -64,4 +64,14 @@ config RPMSG_VIRTIO
 	select RPMSG
 	select VIRTIO
 
+config RPMSG_VHOST
+	tristate "Vhost RPMSG bus driver"
+	depends on HAS_DMA
+	select RPMSG
+	select VHOST
+	help
+	  Say y here to enable support for the RPMSG VHOST driver
+	  providing communication channels to remote processors running
+	  RPMSG VIRTIO driver.
+
 endmenu
diff --git a/drivers/rpmsg/Makefile b/drivers/rpmsg/Makefile
index 047acfda518a..44023b0abe9e 100644
--- a/drivers/rpmsg/Makefile
+++ b/drivers/rpmsg/Makefile
@@ -7,3 +7,4 @@ obj-$(CONFIG_RPMSG_QCOM_GLINK_NATIVE) += qcom_glink_native.o
 obj-$(CONFIG_RPMSG_QCOM_GLINK_SMEM) += qcom_glink_smem.o
 obj-$(CONFIG_RPMSG_QCOM_SMD)	+= qcom_smd.o
 obj-$(CONFIG_RPMSG_VIRTIO)	+= virtio_rpmsg_bus.o
+obj-$(CONFIG_RPMSG_VHOST)	+= vhost_rpmsg_bus.o
diff --git a/drivers/rpmsg/vhost_rpmsg_bus.c b/drivers/rpmsg/vhost_rpmsg_bus.c
new file mode 100644
index 000000000000..6bfb5c64c95a
--- /dev/null
+++ b/drivers/rpmsg/vhost_rpmsg_bus.c
@@ -0,0 +1,1151 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Vhost-based remote processor messaging bus
+ *
+ * Based on virtio_rpmsg_bus.c
+ *
+ * Copyright (C) 2020 Texas Instruments
+ * Author: Kishon Vijay Abraham I <kishon@xxxxxx>
+ *
+ */
+
+#define pr_fmt(fmt) "%s: " fmt, __func__
+
+#include <linux/configfs.h>
+#include <linux/idr.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/rpmsg.h>
+#include <linux/vhost.h>
+#include <linux/virtio_ids.h>
+
+#include "rpmsg_internal.h"
+
+/**
+ * struct virtproc_info - virtual remote processor state
+ * @vdev:	the virtio device
+ * @rvq:	rx vhost_virtqueue
+ * @svq:	tx vhost_virtqueue
+ * @buf_size:   size of one rx or tx buffer
+ * @tx_lock:	protects svq, sbufs and sleepers, to allow concurrent senders.
+ *		sending a message might require waking up a dozing remote
+ *		processor, which involves sleeping, hence the mutex.
+ * @endpoints:	idr of local endpoints, allows fast retrieval
+ * @endpoints_lock: lock of the endpoints set
+ * @sendq:	wait queue of sending contexts waiting for a tx buffers
+ * @sleepers:	number of senders that are waiting for a tx buffer
+ * @as_ept:	the bus's address service endpoint
+ * @nb:		notifier block for receiving notifications from vhost device
+ *              driver
+ * @list:	maintain list of client drivers bound to rpmsg vhost device
+ * @list_lock:  mutex to protect updating the list
+ *
+ * This structure stores the rpmsg state of a given vhost remote processor
+ * device (there might be several virtio proc devices for each physical
+ * remote processor).
+ */
+struct virtproc_info {
+	struct vhost_dev *vdev;
+	struct vhost_virtqueue *rvq, *svq;
+	unsigned int buf_size;
+	/* mutex to protect sending messages */
+	struct mutex tx_lock;
+	/* mutex to protect receiving messages */
+	struct mutex rx_lock;
+	struct idr endpoints;
+	/* mutex to protect receiving accessing idr */
+	struct mutex endpoints_lock;
+	wait_queue_head_t sendq;
+	atomic_t sleepers;
+	struct rpmsg_endpoint *as_ept;
+	struct notifier_block nb;
+	struct list_head list;
+	/* mutex to protect updating pending rpdev in vrp */
+	struct mutex list_lock;
+};
+
+/**
+ * @vrp: the remote processor this channel belongs to
+ */
+struct vhost_rpmsg_channel {
+	struct rpmsg_device rpdev;
+
+	struct virtproc_info *vrp;
+};
+
+#define to_vhost_rpmsg_channel(_rpdev) \
+	container_of(_rpdev, struct vhost_rpmsg_channel, rpdev)
+
+static void vhost_rpmsg_destroy_ept(struct rpmsg_endpoint *ept);
+static int vhost_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len);
+static int vhost_rpmsg_sendto(struct rpmsg_endpoint *ept, void *data, int len,
+			      u32 dst);
+static int vhost_rpmsg_send_offchannel(struct rpmsg_endpoint *ept, u32 src,
+				       u32 dst, void *data, int len);
+static int vhost_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len);
+static int vhost_rpmsg_trysendto(struct rpmsg_endpoint *ept, void *data,
+				 int len, u32 dst);
+static int vhost_rpmsg_trysend_offchannel(struct rpmsg_endpoint *ept, u32 src,
+					  u32 dst, void *data, int len);
+
+static const struct rpmsg_endpoint_ops vhost_endpoint_ops = {
+	.destroy_ept = vhost_rpmsg_destroy_ept,
+	.send = vhost_rpmsg_send,
+	.sendto = vhost_rpmsg_sendto,
+	.send_offchannel = vhost_rpmsg_send_offchannel,
+	.trysend = vhost_rpmsg_trysend,
+	.trysendto = vhost_rpmsg_trysendto,
+	.trysend_offchannel = vhost_rpmsg_trysend_offchannel,
+};
+
+/**
+ * __ept_release() - deallocate an rpmsg endpoint
+ * @kref: the ept's reference count
+ *
+ * This function deallocates an ept, and is invoked when its @kref refcount
+ * drops to zero.
+ *
+ * Never invoke this function directly!
+ */
+static void __ept_release(struct kref *kref)
+{
+	struct rpmsg_endpoint *ept = container_of(kref, struct rpmsg_endpoint,
+						  refcount);
+	/*
+	 * At this point no one holds a reference to ept anymore,
+	 * so we can directly free it
+	 */
+	kfree(ept);
+}
+
+/**
+ * __rpmsg_create_ept() - Create rpmsg endpoint
+ * @vrp: virtual remote processor of the vhost device where endpoint has to be
+ *       created
+ * @rpdev: rpmsg device on which endpoint has to be created
+ * @cb: callback associated with the endpoint
+ * @priv: private data for the driver's use
+ * @addr: channel_info with the local rpmsg address to bind with @cb
+ *
+ * Allows drivers to create an endpoint, and bind a callback with some
+ * private data, to an rpmsg address.
+ */
+static struct rpmsg_endpoint *__rpmsg_create_ept(struct virtproc_info *vrp,
+						 struct rpmsg_device *rpdev,
+						 rpmsg_rx_cb_t cb,
+						 void *priv, u32 addr)
+{
+	int id_min, id_max, id;
+	struct rpmsg_endpoint *ept;
+	struct device *dev = rpdev ? &rpdev->dev : &vrp->vdev->dev;
+
+	ept = kzalloc(sizeof(*ept), GFP_KERNEL);
+	if (!ept)
+		return NULL;
+
+	kref_init(&ept->refcount);
+	mutex_init(&ept->cb_lock);
+
+	ept->rpdev = rpdev;
+	ept->cb = cb;
+	ept->priv = priv;
+	ept->ops = &vhost_endpoint_ops;
+
+	/* do we need to allocate a local address ? */
+	if (addr == RPMSG_ADDR_ANY) {
+		id_min = RPMSG_RESERVED_ADDRESSES;
+		id_max = 0;
+	} else {
+		id_min = addr;
+		id_max = addr + 1;
+	}
+
+	mutex_lock(&vrp->endpoints_lock);
+
+	/* bind the endpoint to an rpmsg address (and allocate one if needed) */
+	id = idr_alloc(&vrp->endpoints, ept, id_min, id_max, GFP_KERNEL);
+	if (id < 0) {
+		dev_err(dev, "idr_alloc failed: %d\n", id);
+		goto free_ept;
+	}
+	ept->addr = id;
+
+	mutex_unlock(&vrp->endpoints_lock);
+
+	return ept;
+
+free_ept:
+	mutex_unlock(&vrp->endpoints_lock);
+	kref_put(&ept->refcount, __ept_release);
+	return NULL;
+}
+
+/**
+ * vhost_rpmsg_create_ept() - Create rpmsg endpoint
+ * @rpdev: rpmsg device on which endpoint has to be created
+ * @cb: callback associated with the endpoint
+ * @priv: private data for the driver's use
+ * @chinfo: channel_info with the local rpmsg address to bind with @cb
+ *
+ * Wrapper to __rpmsg_create_ept() to create rpmsg endpoint
+ */
+static struct rpmsg_endpoint
+*vhost_rpmsg_create_ept(struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv,
+			struct rpmsg_channel_info chinfo)
+{
+	struct vhost_rpmsg_channel *vch = to_vhost_rpmsg_channel(rpdev);
+
+	return __rpmsg_create_ept(vch->vrp, rpdev, cb, priv, chinfo.src);
+}
+
+/**
+ * __rpmsg_destroy_ept() - destroy an existing rpmsg endpoint
+ * @vrp: virtproc which owns this ept
+ * @ept: endpoing to destroy
+ *
+ * An internal function which destroy an ept without assuming it is
+ * bound to an rpmsg channel. This is needed for handling the internal
+ * name service endpoint, which isn't bound to an rpmsg channel.
+ * See also __rpmsg_create_ept().
+ */
+static void
+__rpmsg_destroy_ept(struct virtproc_info *vrp, struct rpmsg_endpoint *ept)
+{
+	/* make sure new inbound messages can't find this ept anymore */
+	mutex_lock(&vrp->endpoints_lock);
+	idr_remove(&vrp->endpoints, ept->addr);
+	mutex_unlock(&vrp->endpoints_lock);
+
+	/* make sure in-flight inbound messages won't invoke cb anymore */
+	mutex_lock(&ept->cb_lock);
+	ept->cb = NULL;
+	mutex_unlock(&ept->cb_lock);
+
+	kref_put(&ept->refcount, __ept_release);
+}
+
+/**
+ * vhost_rpmsg_destroy_ept() - destroy an existing rpmsg endpoint
+ * @ept: endpoing to destroy
+ *
+ * Wrapper to __rpmsg_destroy_ept() to destroy rpmsg endpoint
+ */
+static void vhost_rpmsg_destroy_ept(struct rpmsg_endpoint *ept)
+{
+	struct vhost_rpmsg_channel *vch = to_vhost_rpmsg_channel(ept->rpdev);
+
+	__rpmsg_destroy_ept(vch->vrp, ept);
+}
+
+/**
+ * vhost_rpmsg_announce_create() - Announce creation of new channel
+ * @rpdev: rpmsg device on which new endpoint channel is created
+ *
+ * Send a message to the remote processor's name service about the
+ * creation of this channel.
+ */
+static int vhost_rpmsg_announce_create(struct rpmsg_device *rpdev)
+{
+	struct vhost_rpmsg_channel *vch = to_vhost_rpmsg_channel(rpdev);
+	struct virtproc_info *vrp = vch->vrp;
+	struct device *dev = &rpdev->dev;
+	int err = 0;
+
+	/* need to tell remote processor's name service about this channel ? */
+	if (rpdev->ept && vhost_has_feature(vrp->vdev, VIRTIO_RPMSG_F_NS)) {
+		struct rpmsg_ns_msg nsm;
+
+		strncpy(nsm.name, rpdev->id.name, RPMSG_NAME_SIZE);
+		nsm.addr = rpdev->ept->addr;
+		nsm.flags = RPMSG_NS_CREATE | RPMSG_AS_ANNOUNCE;
+
+		err = rpmsg_sendto(rpdev->ept, &nsm, sizeof(nsm), RPMSG_NS_ADDR);
+		if (err)
+			dev_err(dev, "failed to announce service %d\n", err);
+	}
+
+	return err;
+}
+
+/**
+ * vhost_rpmsg_announce_destroy() - Announce deletion of channel
+ * @rpdev: rpmsg device on which this endpoint channel is created
+ *
+ * Send a message to the remote processor's name service about the
+ * deletion of this channel.
+ */
+static int vhost_rpmsg_announce_destroy(struct rpmsg_device *rpdev)
+{
+	struct vhost_rpmsg_channel *vch = to_vhost_rpmsg_channel(rpdev);
+	struct virtproc_info *vrp = vch->vrp;
+	struct device *dev = &rpdev->dev;
+	int err = 0;
+
+	/* tell remote processor's name service we're removing this channel */
+	if (rpdev->announce && rpdev->ept &&
+	    vhost_has_feature(vrp->vdev, VIRTIO_RPMSG_F_NS)) {
+		struct rpmsg_ns_msg nsm;
+
+		strncpy(nsm.name, rpdev->id.name, RPMSG_NAME_SIZE);
+		nsm.addr = rpdev->ept->addr;
+		nsm.flags = RPMSG_NS_DESTROY;
+
+		err = rpmsg_sendto(rpdev->ept, &nsm, sizeof(nsm), RPMSG_NS_ADDR);
+		if (err)
+			dev_err(dev, "failed to announce service %d\n", err);
+	}
+
+	return err;
+}
+
+static const struct rpmsg_device_ops vhost_rpmsg_ops = {
+	.create_ept = vhost_rpmsg_create_ept,
+	.announce_create = vhost_rpmsg_announce_create,
+	.announce_destroy = vhost_rpmsg_announce_destroy,
+};
+
+/**
+ * vhost_rpmsg_release_device() - Callback to free vhost_rpmsg_channel
+ * @dev: struct device of rpmsg_device
+ *
+ * Invoked from device core after all references to "dev" is removed
+ * to free the wrapper vhost_rpmsg_channel.
+ */
+static void vhost_rpmsg_release_device(struct device *dev)
+{
+	struct rpmsg_device *rpdev = to_rpmsg_device(dev);
+	struct vhost_rpmsg_channel *vch = to_vhost_rpmsg_channel(rpdev);
+
+	kfree(vch);
+}
+
+/**
+ * vhost_rpmsg_create_channel - Create an rpmsg channel
+ * @dev: struct device of vhost_dev
+ * @name: name of the rpmsg channel to be created
+ *
+ * Create an rpmsg channel using its name. Invokes rpmsg_register_device()
+ * only if status is VIRTIO_CONFIG_S_DRIVER_OK or else just adds it to
+ * list of pending rpmsg devices. This is because if the rpmsg client
+ * driver is already loaded when rpmsg is being registered, it'll try
+ * to start accessing virtqueue which will be ready only after VIRTIO
+ * sets status as VIRTIO_CONFIG_S_DRIVER_OK.
+ */
+struct device *vhost_rpmsg_create_channel(struct device *dev, const char *name)
+{
+	struct vhost_rpmsg_channel *vch;
+	struct rpmsg_device *rpdev;
+	struct virtproc_info *vrp;
+	struct vhost_dev *vdev;
+	u8 status;
+	int ret;
+
+	vdev = to_vhost_dev(dev);
+	status = vhost_get_status(vdev);
+	vrp = vhost_get_drvdata(vdev);
+
+	vch = kzalloc(sizeof(*vch), GFP_KERNEL);
+	if (!vch)
+		return ERR_PTR(-ENOMEM);
+
+	/* Link the channel to our vrp */
+	vch->vrp = vrp;
+
+	/* Assign public information to the rpmsg_device */
+	rpdev = &vch->rpdev;
+	rpdev->src = RPMSG_ADDR_ANY;
+	rpdev->dst = RPMSG_ADDR_ANY;
+	rpdev->ops = &vhost_rpmsg_ops;
+
+	rpdev->announce = true;
+
+	strncpy(rpdev->id.name, name, RPMSG_NAME_SIZE);
+
+	rpdev->dev.parent = &vrp->vdev->dev;
+	rpdev->dev.release = vhost_rpmsg_release_device;
+	if (!(status & VIRTIO_CONFIG_S_DRIVER_OK)) {
+		mutex_lock(&vrp->list_lock);
+		list_add_tail(&rpdev->list, &vrp->list);
+		mutex_unlock(&vrp->list_lock);
+	} else {
+		ret = rpmsg_register_device(rpdev);
+		if (ret)
+			return ERR_PTR(-EINVAL);
+	}
+
+	return &rpdev->dev;
+}
+EXPORT_SYMBOL_GPL(vhost_rpmsg_create_channel);
+
+/**
+ * vhost_rpmsg_delete_channel - Delete an rpmsg channel
+ * @dev: struct device of rpmsg_device
+ *
+ * Delete channel created using vhost_rpmsg_create_channel()
+ */
+void vhost_rpmsg_delete_channel(struct device *dev)
+{
+	struct rpmsg_device *rpdev = to_rpmsg_device(dev);
+	struct vhost_rpmsg_channel *vch;
+	struct virtproc_info *vrp;
+	struct vhost_dev *vdev;
+	u8 status;
+
+	vch = to_vhost_rpmsg_channel(rpdev);
+	vrp = vch->vrp;
+	vdev = vrp->vdev;
+	status = vhost_get_status(vdev);
+
+	if (!(status & VIRTIO_CONFIG_S_DRIVER_OK)) {
+		mutex_lock(&vrp->list_lock);
+		list_del(&rpdev->list);
+		mutex_unlock(&vrp->list_lock);
+		kfree(vch);
+	} else {
+		device_unregister(dev);
+	}
+}
+EXPORT_SYMBOL_GPL(vhost_rpmsg_delete_channel);
+
+static const struct rpmsg_virtproc_ops vhost_rpmsg_virtproc_ops = {
+	.create_channel = vhost_rpmsg_create_channel,
+	.delete_channel = vhost_rpmsg_delete_channel,
+};
+
+/**
+ * rpmsg_upref_sleepers() - enable "tx-complete" interrupts, if needed
+ * @vrp: virtual remote processor state
+ *
+ * This function is called before a sender is blocked, waiting for
+ * a tx buffer to become available.
+ *
+ * If we already have blocking senders, this function merely increases
+ * the "sleepers" reference count, and exits.
+ *
+ * Otherwise, if this is the first sender to block, we also enable
+ * virtio's tx callbacks, so we'd be immediately notified when a tx
+ * buffer is consumed (we rely on virtio's tx callback in order
+ * to wake up sleeping senders as soon as a tx buffer is used by the
+ * remote processor).
+ */
+static void rpmsg_upref_sleepers(struct virtproc_info *vrp)
+{
+	/* support multiple concurrent senders */
+	mutex_lock(&vrp->tx_lock);
+
+	/* are we the first sleeping context waiting for tx buffers ? */
+	if (atomic_inc_return(&vrp->sleepers) == 1)
+		/* enable "tx-complete" interrupts before dozing off */
+		vhost_virtqueue_enable_cb(vrp->svq);
+
+	mutex_unlock(&vrp->tx_lock);
+}
+
+/**
+ * rpmsg_downref_sleepers() - disable "tx-complete" interrupts, if needed
+ * @vrp: virtual remote processor state
+ *
+ * This function is called after a sender, that waited for a tx buffer
+ * to become available, is unblocked.
+ *
+ * If we still have blocking senders, this function merely decreases
+ * the "sleepers" reference count, and exits.
+ *
+ * Otherwise, if there are no more blocking senders, we also disable
+ * virtio's tx callbacks, to avoid the overhead incurred with handling
+ * those (now redundant) interrupts.
+ */
+static void rpmsg_downref_sleepers(struct virtproc_info *vrp)
+{
+	/* support multiple concurrent senders */
+	mutex_lock(&vrp->tx_lock);
+
+	/* are we the last sleeping context waiting for tx buffers ? */
+	if (atomic_dec_and_test(&vrp->sleepers))
+		/* disable "tx-complete" interrupts */
+		vhost_virtqueue_disable_cb(vrp->svq);
+
+	mutex_unlock(&vrp->tx_lock);
+}
+
+/**
+ * rpmsg_send_offchannel_raw() - send a message across to the remote processor
+ * @rpdev: the rpmsg channel
+ * @src: source address
+ * @dst: destination address
+ * @data: payload of message
+ * @len: length of payload
+ * @wait: indicates whether caller should block in case no TX buffers available
+ *
+ * This function is the base implementation for all of the rpmsg sending API.
+ *
+ * It will send @data of length @len to @dst, and say it's from @src. The
+ * message will be sent to the remote processor which the @rpdev channel
+ * belongs to.
+ *
+ * The message is sent using one of the TX buffers that are available for
+ * communication with this remote processor.
+ *
+ * If @wait is true, the caller will be blocked until either a TX buffer is
+ * available, or 15 seconds elapses (we don't want callers to
+ * sleep indefinitely due to misbehaving remote processors), and in that
+ * case -ERESTARTSYS is returned. The number '15' itself was picked
+ * arbitrarily; there's little point in asking drivers to provide a timeout
+ * value themselves.
+ *
+ * Otherwise, if @wait is false, and there are no TX buffers available,
+ * the function will immediately fail, and -ENOMEM will be returned.
+ *
+ * Normally drivers shouldn't use this function directly; instead, drivers
+ * should use the appropriate rpmsg_{try}send{to, _offchannel} API
+ * (see include/linux/rpmsg.h).
+ *
+ * Returns 0 on success and an appropriate error value on failure.
+ */
+static int rpmsg_send_offchannel_raw(struct rpmsg_device *rpdev,
+				     u32 src, u32 dst,
+				     void *data, int len, bool wait)
+{
+	struct vhost_rpmsg_channel *vch = to_vhost_rpmsg_channel(rpdev);
+	struct virtproc_info *vrp = vch->vrp;
+	struct vhost_virtqueue *svq = vrp->svq;
+	struct vhost_dev *vdev = svq->dev;
+	struct device *dev = &rpdev->dev;
+	struct rpmsg_hdr msg;
+	int length;
+	u16 head;
+	u64 base;
+	int err;
+
+	/*
+	 * We currently use fixed-sized buffers, and therefore the payload
+	 * length is limited.
+	 *
+	 * One of the possible improvements here is either to support
+	 * user-provided buffers (and then we can also support zero-copy
+	 * messaging), or to improve the buffer allocator, to support
+	 * variable-length buffer sizes.
+	 */
+	if (len > vrp->buf_size - sizeof(struct rpmsg_hdr)) {
+		dev_err(dev, "message is too big (%d)\n", len);
+		return -EMSGSIZE;
+	}
+
+	mutex_lock(&vrp->tx_lock);
+	/* grab a buffer */
+	base = vhost_virtqueue_get_outbuf(svq, &head, &length);
+	if (!base && !wait) {
+		dev_err(dev, "Failed to get buffer for OUT transfers\n");
+		err = -ENOMEM;
+		goto out;
+	}
+
+	/* no free buffer ? wait for one (but bail after 15 seconds) */
+	while (!base) {
+		/* enable "tx-complete" interrupts, if not already enabled */
+		rpmsg_upref_sleepers(vrp);
+
+		/*
+		 * sleep until a free buffer is available or 15 secs elapse.
+		 * the timeout period is not configurable because there's
+		 * little point in asking drivers to specify that.
+		 * if later this happens to be required, it'd be easy to add.
+		 */
+		err = wait_event_interruptible_timeout
+			(vrp->sendq, (base =
+				      vhost_virtqueue_get_outbuf(svq, &head,
+								 &length)),
+			 msecs_to_jiffies(15000));
+
+		/* disable "tx-complete" interrupts if we're the last sleeper */
+		rpmsg_downref_sleepers(vrp);
+
+		/* timeout ? */
+		if (!err) {
+			dev_err(dev, "timeout waiting for a tx buffer\n");
+			err = -ERESTARTSYS;
+			goto out;
+		}
+	}
+
+	msg.len = len;
+	msg.flags = 0;
+	msg.src = src;
+	msg.dst = dst;
+	msg.reserved = 0;
+	/*
+	 * Perform two writes, one for rpmsg header and other for actual buffer
+	 * data, instead of squashing the data into one buffer and then send
+	 * them to the vhost layer.
+	 */
+	err = vhost_write(vdev, base, &msg, sizeof(struct rpmsg_hdr));
+	if (err) {
+		dev_err(dev, "Failed to write rpmsg header to remote buffer\n");
+		goto out;
+	}
+
+	err = vhost_write(vdev, base + sizeof(struct rpmsg_hdr), data, len);
+	if (err) {
+		dev_err(dev, "Failed to write buffer data to remote buffer\n");
+		goto out;
+	}
+
+	dev_dbg(dev, "TX From 0x%x, To 0x%x, Len %d, Flags %d, Reserved %d\n",
+		msg.src, msg.dst, msg.len, msg.flags, msg.reserved);
+#if defined(CONFIG_DYNAMIC_DEBUG)
+	dynamic_hex_dump("rpmsg_virtio TX: ", DUMP_PREFIX_NONE, 16, 1,
+			 &msg, sizeof(msg) + msg.len, true);
+#endif
+
+	vhost_virtqueue_put_buf(svq, head, len + sizeof(struct rpmsg_hdr));
+
+	/* tell the remote processor it has a pending message to read */
+	vhost_virtqueue_kick(vrp->svq);
+
+out:
+	mutex_unlock(&vrp->tx_lock);
+
+	return err;
+}
+
+static int vhost_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len)
+{
+	struct rpmsg_device *rpdev = ept->rpdev;
+	u32 src = ept->addr, dst = rpdev->dst;
+
+	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
+}
+
+static int vhost_rpmsg_sendto(struct rpmsg_endpoint *ept, void *data, int len,
+			      u32 dst)
+{
+	struct rpmsg_device *rpdev = ept->rpdev;
+	u32 src = ept->addr;
+
+	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
+}
+
+static int vhost_rpmsg_send_offchannel(struct rpmsg_endpoint *ept, u32 src,
+				       u32 dst, void *data, int len)
+{
+	struct rpmsg_device *rpdev = ept->rpdev;
+
+	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, true);
+}
+
+static int vhost_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len)
+{
+	struct rpmsg_device *rpdev = ept->rpdev;
+	u32 src = ept->addr, dst = rpdev->dst;
+
+	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
+}
+
+static int vhost_rpmsg_trysendto(struct rpmsg_endpoint *ept, void *data,
+				 int len, u32 dst)
+{
+	struct rpmsg_device *rpdev = ept->rpdev;
+	u32 src = ept->addr;
+
+	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
+}
+
+static int vhost_rpmsg_trysend_offchannel(struct rpmsg_endpoint *ept, u32 src,
+					  u32 dst, void *data, int len)
+{
+	struct rpmsg_device *rpdev = ept->rpdev;
+
+	return rpmsg_send_offchannel_raw(rpdev, src, dst, data, len, false);
+}
+
+/**
+ * rpmsg_recv_single - Invoked when a buffer is received from remote VIRTIO dev
+ * @vrp: virtual remote processor of the vhost device which has received a msg
+ * @dev: struct device of vhost_dev
+ * @msg: pointer to the rpmsg_hdr
+ * @len: length of the received buffer
+ *
+ * Invoked when a buffer is received from remote VIRTIO device. It gets the
+ * destination address from rpmsg_hdr and invokes the callback of the endpoint
+ * corresponding to the address
+ */
+static int rpmsg_recv_single(struct virtproc_info *vrp, struct device *dev,
+			     struct rpmsg_hdr *msg, unsigned int len)
+{
+	struct rpmsg_endpoint *ept;
+
+	dev_dbg(dev, "From: 0x%x, To: 0x%x, Len: %d, Flags: %d, Reserved: %d\n",
+		msg->src, msg->dst, msg->len, msg->flags, msg->reserved);
+#if defined(CONFIG_DYNAMIC_DEBUG)
+	dynamic_hex_dump("rpmsg_virtio RX: ", DUMP_PREFIX_NONE, 16, 1,
+			 msg, sizeof(*msg) + msg->len, true);
+#endif
+
+	/*
+	 * We currently use fixed-sized buffers, so trivially sanitize
+	 * the reported payload length.
+	 */
+	if (len > vrp->buf_size ||
+	    msg->len > (len - sizeof(struct rpmsg_hdr))) {
+		dev_warn(dev, "inbound msg too big: (%d, %d)\n", len, msg->len);
+		return -EINVAL;
+	}
+
+	/* use the dst addr to fetch the callback of the appropriate user */
+	mutex_lock(&vrp->endpoints_lock);
+
+	ept = idr_find(&vrp->endpoints, msg->dst);
+
+	/* let's make sure no one deallocates ept while we use it */
+	if (ept)
+		kref_get(&ept->refcount);
+
+	mutex_unlock(&vrp->endpoints_lock);
+
+	if (ept) {
+		/* make sure ept->cb doesn't go away while we use it */
+		mutex_lock(&ept->cb_lock);
+
+		if (ept->cb)
+			ept->cb(ept->rpdev, msg->data, msg->len, ept->priv,
+				msg->src);
+
+		mutex_unlock(&ept->cb_lock);
+
+		/* farewell, ept, we don't need you anymore */
+		kref_put(&ept->refcount, __ept_release);
+	} else {
+		dev_warn(dev, "msg received with no recipient\n");
+	}
+
+	return 0;
+}
+
+/**
+ * vhost_rpmsg_recv_done - Callback of the receive virtqueue
+ * @rvq: Receive virtqueue
+ *
+ * Invoked when the remote VIRTIO device sends a notification on the receive
+ * virtqueue. It gets base address of the input buffer and repeatedly calls
+ * rpmsg_recv_single() until no more buffers are left to be read.
+ */
+static void vhost_rpmsg_recv_done(struct vhost_virtqueue *rvq)
+{
+	struct vhost_dev *vdev = rvq->dev;
+	struct virtproc_info *vrp = vhost_get_drvdata(vdev);
+	unsigned int len, msgs_received = 0;
+	struct device *dev = &vdev->dev;
+	struct rpmsg_hdr *msg;
+	u64 base;
+	u16 head;
+	int err;
+
+	base = vhost_virtqueue_get_inbuf(rvq, &head, &len);
+	if (!base) {
+		dev_err(dev, "uhm, incoming signal, but no used buffer ?\n");
+		return;
+	}
+
+	vhost_virtqueue_disable_cb(rvq);
+	while (base) {
+		msg =  kzalloc(len, GFP_KERNEL);
+		if (!msg)
+			return;
+
+		vhost_read(rvq->dev, msg, base, len);
+
+		err = rpmsg_recv_single(vrp, dev, msg, len);
+		if (err)
+			break;
+
+		kfree(msg);
+		vhost_virtqueue_put_buf(rvq, head, len);
+		msgs_received++;
+
+		base = vhost_virtqueue_get_inbuf(rvq, &head, &len);
+	}
+	vhost_virtqueue_enable_cb(rvq);
+
+	dev_dbg(dev, "Received %u messages\n", msgs_received);
+
+	/* tell the remote processor we added another available rx buffer */
+	if (msgs_received)
+		vhost_virtqueue_kick(vrp->rvq);
+}
+
+/**
+ * vhost_rpmsg_xmit_done - Callback of the receive virtqueue
+ * @svq: Send virtqueue
+ *
+ * This is invoked whenever the remote processor completed processing
+ * a TX msg we just sent it, and the buffer is put back to the used ring.
+ *
+ * Normally, though, we suppress this "tx complete" interrupt in order to
+ * avoid the incurred overhead.
+ */
+static void vhost_rpmsg_xmit_done(struct vhost_virtqueue *svq)
+{
+	struct vhost_dev *vdev = svq->dev;
+	struct virtproc_info *vrp = vhost_get_drvdata(vdev);
+	struct device *dev = &vdev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/* wake up potential senders that are waiting for a tx buffer */
+	wake_up_interruptible(&vrp->sendq);
+}
+
+/**
+ * vhost_rpmsg_as_cb - Callback of address service announcement
+ * @data: rpmsg_as_msg sent by remote VIRTIO device
+ * @len: length of the received message
+ * @priv: private data for the driver's use
+ * @src: source address of the remote VIRTIO device that sent the AS
+ *       announcement
+ *
+ * Invoked when a address service announcement arrives to assign the
+ * destination address of the rpmsg device.
+ */
+static int vhost_rpmsg_as_cb(struct rpmsg_device *rpdev, void *data, int len,
+			     void *priv, u32 hdr_src)
+{
+	struct virtproc_info *vrp = priv;
+	struct device *dev = &vrp->vdev->dev;
+	struct rpmsg_channel_info chinfo;
+	struct rpmsg_as_msg *msg = data;
+	struct rpmsg_device *rpmsg_dev;
+	struct device *rdev;
+	int ret = 0;
+	u32 flags;
+	u32 src;
+	u32 dst;
+
+#if defined(CONFIG_DYNAMIC_DEBUG)
+	dynamic_hex_dump("AS announcement: ", DUMP_PREFIX_NONE, 16, 1,
+			 data, len, true);
+#endif
+
+	if (len == sizeof(*msg)) {
+		src = msg->src;
+		dst = msg->dst;
+		flags = msg->flags;
+	} else {
+		dev_err(dev, "malformed AS msg (%d)\n", len);
+		return -EINVAL;
+	}
+
+	/*
+	 * the name service ept does _not_ belong to a real rpmsg channel,
+	 * and is handled by the rpmsg bus itself.
+	 * for sanity reasons, make sure a valid rpdev has _not_ sneaked
+	 * in somehow.
+	 */
+	if (rpdev) {
+		dev_err(dev, "anomaly: ns ept has an rpdev handle\n");
+		return -EINVAL;
+	}
+
+	/* don't trust the remote processor for null terminating the name */
+	msg->name[RPMSG_NAME_SIZE - 1] = '\0';
+
+	dev_info(dev, "%sing dst addr 0x%x to channel %s src 0x%x\n",
+		 flags & RPMSG_AS_ASSIGN ? "Assign" : "Free",
+		 dst, msg->name, src);
+
+	strncpy(chinfo.name, msg->name, sizeof(chinfo.name));
+	chinfo.src = src;
+	chinfo.dst = RPMSG_ADDR_ANY;
+
+	/* Find a similar channel */
+	rdev = rpmsg_find_device(dev, &chinfo);
+	if (!rdev) {
+		ret = -ENODEV;
+		goto err_find_device;
+	}
+
+	rpmsg_dev = to_rpmsg_device(rdev);
+	if (flags & RPMSG_AS_ASSIGN) {
+		if (rpmsg_dev->dst != RPMSG_ADDR_ANY) {
+			dev_err(dev, "Address bound to channel %s src 0x%x\n",
+				msg->name, src);
+			ret = -EBUSY;
+			goto err_find_device;
+		}
+		rpmsg_dev->dst = dst;
+	} else {
+		rpmsg_dev->dst = RPMSG_ADDR_ANY;
+	}
+
+err_find_device:
+	put_device(rdev);
+
+	return ret;
+}
+
+/**
+ * vhost_rpmsg_finalize_feature - Perform initializations for negotiated
+ *   features
+ * @vrp: virtual remote processor of the vhost device where the feature has been
+ *       negotiated
+ *
+ * Invoked when features negotiation between VHOST and VIRTIO device is
+ * completed.
+ */
+static int vhost_rpmsg_finalize_feature(struct virtproc_info *vrp)
+{
+	struct vhost_dev *vdev = vrp->vdev;
+
+	/* if supported by the remote processor, enable the address service */
+	if (vhost_has_feature(vdev, VIRTIO_RPMSG_F_AS)) {
+		/* a dedicated endpoint handles the name service msgs */
+		vrp->as_ept = __rpmsg_create_ept(vrp, NULL, vhost_rpmsg_as_cb,
+						 vrp, RPMSG_AS_ADDR);
+		if (!vrp->as_ept) {
+			dev_err(&vdev->dev, "failed to create the as ept\n");
+			return -ENOMEM;
+		}
+	} else {
+		dev_err(&vdev->dev, "Address Service not supported\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+/**
+ * vhost_rpmsg_set_status - Perform initialization when remote VIRTIO device
+ *   updates status
+ * @vrp: virtual remote processor of the vhost device whose status has been
+ *       updated
+ *
+ * Invoked when the remote VIRTIO device updates status. If status is set
+ * as VIRTIO_CONFIG_S_DRIVER_OK, invoke rpmsg_register_device() for every
+ * un-registered rpmsg device.
+ */
+static int vhost_rpmsg_set_status(struct virtproc_info *vrp)
+{
+	struct vhost_dev *vdev = vrp->vdev;
+	struct rpmsg_device *rpdev;
+	u8 status;
+	int ret;
+
+	status = vhost_get_status(vdev);
+
+	if (status & VIRTIO_CONFIG_S_DRIVER_OK) {
+		mutex_lock(&vrp->list_lock);
+		list_for_each_entry(rpdev, &vrp->list, list) {
+			ret = rpmsg_register_device(rpdev);
+			if (ret) {
+				mutex_unlock(&vrp->list_lock);
+				return -EINVAL;
+			}
+		}
+		list_del(&vrp->list);
+		mutex_unlock(&vrp->list_lock);
+	}
+
+	return 0;
+}
+
+/**
+ * vhost_rpmsg_notifier - Notifier to notify updates from remote VIRTIO device
+ * @nb: notifier block associated with this virtual remote processor
+ * @notify_reason: Indicate the updates (finalize feature or set status) by
+ *                 remote host
+ * @data: un-used here
+ *
+ * Invoked when the remote VIRTIO device updates status or finalize features.
+ */
+static int vhost_rpmsg_notifier(struct notifier_block *nb, unsigned long notify_reason,
+				void *data)
+{
+	struct virtproc_info *vrp = container_of(nb, struct virtproc_info, nb);
+	struct vhost_dev *vdev = vrp->vdev;
+	int ret;
+
+	switch (notify_reason) {
+	case NOTIFY_FINALIZE_FEATURES:
+		ret = vhost_rpmsg_finalize_feature(vrp);
+		if (ret)
+			dev_err(&vdev->dev, "failed to finalize features\n");
+		break;
+	case NOTIFY_SET_STATUS:
+		ret = vhost_rpmsg_set_status(vrp);
+		if (ret)
+			dev_err(&vdev->dev, "failed to set status\n");
+		break;
+	default:
+		dev_err(&vdev->dev, "Unsupported notification\n");
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static unsigned int vhost_rpmsg_features[] = {
+	VIRTIO_RPMSG_F_AS,
+	VIRTIO_RPMSG_F_NS,
+};
+
+/**
+ * vhost_rpmsg_set_features - Sets supported features on the VHOST device
+ *
+ * Build supported features from the feature table and invoke
+ * vhost_set_features() to set the supported features on the VHOST device
+ */
+static int vhost_rpmsg_set_features(struct vhost_dev *vdev)
+{
+	unsigned int feature_table_size;
+	unsigned int feature;
+	u64 device_features;
+	int ret, i;
+
+	feature_table_size =  ARRAY_SIZE(vhost_rpmsg_features);
+	for (i = 0; i < feature_table_size; i++) {
+		feature = vhost_rpmsg_features[i];
+		WARN_ON(feature >= 64);
+		device_features |= (1ULL << feature);
+	}
+
+	ret = vhost_set_features(vdev, device_features);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/**
+ * vhost_rpmsg_probe - Create virtual remote processor for the VHOST device
+ * @vdev - VHOST device with vendor ID and device ID supported by this driver
+ *
+ * Invoked when VHOST device is registered with vendor ID and device ID
+ * supported by this driver. Creates and initializes the virtual remote
+ * processor for the VHOST device
+ */
+static int vhost_rpmsg_probe(struct vhost_dev *vdev)
+{
+	vhost_vq_callback_t *vq_cbs[] = { vhost_rpmsg_xmit_done, vhost_rpmsg_recv_done };
+	static const char * const names[] = { "output", "input" };
+	struct device *dev = &vdev->dev;
+	struct vhost_virtqueue *vqs[2];
+	struct config_group *group;
+	struct virtproc_info *vrp;
+	int err;
+
+	vrp = devm_kzalloc(dev, sizeof(*vrp), GFP_KERNEL);
+	if (!vrp)
+		return -ENOMEM;
+
+	vrp->vdev = vdev;
+
+	idr_init(&vrp->endpoints);
+	mutex_init(&vrp->endpoints_lock);
+	mutex_init(&vrp->tx_lock);
+	mutex_init(&vrp->rx_lock);
+	mutex_init(&vrp->list_lock);
+	init_waitqueue_head(&vrp->sendq);
+
+	err = vhost_rpmsg_set_features(vdev);
+	if (err) {
+		dev_err(dev, "Failed to set features\n");
+		return err;
+	}
+
+	/* We expect two vhost_virtqueues, tx and rx (and in this order) */
+	err = vhost_create_vqs(vdev, 2, MAX_RPMSG_NUM_BUFS / 2, vqs, vq_cbs,
+			       names);
+	if (err) {
+		dev_err(dev, "Failed to create virtqueues\n");
+		return err;
+	}
+
+	vrp->svq = vqs[0];
+	vrp->rvq = vqs[1];
+
+	vrp->buf_size = MAX_RPMSG_BUF_SIZE;
+
+	vhost_set_drvdata(vdev, vrp);
+
+	vrp->nb.notifier_call = vhost_rpmsg_notifier;
+	vhost_register_notifier(vdev, &vrp->nb);
+	INIT_LIST_HEAD(&vrp->list);
+
+	group = rpmsg_cfs_add_virtproc_group(dev,
+					     &vhost_rpmsg_virtproc_ops);
+	if (IS_ERR(group)) {
+		err = PTR_ERR(group);
+		goto err;
+	}
+
+	dev_info(&vdev->dev, "vhost rpmsg host is online\n");
+
+	return 0;
+
+err:
+	vhost_del_vqs(vdev);
+
+	return err;
+}
+
+static int vhost_rpmsg_remove_device(struct device *dev, void *data)
+{
+	device_unregister(dev);
+
+	return 0;
+}
+
+static int vhost_rpmsg_remove(struct vhost_dev *vdev)
+{
+	struct virtproc_info *vrp = vhost_get_drvdata(vdev);
+	int ret;
+
+	ret = device_for_each_child(&vdev->dev, NULL, vhost_rpmsg_remove_device);
+	if (ret)
+		dev_warn(&vdev->dev, "can't remove rpmsg device: %d\n", ret);
+
+	if (vrp->as_ept)
+		__rpmsg_destroy_ept(vrp, vrp->as_ept);
+
+	idr_destroy(&vrp->endpoints);
+
+	vhost_del_vqs(vdev);
+
+	kfree(vrp);
+	return 0;
+}
+
+static struct vhost_device_id vhost_rpmsg_id_table[] = {
+	{ VIRTIO_ID_RPMSG, VIRTIO_DEV_ANY_ID },
+	{ 0 },
+};
+
+static struct vhost_driver vhost_rpmsg_driver = {
+	.driver.name	= KBUILD_MODNAME,
+	.driver.owner	= THIS_MODULE,
+	.id_table	= vhost_rpmsg_id_table,
+	.probe		= vhost_rpmsg_probe,
+	.remove		= vhost_rpmsg_remove,
+};
+
+static int __init vhost_rpmsg_init(void)
+{
+	int ret;
+
+	ret = vhost_register_driver(&vhost_rpmsg_driver);
+	if (ret)
+		pr_err("Failed to register vhost rpmsg driver: %d\n", ret);
+
+	return ret;
+}
+module_init(vhost_rpmsg_init);
+
+static void __exit vhost_rpmsg_exit(void)
+{
+	vhost_unregister_driver(&vhost_rpmsg_driver);
+}
+module_exit(vhost_rpmsg_exit);
+
+MODULE_DEVICE_TABLE(vhost, vhost_rpmsg_id_table);
+MODULE_DESCRIPTION("Vhost-based remote processor messaging bus");
+MODULE_LICENSE("GPL v2");
diff --git a/include/linux/rpmsg.h b/include/linux/rpmsg.h
index b9d9283b46ac..5d60d13efaf4 100644
--- a/include/linux/rpmsg.h
+++ b/include/linux/rpmsg.h
@@ -55,6 +55,7 @@ struct rpmsg_device {
 	u32 dst;
 	struct rpmsg_endpoint *ept;
 	bool announce;
+	struct list_head list;
 
 	const struct rpmsg_device_ops *ops;
 };
-- 
2.17.1