grml...

[seabios.git] / src / virtio-pci.c

diff --git a/src/virtio-pci.c b/src/virtio-pci.c
index db19e974a8560b5aae4b8cd8943145723aa39c3c..4b9ad0acb8324eed31e2e52edee036de257a4f21 100644 (file)
--- a/src/virtio-pci.c
+++ b/src/virtio-pci.c
@@ -19,14 +19,22 @@
 #include "virtio-pci.h"
 #include "config.h" // CONFIG_DEBUG_LEVEL
 #include "util.h" // dprintf
+#include "pci.h" // pci_config_readl
+#include "pci_regs.h" // PCI_BASE_ADDRESS_0
 
 int vp_find_vq(unsigned int ioaddr, int queue_index,
-               struct vring_virtqueue *vq)
+               struct vring_virtqueue **p_vq)
 {
-   struct vring * vr = &vq->vring;
    u16 num;
 
    ASSERT32FLAT();
+   struct vring_virtqueue *vq = *p_vq = memalign_low(PAGE_SIZE, sizeof(*vq));
+   if (!vq) {
+       warn_noalloc();
+       goto fail;
+   }
+   memset(vq, 0, sizeof(*vq));
+
    /* select the queue */
 
    outw(queue_index, ioaddr + VIRTIO_PCI_QUEUE_SEL);
@@ -36,25 +44,26 @@ int vp_find_vq(unsigned int ioaddr, int queue_index,
    num = inw(ioaddr + VIRTIO_PCI_QUEUE_NUM);
    if (!num) {
        dprintf(1, "ERROR: queue size is 0\n");
-       return -1;
+       goto fail;
    }
 
    if (num > MAX_QUEUE_NUM) {
        dprintf(1, "ERROR: queue size %d > %d\n", num, MAX_QUEUE_NUM);
-       return -1;
+       goto fail;
    }
 
    /* check if the queue is already active */
 
    if (inl(ioaddr + VIRTIO_PCI_QUEUE_PFN)) {
        dprintf(1, "ERROR: queue already active\n");
-       return -1;
+       goto fail;
    }
 
    vq->queue_index = queue_index;
 
    /* initialize the queue */
 
+   struct vring * vr = &vq->vring;
    vring_init(vr, num, (unsigned char*)&vq->queue);
 
    /* activate the queue
@@ -66,4 +75,20 @@ int vp_find_vq(unsigned int ioaddr, int queue_index,
         ioaddr + VIRTIO_PCI_QUEUE_PFN);
 
    return num;
+
+fail:
+   free(vq);
+   *p_vq = NULL;
+   return -1;
+}
+
+u16 vp_init_simple(u16 bdf)
+{
+    u16 ioaddr = pci_config_readl(bdf, PCI_BASE_ADDRESS_0) &
+        PCI_BASE_ADDRESS_IO_MASK;
+
+    vp_reset(ioaddr);
+    vp_set_status(ioaddr, VIRTIO_CONFIG_S_ACKNOWLEDGE |
+                  VIRTIO_CONFIG_S_DRIVER );
+    return ioaddr;
 }