From 9cab89df960816a5f2946f1da3b67377d9ccf6ce Mon Sep 17 00:00:00 2001
From: Samuel Jero <sj323707@ohio.edu>
Date: Wed, 13 Feb 2013 22:33:18 -0500
Subject: [PATCH] Add Support for Ethernet VLAN tags

---
 encap.c | 73 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 encap.h |  1 +
 2 files changed, 74 insertions(+)

diff --git a/encap.c b/encap.c
index 6f79307..d2c06b1 100644
--- a/encap.c
+++ b/encap.c
@@ -24,6 +24,7 @@ Date: 11/2012
 #include "encap.h"
 #include "checksums.h"
 #include <pcap/sll.h>
+#include <pcap/vlan.h>
 #include <netinet/ip6.h>
 #include <netdb.h>
 
@@ -114,6 +115,11 @@ int ethernet_encap(struct packet *new, const struct const_packet *old)
 							return 0;
 					}
 					break;
+			case ETHERTYPE_VLAN:
+					if(!ethernet_vlan_encap(&nnew, &nold)){
+							return 0;
+					}
+					break;
 			default:
 					dbgprintf(1, "Unknown Next Protocol at Ethernet\n");
 					return 0;
@@ -125,6 +131,73 @@ int ethernet_encap(struct packet *new, const struct const_packet *old)
 return 1;
 }
 
+/*Ethernet 802.1Q VLAN Encapsulation*/
+int ethernet_vlan_encap(struct packet *new, const struct const_packet *old)
+{
+		struct vlan_tag		*tag;
+		struct const_packet nold;
+		struct packet 		nnew;
+
+		/*Safety checks*/
+		if(!new || !old || !new->data || !old->data || !new->h || !old->h){
+			dbgprintf(0,"Error: Ethernet VLAN Encapsulation Function given bad data!\n");
+			return 0;
+		}
+		if(old->length < sizeof(struct vlan_tag) || new->length < sizeof(struct vlan_tag)){
+			dbgprintf(0, "Error: Ethernet VLAN Encapsulation Function given packet of wrong size!\n");
+			return 0;
+		}
+
+		/*Copy VLAN tag over*/
+		memcpy(new->data, old->data, sizeof(struct vlan_tag));
+
+		/*Cast Pointer*/
+		tag=(struct vlan_tag*)(new->data);
+
+		/*Adjust pointers and lengths*/
+		nold.data= old->data+ sizeof(struct vlan_tag);
+		nnew.data= new->data + sizeof(struct vlan_tag);
+		nold.length= old->length - sizeof(struct vlan_tag);
+		nnew.length= new->length - sizeof(struct vlan_tag);
+		nnew.h=new->h;
+		nold.h=old->h;
+		nnew.print_id=NULL;
+		nnew.dest_id=NULL;
+		nnew.src_id=NULL;
+		nnew.id_len=0;
+		nold.print_id=NULL;
+		nold.dest_id=NULL;
+		nold.src_id=NULL;
+		nold.id_len=0;
+
+		/*Select Next Protocol*/
+		switch(ntohs(tag->vlan_tci)){
+			case ETHERTYPE_IP:
+					if(!ipv4_encap(&nnew, &nold)){
+							return 0;
+					}
+					break;
+			case ETHERTYPE_IPV6:
+					if(!ipv6_encap(&nnew, &nold)){
+							return 0;
+					}
+					break;
+			case ETHERTYPE_VLAN:
+					if(!ethernet_vlan_encap(&nnew, &nold)){
+							return 0;
+					}
+					break;
+			default:
+					dbgprintf(1, "Unknown Next Protocol at Ethernet VLAN tag\n");
+					return 0;
+					break;
+		}
+
+		/*Adjust length*/
+		new->length=nnew.length + sizeof(struct vlan_tag);
+return 1;
+}
+
 /*IPv6 Encapsulation*/
 int ipv6_encap(struct packet *new, const struct const_packet *old)
 {
diff --git a/encap.h b/encap.h
index 890335a..010cac2 100644
--- a/encap.h
+++ b/encap.h
@@ -76,6 +76,7 @@ int convert_packet(struct packet *new, const struct const_packet *old);
 
 /*Standard Encapsulation Functions*/
 int ethernet_encap(struct packet *new, const struct const_packet *old);
+int ethernet_vlan_encap(struct packet *new, const struct const_packet *old);
 int linux_cooked_encap(struct packet *new, const struct const_packet *old);
 int ipv4_encap(struct packet *new, const struct const_packet *old);
 int ipv6_encap(struct packet *new, const struct const_packet *old);
-- 
2.39.2