[RFC PATCH 7/9] vhost: Add vhost_svq_inject

["Eugenio Pérez" <eperezma@redhat.com>]

This allows qemu to inject packets to the device without guest's notice.

This will be use to inject net CVQ messages to restore status in the destination

Signed-off-by: Eugenio Pérez <eperezma@redhat.com>
---
 hw/virtio/vhost-shadow-virtqueue.h |   2 +
 hw/virtio/vhost-shadow-virtqueue.c | 142 ++++++++++++++++++++++++-----
 2 files changed, 123 insertions(+), 21 deletions(-)

diff --git a/hw/virtio/vhost-shadow-virtqueue.h b/hw/virtio/vhost-shadow-virtqueue.h
index bf3b658889..767b0a82ba 100644
--- a/hw/virtio/vhost-shadow-virtqueue.h
+++ b/hw/virtio/vhost-shadow-virtqueue.h
@@ -38,6 +38,8 @@ bool vhost_svq_ack_guest_features(uint64_t dev_features,
                                   uint64_t guest_features,
                                   uint64_t *acked_features);
 
+bool vhost_svq_inject(VhostShadowVirtqueue *svq, const struct iovec *iov,
+                      size_t out_num, size_t in_num);
 void vhost_svq_set_svq_kick_fd(VhostShadowVirtqueue *svq, int svq_kick_fd);
 void vhost_svq_set_guest_call_notifier(VhostShadowVirtqueue *svq, int call_fd);
 const EventNotifier *vhost_svq_get_dev_kick_notifier(
diff --git a/hw/virtio/vhost-shadow-virtqueue.c b/hw/virtio/vhost-shadow-virtqueue.c
index 2ba3c2966a..26938b059c 100644
--- a/hw/virtio/vhost-shadow-virtqueue.c
+++ b/hw/virtio/vhost-shadow-virtqueue.c
@@ -16,9 +16,11 @@
 
 #include "qemu/error-report.h"
 #include "qemu/main-loop.h"
+#include "qemu/iov.h"
 
 typedef struct SVQElement {
     VirtQueueElement elem;
+    bool not_from_guest;
 } SVQElement;
 
 /* Shadow virtqueue to relay notifications */
@@ -311,19 +313,53 @@ static bool vhost_svq_add_split(VhostShadowVirtqueue *svq,
     /* We need some descriptors here */
     assert(elem->out_num || elem->in_num);
 
-    ok = vhost_svq_translate_addr(svq, sgs, elem->out_sg, elem->out_num);
-    if (unlikely(!ok)) {
-        return false;
+    if (elem->out_sg && svq_elem->not_from_guest) {
+        DMAMap map = {
+            .translated_addr = (hwaddr)svq_elem->elem.out_sg->iov_base,
+            .size = ROUND_UP(elem->out_sg->iov_len, 4096) - 1,
+            .perm = IOMMU_RO,
+        };
+        int r = vhost_iova_tree_map_alloc(svq->iova_tree, &map);
+
+        elem->out_addr[0] = map.iova;
+        assert(elem->out_num == 1);
+        assert(r == IOVA_OK);
+
+        r = svq->map_ops->map(map.iova, map.size, (void *)map.translated_addr,
+                              true, svq->map_ops_opaque);
+        assert(r == 0);
+        sgs[0] = (void *)map.iova;
+    } else {
+        ok = vhost_svq_translate_addr(svq, sgs, elem->out_sg, elem->out_num);
+        if (unlikely(!ok)) {
+            return false;
+        }
     }
     vhost_vring_write_descs(svq, sgs, elem->out_sg, elem->out_num,
                             elem->in_num > 0, false);
 
+    if (elem->in_sg && svq_elem->not_from_guest) {
+        DMAMap map = {
+            .translated_addr = (hwaddr)svq_elem->elem.in_sg->iov_base,
+            .size = ROUND_UP(elem->in_sg->iov_len, 4096) - 1,
+            .perm = IOMMU_RW,
+        };
+        int r = vhost_iova_tree_map_alloc(svq->iova_tree, &map);
 
-    ok = vhost_svq_translate_addr(svq, sgs, elem->in_sg, elem->in_num);
-    if (unlikely(!ok)) {
-        return false;
-    }
+        elem->in_addr[0] = map.iova;
+        assert(elem->out_num == 1);
+        assert(r == IOVA_OK);
 
+        r = svq->map_ops->map(map.iova, map.size, (void *)map.translated_addr,
+                              false, svq->map_ops_opaque);
+        assert(r == 0);
+        sgs[0] = (void *)map.iova;
+    } else {
+        ok = vhost_svq_translate_addr(svq, sgs, elem->in_sg, elem->in_num);
+        if (unlikely(!ok)) {
+            return false;
+        }
+    }
     vhost_vring_write_descs(svq, sgs, elem->in_sg, elem->in_num, false, true);
 
     /*
@@ -364,6 +400,43 @@ static void vhost_svq_kick(VhostShadowVirtqueue *svq)
     event_notifier_set(&svq->hdev_kick);
 }
 
+bool vhost_svq_inject(VhostShadowVirtqueue *svq, const struct iovec *iov,
+                      size_t out_num, size_t in_num)
+{
+    size_t out_size = iov_size(iov, out_num);
+    size_t out_buf_size = ROUND_UP(out_size, 4096);
+    size_t in_size = iov_size(iov + out_num, in_num);
+    size_t in_buf_size = ROUND_UP(in_size, 4096);
+    SVQElement *svq_elem;
+    uint16_t num_slots = (in_num ? 1 : 0) + (out_num ? 1 : 0);
+
+    if (unlikely(num_slots == 0 || svq->next_guest_avail_elem ||
+        vhost_svq_available_slots(svq) < num_slots)) {
+        return false;
+    }
+
+    svq_elem = virtqueue_alloc_element(sizeof(SVQElement), 1, 1);
+    if (out_num) {
+        void *out = qemu_memalign(4096, out_buf_size);
+        svq_elem->elem.out_sg[0].iov_base = out;
+        svq_elem->elem.out_sg[0].iov_len = out_size;
+        iov_to_buf(iov, out_num, 0, out, out_size);
+        memset(out + out_size, 0, out_buf_size - out_size);
+    }
+    if (in_num) {
+        void *in = qemu_memalign(4096, in_buf_size);
+        svq_elem->elem.in_sg[0].iov_base = in;
+        svq_elem->elem.in_sg[0].iov_len = in_size;
+        memset(in, 0, in_buf_size);
+    }
+
+    svq_elem->not_from_guest = true;
+    vhost_svq_add(svq, svq_elem);
+    vhost_svq_kick(svq);
+
+    return true;
+}
+
 /**
  * Forward available buffers.
  *
@@ -512,23 +585,50 @@ static void vhost_svq_flush(VhostShadowVirtqueue *svq,
             }
 
             elem = &svq_elem->elem;
-            if (unlikely(i >= svq->vring.num)) {
-                virtio_error(svq->vdev,
-                         "More than %u used buffers obtained in a %u size SVQ",
-                         i, svq->vring.num);
-                virtqueue_fill(vq, elem, elem->len, i);
-                virtqueue_flush(vq, i);
-                i = 0;
-            }
-            virtqueue_fill(vq, elem, elem->len, i++);
-
             if (svq->ops && svq->ops->used_elem_handler) {
                 svq->ops->used_elem_handler(svq->vdev, elem);
             }
+
+            if (svq_elem->not_from_guest) {
+                const DMAMap out_map = {
+                    .iova = elem->out_addr[0],
+                    .translated_addr = (hwaddr)elem->out_sg[0].iov_base,
+                    .size = elem->out_sg[0].iov_len,
+                };
+                const DMAMap in_map = {
+                    .iova = elem->in_addr[0],
+                    .translated_addr = (hwaddr)elem->in_sg[0].iov_base,
+                    .size = elem->in_sg[0].iov_len,
+                };
+                vhost_iova_tree_remove(svq->iova_tree, &out_map);
+                if (svq->map_ops->unmap) {
+                    svq->map_ops->unmap(out_map.iova, in_map.size,
+                                        svq->map_ops_opaque);
+                }
+                qemu_vfree(elem->out_sg[0].iov_base);
+                vhost_iova_tree_remove(svq->iova_tree, &in_map);
+                if (svq->map_ops->unmap) {
+                    svq->map_ops->unmap(out_map.iova, out_map.size,
+                                        svq->map_ops_opaque);
+                }
+                qemu_vfree(elem->in_sg[0].iov_base);
+            } else {
+                if (unlikely(i >= svq->vring.num)) {
+                    virtio_error(svq->vdev,
+                        "More than %u used buffers obtained in a %u size SVQ",
+                        i, svq->vring.num);
+                    virtqueue_fill(vq, elem, elem->len, i);
+                    virtqueue_flush(vq, i);
+                    i = 0;
+                }
+                virtqueue_fill(vq, elem, elem->len, i++);
+            }
         }
 
-        virtqueue_flush(vq, i);
-        event_notifier_set(&svq->svq_call);
+        if (i > 0) {
+            virtqueue_flush(vq, i);
+            event_notifier_set(&svq->svq_call);
+        }
 
         if (check_for_avail_queue && svq->next_guest_avail_elem) {
             /*
@@ -704,14 +804,14 @@ void vhost_svq_stop(VhostShadowVirtqueue *svq)
     for (unsigned i = 0; i < svq->vring.num; ++i) {
         g_autofree SVQElement *svq_elem = NULL;
         svq_elem = g_steal_pointer(&svq->ring_id_maps[i]);
-        if (svq_elem)  {
+        if (svq_elem && !svq_elem->not_from_guest)  {
             virtqueue_detach_element(svq->vq, &svq_elem->elem,
                                      svq_elem->elem.len);
         }
     }
 
     next_avail_elem = g_steal_pointer(&svq->next_guest_avail_elem);
-    if (next_avail_elem) {
+    if (next_avail_elem && !next_avail_elem->not_from_guest) {
         virtqueue_detach_element(svq->vq, &next_avail_elem->elem,
                                  next_avail_elem->elem.len);
     }
-- 
2.27.0