1 files changed, 65 insertions, 3 deletions

diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index fe576e75a538..67bb583b7fc9 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -37,10 +37,12 @@ module_param(gso, bool, 0444);
 #define MAX_PACKET_LEN (ETH_HLEN + VLAN_HLEN + ETH_DATA_LEN)
 #define GOOD_COPY_LEN   128
 
+#define VIRTNET_SEND_COMMAND_SG_MAX    0
+
 struct virtnet_info
 {
         struct virtio_device *vdev;
-        struct virtqueue *rvq, *svq;
+        struct virtqueue *rvq, *svq, *cvq;
         struct net_device *dev;
         struct napi_struct napi;
         unsigned int status;
@@ -589,6 +591,53 @@ static int virtnet_open(struct net_device *dev)
         return 0;
 }
 
+/*
+ * Send command via the control virtqueue and check status.  Commands
+ * supported by the hypervisor, as indicated by feature bits, should
+ * never fail unless improperly formated.
+ */
+static bool virtnet_send_command(struct virtnet_info *vi, u8 class, u8 cmd,
+                                 struct scatterlist *data, int out, int in)
+{
+        struct scatterlist sg[VIRTNET_SEND_COMMAND_SG_MAX + 2];
+        struct virtio_net_ctrl_hdr ctrl;
+        virtio_net_ctrl_ack status = ~0;
+        unsigned int tmp;
+
+        if (!virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ)) {
+                BUG();  /* Caller should know better */
+                return false;
+        }
+
+        BUG_ON(out + in > VIRTNET_SEND_COMMAND_SG_MAX);
+
+        out++; /* Add header */
+        in++; /* Add return status */
+
+        ctrl.class = class;
+        ctrl.cmd = cmd;
+
+        sg_init_table(sg, out + in);
+
+        sg_set_buf(&sg[0], &ctrl, sizeof(ctrl));
+        memcpy(&sg[1], data, sizeof(struct scatterlist) * (out + in - 2));
+        sg_set_buf(&sg[out + in - 1], &status, sizeof(status));
+
+        if (vi->cvq->vq_ops->add_buf(vi->cvq, sg, out, in, vi) != 0)
+                BUG();
+
+        vi->cvq->vq_ops->kick(vi->cvq);
+
+        /*
+         * Spin for a response, the kick causes an ioport write, trapping
+         * into the hypervisor, so the request should be handled immediately.
+         */
+        while (!vi->cvq->vq_ops->get_buf(vi->cvq, &tmp))
+                cpu_relax();
+
+        return status == VIRTIO_NET_OK;
+}
+
 static int virtnet_close(struct net_device *dev)
 {
         struct virtnet_info *vi = netdev_priv(dev);
@@ -752,6 +801,14 @@ static int virtnet_probe(struct virtio_device *vdev)
                 goto free_recv;
         }
 
+        if (virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ)) {
+                vi->cvq = vdev->config->find_vq(vdev, 2, NULL);
+                if (IS_ERR(vi->cvq)) {
+                        err = PTR_ERR(vi->svq);
+                        goto free_send;
+                }
+        }
+
         /* Initialize our empty receive and send queues. */
         skb_queue_head_init(&vi->recv);
         skb_queue_head_init(&vi->send);
@@ -764,7 +821,7 @@ static int virtnet_probe(struct virtio_device *vdev)
         err = register_netdev(dev);
         if (err) {
                 pr_debug("virtio_net: registering device failed\n");
-                goto free_send;
+                goto free_ctrl;
         }
 
         /* Last of all, set up some receive buffers. */
@@ -784,6 +841,9 @@ static int virtnet_probe(struct virtio_device *vdev)
 
 unregister:
         unregister_netdev(dev);
+free_ctrl:
+        if (virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ))
+                vdev->config->del_vq(vi->cvq);
 free_send:
         vdev->config->del_vq(vi->svq);
 free_recv:
@@ -815,6 +875,8 @@ static void virtnet_remove(struct virtio_device *vdev)
 
         vdev->config->del_vq(vi->svq);
         vdev->config->del_vq(vi->rvq);
+        if (virtio_has_feature(vi->vdev, VIRTIO_NET_F_CTRL_VQ))
+                vdev->config->del_vq(vi->cvq);
         unregister_netdev(vi->dev);
 
         while (vi->pages)
@@ -834,7 +896,7 @@ static unsigned int features[] = {
         VIRTIO_NET_F_HOST_TSO4, VIRTIO_NET_F_HOST_UFO, VIRTIO_NET_F_HOST_TSO6,
         VIRTIO_NET_F_HOST_ECN, VIRTIO_NET_F_GUEST_TSO4, VIRTIO_NET_F_GUEST_TSO6,
         VIRTIO_NET_F_GUEST_ECN, /* We don't yet handle UFO input. */
-        VIRTIO_NET_F_MRG_RXBUF, VIRTIO_NET_F_STATUS,
+        VIRTIO_NET_F_MRG_RXBUF, VIRTIO_NET_F_STATUS, VIRTIO_NET_F_CTRL_VQ,
         VIRTIO_F_NOTIFY_ON_EMPTY,
 };