virtio: introduce helper for initializing virt queue

The routine takes care to allocate and set the queue address in the
device. Add these calls in virtio-net, virtio-blk and virtio-9p.

With the lack of this routine, devices like virtio-blk and virtio-9p did
not do a device reset in the driver initialization code. This helper
will fix that problem

Change the signature of virtio_set_qaddr, accepting queue address as
unsigned long argument.

Signed-off-by: Nikunj A Dadhania <nikunj@linux.vnet.ibm.com>
Reviewed-by: Thomas Huth <thuth@redhat.com>
Signed-off-by: Alexey Kardashevskiy <aik@ozlabs.ru>

6 files changed, 53 insertions, 51 deletions

diff --git a/lib/libvirtio/virtio-9p.c b/lib/libvirtio/virtio-9p.c
index 95d7db9..99ab7d0 100644
--- a/lib/libvirtio/virtio-9p.c
+++ b/lib/libvirtio/virtio-9p.c
@@ -19,6 +19,7 @@
 #include "virtio-9p.h"
 #include "p9.h"
 
+static struct vqs vq;
 
 /**
  * Notes for 9P Server config:
@@ -189,6 +190,12 @@ int virtio_9p_init(struct virtio_device *dev, void *tx_buf, void *rx_buf,
 	/* Device specific setup - we do not support special features */
 	virtio_set_guest_features(dev,  0);
 
+	if (virtio_queue_init_vq(dev, &vq, 0)) {
+		virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
+				  |VIRTIO_STAT_FAILED);
+		return -1;
+	}
+
 	vq_avail = virtio_get_vring_avail(dev, 0);
 	vq_avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
 	vq_avail->idx = 0;
diff --git a/lib/libvirtio/virtio-blk.c b/lib/libvirtio/virtio-blk.c
index 6eb4a64..b78ada5 100644
--- a/lib/libvirtio/virtio-blk.c
+++ b/lib/libvirtio/virtio-blk.c
@@ -18,6 +18,8 @@
 
 #define DEFAULT_SECTOR_SIZE 512
 
+static struct vqs vq;
+
 /**
  * Initialize virtio-block device.
  * @param  dev  pointer to virtio device information
@@ -44,6 +46,12 @@ virtioblk_init(struct virtio_device *dev)
 	/* Device specific setup - we support F_BLK_SIZE */
 	virtio_set_guest_features(dev,  VIRTIO_BLK_F_BLK_SIZE);
 
+	if (virtio_queue_init_vq(dev, &vq, 0)) {
+		virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
+				  |VIRTIO_STAT_FAILED);
+		return 0;
+	}
+
 	vq_avail = virtio_get_vring_avail(dev, 0);
 	vq_avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
 	vq_avail->idx = 0;
diff --git a/lib/libvirtio/virtio-net.c b/lib/libvirtio/virtio-net.c
index d8ce9a3..2c0e50a 100644
--- a/lib/libvirtio/virtio-net.c
+++ b/lib/libvirtio/virtio-net.c
@@ -37,18 +37,6 @@
 
 #define sync()  asm volatile (" sync \n" ::: "memory")
 
-/* PCI virtio header offsets */
-#define VIRTIOHDR_DEVICE_FEATURES       0
-#define VIRTIOHDR_GUEST_FEATURES        4
-#define VIRTIOHDR_QUEUE_ADDRESS         8
-#define VIRTIOHDR_QUEUE_SIZE            12
-#define VIRTIOHDR_QUEUE_SELECT          14
-#define VIRTIOHDR_QUEUE_NOTIFY          16
-#define VIRTIOHDR_DEVICE_STATUS         18
-#define VIRTIOHDR_ISR_STATUS            19
-#define VIRTIOHDR_DEVICE_CONFIG         20
-#define VIRTIOHDR_MAC_ADDRESS           20
-
 struct virtio_device virtiodev;
 struct vqs vq[2];     /* Information about virtqueues */
 
