virtio-blk: support variable block size

Current code only works with 512 bytes read.

Moreover, Qemu ignores the guest set features request. In the set
features request SLOF indicates to qemu that it is not support
VIRTIO_BLK_F_BLK_SIZE feature. Code in qemu suggests that virtio-blk
is not implementing set_guest_feature.

Tested-by: Bharata B Rao <bharata@linux.ibm.com>
Reviewed-by: Thomas Huth <thuth@linux.vnet.ibm.com>
Signed-off-by: Nikunj A Dadhania <nikunj@linux.vnet.ibm.com>

5 files changed, 49 insertions, 15 deletions

diff --git a/lib/libvirtio/virtio-blk.c b/lib/libvirtio/virtio-blk.c
index 544068f..826f2ea 100644
--- a/lib/libvirtio/virtio-blk.c
+++ b/lib/libvirtio/virtio-blk.c
@@ -16,6 +16,8 @@
 #include "virtio.h"
 #include "virtio-blk.h"
 
+#define DEFAULT_SECTOR_SIZE 512
+
 /**
  * Initialize virtio-block device.
  * @param  dev  pointer to virtio device information
@@ -24,6 +26,8 @@ int
 virtioblk_init(struct virtio_device *dev)
 {
 	struct vring_avail *vq_avail;
+	int blk_size = DEFAULT_SECTOR_SIZE;
+	int features;
 
 	/* Reset device */
 	// XXX That will clear the virtq base. We need to move
@@ -37,8 +41,8 @@ virtioblk_init(struct virtio_device *dev)
 	/* Tell HV that we know how to drive the device. */
 	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER);
 
-	/* Device specific setup - we do not support special features right now */
-	virtio_set_guest_features(dev,  0);
+	/* Device specific setup - we support F_BLK_SIZE */
+	virtio_set_guest_features(dev,  VIRTIO_BLK_F_BLK_SIZE);
 
 	vq_avail = virtio_get_vring_avail(dev, 0);
 	vq_avail->flags = VRING_AVAIL_F_NO_INTERRUPT;
@@ -48,7 +52,14 @@ virtioblk_init(struct virtio_device *dev)
 	virtio_set_status(dev, VIRTIO_STAT_ACKNOWLEDGE|VIRTIO_STAT_DRIVER
 				|VIRTIO_STAT_DRIVER_OK);
 
-	return 0;
+	virtio_get_host_features(dev, &features);
+	if (features & VIRTIO_BLK_F_BLK_SIZE) {
+		blk_size = virtio_get_config(dev,
+				offset_of(struct virtio_blk_cfg, blk_size),
+				sizeof(blk_size));
+	}
+
+	return blk_size;
 }
 
 
@@ -90,17 +101,28 @@ virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt)
 	volatile uint8_t status = -1;
 	volatile uint16_t *current_used_idx;
 	uint16_t last_used_idx;
+	int blk_size = DEFAULT_SECTOR_SIZE;
 
 	//printf("virtioblk_read: dev=%p buf=%p blocknum=%li count=%li\n",
 	//	dev, buf, blocknum, cnt);
 
 	/* Check whether request is within disk capacity */
-	capacity = virtio_get_config(dev, 0, sizeof(capacity));
+	capacity = virtio_get_config(dev,
+			offset_of(struct virtio_blk_cfg, capacity),
+			sizeof(capacity));
 	if (blocknum + cnt - 1 > capacity) {
 		puts("virtioblk_read: Access beyond end of device!");
 		return 0;
 	}
 
+	blk_size = virtio_get_config(dev,
+			offset_of(struct virtio_blk_cfg, blk_size),
+			sizeof(blk_size));
+	if (blk_size % DEFAULT_SECTOR_SIZE) {
+		fprintf(stderr, "virtio-blk: Unaligned sector read %d\n", blk_size);
+		return 0;
+	}
+
 	vq_size = virtio_get_qsize(dev, 0);
 	vq_desc = virtio_get_vring_desc(dev, 0);
 	vq_avail = virtio_get_vring_avail(dev, 0);
@@ -112,7 +134,7 @@ virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt)
 	/* Set up header */
 	blkhdr.type = VIRTIO_BLK_T_IN | VIRTIO_BLK_T_BARRIER;
 	blkhdr.ioprio = 1;
