[v1,15/17] virtio-blk: support multi queue for non-dataplane

diff --git a/hw/block/virtio-blk.c b/hw/block/virtio-blk.c
index 2a11bc4..baec8f8 100644
--- a/hw/block/virtio-blk.c
+++ b/hw/block/virtio-blk.c
@@ -58,12 +58,13 @@  static void virtio_blk_complete_request(VirtIOBlockReq *req,
 {
     VirtIOBlock *s = req->dev;
     VirtIODevice *vdev = VIRTIO_DEVICE(s);
+    unsigned qid = req->qid;
 
     trace_virtio_blk_req_complete(req, status);
 
     stb_p(&req->in->status, status);
-    virtqueue_push(s->vq, &req->elem, req->qiov.size + sizeof(*req->in));
-    virtio_notify(vdev, s->vq);
+    virtqueue_push(s->vqs[qid], &req->elem, req->qiov.size + sizeof(*req->in));
+    virtio_notify(vdev, s->vqs[qid]);
 }
 
 static void virtio_blk_req_complete(VirtIOBlockReq *req, unsigned char status)
@@ -123,11 +124,12 @@  static void virtio_blk_flush_complete(void *opaque, int ret)
     virtio_blk_free_request(req);
 }
 
-static VirtIOBlockReq *virtio_blk_get_request(VirtIOBlock *s)
+static VirtIOBlockReq *virtio_blk_get_request(VirtIOBlock *s, unsigned qid)
 {
     VirtIOBlockReq *req = virtio_blk_alloc_request(s);
 
-    if (!virtqueue_pop(s->vq, &req->elem)) {
+    req->qid = qid;
+    if (!virtqueue_pop(s->vqs[qid], &req->elem)) {
         virtio_blk_free_request(req);
         return NULL;
     }
@@ -439,6 +441,7 @@  static void virtio_blk_handle_output(VirtIODevice *vdev, VirtQueue *vq)
     MultiReqBuffer mrb = {
         .num_writes = 0,
     };
+    unsigned qid = virtio_get_queue_index(vq);
 
 #ifdef CONFIG_VIRTIO_BLK_DATA_PLANE
     /* Some guests kick before setting VIRTIO_CONFIG_S_DRIVER_OK so start
@@ -450,7 +453,7 @@  static void virtio_blk_handle_output(VirtIODevice *vdev, VirtQueue *vq)
     }
 #endif
 
-    while ((req = virtio_blk_get_request(s))) {
+    while ((req = virtio_blk_get_request(s, qid))) {
         virtio_blk_handle_request(req, &mrb);
     }
 
@@ -556,6 +559,7 @@  static void virtio_blk_update_config(VirtIODevice *vdev, uint8_t *config)
     blkcfg.physical_block_exp = get_physical_block_exp(s->conf);
     blkcfg.alignment_offset = 0;
     blkcfg.wce = bdrv_enable_write_cache(s->bs);
+    stw_p(&blkcfg.num_queues, s->blk.num_queues);
     memcpy(config, &blkcfg, sizeof(struct virtio_blk_config));
 }
 
@@ -590,6 +594,10 @@  static uint32_t virtio_blk_get_features(VirtIODevice *vdev, uint32_t features)
     if (bdrv_is_read_only(s->bs))
         features |= 1 << VIRTIO_BLK_F_RO;
 
+    if (s->blk.num_queues > 1) {
+        features |= 1 << VIRTIO_BLK_F_MQ;
+    }
+
     return features;
 }
 
@@ -739,6 +747,7 @@  static void virtio_blk_device_realize(DeviceState *dev, Error **errp)
 #ifdef CONFIG_VIRTIO_BLK_DATA_PLANE
     Error *err = NULL;
 #endif
+    int i;
     static int virtio_blk_id;
 
     if (!blk->conf.bs) {
@@ -765,7 +774,9 @@  static void virtio_blk_device_realize(DeviceState *dev, Error **errp)
     s->rq = NULL;
     s->sector_mask = (s->conf->logical_block_size / BDRV_SECTOR_SIZE) - 1;
 
-    s->vq = virtio_add_queue(vdev, 128, virtio_blk_handle_output);
+    s->vqs = g_malloc0(sizeof(VirtQueue *) * blk->num_queues);
+    for (i = 0; i < blk->num_queues; i++)
+        s->vqs[i] = virtio_add_queue(vdev, 128, virtio_blk_handle_output);
     s->complete_request = virtio_blk_complete_request;
 #ifdef CONFIG_VIRTIO_BLK_DATA_PLANE
     virtio_blk_data_plane_create(vdev, blk, &s->dataplane, &err);
@@ -802,6 +813,7 @@  static void virtio_blk_device_unrealize(DeviceState *dev, Error **errp)
     qemu_del_vm_change_state_handler(s->change);
     unregister_savevm(dev, "virtio-blk", s);
     blockdev_mark_auto_del(s->bs);
+    g_free(s->vqs);
     virtio_cleanup(vdev);
 }
 
@@ -809,6 +821,7 @@  static void virtio_blk_instance_init(Object *obj)
 {
     VirtIOBlock *s = VIRTIO_BLK(obj);
 
+    s->blk.num_queues = 1;    /* num of queue has to be at least 1 */
     s->obj_pool = NULL;
     object_property_add_link(obj, "iothread", TYPE_IOTHREAD,
                              (Object **)&s->blk.iothread,
diff --git a/include/hw/virtio/virtio-blk.h b/include/hw/virtio/virtio-blk.h
index 5b0fb91..79c3017 100644
--- a/include/hw/virtio/virtio-blk.h
+++ b/include/hw/virtio/virtio-blk.h
@@ -122,6 +122,7 @@  struct VirtIOBlkConf
     uint32_t scsi;
     uint32_t config_wce;
     uint32_t data_plane;
+    uint32_t num_queues;
 };
 
 struct VirtIOBlockDataPlane;
@@ -130,7 +131,7 @@  struct VirtIOBlockReq;
 typedef struct VirtIOBlock {
     VirtIODevice parent_obj;
     BlockDriverState *bs;
-    VirtQueue *vq;
+    VirtQueue **vqs;
     void *rq;
     QEMUBH *bh;
     BlockConf *conf;
@@ -160,6 +161,7 @@  typedef struct VirtIOBlockReq {
     QEMUIOVector qiov;
     struct VirtIOBlockReq *next;
     BlockAcctCookie acct;
+    unsigned qid;
 } VirtIOBlockReq;
 
 VirtIOBlockReq *virtio_blk_alloc_request(VirtIOBlock *s);