From 95d089c8b532f4fdbba5e03832371602525fc820 Mon Sep 17 00:00:00 2001
From: anchao <anchao@pinecone.net>
Date: Mon, 10 Dec 2018 16:26:39 +0800
Subject: [PATCH] Negotiate individual buffer size dynamically

If slave support VIRTIO_RPMSG_F_BUFSZ(0x04) feature, master
determine the buffer size from config space(first 4 bytes),
otherwise the default size(512 bytes) will be used.

Signed-off-by: anchao <anchao@pinecone.net>
---
 lib/include/openamp/rpmsg_virtio.h | 12 +++++++++++-
 lib/rpmsg/rpmsg_virtio.c           | 20 ++++++++++++++------
 2 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/lib/include/openamp/rpmsg_virtio.h b/lib/include/openamp/rpmsg_virtio.h
index b1369cacb..16c6ca0d8 100644
--- a/lib/include/openamp/rpmsg_virtio.h
+++ b/lib/include/openamp/rpmsg_virtio.h
@@ -27,7 +27,8 @@ extern "C" {
 #endif
 
 /* The feature bitmap for virtio rpmsg */
-#define VIRTIO_RPMSG_F_NS 0 /* RP supports name service notifications */
+#define VIRTIO_RPMSG_F_NS	0 /* RP supports name service notifications */
+#define VIRTIO_RPMSG_F_BUFSZ	2 /* RP supports get buffer size from config space */
 
 struct rpmsg_virtio_shm_pool;
 /**
@@ -52,6 +53,7 @@ struct rpmsg_virtio_shm_pool {
  * @shbuf_io: pointer to the shared buffer I/O region
  * @shpool: pointer to the shared buffers pool
  * @endpoints: list of endpoints.
+ * @shbuf_size: size of each rpmsg buffer
  */
 struct rpmsg_virtio_device {
 	struct rpmsg_device rdev;
@@ -60,6 +62,7 @@ struct rpmsg_virtio_device {
 	struct virtqueue *svq;
 	struct metal_io_region *shbuf_io;
 	struct rpmsg_virtio_shm_pool *shpool;
+	uint32_t shbuf_size;
 };
 
 #define RPMSG_REMOTE	VIRTIO_DEV_SLAVE
@@ -87,6 +90,13 @@ static inline uint32_t
 	return rvdev->vdev->func->get_features(rvdev->vdev);
 }
 
+static inline void
+rpmsg_virtio_read_config(struct rpmsg_virtio_device *rvdev,
+			 uint32_t offset, void *dst, int length)
+{
+	rvdev->vdev->func->read_config(rvdev->vdev, offset, dst, length);
+}
+
 static inline int
 	rpmsg_virtio_create_virtqueues(struct rpmsg_virtio_device *rvdev,
 				       int flags, unsigned int nvqs,
diff --git a/lib/rpmsg/rpmsg_virtio.c b/lib/rpmsg/rpmsg_virtio.c
index c7ef1b5a0..4a454e9c5 100644
--- a/lib/rpmsg/rpmsg_virtio.c
+++ b/lib/rpmsg/rpmsg_virtio.c
@@ -151,8 +151,8 @@ static void *rpmsg_virtio_get_tx_buffer(struct rpmsg_virtio_device *rvdev,
 		data = virtqueue_get_buffer(rvdev->svq, (uint32_t *)len, idx);
 		if (data == NULL) {
 			data = rpmsg_virtio_shm_pool_get_buffer(rvdev->shpool,
-							RPMSG_BUFFER_SIZE);
-			*len = RPMSG_BUFFER_SIZE;
+							rvdev->shbuf_size);
+			*len = rvdev->shbuf_size;
 		}
 	}
 #endif /*!VIRTIO_SLAVE_ONLY*/
@@ -258,7 +258,7 @@ static int _rpmsg_virtio_get_buffer_size(struct rpmsg_virtio_device *rvdev)
 		 * If device role is Remote then buffers are provided by us
 		 * (RPMSG Master), so just provide the macro.
 		 */
-		length = RPMSG_BUFFER_SIZE - sizeof(struct rpmsg_hdr);
+		length = rvdev->shbuf_size - sizeof(struct rpmsg_hdr);
 	}
 #endif /*!VIRTIO_SLAVE_ONLY*/
 
@@ -545,6 +545,14 @@ int rpmsg_init_vdev(struct rpmsg_virtio_device *rvdev,
 	rdev->ns_bind_cb = ns_bind_cb;
 	vdev->priv = rvdev;
 	rdev->ops.send_offchannel_raw = rpmsg_virtio_send_offchannel_raw;
+
+	if (vdev->features & (1 << VIRTIO_RPMSG_F_BUFSZ)) {
+		rpmsg_virtio_read_config(rvdev, 0,
+					 &rvdev->shbuf_size,
+					 sizeof(rvdev->shbuf_size));
+	} else
+		rvdev->shbuf_size = RPMSG_BUFFER_SIZE;
+
 	role = rpmsg_virtio_get_role(rvdev);
 
 #ifndef VIRTIO_SLAVE_ONLY
@@ -608,11 +616,11 @@ int rpmsg_init_vdev(struct rpmsg_virtio_device *rvdev,
 		unsigned int idx;
 		void *buffer;
 
-		vqbuf.len = RPMSG_BUFFER_SIZE;
+		vqbuf.len = rvdev->shbuf_size;
 		for (idx = 0; idx < rvdev->rvq->vq_nentries; idx++) {
 			/* Initialize TX virtqueue buffers for remote device */
 			buffer = rpmsg_virtio_shm_pool_get_buffer(shpool,
-							RPMSG_BUFFER_SIZE);
+							rvdev->shbuf_size);
 
 			if (!buffer) {
 				return RPMSG_ERR_NO_BUFF;
@@ -623,7 +631,7 @@ int rpmsg_init_vdev(struct rpmsg_virtio_device *rvdev,
 			metal_io_block_set(shm_io,
 					   metal_io_virt_to_offset(shm_io,
 								   buffer),
-					   0x00, RPMSG_BUFFER_SIZE);
+					   0x00, rvdev->shbuf_size);
 			status =
 				virtqueue_add_buffer(rvdev->rvq, &vqbuf, 0, 1,
 						     buffer);