[PATCH 7/7] qemu:virtio-net: Add VLAN filtering

[Alex Williamson <alex.williamson@hp.com>]

Use the control virtqueue to allow the guest to enable and manipulate
a VLAN filter table.  This allows us to drop more packets the guest
doesn't want to see.  We define a new VLAN class for the control
virtqueue with commands ENABLE, ADD, and DEL with usage defined in
virtio-net.h.  By default VLAN filtering is disabled to allow backwards
compatibility with guest drivers.

Signed-off-by: Alex Williamson <alex.williamson@hp.com>
---

 Updated to reflect VLAN_KILL -> VLAN_DEL rename in the guest driver
 Updated to reflect change in receive_filter() looking past vnet_hdr.

 qemu/hw/virtio-net.c |   70 +++++++++++++++++++++++++++++++++++++++++++++++++-
 qemu/hw/virtio-net.h |   15 +++++++++++
 2 files changed, 84 insertions(+), 1 deletions(-)

diff --git a/qemu/hw/virtio-net.c b/qemu/hw/virtio-net.c
index 528171e..8f3c41d 100644
--- a/qemu/hw/virtio-net.c
+++ b/qemu/hw/virtio-net.c
@@ -21,9 +21,10 @@
 
 #define TAP_VNET_HDR
 
-#define VIRTIO_NET_VM_VERSION    5
+#define VIRTIO_NET_VM_VERSION    6
 
 #define ETH_ALEN    6
+#define MAX_VLAN    (1 << 12)   /* Per 802.1Q definition */
 
 typedef struct VirtIONet
 {
@@ -44,6 +45,10 @@ typedef struct VirtIONet
         int in_use;
         uint8_t *macs;
     } mac_table;
