1 files changed, 42 insertions, 15 deletions

diff --git a/hw/block/virtio-blk.c b/hw/block/virtio-blk.c
index 284e64667c..fb43bbaa46 100644
--- a/hw/block/virtio-blk.c
+++ b/hw/block/virtio-blk.c
@@ -29,9 +29,11 @@
 #include "hw/virtio/virtio-bus.h"
 #include "hw/virtio/virtio-access.h"
 
-void virtio_blk_init_request(VirtIOBlock *s, VirtIOBlockReq *req)
+void virtio_blk_init_request(VirtIOBlock *s, VirtQueue *vq,
+                             VirtIOBlockReq *req)
 {
     req->dev = s;
+    req->vq = vq;
     req->qiov.size = 0;
     req->in_len = 0;
     req->next = NULL;
@@ -53,11 +55,11 @@ static void virtio_blk_req_complete(VirtIOBlockReq *req, unsigned char status)
     trace_virtio_blk_req_complete(req, status);
 
     stb_p(&req->in->status, status);
-    virtqueue_push(s->vq, &req->elem, req->in_len);
+    virtqueue_push(req->vq, &req->elem, req->in_len);
     if (s->dataplane_started && !s->dataplane_disabled) {
-        virtio_blk_data_plane_notify(s->dataplane);
+        virtio_blk_data_plane_notify(s->dataplane, req->vq);
     } else {
-        virtio_notify(vdev, s->vq);
+        virtio_notify(vdev, req->vq);
     }
 }
 
@@ -187,12 +189,12 @@ out:
 
 #endif
 
-static VirtIOBlockReq *virtio_blk_get_request(VirtIOBlock *s)
+static VirtIOBlockReq *virtio_blk_get_request(VirtIOBlock *s, VirtQueue *vq)
 {
-    VirtIOBlockReq *req = virtqueue_pop(s->vq, sizeof(VirtIOBlockReq));
+    VirtIOBlockReq *req = virtqueue_pop(vq, sizeof(VirtIOBlockReq));
 
     if (req) {
-        virtio_blk_init_request(s, req);
+        virtio_blk_init_request(s, vq, req);
     }
     return req;
 }
@@ -583,7 +585,7 @@ void virtio_blk_handle_vq(VirtIOBlock *s, VirtQueue *vq)
 
     blk_io_plug(s->blk);
 
-    while ((req = virtio_blk_get_request(s))) {
+    while ((req = virtio_blk_get_request(s, vq))) {
         virtio_blk_handle_request(req, &mrb);
     }
 
@@ -708,6 +710,7 @@ static void virtio_blk_update_config(VirtIODevice *vdev, uint8_t *config)
     blkcfg.physical_block_exp = get_physical_block_exp(conf);
     blkcfg.alignment_offset = 0;
     blkcfg.wce = blk_enable_write_cache(s->blk);
+    virtio_stw_p(vdev, &blkcfg.num_queues, s->conf.num_queues);
     memcpy(config, &blkcfg, sizeof(struct virtio_blk_config));
 }
 
@@ -751,6 +754,9 @@ static uint64_t virtio_blk_get_features(VirtIODevice *vdev, uint64_t features,
     if (blk_is_read_only(s->blk)) {
         virtio_add_feature(&features, VIRTIO_BLK_F_RO);
     }
+    if (s->conf.num_queues > 1) {
+        virtio_add_feature(&features, VIRTIO_BLK_F_MQ);
+    }
 
     return features;
 }
@@ -795,11 +801,6 @@ static void virtio_blk_set_status(VirtIODevice *vdev, uint8_t status)
 static void virtio_blk_save(QEMUFile *f, void *opaque)
 {
     VirtIODevice *vdev = VIRTIO_DEVICE(opaque);
-    VirtIOBlock *s = VIRTIO_BLK(vdev);
-
-    if (s->dataplane) {
-        virtio_blk_data_plane_stop(s->dataplane);
-    }
 
     virtio_save(vdev, f);
 }
@@ -811,6 +812,11 @@ static void virtio_blk_save_device(VirtIODevice *vdev, QEMUFile *f)
 
     while (req) {
         qemu_put_sbyte(f, 1);
+
+        if (s->conf.num_queues > 1) {
+            qemu_put_be32(f, virtio_get_queue_index(req->vq));
+        }
+
         qemu_put_virtqueue_element(f, &req->elem);
         req = req->next;
     }
@@ -834,9 +840,22 @@ static int virtio_blk_load_device(VirtIODevice *vdev, QEMUFile *f,
     VirtIOBlock *s = VIRTIO_BLK(vdev);
 
     while (qemu_get_sbyte(f)) {
+        unsigned nvqs = s->conf.num_queues;
+        unsigned vq_idx = 0;
         VirtIOBlockReq *req;
+
+        if (nvqs > 1) {
+            vq_idx = qemu_get_be32(f);
+
+            if (vq_idx >= nvqs) {
+                error_report("Invalid virtqueue index in request list: %#x",
+                             vq_idx);
+                return -EINVAL;
+            }
+        }
+
         req = qemu_get_virtqueue_element(f, sizeof(VirtIOBlockReq));
-        virtio_blk_init_request(s, req);
+        virtio_blk_init_request(s, virtio_get_queue(vdev, vq_idx), req);
         req->next = s->rq;
         s->rq = req;
     }
@@ -862,6 +881,7 @@ static void virtio_blk_device_realize(DeviceState *dev, Error **errp)
     VirtIOBlkConf *conf = &s->conf;
     Error *err = NULL;
     static int virtio_blk_id;
+    unsigned i;
 
     if (!conf->conf.blk) {
         error_setg(errp, "drive property not set");
@@ -871,6 +891,10 @@ static void virtio_blk_device_realize(DeviceState *dev, Error **errp)
         error_setg(errp, "Device needs media, but drive is empty");
         return;
     }
+    if (!conf->num_queues) {
+        error_setg(errp, "num-queues property must be larger than 0");
+        return;
+    }
 
     blkconf_serial(&conf->conf, &conf->serial);
     s->original_wce = blk_enable_write_cache(conf->conf.blk);
@@ -888,7 +912,9 @@ static void virtio_blk_device_realize(DeviceState *dev, Error **errp)
     s->rq = NULL;
     s->sector_mask = (s->conf.conf.logical_block_size / BDRV_SECTOR_SIZE) - 1;
 
-    s->vq = virtio_add_queue(vdev, 128, virtio_blk_handle_output);
+    for (i = 0; i < conf->num_queues; i++) {
+        virtio_add_queue(vdev, 128, virtio_blk_handle_output);
+    }
     virtio_blk_data_plane_create(vdev, conf, &s->dataplane, &err);
     if (err != NULL) {
         error_propagate(errp, err);
@@ -941,6 +967,7 @@ static Property virtio_blk_properties[] = {
 #endif
     DEFINE_PROP_BIT("request-merging", VirtIOBlock, conf.request_merging, 0,
                     true),
+    DEFINE_PROP_UINT16("num-queues", VirtIOBlock, conf.num_queues, 1),
     DEFINE_PROP_END_OF_LIST(),
 };