[SeaBIOS] [PATCH 14/18] virtio: add version 1.0 support to vp_find_vq

[Gerd Hoffmann]

Signed-off-by: Gerd Hoffmann 
---
 src/hw/virtio-pci.c | 61 +++++++++++++++++++++++++++++++++++++++++------------
 1 file changed, 47 insertions(+), 14 deletions(-)

diff --git a/src/hw/virtio-pci.c b/src/hw/virtio-pci.c
index 8097901..c80d38d 100644
--- a/src/hw/virtio-pci.c
+++ b/src/hw/virtio-pci.c
@@ -139,7 +139,6 @@ void vp_notify(struct vp_device *vp, struct vring_virtqueue *vq)
 int vp_find_vq(struct vp_device *vp, int queue_index,
                struct vring_virtqueue **p_vq)
 {
-   int ioaddr = GET_LOWFLAT(vp->ioaddr);
    u16 num;

ASSERT32FLAT();
@@ -150,34 +149,49 @@ int vp_find_vq(struct vp_device *vp, int queue_index,
    }
    memset(vq, 0, sizeof(*vq));

+
    /* select the queue */
-
-   outw(queue_index, ioaddr + VIRTIO_PCI_QUEUE_SEL);
+   if (vp->use_modern) {
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_select, queue_index);
+   } else {
+       outw(queue_index, vp->ioaddr + VIRTIO_PCI_QUEUE_SEL);
+   }

/* check if the queue is available */
-
-   num = inw(ioaddr + VIRTIO_PCI_QUEUE_NUM);
+   if (vp->use_modern) {
+       vp_modern_read(vp->common, virtio_pci_common_cfg,
+                      queue_size, num);
+       if (num > MAX_QUEUE_NUM) {
+           vp_modern_write(vp->common, virtio_pci_common_cfg,
+                           queue_size, MAX_QUEUE_NUM);
+           vp_modern_read(vp->common, virtio_pci_common_cfg,
+                          queue_size, num);
+       }
+   } else {
+       num = inw(vp->ioaddr + VIRTIO_PCI_QUEUE_NUM);
+   }
    if (!num) {
        dprintf(1, "ERROR: queue size is 0\n");
        goto fail;
    }
-
    if (num > MAX_QUEUE_NUM) {
        dprintf(1, "ERROR: queue size %d > %d\n", num, MAX_QUEUE_NUM);
        goto fail;
    }

/* check if the queue is already active */
-
-   if (inl(ioaddr + VIRTIO_PCI_QUEUE_PFN)) {
-       dprintf(1, "ERROR: queue already active\n");
-       goto fail;
+   if (vp->use_modern) {
+       /* WIP */
+   } else {
+       if (inl(vp->ioaddr + VIRTIO_PCI_QUEUE_PFN)) {
+           dprintf(1, "ERROR: queue already active\n");
+           goto fail;
+       }
    }
-
    vq->queue_index = queue_index;

/* initialize the queue */
-
    struct vring * vr = &vq->vring;
    vring_init(vr, num, (unsigned char*)&vq->queue);

@@ -186,8 +200,27 @@ int vp_find_vq(struct vp_device *vp, int queue_index,
     * NOTE: vr->desc is initialized by vring_init()
     */

-   outl((unsigned long)virt_to_phys(vr->desc) >> PAGE_SHIFT,
-        ioaddr + VIRTIO_PCI_QUEUE_PFN);
+   if (vp->use_modern) {
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_desc_lo, (unsigned long)virt_to_phys(vr->desc));
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_desc_hi, 0);
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_avail_lo, (unsigned long)virt_to_phys(vr->avail));
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_avail_hi, 0);
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_used_lo, (unsigned long)virt_to_phys(vr->used));
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_used_hi, 0);
+       vp_modern_write(vp->common, virtio_pci_common_cfg,
+                       queue_enable, 1);
+       vp_modern_read(vp->common, virtio_pci_common_cfg,
+                      queue_notify_off, vq->queue_notify_off);
+   } else {
+       outl((unsigned long)virt_to_phys(vr->desc) >> PAGE_SHIFT,
+            vp->ioaddr + VIRTIO_PCI_QUEUE_PFN);
+   }

return num;