@@ -72,8 +60,6 @@ static uint16_t last_rx_idx;	/* Last index in RX "used" ring */
  */
 static int virtionet_init_pci(struct virtio_device *dev)
 {
-	int i;
-
 	dprintf("virtionet: doing virtionet_init_pci!\n");
 
 	if (!dev)
@@ -90,29 +76,11 @@ static int virtionet_init_pci(struct virtio_device *dev)
 	 * second the transmit queue, and the forth is the control queue for
 	 * networking options.
 	 * We are only interested in the receive and transmit queue here. */
-
-	for (i=VQ_RX; i<=VQ_TX; i++) {
-		/* Select ring (0=RX, 1=TX): */
-		vq[i].id = i-VQ_RX;
-		ci_write_16(virtiodev.base+VIRTIOHDR_QUEUE_SELECT,
-			    cpu_to_le16(vq[i].id));
-
-		vq[i].size = le16_to_cpu(ci_read_16(virtiodev.base+VIRTIOHDR_QUEUE_SIZE));
-		vq[i].desc = SLOF_alloc_mem_aligned(virtio_vring_size(vq[i].size), 4096);
-		if (!vq[i].desc) {
-			printf("memory allocation failed!\n");
-			return -1;
-		}
-		memset(vq[i].desc, 0, virtio_vring_size(vq[i].size));
-		ci_write_32(virtiodev.base+VIRTIOHDR_QUEUE_ADDRESS,
-			    cpu_to_le32((long)vq[i].desc / 4096));
-		vq[i].avail = (void*)vq[i].desc
-				    + vq[i].size * sizeof(struct vring_desc);
-		vq[i].used = (void*)VQ_ALIGN((long)vq[i].avail
-				    + vq[i].size * sizeof(struct vring_avail));
-
-		dprintf("%i: vq.id = %llx\nvq.size =%x\n vq.avail =%p\nvq.used=%p\n",
-			i, vq[i].id, vq[i].size, vq[i].avail, vq[i].used);
+	if (virtio_queue_init_vq(dev, &vq[VQ_RX], VQ_RX) ||
+	    virtio_queue_init_vq(dev, &vq[VQ_TX], VQ_TX)) {
+		virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
+				  |VIRTIO_STAT_FAILED);
+		return -1;
 	}
 
 	/* Acknowledge device. */
diff --git a/lib/libvirtio/virtio-net.h b/lib/libvirtio/virtio-net.h
index bc7a189..2196f87 100644
--- a/lib/libvirtio/virtio-net.h
+++ b/lib/libvirtio/virtio-net.h
@@ -23,18 +23,6 @@ enum {
 	VQ_TX = 1,	/* Transmit Queue */
 };
 
-struct vqs {
-	uint64_t id;	/* Queue ID */
-	uint32_t size;
-	void *buf_mem;
-	struct vring_desc *desc;
-	struct vring_avail *avail;
-	struct vring_used *used;
-};
-
-/* Device is identified by RX queue ID: */
-#define DEVICE_ID  vq[0].id
-
 extern net_driver_t *virtionet_open(char *mac_addr, int len, struct virtio_device *dev);
 extern void virtionet_close(net_driver_t *driver);
 extern int virtionet_read(char *buf, int len);
diff --git a/lib/libvirtio/virtio.c b/lib/libvirtio/virtio.c
index 8da10c8..063a621 100644
--- a/lib/libvirtio/virtio.c
+++ b/lib/libvirtio/virtio.c
@@ -10,10 +10,15 @@
  *     IBM Corporation - initial implementation
  *****************************************************************************/
 
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
 #include <cpu.h>
 #include <cache.h>
 #include <byteorder.h>
 #include "virtio.h"
+#include "helpers.h"
 
 /* PCI virtio header offsets */
 #define VIRTIOHDR_DEVICE_FEATURES	0
@@ -133,7 +138,7 @@ void virtio_queue_notify(struct virtio_device *dev, int queue)
 /**
  * Set queue address
  */
-void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr)
+void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned long qaddr)
 {
 	if (dev->type == VIRTIO_TYPE_PCI) {
 		uint32_t val = qaddr;
@@ -146,6 +151,22 @@ void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr)
 	}
 }
 
+int virtio_queue_init_vq(struct virtio_device *dev, struct vqs *vq, unsigned int id)
+{
+	vq->size = virtio_get_qsize(dev, id);
+	vq->desc = SLOF_alloc_mem_aligned(virtio_vring_size(vq->size), 4096);
+	if (!vq->desc) {
+		printf("memory allocation failed!\n");
+		return -1;
+	}
+	memset(vq->desc, 0, virtio_vring_size(vq->size));
+	virtio_set_qaddr(dev, id, (unsigned long)vq->desc);
+	vq->avail = virtio_get_vring_avail(dev, id);
+	vq->used = virtio_get_vring_used(dev, id);
+	vq->id = id;
+	return 0;
+}
+
 /**
  * Set device status bits
  */
diff --git a/lib/libvirtio/virtio.h b/lib/libvirtio/virtio.h
index 9d6d8da..e41f47d 100644
--- a/lib/libvirtio/virtio.h
+++ b/lib/libvirtio/virtio.h
@@ -66,6 +66,15 @@ struct virtio_device {
 	int type;		/* VIRTIO_TYPE_PCI or VIRTIO_TYPE_VIO */
 };
 
+struct vqs {
+	uint64_t id;	/* Queue ID */
+	uint32_t size;
+	void *buf_mem;
+	struct vring_desc *desc;
+	struct vring_avail *avail;
+	struct vring_used *used;
+};
+
 /* Parts of the virtqueue are aligned on a 4096 byte page boundary */
 #define VQ_ALIGN(addr)	(((addr) + 0xfff) & ~0xfff)
 
@@ -74,11 +83,12 @@ extern int virtio_get_qsize(struct virtio_device *dev, int queue);
 extern struct vring_desc *virtio_get_vring_desc(struct virtio_device *dev, int queue);
 extern struct vring_avail *virtio_get_vring_avail(struct virtio_device *dev, int queue);
 extern struct vring_used *virtio_get_vring_used(struct virtio_device *dev, int queue);
+extern int virtio_queue_init_vq(struct virtio_device *dev, struct vqs *vq, unsigned int id);
 
 extern void virtio_reset_device(struct virtio_device *dev);
 extern void virtio_queue_notify(struct virtio_device *dev, int queue);
 extern void virtio_set_status(struct virtio_device *dev, int status);
-extern void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned int qaddr);
+extern void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned long qaddr);
 extern void virtio_set_guest_features(struct virtio_device *dev, int features);
 extern void virtio_get_host_features(struct virtio_device *dev, int *features);
 extern uint64_t virtio_get_config(struct virtio_device *dev, int offset, int size);