+    struct {
+        int enabled;
+        uint32_t *vlans;
+    } vlan_table;
 } VirtIONet;
 
 /* TODO
@@ -101,6 +106,9 @@ static void virtio_net_reset(VirtIODevice *vdev)
     n->mac_table.entries = 0;
     qemu_free(n->mac_table.macs);
     n->mac_table.macs = NULL;
+
+    n->vlan_table.enabled = 0;
+    memset(n->vlan_table.vlans, 0, MAX_VLAN >> 3);
 }
 
 static uint32_t virtio_net_get_features(VirtIODevice *vdev)
@@ -223,6 +231,45 @@ static int virtio_net_handle_mac_table(VirtIONet *n, uint8_t cmd,
     return VIRTIO_NET_ERR;
 }
 
+static int virtio_net_handle_vlan_table(VirtIONet *n, uint8_t cmd,
+                                        VirtQueueElement *elem)
+{
+    uint16_t *vid;
+
+    if (cmd == VIRTIO_NET_CTRL_VLAN_ENABLE) {
+        uint8_t *on;
+
+        if (elem->out_num != 2 || elem->out_sg[1].iov_len != sizeof(*on)) {
+            fprintf(stderr, "virtio-net ctrl invalid vlan command\n");
+            exit(1);
+        }
+
+        on = elem->out_sg[1].iov_base;
+
+        n->vlan_table.enabled = *on;
+        return VIRTIO_NET_OK;
+    }
+
+    if (elem->out_num != 2 || elem->out_sg[1].iov_len != sizeof(*vid)) {
+        fprintf(stderr, "virtio-net ctrl invalid vlan command\n");
+        exit(1);
+    }
+
+    vid = elem->out_sg[1].iov_base;
+
+    if (*vid >= MAX_VLAN)
+        return VIRTIO_NET_ERR;
+
+    if (cmd == VIRTIO_NET_CTRL_VLAN_ADD)
+        n->vlan_table.vlans[*vid >> 5] |= (1U << (*vid & 0x1f));
+    else if (cmd == VIRTIO_NET_CTRL_VLAN_DEL)
+        n->vlan_table.vlans[*vid >> 5] &= ~(1U << (*vid & 0x1f));
+    else
+        return VIRTIO_NET_ERR;
+
+    return VIRTIO_NET_OK;
+}
+
 static void virtio_net_handle_ctrl(VirtIODevice *vdev, VirtQueue *vq)
 {
     VirtIONet *n = to_virtio_net(vdev);
@@ -250,6 +297,8 @@ static void virtio_net_handle_ctrl(VirtIODevice *vdev, VirtQueue *vq)
             *status = virtio_net_handle_rx_mode(n, ctrl->cmd, &elem);
         else if (ctrl->class == VIRTIO_NET_CTRL_MAC_TABLE)
             *status = virtio_net_handle_mac_table(n, ctrl->cmd, &elem);
+        else if (ctrl->class == VIRTIO_NET_CTRL_VLAN)
+            *status = virtio_net_handle_vlan_table(n, ctrl->cmd, &elem);
 
         virtqueue_push(vq, &elem, sizeof(*status));
         virtio_notify(vdev, vq);
@@ -366,6 +415,7 @@ static int receive_header(VirtIONet *n, struct iovec *iov, int iovcnt,
 static int receive_filter(VirtIONet *n, const uint8_t *buf, int size)
 {
     static uint8_t bcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+    static uint8_t vlan[] = {0x81, 0x00};
     uint8_t *ptr = (uint8_t *)buf;
     int i;
 
@@ -374,6 +424,12 @@ static int receive_filter(VirtIONet *n, const uint8_t *buf, int size)
         ptr += sizeof(struct virtio_net_hdr);
 #endif
 
+    if (n->vlan_table.enabled && !memcmp(&ptr[12], vlan, sizeof(vlan))) {
+        int vid = be16_to_cpup((uint16_t *)(ptr + 14)) & 0xfff;
+        if (!(n->vlan_table.vlans[vid >> 5] & (1U << (vid & 0x1f))))
+            return 0;
+    }
+
     if (n->promisc)
         return 1;
 
@@ -573,6 +629,8 @@ static void virtio_net_save(QEMUFile *f, void *opaque)
     qemu_put_be32(f, n->mac_table.in_use);
     if (n->mac_table.entries)
         qemu_put_buffer(f, n->mac_table.macs, n->mac_table.entries * ETH_ALEN);
+    qemu_put_be32(f, n->vlan_table.enabled);
+    qemu_put_buffer(f, (uint8_t *)n->vlan_table.vlans, MAX_VLAN >> 3);
 }
 
 static int virtio_net_load(QEMUFile *f, void *opaque, int version_id)
@@ -614,6 +672,11 @@ static int virtio_net_load(QEMUFile *f, void *opaque, int version_id)
         }
     }
  
+    if (version_id >= 6) {
+        n->vlan_table.enabled = qemu_get_be32(f);
+        qemu_get_buffer(f, (uint8_t *)n->vlan_table.vlans, MAX_VLAN >> 3);
+    }
+
     if (n->tx_timer_active) {
         qemu_mod_timer(n->tx_timer,
                        qemu_get_clock(vm_clock) + TX_TIMER_INTERVAL);
@@ -656,6 +719,11 @@ PCIDevice *virtio_net_init(PCIBus *bus, NICInfo *nd, int devfn)
     n->mergeable_rx_bufs = 0;
     n->promisc = 1; /* for compatibility */
 
+    /* VLAN filter table starts disabled for compatibility */
+    n->vlan_table.vlans = qemu_mallocz(MAX_VLAN >> 3);
+    if (!n->vlan_table.vlans)
+        return NULL;
+
     register_savevm("virtio-net", virtio_net_id++, VIRTIO_NET_VM_VERSION,
                     virtio_net_save, virtio_net_load, n);
 
diff --git a/qemu/hw/virtio-net.h b/qemu/hw/virtio-net.h
index 6faf497..bf40207 100644
--- a/qemu/hw/virtio-net.h
+++ b/qemu/hw/virtio-net.h
@@ -128,4 +128,19 @@ typedef uint8_t virtio_net_ctrl_ack;
  #define VIRTIO_NET_CTRL_MAC_TABLE_ALLOC      0
  #define VIRTIO_NET_CTRL_MAC_TABLE_SET        1
 
+/*
+ * Control VLAN filtering
+ *
+ * The VLAN filter table is controlled via a simple ADD/DEL interface.
+ * VLAN IDs not added will be dropped.  Del is the opposite of add.
+ * Both commands expect an out entry containing a 2 byte VLAN ID.
+ * The ENABLE command expects an out entry containing a single byte,
+ * zero to disable, non-zero to enable.  The default state is disabled
+ * for compatibility.
+ */
+#define VIRTIO_NET_CTRL_VLAN       2
+ #define VIRTIO_NET_CTRL_VLAN_ENABLE          0
+ #define VIRTIO_NET_CTRL_VLAN_ADD             1
+ #define VIRTIO_NET_CTRL_VLAN_DEL             2
+
 #endif