diff --git a/lib/include/openamp/virtqueue.h b/lib/include/openamp/virtqueue.h index 81943987..865129ad 100644 --- a/lib/include/openamp/virtqueue.h +++ b/lib/include/openamp/virtqueue.h @@ -72,6 +72,21 @@ struct virtqueue_buf { int len; }; +/** @brief Virtqueue buffers descriptor. */ +struct virtqueue_bufs { + /** The descriptor index of the first available buffer */ + uint16_t head; + + /** The capacity of the virtqueue buffers */ + unsigned int vb_capacity; + + /** The real number of the virtqueue buffers */ + unsigned int vb_num; + + /** The virtqueue buffers */ + struct virtqueue_buf vb[0]; +}; + /** @brief Vring descriptor extra information for buffer list management. */ struct vq_desc_extra { /** Pointer to first descriptor. */ @@ -289,8 +304,20 @@ void *virtqueue_get_buffer(struct virtqueue *vq, uint32_t *len, uint16_t *idx); * * @return Pointer to available buffer */ -void *virtqueue_get_available_buffer(struct virtqueue *vq, uint16_t *avail_idx, - uint32_t *len); +void *virtqueue_get_first_avail_buffer(struct virtqueue *vq, uint16_t *avail_idx, + uint32_t *len); + +/** + * @internal + * + * @brief Returns buffer available for use in the VirtIO queue + * + * @param vq Pointer to VirtIO queue control block + * @param vbs Pointer to virtqueue buffers to store available buffers + * + * @return Function status + */ +int virtqueue_get_available_buffer(struct virtqueue *vq, struct virtqueue_bufs *vbs); /** * @internal diff --git a/lib/rpmsg/rpmsg_virtio.c b/lib/rpmsg/rpmsg_virtio.c index ac75c836..6ff9a5bf 100644 --- a/lib/rpmsg/rpmsg_virtio.c +++ b/lib/rpmsg/rpmsg_virtio.c @@ -207,7 +207,7 @@ static void *rpmsg_virtio_get_tx_buffer(struct rpmsg_virtio_device *rvdev, *idx = 0; } } else if (VIRTIO_ROLE_IS_DEVICE(rvdev->vdev)) { - data = virtqueue_get_available_buffer(rvdev->svq, idx, len); + data = virtqueue_get_first_avail_buffer(rvdev->svq, idx, len); } return data; @@ -235,7 +235,7 @@ static void *rpmsg_virtio_get_rx_buffer(struct rpmsg_virtio_device *rvdev, if (VIRTIO_ROLE_IS_DEVICE(rvdev->vdev)) { data = - virtqueue_get_available_buffer(rvdev->rvq, idx, len); + virtqueue_get_first_avail_buffer(rvdev->rvq, idx, len); } /* Invalidate the buffer before returning it */ diff --git a/lib/virtio/virtqueue.c b/lib/virtio/virtqueue.c index 363fda8a..442f3c64 100644 --- a/lib/virtio/virtqueue.c +++ b/lib/virtio/virtqueue.c @@ -23,6 +23,8 @@ static int vq_ring_must_notify(struct virtqueue *vq); static void vq_ring_notify(struct virtqueue *vq); static int virtqueue_nused(struct virtqueue *vq); static int virtqueue_navail(struct virtqueue *vq); +static void *virtqueue_get_next_avail_buffer(struct virtqueue *vq, uint16_t idx, + uint16_t *next_idx, uint32_t *next_len); /* Default implementation of P2V based on libmetal */ static inline void *virtqueue_phys_to_virt(struct virtqueue *vq, @@ -200,8 +202,8 @@ void virtqueue_free(struct virtqueue *vq) } } -void *virtqueue_get_available_buffer(struct virtqueue *vq, uint16_t *avail_idx, - uint32_t *len) +void *virtqueue_get_first_avail_buffer(struct virtqueue *vq, uint16_t *avail_idx, + uint32_t *len) { uint16_t head_idx = 0; void *buffer; @@ -231,6 +233,40 @@ void *virtqueue_get_available_buffer(struct virtqueue *vq, uint16_t *avail_idx, return buffer; } +int virtqueue_get_available_buffer(struct virtqueue *vq, struct virtqueue_bufs *vbs) +{ + unsigned int i; + uint16_t head; + uint16_t idx; + uint32_t len; + void *buf; + + buf = virtqueue_get_first_avail_buffer(vq, &head, &len); + if (!buf) + return ERROR_VRING_NO_BUFF; + + vbs->head = head; + vbs->vb[0].buf = buf; + vbs->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 >= vbs->vb_capacity) { + metal_log(METAL_LOG_ERROR, "capacity %u is not enough\n", + vbs->vb_capacity); + return ERROR_VQUEUE_INVLD_PARAM; + } + + vbs->vb[i].buf = buf; + vbs->vb[i].len = len; + } + + vbs->vb_num = i; + return 0; +} + int virtqueue_add_consumed_buffer(struct virtqueue *vq, uint16_t head_idx, uint32_t len) { @@ -697,3 +733,25 @@ static int virtqueue_navail(struct virtqueue *vq) return navail; } + +static 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; +}