From cdb4e013c3639a82219d9a02834ef611b4da85d3 Mon Sep 17 00:00:00 2001
From: Bowen Wang <wangbowen6@xiaomi.com>
Date: Mon, 4 Nov 2024 10:26:52 +0800
Subject: [PATCH] virtqueue: add virtqueue_get_next_avail_buffer() and
 virtqueue_get_available_buffers()

In virtio device side, we always need to get the next avaiable
buffer based on current buffer index. So add these two APIs for
convinience use.

For example, virtio blk driver origanize the buffer:
+----------+
| Reqeust  | (Flags: Read | Next)
+----------+
| Buffer   | (Flags: Read/Write | Next)
+----------+
| Response | (Flags: Write)
+----------+

For the virtio blk device size, we need get the Buffer and Response buffer
based on the Request buffer index.

Signed-off-by: Bowen Wang <wangbowen6@xiaomi.com>
Signed-off-by: Yongrong Wang <wangyongrong@xiaomi.com>
---
 lib/include/openamp/virtqueue.h | 31 ++++++++++++++++++
 lib/virtio/virtqueue.c          | 57 +++++++++++++++++++++++++++++++++
 2 files changed, 88 insertions(+)

diff --git a/lib/include/openamp/virtqueue.h b/lib/include/openamp/virtqueue.h
index 81943987..b24679d9 100644
--- a/lib/include/openamp/virtqueue.h
+++ b/lib/include/openamp/virtqueue.h
@@ -292,6 +292,37 @@ void *virtqueue_get_buffer(struct virtqueue *vq, uint32_t *len, uint16_t *idx);
 void *virtqueue_get_available_buffer(struct virtqueue *vq, uint16_t *avail_idx,
 				     uint32_t *len);
 
+/**
+ * @internal
+ *
+ * @brief Returns next available buffer in the VirtIO queue
+ *
+ * @param vq		Pointer to VirtIO queue control block
+ * @param idx		Index of the buffer in vring desc table
+ * @param next_idx	Pointer to index of next buffer in vring desc table
+ * @param next_len	Pointer to length of next buffer in vring desc table
+ *
+ * @return Pointer to next available buffer
+ */
+void *virtqueue_get_next_avail_buffer(struct virtqueue *vq, uint16_t idx,
+				      uint16_t *next_idx, uint32_t *next_len);
+
+/**
+ * @internal
+ *
+ * @brief Get all the available buffers in the VirtIO queue
+ *
+ * @param vq		Pointer to VirtIO queue control block
+ * @param vb		Pointer to list of buffers
+ * @param vbsize	Size of buffer list
+ * @param vbcnt		Number of buffers returned
+ *
+ * @return 0 on success, otherwise error code.
+ */
+int virtqueue_get_available_buffers(struct virtqueue *vq,
+				    struct virtqueue_buf *vb, int vbsize,
+				    int *vbcnt);
+
 /**
  * @internal
  *
diff --git a/lib/virtio/virtqueue.c b/lib/virtio/virtqueue.c
index 363fda8a..edb87b08 100644
--- a/lib/virtio/virtqueue.c
+++ b/lib/virtio/virtqueue.c
@@ -231,6 +231,63 @@ void *virtqueue_get_available_buffer(struct virtqueue *vq, uint16_t *avail_idx,
 	return buffer;
 }
 
+void *virtqueue_get_next_avail_buffer(struct virtqueue *vq, uint16_t idx,
+				      uint16_t *next_idx, uint32_t *next_len)
+{
+	void *buffer;
+	uint16_t next;
+
+	VRING_INVALIDATE(vq->vq_ring.desc[idx], sizeof(struct vring_desc));
+	if (!(vq->vq_ring.desc[idx].flags & VRING_DESC_F_NEXT))
+		return NULL;
+
+	next = vq->vq_ring.desc[idx].next;
+	if (next_idx)
+		*next_idx = next;
+
+	VRING_INVALIDATE(vq->vq_ring.desc[next], sizeof(struct vring_desc));
+	buffer = virtqueue_phys_to_virt(vq, vq->vq_ring.desc[next].addr);
+	if (next_len)
+		*next_len = vq->vq_ring.desc[next].len;
+
+	return buffer;
+}
+
+int virtqueue_get_available_buffers(struct virtqueue *vq,
+				    struct virtqueue_buf *vb, int vbsize,
+				    int *vbcnt)
+{
+	uint16_t head;
+	uint16_t idx;
+	uint32_t len;
+	void *buf;
+	int i;
+
+	buf = virtqueue_get_available_buffer(vq, &head, &len);
+	if (!buf)
+		return ERROR_VRING_NO_BUFF;
+
+	vb[0].buf = buf;
+	vb[0].len = len;
+
+	for (i = 1, idx = head; ; i++) {
+		buf = virtqueue_get_next_avail_buffer(vq, idx, &idx, &len);
+		if (!buf)
+			break;
+		else if (i >= vbsize) {
+			metal_log(METAL_LOG_ERROR, "vbsize %d is not enough\n",
+				  vbsize);
+			return ERROR_VQUEUE_INVLD_PARAM;
+		}
+
+		vb[i].buf = buf;
+		vb[i].len = len;
+	}
+
+	*vbcnt = i;
+	return head;
+}
+
 int virtqueue_add_consumed_buffer(struct virtqueue *vq, uint16_t head_idx,
 				  uint32_t len)
 {