2 files changed, 44 insertions, 0 deletions

diff --git a/lib/libvirtio/virtio.c b/lib/libvirtio/virtio.c
index ba7aced..276d69f 100644
--- a/lib/libvirtio/virtio.c
+++ b/lib/libvirtio/virtio.c
@@ -297,6 +297,18 @@ void virtio_set_status(struct virtio_device *dev, int status)
 	}
 }
 
+/**
+ * Get device status bits
+ */
+void virtio_get_status(struct virtio_device *dev, int *status)
+{
+	if (dev->is_modern) {
+		*status = ci_read_8(dev->common.addr +
+				    offset_of(struct virtio_dev_common, dev_status));
+	} else {
+		*status = ci_read_8(dev->base+VIRTIOHDR_DEVICE_STATUS);
+	}
+}
 
 /**
  * Set guest feature bits
@@ -350,6 +362,36 @@ uint64_t virtio_get_host_features(struct virtio_device *dev)
 	return features;
 }
 
+int virtio_negotiate_guest_features(struct virtio_device *dev, uint64_t features)
+{
+	uint64_t host_features = 0;
+	int status;
+
+	/* Negotiate features */
+	host_features = virtio_get_host_features(dev);
+	if (!(host_features & VIRTIO_F_VERSION_1)) {
+		fprintf(stderr, "Device does not support virtio 1.0 %llx\n", host_features);
+		return -1;
+	}
+
+	virtio_set_guest_features(dev,  features);
+	host_features = virtio_get_host_features(dev);
+	if ((host_features & features) != features) {
+		fprintf(stderr, "Features error %llx\n", features);
+		return -1;
+	}
+
+	virtio_get_status(dev, &status);
+	status |= VIRTIO_STAT_FEATURES_OK;
+	virtio_set_status(dev, status);
+
+	/* Read back to verify the FEATURES_OK bit */
+	virtio_get_status(dev, &status);
+	if ((status & VIRTIO_STAT_FEATURES_OK) != VIRTIO_STAT_FEATURES_OK)
+		return -1;
+
+	return 0;
+}
 
 /**
  * Get additional config values
diff --git a/lib/libvirtio/virtio.h b/lib/libvirtio/virtio.h
index bf8ea28..3ff2bac 100644
--- a/lib/libvirtio/virtio.h
+++ b/lib/libvirtio/virtio.h
@@ -116,9 +116,11 @@ extern int virtio_queue_init_vq(struct virtio_device *dev, struct vqs *vq, unsig
 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_get_status(struct virtio_device *dev, int *status);
 extern void virtio_set_qaddr(struct virtio_device *dev, int queue, unsigned long qaddr);
 extern void virtio_set_guest_features(struct virtio_device *dev, uint64_t features);
 extern uint64_t virtio_get_host_features(struct virtio_device *dev);
+extern int virtio_negotiate_guest_features(struct virtio_device *dev, uint64_t 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);