From 96415649fcd69411a3ef7c12c84e7ccc909a665f Mon Sep 17 00:00:00 2001
From: chenxm <chen@xiaming.me>
Date: Thu, 12 May 2016 10:45:11 +0800
Subject: [PATCH] Support IEEE 802.1Q, VLAN

---
 pkt2flow.c  | 20 ++++++++++++++++----
 pkt2flow.h  |  7 +++++++
 utilities.c | 12 +++++++++++-
 3 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/pkt2flow.c b/pkt2flow.c
index 94cf81b..63573ec 100644
--- a/pkt2flow.c
+++ b/pkt2flow.c
@@ -34,7 +34,6 @@
 #include <getopt.h>
 #include <net/ethernet.h>
 #include <netinet/in.h>
-#include <netinet/in.h>
 #include <netinet/ip.h>
 #include <netinet/ip6.h>
 #include <netinet/tcp.h>
@@ -222,7 +221,7 @@ static int pcap_handle_layer4(struct af_6tuple *af_6tuple, const u_char *bytes,
 #endif
 
 #ifdef darwin
-            if (tcphdr->th_flags == TH_SYN)
+        if (tcphdr->th_flags == TH_SYN)
 #else
 		if (tcphdr->syn)
 #endif
@@ -347,8 +346,21 @@ static int pcap_handle_ethernet(struct af_6tuple *af_6tuple,
 	len -= sizeof(*ethhdr);
 	bytes += sizeof(*ethhdr);
 
-	if (ntohs(ethhdr->ether_type) != ETHERTYPE_IP &&
-	    ntohs(ethhdr->ether_type) != ETHERTYPE_IPV6)
+	struct vlan_header *vlanhdr;
+	uint16_t etype = ntohs(ethhdr->ether_type);
+
+	/* VLAN header, IEEE 802.1Q */
+	if (etype == ETHERTYPE_VLAN) {
+		vlanhdr = (struct vlan_header *)bytes;
+		etype = ntohs(vlanhdr->tpid);
+		bytes += sizeof(*vlanhdr);
+		len -= sizeof(*vlanhdr);
+		af_6tuple->is_vlan = 1;
+	} else {
+		af_6tuple->is_vlan = 0;
+	}
+
+	if (etype != ETHERTYPE_IP && etype != ETHERTYPE_IPV6)
 		return -1;
 
 	return pcap_handle_ip(af_6tuple, bytes, len);
diff --git a/pkt2flow.h b/pkt2flow.h
index e2e1b63..7142555 100644
--- a/pkt2flow.h
+++ b/pkt2flow.h
@@ -68,6 +68,12 @@ struct pkt_dump_file {
 	unsigned long start_time;
 };
 
+/* VLAN header, IEEE 802.1Q */
+struct vlan_header {
+	uint16_t tci;   /* Priority 3bits, CFI 1bit, ID 12bits */
+	uint16_t tpid;
+};
+
 union ip_address {
 	struct in_addr v4;
 	struct in6_addr v6;
@@ -78,6 +84,7 @@ struct af_6tuple {
 	int protocol;
 	union ip_address ip1, ip2;
 	uint16_t port1, port2;
+	uint8_t is_vlan;
 };
 
 struct ip_pair {
diff --git a/utilities.c b/utilities.c
index f0793dd..c59b44b 100644
--- a/utilities.c
+++ b/utilities.c
@@ -57,9 +57,19 @@ char *new_file_name(struct af_6tuple af_6tuple, unsigned long timestamp)
 		break;
 	}
 
-	ret = asprintf(&fname, "%s_%"PRIu16"_%s_%"PRIu16"_%lu.pcap",
+	switch (af_6tuple.is_vlan) {
+	case 0:
+		ret = asprintf(&fname, "%s_%"PRIu16"_%s_%"PRIu16"_%lu.pcap",
 		       src_ip_str, af_6tuple.port1, dst_ip_str, af_6tuple.port2,
 		       timestamp);
+		break;
+	case 1:
+		ret = asprintf(&fname, "%s_%"PRIu16"_%s_%"PRIu16"_%lu_vlan.pcap",
+		       src_ip_str, af_6tuple.port1, dst_ip_str, af_6tuple.port2,
+		       timestamp);
+		break;
+	}
+
 	if (ret < 0)
 		fname = NULL;