[SLOF] [PATCH v1 15/27] virtio: make virtio_set_qaddr 1.0 aware

[Nikunj A Dadhania]

Signed-off-by: Nikunj A Dadhania <nikunj at linux.vnet.ibm.com>
---
 lib/libvirtio/virtio.c | 32 +++++++++++++++++++++++---------
 1 file changed, 23 insertions(+), 9 deletions(-)

diff --git a/lib/libvirtio/virtio.c b/lib/libvirtio/virtio.c
index 32e62e4..86e9bae 100644
--- a/lib/libvirtio/virtio.c
+++ b/lib/libvirtio/virtio.c
@@ -351,15 +351,29 @@ void virtio_queue_notify(struct virtio_device *dev, int queue)
  */
 void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr)
 {
-        if (dev->type == VIRTIO_TYPE_PCI) {
-                uint32_t val = qaddr;
-                val = val >> 12;
-                ci_write_16(dev->base+VIRTIOHDR_QUEUE_SELECT,
-                            cpu_to_le16(queue));
-                eieio();
-                ci_write_32(dev->base+VIRTIOHDR_QUEUE_ADDRESS,
-                            cpu_to_le32(val));
-        }
+	if (dev->type != VIRTIO_TYPE_PCI)
+		return;
+	if (dev->is_modern) {
+		uint64_t q_desc = qaddr;
+		uint64_t q_avail;
+		uint64_t q_used;
+		uint32_t q_size = virtio_get_qsize(dev, queue);
+
+		virtio_write64(dev->common.addr + offset_of(struct virtio_dev_common, q_desc), q_desc);
+		q_avail = q_desc + q_size * sizeof(struct vring_desc);
+		virtio_write64(dev->common.addr + offset_of(struct virtio_dev_common, q_avail), q_avail);
+		q_used = VQ_ALIGN(q_avail + sizeof(struct vring_avail) + sizeof(uint16_t) * q_size);
+		virtio_write64(dev->common.addr + offset_of(struct virtio_dev_common, q_used), q_used);
+		ci_write_16(dev->common.addr + offset_of(struct virtio_dev_common, q_enable), cpu_to_le16(1));
+	} else {
+		uint32_t val = qaddr;
+		val = val >> 12;
+		ci_write_16(dev->base+VIRTIOHDR_QUEUE_SELECT,
+			    cpu_to_le16(queue));
+		eieio();
+		ci_write_32(dev->base+VIRTIOHDR_QUEUE_ADDRESS,
+			    cpu_to_le32(val));
+	}
 }
 
 int virtio_queue_init_vq(struct virtio_device *dev, struct vqs *vq, unsigned int id)
-- 
2.5.0