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) {