Currently only supports sending (outbufs), doesn't have any bells or whistles.
Signed-off-by: Andrew Jones <[email protected]> --- lib/virtio.c | 177 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ lib/virtio.h | 107 +++++++++++++++++++++++++++++++++++- 2 files changed, 282 insertions(+), 2 deletions(-) diff --git a/lib/virtio.c b/lib/virtio.c index 8e48d364bec7e..5e61965738d9c 100644 --- a/lib/virtio.c +++ b/lib/virtio.c @@ -6,6 +6,7 @@ #include "libcflat.h" #include "alloc.h" #include "devicetree.h" +#include "asm/page.h" #include "asm/io.h" #include "virtio.h" @@ -61,6 +62,121 @@ struct virtio_device *virtio_bind(u32 devid) return NULL; } +static void vring_init(struct vring *vr, unsigned int num, void *p, + unsigned long align) +{ + vr->num = num; + vr->desc = p; + vr->avail = p + num*sizeof(struct vring_desc); + vr->used = (void *)(((unsigned long)&vr->avail->ring[num] + sizeof(u16) + + align-1) & ~(align - 1)); +} + +static void +vring_init_virtqueue(struct vring_virtqueue *vq, + unsigned index, unsigned num, unsigned vring_align, + struct virtio_device *vdev, void *pages, + bool (*notify)(struct virtqueue *), + void (*callback)(struct virtqueue *), + const char *name) +{ + unsigned i; + + vring_init(&vq->vring, num, pages, vring_align); + vq->vq.callback = callback; + vq->vq.vdev = vdev; + vq->vq.name = name; + vq->vq.num_free = num; + vq->vq.index = index; + vq->notify = notify; + vq->last_used_idx = 0; + vq->num_added = 0; + vq->free_head = 0; + + for (i = 0; i < num-1; i++) { + vq->vring.desc[i].next = i+1; + vq->data[i] = NULL; + } + vq->data[i] = NULL; +} + +int virtqueue_add_outbuf(struct virtqueue *_vq, char *buf, size_t len) +{ + struct vring_virtqueue *vq = to_vvq(_vq); + unsigned avail; + int head; + + assert(buf != NULL); + assert(len != 0); + + if (!vq->vq.num_free) + return -1; + + --vq->vq.num_free; + + head = vq->free_head; + + vq->vring.desc[head].flags = 0; + vq->vring.desc[head].addr = virt_to_phys(buf); + vq->vring.desc[head].len = len; + + vq->free_head = vq->vring.desc[head].next; + + vq->data[head] = buf; + + avail = (vq->vring.avail->idx & (vq->vring.num-1)); + vq->vring.avail->ring[avail] = head; + wmb(); + vq->vring.avail->idx++; + vq->num_added++; + + return 0; +} + +bool virtqueue_kick(struct virtqueue *_vq) +{ + struct vring_virtqueue *vq = to_vvq(_vq); + mb(); + return vq->notify(_vq); +} + +static void detach_buf(struct vring_virtqueue *vq, unsigned head) +{ + unsigned i = head; + + vq->data[head] = NULL; + + while (vq->vring.desc[i].flags & VRING_DESC_F_NEXT) { + i = vq->vring.desc[i].next; + vq->vq.num_free++; + } + + vq->vring.desc[i].next = vq->free_head; + vq->free_head = head; + vq->vq.num_free++; +} + +void *virtqueue_get_buf(struct virtqueue *_vq, unsigned int *len) +{ + struct vring_virtqueue *vq = to_vvq(_vq); + u16 last_used; + unsigned i; + void *ret; + + rmb(); + + last_used = (vq->last_used_idx & (vq->vring.num-1)); + i = vq->vring.used->ring[last_used].id; + *len = vq->vring.used->ring[last_used].len; + + ret = vq->data[i]; + detach_buf(vq, i); + + vq->last_used_idx++; + + return ret; +} + /****************************************************** * virtio-mmio support (config space only) ******************************************************/ @@ -87,9 +203,68 @@ static void vm_set(struct virtio_device *vdev, unsigned offset, writeb(p[i], vm_dev->base + VIRTIO_MMIO_CONFIG + offset + i); } +static bool vm_notify(struct virtqueue *vq) +{ + struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vq->vdev); + writel(vq->index, vm_dev->base + VIRTIO_MMIO_QUEUE_NOTIFY); + return true; +} + +static struct virtqueue *vm_setup_vq(struct virtio_device *vdev, + unsigned index, + void (*callback)(struct virtqueue *vq), + const char *name) +{ + struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev); + struct vring_virtqueue *vq; + void *queue; + unsigned num = VIRTIO_MMIO_QUEUE_NUM_MIN; + + vq = alloc(sizeof(*vq)); + queue = alloc_aligned(VIRTIO_MMIO_QUEUE_SIZE_MIN, PAGE_SIZE); + if (!vq || !queue) + return NULL; + + writel(index, vm_dev->base + VIRTIO_MMIO_QUEUE_SEL); + + assert(readl(vm_dev->base + VIRTIO_MMIO_QUEUE_NUM_MAX) >= num); + + if (readl(vm_dev->base + VIRTIO_MMIO_QUEUE_PFN) != 0) { + printf("%s: virtqueue %d already setup! base=%p\n", + __func__, index, vm_dev->base); + return NULL; + } + + writel(num, vm_dev->base + VIRTIO_MMIO_QUEUE_NUM); + writel(VIRTIO_MMIO_VRING_ALIGN, + vm_dev->base + VIRTIO_MMIO_QUEUE_ALIGN); + writel(virt_to_pfn(queue), vm_dev->base + VIRTIO_MMIO_QUEUE_PFN); + + vring_init_virtqueue(vq, index, num, VIRTIO_MMIO_VRING_ALIGN, + vdev, queue, vm_notify, callback, name); + + return &vq->vq; +} + +static int vm_find_vqs(struct virtio_device *vdev, unsigned nvqs, + struct virtqueue *vqs[], vq_callback_t *callbacks[], + const char *names[]) +{ + unsigned i; + + for (i = 0; i < nvqs; ++i) { + vqs[i] = vm_setup_vq(vdev, i, callbacks[i], names[i]); + if (vqs[i] == NULL) + return -1; + } + + return 0; +} + static const struct virtio_config_ops vm_config_ops = { .get = vm_get, .set = vm_set, + .find_vqs = vm_find_vqs, }; static void vm_device_init(struct virtio_mmio_device *vm_dev) @@ -97,6 +272,8 @@ static void vm_device_init(struct virtio_mmio_device *vm_dev) vm_dev->vdev.id.device = readl(vm_dev->base + VIRTIO_MMIO_DEVICE_ID); vm_dev->vdev.id.vendor = readl(vm_dev->base + VIRTIO_MMIO_VENDOR_ID); vm_dev->vdev.config = &vm_config_ops; + + writel(PAGE_SIZE, vm_dev->base + VIRTIO_MMIO_GUEST_PAGE_SIZE); } /****************************************************** diff --git a/lib/virtio.h b/lib/virtio.h index 16ebe7e0a7e70..110a066c8591c 100644 --- a/lib/virtio.h +++ b/lib/virtio.h @@ -9,6 +9,7 @@ * This work is licensed under the terms of the GNU LGPL, version 2. */ #include "libcflat.h" +#include "asm/page.h" struct virtio_device_id { u32 device; @@ -20,11 +21,25 @@ struct virtio_device { const struct virtio_config_ops *config; }; +struct virtqueue { + void (*callback)(struct virtqueue *vq); + const char *name; + struct virtio_device *vdev; + unsigned int index; + unsigned int num_free; + void *priv; +}; + +typedef void vq_callback_t(struct virtqueue *); struct virtio_config_ops { void (*get)(struct virtio_device *vdev, unsigned offset, void *buf, unsigned len); void (*set)(struct virtio_device *vdev, unsigned offset, const void *buf, unsigned len); + int (*find_vqs)(struct virtio_device *vdev, unsigned nvqs, + struct virtqueue *vqs[], + vq_callback_t *callbacks[], + const char *names[]); }; extern struct virtio_device *virtio_bind(u32 devid); @@ -71,12 +86,100 @@ virtio_config_writel(struct virtio_device *vdev, unsigned offset, u32 val) vdev->config->set(vdev, offset, &val, 4); } +#define VRING_DESC_F_NEXT 1 +#define VRING_DESC_F_WRITE 2 + +struct vring_desc { + u64 addr; + u32 len; + u16 flags; + u16 next; +}; + +struct vring_avail { + u16 flags; + u16 idx; + u16 ring[]; +}; + +struct vring_used_elem { + u32 id; + u32 len; +}; + +struct vring_used { + u16 flags; + u16 idx; + struct vring_used_elem ring[]; +}; + +struct vring { + unsigned int num; + struct vring_desc *desc; + struct vring_avail *avail; + struct vring_used *used; +}; + +struct vring_virtqueue { + struct virtqueue vq; + struct vring vring; + unsigned int free_head; + unsigned int num_added; + u16 last_used_idx; + bool (*notify)(struct virtqueue *vq); + void *data[]; +}; + +#define to_vvq(_vq) container_of(_vq, struct vring_virtqueue, vq) + +extern int virtqueue_add_outbuf(struct virtqueue *vq, char *buf, size_t len); +extern bool virtqueue_kick(struct virtqueue *vq); +extern void *virtqueue_get_buf(struct virtqueue *_vq, unsigned int *len); + /****************************************************** * virtio-mmio ******************************************************/ -#define VIRTIO_MMIO_DEVICE_ID 0x008 -#define VIRTIO_MMIO_CONFIG 0x100 +#define VIRTIO_MMIO_MAGIC_VALUE 0x000 +#define VIRTIO_MMIO_VERSION 0x004 +#define VIRTIO_MMIO_DEVICE_ID 0x008 +#define VIRTIO_MMIO_VENDOR_ID 0x00c +#define VIRTIO_MMIO_HOST_FEATURES 0x010 +#define VIRTIO_MMIO_HOST_FEATURES_SEL 0x014 +#define VIRTIO_MMIO_GUEST_FEATURES 0x020 +#define VIRTIO_MMIO_GUEST_FEATURES_SEL 0x024 +#define VIRTIO_MMIO_GUEST_PAGE_SIZE 0x028 +#define VIRTIO_MMIO_QUEUE_SEL 0x030 +#define VIRTIO_MMIO_QUEUE_NUM_MAX 0x034 +#define VIRTIO_MMIO_QUEUE_NUM 0x038 +#define VIRTIO_MMIO_QUEUE_ALIGN 0x03c +#define VIRTIO_MMIO_QUEUE_PFN 0x040 +#define VIRTIO_MMIO_QUEUE_NOTIFY 0x050 +#define VIRTIO_MMIO_INTERRUPT_STATUS 0x060 +#define VIRTIO_MMIO_INTERRUPT_ACK 0x064 +#define VIRTIO_MMIO_STATUS 0x070 +#define VIRTIO_MMIO_CONFIG 0x100 + +#define VIRTIO_MMIO_INT_VRING (1 << 0) +#define VIRTIO_MMIO_INT_CONFIG (1 << 1) + +#define VIRTIO_MMIO_VRING_ALIGN PAGE_SIZE + +/* + * The minimum queue size is 2*VIRTIO_MMIO_VRING_ALIGN, which + * means the largest queue num for the minimum queue size is 128, i.e. + * 2*VIRTIO_MMIO_VRING_ALIGN = vring_size(128, VIRTIO_MMIO_VRING_ALIGN), + * where vring_size is + * + * unsigned vring_size(unsigned num, unsigned long align) + * { + * return ((sizeof(struct vring_desc) * num + sizeof(u16) * (3 + num) + * + align - 1) & ~(align - 1)) + * + sizeof(u16) * 3 + sizeof(struct vring_used_elem) * num; + * } + */ +#define VIRTIO_MMIO_QUEUE_SIZE_MIN (2*VIRTIO_MMIO_VRING_ALIGN) +#define VIRTIO_MMIO_QUEUE_NUM_MIN 128 #define to_virtio_mmio_device(vdev_ptr) \ container_of(vdev_ptr, struct virtio_mmio_device, vdev) -- 1.9.3 -- To unsubscribe from this list: send the line "unsubscribe kvm" in the body of a message to [email protected] More majordomo info at http://vger.kernel.org/majordomo-info.html
