[SeaBIOS] [PATCH v3 14/25] virtio: add version 1.0 support to vp_notify

[Gerd Hoffmann]

Signed-off-by: Gerd Hoffmann <kraxel at redhat.com>
---
 src/hw/virtio-pci.c  | 22 +++++++++++++++++++++-
 src/hw/virtio-pci.h  |  7 ++-----
 src/hw/virtio-ring.c |  2 +-
 src/hw/virtio-ring.h |  1 +
 4 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/src/hw/virtio-pci.c b/src/hw/virtio-pci.c
index bd51d8a..240cd2f 100644
--- a/src/hw/virtio-pci.c
+++ b/src/hw/virtio-pci.c
@@ -97,6 +97,24 @@ void vp_reset(struct vp_device *vp)
     }
 }
 
+void vp_notify(struct vp_device *vp, struct vring_virtqueue *vq)
+{
+    if (vp->use_modern) {
+        u32 addr = vp->notify.addr +
+            vq->queue_notify_off *
+            vp->notify_off_multiplier;
+        if (vp->notify.is_io) {
+            outw(vq->queue_index, addr);
+        } else {
+            writew((void*)addr, vq->queue_index);
+        }
+        dprintf(9, "vp notify %x (%d) -- 0x%x\n",
+                addr, 2, vq->queue_index);
+    } else {
+        vp_write(&vp->legacy, virtio_pci_legacy, queue_notify, vq->queue_index);
+    }
+}
+
 int vp_find_vq(struct vp_device *vp, int queue_index,
                struct vring_virtqueue **p_vq)
 {
@@ -162,7 +180,7 @@ void vp_init_simple(struct vp_device *vp, struct pci_device *pci)
 {
     u8 cap = pci_find_capability(pci, PCI_CAP_ID_VNDR, 0);
     struct vp_cap *vp_cap;
-    u32 addr, offset;
+    u32 addr, offset, mul;
     u8 type;
 
     memset(vp, 0, sizeof(*vp));
@@ -175,6 +193,8 @@ void vp_init_simple(struct vp_device *vp, struct pci_device *pci)
             break;
         case VIRTIO_PCI_CAP_NOTIFY_CFG:
             vp_cap = &vp->notify;
+            mul = offsetof(struct virtio_pci_notify_cap, notify_off_multiplier);
+            vp->notify_off_multiplier = pci_config_readl(pci->bdf, cap + mul);
             break;
         case VIRTIO_PCI_CAP_ISR_CFG:
             vp_cap = &vp->isr;
diff --git a/src/hw/virtio-pci.h b/src/hw/virtio-pci.h
index f2ae5b9..3054a13 100644
--- a/src/hw/virtio-pci.h
+++ b/src/hw/virtio-pci.h
@@ -125,6 +125,7 @@ struct vp_cap {
 struct vp_device {
     unsigned int ioaddr;
     struct vp_cap common, notify, isr, device, legacy;
+    u32 notify_off_multiplier;
     u8 use_modern;
 };
 
@@ -233,11 +234,6 @@ void vp_set_status(struct vp_device *vp, u8 status);
 u8 vp_get_isr(struct vp_device *vp);
 void vp_reset(struct vp_device *vp);
 
-static inline void vp_notify(struct vp_device *vp, int queue_index)
-{
-    outw(queue_index, GET_LOWFLAT(vp->ioaddr) + VIRTIO_PCI_QUEUE_NOTIFY);
-}
-
 static inline void vp_del_vq(struct vp_device *vp, int queue_index)
 {
    int ioaddr = GET_LOWFLAT(vp->ioaddr);
@@ -252,6 +248,7 @@ static inline void vp_del_vq(struct vp_device *vp, int queue_index)
 struct pci_device;
 struct vring_virtqueue;
 void vp_init_simple(struct vp_device *vp, struct pci_device *pci);
+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);
 #endif /* _VIRTIO_PCI_H_ */
diff --git a/src/hw/virtio-ring.c b/src/hw/virtio-ring.c
index 5c6a32e..6c86c38 100644
--- a/src/hw/virtio-ring.c
+++ b/src/hw/virtio-ring.c
@@ -145,5 +145,5 @@ void vring_kick(struct vp_device *vp, struct vring_virtqueue *vq, int num_added)
     smp_wmb();
     SET_LOWFLAT(avail->idx, GET_LOWFLAT(avail->idx) + num_added);
 
-    vp_notify(vp, GET_LOWFLAT(vq->queue_index));
+    vp_notify(vp, vq);
 }
diff --git a/src/hw/virtio-ring.h b/src/hw/virtio-ring.h
index 553a508..7df9004 100644
--- a/src/hw/virtio-ring.h
+++ b/src/hw/virtio-ring.h
@@ -88,6 +88,7 @@ struct vring_virtqueue {
    u16 vdata[MAX_QUEUE_NUM];
    /* PCI */
    int queue_index;
+   int queue_notify_off;
 };
 
 struct vring_list {
-- 
1.8.3.1