From 2bafe51755ef34f34e1a10eb7bbeb0ef0df878b8 Mon Sep 17 00:00:00 2001
From: Guiding Li <liguiding1@xiaomi.com>
Date: Thu, 18 Nov 2021 20:54:45 +0800
Subject: [PATCH 05/12] openamp: add new ops notify_wait() support

This can avoid looping check tx buffer

Signed-off-by: Guiding Li <liguiding1@xiaomi.com>
Signed-off-by: ligd <liguiding1@xiaomi.com>
---
 lib/include/openamp/remoteproc.h        | 12 ++++++++++++
 lib/include/openamp/remoteproc_virtio.h |  2 ++
 lib/include/openamp/rpmsg.h             |  1 +
 lib/include/openamp/rpmsg_virtio.h      |  9 +++++++++
 lib/include/openamp/virtio.h            |  1 +
 lib/remoteproc/remoteproc.c             | 11 +++++++++++
 lib/remoteproc/remoteproc_virtio.c      | 14 ++++++++++++++
 lib/rpmsg/rpmsg_virtio.c                |  7 +++++++
 8 files changed, 57 insertions(+)

diff --git a/lib/include/openamp/remoteproc.h open-amp/lib/include/openamp/remoteproc.h
index e9111ff..d276550 100644
--- a/lib/include/openamp/remoteproc.h
+++ open-amp/lib/include/openamp/remoteproc.h
@@ -428,6 +428,18 @@ struct remoteproc_ops {
 	int (*stop)(struct remoteproc *rproc);
 	int (*shutdown)(struct remoteproc *rproc);
 	int (*notify)(struct remoteproc *rproc, uint32_t id);
+	/**
+	 * notify_wait
+	 *
+	 * Wait for remote notified, when there is no TX buffer anymore.
+	 * Set to NULL means use usleep to wait TX buffer available.
+	 *
+	 * @rproc - pointer to remoteproc instance
+	 * @id    - the notifyid
+	 *
+	 * return 0 means there is notify available, otherwise negative value.
+	 */
+	int (*notify_wait)(struct remoteproc *rproc, uint32_t id);
 	/**
 	 * get_mem
 	 *
diff --git a/lib/include/openamp/remoteproc_virtio.h open-amp/lib/include/openamp/remoteproc_virtio.h
index 70cff97..eaca76a 100644
--- a/lib/include/openamp/remoteproc_virtio.h
+++ open-amp/lib/include/openamp/remoteproc_virtio.h
@@ -22,6 +22,7 @@ extern "C" {
 
 /* define vdev notification function user should implement */
 typedef int (*rpvdev_notify_func)(void *priv, uint32_t id);
+typedef int (*rpvdev_notify_wait)(void *priv, uint32_t id);
 
 /**
  * struct remoteproc_virtio
@@ -37,6 +38,7 @@ struct remoteproc_virtio {
 	void *vdev_rsc;
 	struct metal_io_region *vdev_rsc_io;
 	rpvdev_notify_func notify;
+	rpvdev_notify_wait notify_wait;
 	struct virtio_device vdev;
 	struct metal_list node;
 };
diff --git a/lib/include/openamp/rpmsg.h open-amp/lib/include/openamp/rpmsg.h
index 11c3ccb..6f546e5 100644
--- a/lib/include/openamp/rpmsg.h
+++ open-amp/lib/include/openamp/rpmsg.h
@@ -49,6 +49,7 @@ extern "C" {
 #define RPMSG_ERR_BUFF_SIZE		(RPMSG_ERROR_BASE - 5)
 #define RPMSG_ERR_INIT			(RPMSG_ERROR_BASE - 6)
 #define RPMSG_ERR_ADDR			(RPMSG_ERROR_BASE - 7)
+#define RPMSG_ERR_NXIO			(RPMSG_ERROR_BASE - 8)
 
 struct rpmsg_endpoint;
 struct rpmsg_device;
diff --git a/lib/include/openamp/rpmsg_virtio.h open-amp/lib/include/openamp/rpmsg_virtio.h
index aaba7e1..3ec0b0f 100644
--- a/lib/include/openamp/rpmsg_virtio.h
+++ open-amp/lib/include/openamp/rpmsg_virtio.h
@@ -143,6 +143,15 @@ rpmsg_virtio_create_virtqueues(struct rpmsg_virtio_device *rvdev,
 					callbacks);
 }
 
+static inline int
+rpmsg_virtio_notify_wait(struct rpmsg_virtio_device *rvdev,
+			 struct virtqueue *vq)
+{
+	return rvdev->vdev->func->notify_wait ?
+			rvdev->vdev->func->notify_wait(rvdev->vdev, vq) :
+			RPMSG_ERR_NXIO;
+}
+
 /**
  * rpmsg_virtio_get_buffer_size - get rpmsg virtio buffer size
  *
diff --git a/lib/include/openamp/virtio.h open-amp/lib/include/openamp/virtio.h
index 916132b..0303a5b 100644
--- a/lib/include/openamp/virtio.h
+++ open-amp/lib/include/openamp/virtio.h
@@ -162,6 +162,7 @@ struct virtio_dispatch {
 			     void *src, int length);
 	void (*reset_device)(struct virtio_device *dev);
 	void (*notify)(struct virtqueue *vq);
+	int (*notify_wait)(struct virtio_device *dev, struct virtqueue *vq);
 };
 
 int virtio_create_virtqueues(struct virtio_device *vdev, unsigned int flags,
diff --git a/lib/remoteproc/remoteproc.c open-amp/lib/remoteproc/remoteproc.c
index 9a0cf3e..4c101db 100644
--- a/lib/remoteproc/remoteproc.c
+++ open-amp/lib/remoteproc/remoteproc.c
@@ -880,6 +880,16 @@ static int remoteproc_virtio_notify(void *priv, uint32_t id)
 	return 0;
 }
 
+static int remoteproc_virtio_notify_wait(void *priv, uint32_t id)
+{
+	struct remoteproc *rproc = priv;
+
+	if (rproc->ops->notify_wait)
+		return rproc->ops->notify_wait(rproc, id);
+
+	return 0;
+}
+
 struct virtio_device *
 remoteproc_create_virtio(struct remoteproc *rproc,
 			 int vdev_id, unsigned int role,
@@ -927,6 +937,7 @@ remoteproc_create_virtio(struct remoteproc *rproc,
 	rproc_virtio_wait_remote_ready(vdev);
 
 	rpvdev = metal_container_of(vdev, struct remoteproc_virtio, vdev);
+	rpvdev->notify_wait = remoteproc_virtio_notify_wait;
 	metal_list_add_tail(&rproc->vdevs, &rpvdev->node);
 	num_vrings = vdev_rsc->num_of_vrings;
 
diff --git a/lib/remoteproc/remoteproc_virtio.c open-amp/lib/remoteproc/remoteproc_virtio.c
index cbfd966..ef5eef3 100644
--- a/lib/remoteproc/remoteproc_virtio.c
+++ open-amp/lib/remoteproc/remoteproc_virtio.c
@@ -30,6 +30,19 @@ static void rproc_virtio_virtqueue_notify(struct virtqueue *vq)
 	rpvdev->notify(rpvdev->priv, vring_info->notifyid);
 }
 
+static int rproc_virtio_notify_wait(struct virtio_device *vdev,
+				    struct virtqueue *vq)
+{
+	struct remoteproc_virtio *rpvdev;
+	struct virtio_vring_info *vring_info;
+	unsigned int vq_id = vq->vq_queue_index;
+
+	rpvdev = metal_container_of(vdev, struct remoteproc_virtio, vdev);
+	vring_info = &vdev->vrings_info[vq_id];
+
+	return rpvdev->notify_wait(rpvdev->priv, vring_info->notifyid);
+}
+
 static unsigned char rproc_virtio_get_status(struct virtio_device *vdev)
 {
 	struct remoteproc_virtio *rpvdev;
@@ -179,6 +192,7 @@ static const struct virtio_dispatch remoteproc_virtio_dispatch_funcs = {
 	.get_features = rproc_virtio_get_features,
 	.read_config = rproc_virtio_read_config,
 	.notify = rproc_virtio_virtqueue_notify,
+	.notify_wait = rproc_virtio_notify_wait,
 #ifndef VIRTIO_DEVICE_ONLY
 	/*
 	 * We suppose here that the vdev is in a shared memory so that can
diff --git a/lib/rpmsg/rpmsg_virtio.c open-amp/lib/rpmsg/rpmsg_virtio.c
index d19d3b1..c555101 100644
--- a/lib/rpmsg/rpmsg_virtio.c
+++ open-amp/lib/rpmsg/rpmsg_virtio.c
@@ -339,6 +339,13 @@ static void *rpmsg_virtio_get_tx_payload_buffer(struct rpmsg_device *rdev,
 		metal_mutex_release(&rdev->lock);
 		if (rp_hdr || !tick_count)
 			break;
+
+		status = rpmsg_virtio_notify_wait(rvdev, rvdev->rvq);
+		if (status == RPMSG_SUCCESS)
+			continue;
+		else if (status != RPMSG_ERR_NXIO)
+			break;
+
 		metal_sleep_usec(RPMSG_TICKS_PER_INTERVAL);
 		tick_count--;
 	}
-- 
2.25.1