Currently only supports sending (outbufs), doesn't have any
bells or whistles.
Signed-off-by: Andrew Jones <drjones@xxxxxxxxxx>
---
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 majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
