ebpf: complete vlan support for ebpf bypass filter

ebpf/bypass_filter.c

@@ -27,6 +27,9 @@
 
 #include "bpf_helpers.h"
 
+/* vlan tracking: set it to 0 if you don't use VLAN for flow tracking */
+#define VLAN_TRACKING    1
+
 #define LINUX_VERSION_CODE 263682
 
 struct flowv4_keys {
@@ -71,18 +74,22 @@ struct bpf_map_def SEC("maps") flow_table_v6 = {
     .max_entries = 32768,
 };
 
+struct vlan_hdr {
+    __u16	h_vlan_TCI;
+    __u16	h_vlan_encapsulated_proto;
+};
+
 /**
  * IPv4 filter
  *
  * \return 0 to drop packet out and -1 to accept it
  */
-static __always_inline int ipv4_filter(struct __sk_buff *skb)
+static __always_inline int ipv4_filter(struct __sk_buff *skb, __u16 vlan0, __u16 vlan1)
 {
     __u32 nhoff, verlen;
     struct flowv4_keys tuple;
     struct pair *value;
     __u16 port;
-    __u16 vlan_id = skb->vlan_tci & 0x0fff;
 
     nhoff = skb->cb[0];
 
@@ -105,9 +112,8 @@ static __always_inline int ipv4_filter(struct __sk_buff *skb)
     port = tuple.port16[1];
     tuple.port16[1] = tuple.port16[0];
     tuple.port16[0] = port;
-    tuple.vlan_id[0] = vlan_id;
-    /* FIXME add second vlan layer */
-    tuple.vlan_id[1] = 0;
+    tuple.vlan_id[0] = vlan0;
+    tuple.vlan_id[1] = vlan1;
 
 #if 0
     if ((tuple.port16[0] == 22) || (tuple.port16[1] == 22))
@@ -141,14 +147,13 @@ static __always_inline int ipv4_filter(struct __sk_buff *skb)
  *
  * \return 0 to drop packet out and -1 to accept it
  */
-static __always_inline int ipv6_filter(struct __sk_buff *skb)
+static __always_inline int ipv6_filter(struct __sk_buff *skb, __u16 vlan0, __u16 vlan1)
 {
     __u32 nhoff;
     __u8 nhdr;
     struct flowv6_keys tuple;
     struct pair *value;
     __u16 port;
-    __u16 vlan_id = skb->vlan_tci & 0x0fff;
 
     nhoff = skb->cb[0];
 
@@ -180,9 +185,8 @@ static __always_inline int ipv6_filter(struct __sk_buff *skb)
     tuple.port16[0] = port;
     tuple.ip_proto = nhdr;
 
-    tuple.vlan_id[0] = vlan_id;
-    /* FIXME add second vlan layer */
-    tuple.vlan_id[1] = 0;
+    tuple.vlan_id[0] = vlan0;
+    tuple.vlan_id[1] = vlan1;
 
     //char fmt[] = "Now Got IPv6 port %u and %u\n";
     //bpf_trace_printk(fmt, sizeof(fmt), tuple.port16[0], tuple.port16[1]);
@@ -209,12 +213,39 @@ static __always_inline int ipv6_filter(struct __sk_buff *skb)
 int SEC("filter") hashfilter(struct __sk_buff *skb) {
     __u32 nhoff = ETH_HLEN;
 
+    void *data = (void *)(long)skb->data;
+    void *data_end = (void *)(long)skb->data_end;
+    __u64 proto = load_half(skb, 12);
+    __u16 vlan0 = 0;
+    __u16 vlan1 = 0;
+
+    if (proto == ETH_P_8021AD) {
+        proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
+                    h_vlan_encapsulated_proto));
+#if VLAN_TRACKING
+        if (data + nhoff <= data_end) {
+            vlan0 = ((struct vlan_hdr *)(data + nhoff))->h_vlan_TCI & 0x0fff;
+        }
+#endif
+        nhoff += sizeof(struct vlan_hdr);
+    }
+
+    if (proto == ETH_P_8021Q) {
+        proto = load_half(skb, nhoff + offsetof(struct vlan_hdr,
+                    h_vlan_encapsulated_proto));
+#if VLAN_TRACKING
+        if (data + nhoff <= data_end) {
+            vlan1 = ((struct vlan_hdr *)(data + nhoff))->h_vlan_TCI & 0x0fff;
+        }
+#endif
+        nhoff += sizeof(struct vlan_hdr);
+    }
     skb->cb[0] = nhoff;
     switch (skb->protocol) {
         case __constant_htons(ETH_P_IP):
-            return ipv4_filter(skb);
+            return ipv4_filter(skb, vlan0, vlan1);
         case __constant_htons(ETH_P_IPV6):
-            return ipv6_filter(skb);
+            return ipv6_filter(skb, vlan0, vlan1);
         default:
 #if 0
             {