-	blkhdr.sector = blocknum;
+	blkhdr.sector = blocknum * blk_size / DEFAULT_SECTOR_SIZE;
 
 	/* Determine descriptor index */
 	id = (vq_avail->idx * 3) % vq_size;
@@ -127,7 +149,7 @@ virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt)
 	/* Set up virtqueue descriptor for data */
 	desc = &vq_desc[(id + 1) % vq_size];
 	desc->addr = (uint64_t)buf;
-	desc->len = cnt * 512;
+	desc->len = cnt * blk_size;
 	desc->flags = VRING_DESC_F_NEXT | VRING_DESC_F_WRITE;
 	desc->next = (id + 2) % vq_size;
 
diff --git a/lib/libvirtio/virtio-blk.h b/lib/libvirtio/virtio-blk.h
index 7c2b7e0..ac8bf28 100644
--- a/lib/libvirtio/virtio-blk.h
+++ b/lib/libvirtio/virtio-blk.h
@@ -21,9 +21,7 @@
 #include <stdint.h>
 
 
-/* Device configuration layout */
-/*
-struct virtio_blk_config {
+struct virtio_blk_cfg {
 	uint64_t	capacity;
 	uint32_t	size_max;
 	uint32_t	seg_max;
@@ -34,8 +32,7 @@ struct virtio_blk_config {
 	} geometry;
 	uint32_t	blk_size;
 	uint32_t	sectors_max;
-};
-*/
+} __attribute__ ((packed)) ;
 
 /* Block request */
 struct virtio_blk_req {
@@ -53,6 +50,9 @@ struct virtio_blk_req {
 #define VIRTIO_BLK_T_FLUSH_OUT		5
 #define VIRTIO_BLK_T_BARRIER		0x80000000
 
+/* VIRTIO_BLK Feature bits */
+#define VIRTIO_BLK_F_BLK_SIZE       (1 << 6)
+
 extern int virtioblk_init(struct virtio_device *dev);
 extern void virtioblk_shutdown(struct virtio_device *dev);
 extern int virtioblk_read(struct virtio_device *dev, char *buf, long blocknum, long cnt);
diff --git a/lib/libvirtio/virtio.c b/lib/libvirtio/virtio.c
index 249f803..b010796 100644
--- a/lib/libvirtio/virtio.c
+++ b/lib/libvirtio/virtio.c
@@ -162,7 +162,18 @@ void virtio_set_guest_features(struct virtio_device *dev, int features)
 
 {
 	if (dev->type == VIRTIO_TYPE_PCI) {
-		ci_write_32(dev->base+VIRTIOHDR_GUEST_FEATURES, features);
+		ci_write_32(dev->base+VIRTIOHDR_GUEST_FEATURES, bswap_32(features));
+	}
+}
+
+/**
+ * Get host feature bits
+ */
+void virtio_get_host_features(struct virtio_device *dev, int *features)
+
+{
+	if (dev->type == VIRTIO_TYPE_PCI && features) {
+		*features = bswap_32(ci_read_32(dev->base+VIRTIOHDR_DEVICE_FEATURES));
 	}
 }
 
diff --git a/lib/libvirtio/virtio.code b/lib/libvirtio/virtio.code
index 304849e..258b9bb 100644
--- a/lib/libvirtio/virtio.code
+++ b/lib/libvirtio/virtio.code
@@ -46,10 +46,10 @@ MIRP
 
 /******** virtio-blk ********/
 
-// : virtio-blk-init ( dev -- )
+// : virtio-blk-init ( dev -- blk-size)
 PRIM(virtio_X2d_blk_X2d_init)
-	void *dev = TOS.a; POP;
-	virtioblk_init(dev);
+	void *dev = TOS.a;
+	TOS.u = virtioblk_init(dev);
 MIRP
 
 // : virtio-blk-shutdown ( dev -- )
diff --git a/lib/libvirtio/virtio.h b/lib/libvirtio/virtio.h
index b351930..d5759b4 100644
--- a/lib/libvirtio/virtio.h
+++ b/lib/libvirtio/virtio.h
@@ -81,6 +81,7 @@ 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_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);
 extern int __virtio_read_config(struct virtio_device *dev, void *dst,
 				int offset, int len);