springcute
/
rt-thread
mirror of https://github.com/RT-Thread/rt-thread.git
0
0
Fork 4
Code Issues Releases Wiki Activity
rt-thread/components/drivers/virtio/virtio_queue.c

1220 lines
31 KiB
C
Raw Permalink Blame History

/*
* Copyright (c) 2006-2023, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date Author Notes
* 2023-02-25 GuEe-GUI the first version
*/
#include <rthw.h>
#include <rtthread.h>
#define DBG_TAG "virtio.queue"
#define DBG_LVL DBG_INFO
#include <rtdbg.h>
#include <mm_aspace.h>
#include <drivers/dma.h>
#include <drivers/virtq.h>
#include <drivers/virtio.h>
#include "virtio_internal.h"
struct virtq_buffer_shadow
{
rt_ubase_t dma_handle;
void *dma_buf;
rt_size_t size;
rt_size_t chain_num;
};
struct virtqueue_split
{
struct virtq virtq;
rt_ubase_t virtq_dma;
rt_uint16_t avail_idx_shadow;
rt_uint16_t avail_flags_shadow;
struct virtq_buffer_shadow buffer_shadow[0];
};
struct virtqueue_packed
{
struct virtq virtq;
rt_size_t virtq_size;
rt_ubase_t virtq_dma;
rt_ubase_t driver_event_dma;
rt_ubase_t device_event_dma;
rt_uint16_t last_used_idx;
rt_uint16_t avail_used_flags;
rt_uint16_t avail_wrap_counter;
rt_uint16_t avail_flags_shadow;
rt_uint16_t event_flags_shadow;
struct virtq_buffer_shadow buffer_shadow[0];
};
static struct rt_virtqueue *vq_split_create(struct rt_virtio_device *vdev,
const char *name, int index, int num, rt_uint32_t align,
rt_virtqueue_notifier notify, rt_virtqueue_callback callback,
struct rt_virtqueue_formula *formula)
{
void *pages;
rt_size_t size;
struct virtqueue_split *vq_split;
struct rt_virtqueue *vq = RT_NULL;
struct virtq_buffer_shadow *buffer_shadow;
/*
* Layout:
* +-----------------+
* | rt_virtqueue |
* +-----------------+
* | virtqueue_split |
* +-----------------+
* | buffer_shadow |
* +-----------------+
*/
vq = rt_malloc(sizeof(*vq) +
sizeof(struct virtqueue_split) +
sizeof(struct virtq_buffer_shadow) * num);
if (!vq)
{
return RT_NULL;
}
vq_split = (void *)vq + sizeof(*vq);
buffer_shadow = (void *)vq_split + sizeof(*vq_split);
rt_memset(buffer_shadow, 0, sizeof(*buffer_shadow) * num);
/*
* +------------------+-----------+----------------------+
* | Virtqueue Part | Alignment | Size |
* +------------------+-----------+----------------------+
* | Descriptor Table | 16 | 16 * (Queue Size) |
* +------------------+-----------+----------------------+
* | Available Ring | 2 | 6 + 2 * (Queue Size) |
* +------------------+-----------+----------------------+
* | Used Ring | 4 | 6 + 8 * (Queue Size) |
* +------------------+-----------+----------------------+
*/
size = virtq_size(RT_NULL, num, align);
if (formula->page)
{
pages = formula->page;
}
else
{
pages = rt_dma_alloc(&vdev->parent, size, &vq_split->virtq_dma, RT_DMA_F_LINEAR);
if (!pages)
{
rt_free(vq);
return RT_NULL;
}
}
rt_memset(pages, 0, size);
virtq_init(&vq_split->virtq, num, pages, align);
vq_split->avail_idx_shadow = 0;
vq_split->avail_flags_shadow = 0;
vq->name = name;
vq->index = index;
vq->num_free = num;
vq->callback = callback;
vq->vq_split = vq_split;
vq->packed_ring = RT_FALSE;
vq->notify = notify;
vq->event = rt_virtio_has_feature(vdev, VIRTIO_F_RING_EVENT_IDX);
vq->event_triggered = RT_FALSE;
vq->free_head = 0;
vq->num_added = 0;
vq->last_used_idx = 0;
vq->vdev = vdev;
if (!vq->callback)
{
vq_split->avail_flags_shadow |= VIRTQ_AVAIL_F_NO_INTERRUPT;
if (!vq->event)
{
vq_split->virtq.avail->flags = cpu_to_virtio16(vdev,
vq_split->avail_flags_shadow);
}
}
rt_memcpy(&vq->formula, formula, sizeof(*formula));
return vq;
}
static rt_err_t vq_split_delete(struct rt_virtqueue *vq)
{
rt_err_t err = RT_EOK;
struct virtq *virtq;
struct virtqueue_split *vq_split = vq->vq_split;
struct rt_virtqueue_formula *formula = &vq->formula;
if (!formula->page)
{
virtq = &vq_split->virtq;
rt_dma_free(&vq->vdev->parent, virtq_size(virtq, 0, 0),
(void *)virtq->desc, vq_split->virtq_dma, RT_DMA_F_LINEAR);
}
rt_free(vq);
return err;
}
static rt_err_t vq_split_add_buf(struct rt_virtqueue *vq,
void *dma_buf, rt_size_t size, rt_bool_t is_out)
{
rt_err_t err;
rt_uint16_t buf_id;
rt_ubase_t dma_addr;
rt_size_t virtq_num;
int head, next, flags;
struct virtq *virtq;
struct virtq_desc *desc;
struct virtqueue_split *vq_split;
struct virtq_buffer_shadow *buffer_shadow;
struct rt_virtio_device *vdev = vq->vdev;
if (!vq->num_free)
{
LOG_D("%s.virtqueue[%s(%d)] add buffer.len = %d fail",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index, size);
/* Recvice buffer NOW! */
if (is_out)
{
vq->notify(vq);
}
return -RT_EFULL;
}
err = rt_dma_sync_out_data(&vdev->parent, dma_buf, size, &dma_addr, RT_DMA_F_LINEAR);
if (err)
{
return err;
}
vq_split = vq->vq_split;
virtq = &vq_split->virtq;
virtq_num = virtq->num;
head = vq->free_head++ & (virtq_num - 1);
rt_hw_dsb();
next = vq->free_head & (virtq_num - 1);
desc = &virtq->desc[head];
buffer_shadow = &vq_split->buffer_shadow[head];
buffer_shadow->dma_handle = dma_addr;
buffer_shadow->dma_buf = dma_buf;
buffer_shadow->size = size;
buf_id = (head - vq->num_added) & (virtq_num - 1);
vq_split->buffer_shadow[buf_id].chain_num = vq->num_added + 1;
flags = VIRTQ_DESC_F_NEXT;
if (!is_out)
{
flags |= VIRTQ_DESC_F_WRITE;
}
desc->addr = cpu_to_virtio64(vdev, dma_addr);
desc->len = cpu_to_virtio32(vdev, size);
desc->flags = cpu_to_virtio16(vdev, flags);
desc->next = cpu_to_virtio16(vdev, next);
/* Update index */
++vq->num_added;
--vq->num_free;
LOG_D("%s.virtqueue[%s(%d)] add buffer(%p, size = %d) head = %d",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index, dma_buf, size, head);
return RT_EOK;
}
static rt_bool_t vq_split_submit(struct rt_virtqueue *vq)
{
int head, prev;
rt_uint16_t avail, num_added;
struct rt_virtio_device *vdev = vq->vdev;
struct virtqueue_split *vq_split = vq->vq_split;
struct virtq *virtq = &vq_split->virtq;
rt_size_t virtq_num = virtq->num;
num_added = vq->num_added;
head = vq->free_head;
prev = (head - 1) & (virtq_num - 1);
head = head - num_added;
LOG_D("%s.virtqueue[%s(%d)] submit head = %d, num_added = %d, idx = %d",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index,
head & (virtq_num - 1), num_added, virtq->avail->idx);
/* Reset list info */
vq->num_added = 0;
/* Clear last "next" flags */
virtq->desc[prev].flags &= ~cpu_to_virtio16(vdev, VIRTQ_DESC_F_NEXT);
virtq->desc[prev].next = 0;
/* Tell the device the first index in our chain of descriptors */
avail = vq_split->avail_idx_shadow & (virtq_num - 1);
virtq->avail->ring[avail] = cpu_to_virtio32(vdev, head & (virtq_num - 1));
/* Tell the device another avail ring entry is available */
rt_hw_wmb();
++vq_split->avail_idx_shadow;
virtq->avail->idx = cpu_to_virtio16(vdev, vq_split->avail_idx_shadow);
if (vq->event)
{
rt_uint16_t old = vq_split->avail_idx_shadow - num_added;
rt_uint16_t new = vq_split->avail_idx_shadow;
return virtq_need_event(virtio16_to_cpu(vdev,
*virtq_avail_event(virtq)), new, old);
}
return !(virtq->used->flags & cpu_to_virtio16(vdev, VIRTQ_USED_F_NO_NOTIFY));
}
static rt_bool_t vq_split_poll(struct rt_virtqueue *vq, rt_uint32_t last_used_idx)
{
return (rt_uint16_t)last_used_idx != virtio16_to_cpu(vq->vdev, vq->vq_split->virtq.used->idx);
}
rt_inline rt_bool_t vq_split_pending(struct rt_virtqueue *vq)
{
return vq->last_used_idx != virtio16_to_cpu(vq->vdev, vq->vq_split->virtq.used->idx);
}
static void vq_split_disable_callback(struct rt_virtqueue *vq)
{
struct virtqueue_split *vq_split = vq->vq_split;
struct virtq *virtq = &vq_split->virtq;
if (!(vq_split->avail_flags_shadow & VIRTQ_AVAIL_F_NO_INTERRUPT))
{
vq_split->avail_flags_shadow |= VIRTQ_AVAIL_F_NO_INTERRUPT;
if (vq->event)
{
*virtq_used_event(virtq) = 0;
}
else
{
virtq->avail->flags = cpu_to_virtio16(vq->vdev, vq_split->avail_flags_shadow);
}
}
}
static rt_uint32_t vq_split_enable_callback(struct rt_virtqueue *vq)
{
struct virtqueue_split *vq_split = vq->vq_split;
struct virtq *virtq = &vq_split->virtq;
rt_uint16_t last_used_idx = vq->last_used_idx;
if (vq_split->avail_flags_shadow & VIRTQ_AVAIL_F_NO_INTERRUPT)
{
vq_split->avail_flags_shadow &= ~VIRTQ_AVAIL_F_NO_INTERRUPT;
if (!vq->event)
{
virtq->avail->flags = cpu_to_virtio16(vq->vdev, vq_split->avail_flags_shadow);
}
}
*virtq_used_event(virtq) = cpu_to_virtio16(vq->vdev, last_used_idx);
return last_used_idx;
}
static void *vq_split_read_buf(struct rt_virtqueue *vq, rt_size_t *out_len)
{
void *buf;
rt_err_t err;
rt_uint32_t idx;
rt_uint16_t last_used, next;
rt_size_t chain_num;
struct virtq *virtq;
struct virtqueue_split *vq_split;
struct virtq_buffer_shadow *buffer_shadow;
struct rt_virtio_device *vdev = vq->vdev;
if (!vq_split_pending(vq))
{
LOG_D("%s.virtqueue[%s(%d)] read buffer empty",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index);
return RT_NULL;
}
vq_split = vq->vq_split;
virtq = &vq_split->virtq;
last_used = vq->last_used_idx & (virtq->num - 1);
next = idx = virtio32_to_cpu(vdev, virtq->used->ring[last_used].id);
*out_len = virtio32_to_cpu(vdev, virtq->used->ring[last_used].len);
rt_hw_dsb();
buffer_shadow = &vq_split->buffer_shadow[idx];
buf = buffer_shadow->dma_buf;
chain_num = buffer_shadow->chain_num;
LOG_D("%s.virtqueue[%s(%d)] read head = %d, buffer(%p, size = %d)",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index, idx, buf, *out_len);
for (int i = 0; i < chain_num; ++i)
{
idx = next;
next = virtq->desc[idx].next;
if (virtq->desc[idx].flags & VIRTQ_DESC_F_WRITE ||
vdev->dma_dispatch)
{
buffer_shadow = &vq_split->buffer_shadow[idx];
err = rt_dma_sync_in_data(&vdev->parent,
buffer_shadow->dma_buf,
buffer_shadow->size,
buffer_shadow->dma_handle, RT_DMA_F_LINEAR);
if (err)
{
LOG_E("%s.virtqueue[%s(%d)] read head = %d sync error = %s",
rt_dm_dev_get_name(&vdev->parent),
vq->name, vq->index, idx, rt_strerror(err));
}
}
}
vq->num_free += chain_num;
++vq->last_used_idx;
if (!(vq_split->avail_flags_shadow & VIRTQ_AVAIL_F_NO_INTERRUPT))
{
HWREG16(virtq_used_event(virtq)) = cpu_to_virtio16(vq->vdev, vq->last_used_idx);
rt_hw_dmb();
}
rt_hw_dsb();
return buf;
}
static struct rt_virtqueue *vq_packed_create(struct rt_virtio_device *vdev,
const char *name, int index, int num, int align,
rt_virtqueue_notifier notify, rt_virtqueue_callback callback,
struct rt_virtqueue_formula *formula)
{
rt_size_t size, event_size;
struct virtq *virtq;
struct virtqueue_packed *vq_packed;
struct rt_virtqueue *vq = RT_NULL;
struct virtq_buffer_shadow *buffer_shadow;
/*
* Layout:
* +------------------------+
* | rt_virtqueue |
* +------------------------+
* | virtqueue_packed |
* +------------------------+
* | buffer_shadow |
* +------------------------+
*/
vq = rt_malloc(sizeof(*vq) +
sizeof(struct virtqueue_packed) +
sizeof(struct virtq_buffer_shadow) * num);
if (!vq)
{
return RT_NULL;
}
vq_packed = (void *)vq + sizeof(*vq);
buffer_shadow = (void *)vq_packed + sizeof(*vq_packed);
rt_memset(buffer_shadow, 0, sizeof(*buffer_shadow) * num);
/*
* +--------------------------+-----------+-------------------+
* | Virtqueue Part | Alignment | Size |
* +--------------------------+-----------+-------------------+
* | Descriptor Ring | 16 | 16 * (Queue Size) |
* +--------------------------+-----------+-------------------+
* | Device Event Suppression | 4 | 4 |
* +--------------------------+-----------+-------------------+
* | Driver Event Suppression | 4 | 4 |
* +--------------------------+-----------+-------------------+
*/
virtq = &vq_packed->virtq;
size = num * sizeof(struct virtq_packed_desc);
size = RT_ALIGN(size, 4);
event_size = sizeof(struct virtq_packed_desc_event);
vq_packed->virtq_size = size + event_size * 2;
if (formula->page)
{
virtq->desc_packed = formula->page;
}
else
{
virtq->desc_packed = rt_dma_alloc(&vdev->parent,
vq_packed->virtq_size, &vq_packed->virtq_dma, RT_DMA_F_LINEAR);
if (!virtq->desc_packed)
{
rt_free(vq);
return RT_NULL;
}
}
virtq->driver_event = (void *)virtq->desc_packed + size;
vq_packed->driver_event_dma = vq_packed->virtq_dma + size;
virtq->device_event = (void *)virtq->driver_event + event_size;
vq_packed->device_event_dma = vq_packed->driver_event_dma + event_size;
rt_memset(virtq->desc_packed, 0, size);
rt_memset(virtq->driver_event, 0, event_size);
rt_memset(virtq->device_event, 0, event_size);
virtq->num = num;
virtq->align = align;
vq_packed->avail_wrap_counter = 1;
vq_packed->event_flags_shadow = 0;
vq_packed->avail_used_flags = VIRTQ_DESC_F_AVAIL;
vq->name = name;
vq->index = index;
vq->num_free = num;
vq->callback = callback;
vq->vq_packed = vq_packed;
vq->packed_ring = RT_TRUE;
vq->notify = notify;
vq->event = rt_virtio_has_feature(vdev, VIRTIO_F_RING_EVENT_IDX);
vq->event_triggered = RT_FALSE;
vq->free_head = 0;
vq->num_added = 0;
vq->last_used_idx = 0 | RT_BIT(VIRTQ_PACKED_EVENT_WRAP_CTR);
vq->vdev = vdev;
if (!vq->callback)
{
vq_packed->avail_flags_shadow = VIRTQ_PACKED_EVENT_FLAG_DISABLE;
vq_packed->virtq.avail->flags = rt_cpu_to_le16(vq_packed->avail_flags_shadow);
}
rt_memcpy(&vq->formula, formula, sizeof(*formula));
return vq;
}
static rt_err_t vq_packed_delete(struct rt_virtqueue *vq)
{
rt_err_t err = RT_EOK;
struct virtq *virtq;
struct virtqueue_packed *vq_packed = vq->vq_packed;
struct rt_virtqueue_formula *formula = &vq->formula;
virtq = &vq_packed->virtq;
if (virtq->desc_packed && !formula->page)
{
rt_dma_free(&vq->vdev->parent, vq_packed->virtq_size,
(void *)virtq->desc_packed, vq_packed->virtq_dma, RT_DMA_F_LINEAR);
}
rt_free(vq);
return err;
}
static rt_err_t vq_packed_add_buf(struct rt_virtqueue *vq,
void *dma_buf, rt_size_t size, rt_bool_t is_out)
{
rt_err_t err;
rt_uint16_t buf_id;
rt_ubase_t dma_addr;
rt_size_t virtq_num;
int head, flags;
struct virtq *virtq;
struct virtqueue_packed *vq_packed;
struct virtq_packed_desc *desc_packed;
struct virtq_buffer_shadow *buffer_shadow;
struct rt_virtio_device *vdev = vq->vdev;
if (!vq->num_free)
{
LOG_D("%s.virtqueue[%s(%d)] add buffer.len = %d fail",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index, size);
/* Recvice buffer NOW! */
if (is_out)
{
vq->notify(vq);
}
return -RT_EFULL;
}
err = rt_dma_sync_out_data(&vdev->parent, dma_buf, size, &dma_addr, RT_DMA_F_LINEAR);
if (err)
{
return err;
}
vq_packed = vq->vq_packed;
virtq = &vq_packed->virtq;
virtq_num = virtq->num;
head = vq->free_head++ & (virtq_num - 1);
rt_hw_dsb();
desc_packed = &virtq->desc_packed[head];
buffer_shadow = &vq_packed->buffer_shadow[head];
buffer_shadow->dma_handle = dma_addr;
buffer_shadow->dma_buf = dma_buf;
buffer_shadow->size = size;
buf_id = (head - vq->num_added) & (virtq_num - 1);
vq_packed->buffer_shadow[buf_id].chain_num = vq->num_added + 1;
flags = VIRTQ_DESC_F_NEXT;
if (!is_out)
{
flags |= VIRTQ_DESC_F_WRITE;
}
desc_packed->addr = rt_cpu_to_le64(dma_addr);
desc_packed->len = rt_cpu_to_le32(size);
desc_packed->id = rt_cpu_to_le16(buf_id);
desc_packed->flags = rt_cpu_to_le16(vq_packed->avail_used_flags | flags);
/* Ready fot next */
if (head + 1 >= virtq->num)
{
vq_packed->avail_used_flags ^= VIRTQ_DESC_F_AVAIL | VIRTQ_DESC_F_USED;
/* Toggle the wrap counter */
vq_packed->avail_wrap_counter ^= 1;
}
rt_hw_wmb();
/* Update index */
++vq->num_added;
--vq->num_free;
LOG_D("%s.virtqueue[%s(%d)] add buffer(%p, size = %d) head = %d",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index, dma_buf, size, head);
return RT_EOK;
}
static rt_bool_t vq_packed_submit(struct rt_virtqueue *vq)
{
int head, prev;
rt_uint16_t off_wrap, flags, wrap_counter, event_idx;
struct virtq_packed_desc_event device_event;
struct virtqueue_packed *vq_packed = vq->vq_packed;
struct virtq *virtq = &vq_packed->virtq;
rt_size_t virtq_num = virtq->num;
rt_hw_dmb();
rt_memcpy(&device_event, virtq->device_event, sizeof(device_event));
head = vq->free_head;
prev = (head - 1) & (virtq_num - 1);
head = head - vq->num_added;
LOG_D("%s.virtqueue[%s(%d)] submit head = %d, num_added = %d, idx = %d",
rt_dm_dev_get_name(&vq->vdev->parent), vq->name, vq->index,
head & (virtq_num - 1), vq->num_added, rt_le16_to_cpu(device_event.off_wrap) &
~RT_BIT(VIRTQ_PACKED_EVENT_WRAP_CTR));
/* Reset list info */
vq->num_added = 0;
/* Clear last "next" flags */
virtq->desc_packed[prev].flags &= ~rt_cpu_to_le16(VIRTQ_DESC_F_NEXT);
flags = rt_le16_to_cpu(device_event.flags);
if (flags != VIRTQ_PACKED_EVENT_FLAG_DESC)
{
return (flags != VIRTQ_PACKED_EVENT_FLAG_DISABLE);
}
off_wrap = rt_le16_to_cpu(device_event.off_wrap);
wrap_counter = off_wrap >> VIRTQ_PACKED_EVENT_WRAP_CTR;
event_idx = off_wrap & ~RT_BIT(VIRTQ_PACKED_EVENT_WRAP_CTR);
if (wrap_counter != vq_packed->avail_wrap_counter)
{
event_idx -= virtq->num;
}
return virtq_need_event(event_idx, prev, head);
}
rt_inline rt_bool_t is_used_desc_packed(const struct virtq *virtq,
rt_uint16_t idx, rt_bool_t used_wrap_counter)
{
rt_uint16_t flags;
rt_bool_t avail, used;
flags = rt_le16_to_cpu(virtq->desc_packed[idx].flags);
avail = !!(flags & VIRTQ_DESC_F_AVAIL);
used = !!(flags & VIRTQ_DESC_F_USED);
return avail == used && used == used_wrap_counter;
}
static rt_bool_t packed_used_wrap_counter(rt_uint16_t last_used_idx)
{
return !!(last_used_idx & RT_BIT(VIRTQ_PACKED_EVENT_WRAP_CTR));
}
static rt_uint16_t packed_last_used(rt_uint16_t last_used_idx)
{
return last_used_idx & ~(-RT_BIT(VIRTQ_PACKED_EVENT_WRAP_CTR));
}
static rt_bool_t vq_packed_poll(struct rt_virtqueue *vq, rt_uint32_t last_used_idx)
{
rt_bool_t wrap_counter;
rt_uint16_t off_wrap = last_used_idx, used_idx;
wrap_counter = off_wrap >> VIRTQ_PACKED_EVENT_WRAP_CTR;
used_idx = off_wrap & ~RT_BIT(VIRTQ_PACKED_EVENT_WRAP_CTR);
return is_used_desc_packed(&vq->vq_packed->virtq, used_idx, wrap_counter);
}
rt_inline rt_bool_t vq_packed_pending(struct rt_virtqueue *vq)
{
rt_bool_t wrap_counter;
rt_uint16_t last_used, last_used_idx;
last_used_idx = HWREG16(&vq->last_used_idx);
last_used = packed_last_used(last_used_idx);
wrap_counter = packed_used_wrap_counter(last_used_idx);
return is_used_desc_packed(&vq->vq_packed->virtq, last_used, wrap_counter);
}
static void vq_packed_disable_callback(struct rt_virtqueue *vq)
{
struct virtqueue_packed *vq_packed = vq->vq_packed;
if (vq_packed->event_flags_shadow != VIRTQ_PACKED_EVENT_FLAG_DISABLE)
{
vq_packed->event_flags_shadow = VIRTQ_PACKED_EVENT_FLAG_DISABLE;
/*
* If device triggered an event already it won't trigger one again:
* no need to disable.
*/
if (vq->event_triggered)
{
return;
}
vq_packed->virtq.driver_event->flags = rt_cpu_to_le16(vq_packed->event_flags_shadow);
}
}
static rt_uint32_t vq_packed_enable_callback(struct rt_virtqueue *vq)
{
struct virtqueue_packed *vq_packed = vq->vq_packed;
if (vq->event)
{
vq_packed->virtq.driver_event->off_wrap = rt_cpu_to_le16(vq->last_used_idx);
/* Update event offset and event wrap counter first before updating event flags. */
rt_hw_wmb();
}
if (vq_packed->event_flags_shadow == VIRTQ_PACKED_EVENT_FLAG_DISABLE)
{
vq_packed->event_flags_shadow = vq->event ?
VIRTQ_PACKED_EVENT_FLAG_DESC : VIRTQ_PACKED_EVENT_FLAG_ENABLE;
vq_packed->virtq.driver_event->flags =
rt_cpu_to_le16(vq_packed->event_flags_shadow);
}
return vq->last_used_idx;
}
static void *vq_packed_read_buf(struct rt_virtqueue *vq, rt_size_t *out_len)
{
void *buf;
rt_err_t err;
rt_bool_t wrap_counter;
rt_uint16_t last_used, last_used_idx, id;
rt_size_t chain_num;
struct virtq *virtq;
struct virtqueue_packed *vq_packed;
struct virtq_buffer_shadow *buffer_shadow;
struct rt_virtio_device *vdev = vq->vdev;
if (!vq_packed_pending(vq))
{
LOG_D("%s.virtqueue[%s(%d)] read buffer empty",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index);
return RT_NULL;
}
vq_packed = vq->vq_packed;
virtq = &vq_packed->virtq;
rt_hw_rmb();
last_used_idx = HWREG16(&vq->last_used_idx);
wrap_counter = packed_used_wrap_counter(last_used_idx);
last_used = packed_last_used(last_used_idx);
id = rt_le16_to_cpu(virtq->desc_packed[last_used].id);
*out_len = rt_le32_to_cpu(virtq->desc_packed[last_used].len);
buffer_shadow = &vq->vq_packed->buffer_shadow[id];
buf = buffer_shadow->dma_buf;
chain_num = buffer_shadow->chain_num;
LOG_D("%s.virtqueue[%s(%d)] read head = %d, buffer(%p, size = %d)",
rt_dm_dev_get_name(&vdev->parent), vq->name, vq->index, id, buf, *out_len);
for (int i = 0; i < chain_num; ++i)
{
if (virtq->desc_packed[id].flags & VIRTQ_DESC_F_WRITE ||
vdev->dma_dispatch)
{
buffer_shadow = &vq->vq_packed->buffer_shadow[id];
err = rt_dma_sync_in_data(&vdev->parent,
buffer_shadow->dma_buf,
buffer_shadow->size,
buffer_shadow->dma_handle, RT_DMA_F_LINEAR);
if (err)
{
LOG_E("%s.virtqueue[%s(%d)] read head = %d sync error = %s",
rt_dm_dev_get_name(&vdev->parent),
vq->name, vq->index, id, rt_strerror(err));
}
}
++id;
}
vq->num_free += chain_num;
last_used += chain_num;
if (last_used >= virtq->num)
{
last_used -= virtq->num;
wrap_counter ^= 1;
}
last_used |= (wrap_counter << VIRTQ_PACKED_EVENT_WRAP_CTR);
HWREG16(&vq->last_used_idx) = last_used;
if (vq_packed->event_flags_shadow == VIRTQ_PACKED_EVENT_FLAG_DESC)
{
HWREG16(&virtq->driver_event->off_wrap) = rt_cpu_to_le16(vq->last_used_idx);
rt_hw_dmb();
}
return buf;
}
rt_inline rt_bool_t virtqueue_pending(struct rt_virtqueue *vq)
{
rt_bool_t res = RT_FALSE;
if (vq->packed_ring)
{
res = vq_packed_pending(vq);
}
else
{
res = vq_split_pending(vq);
}
return res;
}
struct rt_virtqueue *rt_virtqueue_create(struct rt_virtio_device *vdev,
const char *name, int index, int num, rt_uint32_t align,
rt_virtqueue_notifier notify, rt_virtqueue_callback callback,
struct rt_virtqueue_formula *formula)
{
struct rt_virtqueue *vq = RT_NULL;
struct rt_virtqueue_formula default_formula = {};
RT_ASSERT(vdev != RT_NULL);
RT_ASSERT(num != 0);
RT_ASSERT((num & (num - 1)) == 0);
RT_ASSERT(notify != RT_NULL);
RT_ASSERT(name != RT_NULL);
formula = formula ? : &default_formula;
if (rt_virtio_has_feature(vdev, VIRTIO_F_RING_PACKED))
{
vq = vq_packed_create(vdev, name, index, num, align, notify, callback, formula);
}
else
{
vq = vq_split_create(vdev, name, index, num, align, notify, callback, formula);
}
if (vq)
{
if (!callback)
{
rt_virtqueue_disable_callback(vq);
}
rt_base_t level = rt_spin_lock_irqsave(&vdev->vq_lock);
rt_list_init(&vq->list);
rt_list_init(&vq->user_list);
rt_list_insert_before(&vdev->vq_node, &vq->list);
rt_spin_unlock_irqrestore(&vdev->vq_lock, level);
}
return vq;
}
rt_err_t rt_virtqueue_delete(struct rt_virtio_device *vdev, struct rt_virtqueue *vq)
{
rt_ubase_t level;
rt_err_t err = RT_EOK;
RT_ASSERT(vdev != RT_NULL);
RT_ASSERT(vq != RT_NULL);
level = rt_spin_lock_irqsave(&vdev->vq_lock);
while (virtqueue_pending(vq))
{
rt_spin_unlock_irqrestore(&vdev->vq_lock, level);
rt_thread_yield();
level = rt_spin_lock_irqsave(&vdev->vq_lock);
}
rt_list_remove(&vq->list);
rt_spin_unlock_irqrestore(&vdev->vq_lock, level);
if (vq->packed_ring)
{
vq_packed_delete(vq);
}
else
{
vq_split_delete(vq);
}
return err;
}
rt_err_t rt_virtqueue_add_outbuf(struct rt_virtqueue *vq, void *dma_buf, rt_size_t size)
{
rt_err_t err = -RT_ENOSYS;
RT_ASSERT(vq != RT_NULL);
RT_ASSERT(dma_buf != RT_NULL);
RT_ASSERT(size != 0);
if (vq->packed_ring)
{
err = vq_packed_add_buf(vq, dma_buf, size, RT_TRUE);
}
else
{
err = vq_split_add_buf(vq, dma_buf, size, RT_TRUE);
}
return err;
}
rt_err_t rt_virtqueue_add_inbuf(struct rt_virtqueue *vq, void *dma_buf, rt_size_t size)
{
rt_err_t err = -RT_ENOSYS;
RT_ASSERT(vq != RT_NULL);
RT_ASSERT(dma_buf != RT_NULL);
RT_ASSERT(size != 0);
if (vq->packed_ring)
{
err = vq_packed_add_buf(vq, dma_buf, size, RT_FALSE);
}
else
{
err = vq_split_add_buf(vq, dma_buf, size, RT_FALSE);
}
return err;
}
rt_bool_t rt_virtqueue_prepare(struct rt_virtqueue *vq, rt_uint32_t nr)
{
return vq->num_free >= nr;
}
void rt_virtqueue_wait_prepare(struct rt_virtqueue *vq, rt_uint32_t nr)
{
while (!rt_virtqueue_prepare(vq, nr))
{
rt_thread_yield();
rt_hw_cpu_relax();
}
}
rt_uint32_t rt_virtqueue_next_buf_index(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
return vq->free_head % (vq->packed_ring ?
vq->vq_packed->virtq.num : vq->vq_split->virtq.num);
}
rt_bool_t rt_virtqueue_submit(struct rt_virtqueue *vq)
{
rt_bool_t res = RT_FALSE;
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
res = vq_packed_submit(vq);
}
else
{
res = vq_split_submit(vq);
}
return res;
}
rt_bool_t rt_virtqueue_notify(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
if (!vq->notify(vq))
{
return RT_FALSE;
}
return RT_TRUE;
}
rt_bool_t rt_virtqueue_kick(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
if (rt_virtqueue_submit(vq))
{
return rt_virtqueue_notify(vq);
}
return RT_TRUE;
}
void rt_virtqueue_isr(int irq, struct rt_virtqueue *vq)
{
if (!virtqueue_pending(vq))
{
LOG_D("%s.virtqueue[%s(%d)] no buffer pending in %s",
rt_dm_dev_get_name(&vq->vdev->parent), vq->name, vq->index, "isr");
return;
}
if (vq->event)
{
vq->event_triggered = RT_TRUE;
}
if (vq->callback)
{
vq->callback(vq);
}
}
rt_bool_t rt_virtqueue_poll(struct rt_virtqueue *vq, rt_uint32_t last_used_idx)
{
rt_bool_t res = RT_FALSE;
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
res = vq_packed_poll(vq, last_used_idx);
}
else
{
res = vq_split_poll(vq, last_used_idx);
}
return res;
}
void rt_virtqueue_disable_callback(struct rt_virtqueue *vq)
{
if (vq->event_triggered)
{
return;
}
if (vq->packed_ring)
{
vq_packed_disable_callback(vq);
}
else
{
vq_split_disable_callback(vq);
}
}
rt_bool_t rt_virtqueue_enable_callback(struct rt_virtqueue *vq,
rt_uint32_t *out_last_used_idx)
{
rt_uint32_t last_used_idx;
if (vq->event_triggered)
{
vq->event_triggered = RT_FALSE;
}
if (vq->packed_ring)
{
last_used_idx = vq_packed_enable_callback(vq);
}
else
{
last_used_idx = vq_split_enable_callback(vq);
}
if (out_last_used_idx)
{
*out_last_used_idx = last_used_idx;
}
return !rt_virtqueue_poll(vq, last_used_idx);
}
void *rt_virtqueue_read_buf(struct rt_virtqueue *vq, rt_size_t *out_len)
{
void *buf = RT_NULL;
rt_size_t len = 0;
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
buf = vq_packed_read_buf(vq, &len);
}
else
{
buf = vq_split_read_buf(vq, &len);
}
if (len && out_len)
{
*out_len = len;
}
return buf;
}
rt_size_t rt_virtqueue_get_virtq_size(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
return (rt_ubase_t)vq->vq_packed->virtq.num;
}
else
{
return (rt_ubase_t)vq->vq_split->virtq.num;
}
}
rt_ubase_t rt_virtqueue_get_desc_addr(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
return (rt_ubase_t)vq->vq_packed->virtq.desc;
}
else
{
return (rt_ubase_t)vq->vq_split->virtq.desc;
}
}
rt_ubase_t rt_virtqueue_get_avail_addr(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
return (rt_ubase_t)vq->vq_packed->virtq.driver_event;
}
else
{
return (rt_ubase_t)vq->vq_split->virtq.avail;
}
}
rt_ubase_t rt_virtqueue_get_used_addr(struct rt_virtqueue *vq)
{
RT_ASSERT(vq != RT_NULL);
if (vq->packed_ring)
{
return (rt_ubase_t)vq->vq_packed->virtq.device_event;
}
else
{
return (rt_ubase_t)vq->vq_split->virtq.used;
}
}
Reference in New Issue View Git Blame Copy Permalink