From e59abf81d3de639e389393989c7823ac8a68f663 Mon Sep 17 00:00:00 2001
From: guy <guy>
Date: Wed, 6 Aug 2008 07:45:00 +0000
Subject: [PATCH] From Patrick McHardy:

VLAN packets sent over devices supporting VLAN tagging/stripping in
hardware don't have a VLAN header when they are received on packet
sockets. The VLAN TCI is available through the PACKET_AUXDATA cmsg,
reconstruct the entire header when necessary.
---
 Makefile.in  |  3 ++-
 pcap-linux.c | 61 ++++++++++++++++++++++++++++++++++++++++++++++++----
 pcap/vlan.h  | 46 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 105 insertions(+), 5 deletions(-)
 create mode 100644 pcap/vlan.h

diff --git a/Makefile.in b/Makefile.in
index 4f14e30b10..058b063253 100644
--- a/Makefile.in
+++ b/Makefile.in
@@ -17,7 +17,7 @@
 #  WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES OF
 #  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 #
-# @(#) $Header: /tcpdump/master/libpcap/Makefile.in,v 1.125 2008-05-30 01:35:33 guy Exp $ (LBL)
+# @(#) $Header: /tcpdump/master/libpcap/Makefile.in,v 1.126 2008-08-06 07:45:00 guy Exp $ (LBL)
 
 #
 # Various configurable paths (remember to edit Makefile.in, not Makefile)
@@ -102,6 +102,7 @@ HDR = \
 	pcap/pcap.h \
 	pcap/sll.h \
 	pcap/usb.h \
+	pcap/vlan.h \
 	pcap.h \
 	pcap-int.h \
 	pcap-namedb.h \
diff --git a/pcap-linux.c b/pcap-linux.c
index 2a22435717..1141595926 100644
--- a/pcap-linux.c
+++ b/pcap-linux.c
@@ -34,7 +34,7 @@
 
 #ifndef lint
 static const char rcsid[] _U_ =
-    "@(#) $Header: /tcpdump/master/libpcap/pcap-linux.c,v 1.151 2008-08-06 07:39:44 guy Exp $ (LBL)";
+    "@(#) $Header: /tcpdump/master/libpcap/pcap-linux.c,v 1.152 2008-08-06 07:45:00 guy Exp $ (LBL)";
 #endif
 
 /*
@@ -108,6 +108,7 @@ static const char rcsid[] _U_ =
 
 #include "pcap-int.h"
 #include "pcap/sll.h"
+#include "pcap/vlan.h"
 
 #ifdef HAVE_DAG_API
 #include "pcap-dag.h"
@@ -165,6 +166,9 @@ static const char rcsid[] _U_ =
   */
 # ifdef PACKET_HOST
 #  define HAVE_PF_PACKET_SOCKETS
+#  ifdef PACKET_AUXDATA
+#   define HAVE_PACKET_AUXDATA
+#  endif /* PACKET_AUXDATA */
 # endif /* PACKET_HOST */
 
 
@@ -629,6 +633,11 @@ pcap_read_packet(pcap_t *handle, pcap_handler callback, u_char *userdata)
 	struct pcap_pkthdr	pcap_header;
 	struct iovec		iov;
 	struct msghdr		msg;
+	struct cmsghdr		*cmsg;
+	union {
+		struct cmsghdr	cmsg;
+		char		buf[CMSG_SPACE(sizeof(struct tpacket_auxdata))];
+	} cmsg_buf;
 #ifdef HAVE_PF_PACKET_SOCKETS
 	/*
 	 * If this is a cooked device, leave extra room for a
@@ -667,8 +676,8 @@ pcap_read_packet(pcap_t *handle, pcap_handler callback, u_char *userdata)
 	msg.msg_namelen		= sizeof(from);
 	msg.msg_iov		= &iov;
 	msg.msg_iovlen		= 1;
-	msg.msg_control		= NULL;
-	msg.msg_controllen	= 0;
+	msg.msg_control		= &cmsg_buf;
+	msg.msg_controllen	= sizeof(cmsg_buf);
 	msg.msg_flags		= 0;
 
 	iov.iov_len		= handle->bufsize - offset;
@@ -774,6 +783,36 @@ pcap_read_packet(pcap_t *handle, pcap_handler callback, u_char *userdata)
 		      from.sll_halen);
 		hdrp->sll_protocol = from.sll_protocol;
 	}
+
+#ifdef HAVE_PACKET_AUXDATA
+	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+		struct tpacket_auxdata *aux;
+		unsigned int len;
+		struct vlan_tag *tag;
+
+		if (cmsg->cmsg_len < CMSG_LEN(sizeof(struct tpacket_auxdata)) ||
+		    cmsg->cmsg_level != SOL_PACKET ||
+		    cmsg->cmsg_type != PACKET_AUXDATA)
+			continue;
+
+		aux = (struct tpacket_auxdata *)CMSG_DATA(cmsg);
+		if (aux->tp_vlan_tci == 0)
+			continue;
+
+		len = packet_len > iov.iov_len ? iov.iov_len : packet_len;
+		if (len < 2 * ETH_ALEN)
+			break;
+
+		bp -= VLAN_TAG_LEN;
+		memmove(bp, bp + VLAN_TAG_LEN, 2 * ETH_ALEN);
+
+		tag = (struct vlan_tag *)(bp + 2 * ETH_ALEN);
+		tag->vlan_tpid = htons(ETH_P_8021Q);
+		tag->vlan_tci = htons(aux->tp_vlan_tci);
+
+		packet_len += VLAN_TAG_LEN;
+	}
+#endif /* HAVE_PACKET_AUXDATA */
 #endif
 
 	/*
@@ -1591,7 +1630,7 @@ static int
 activate_new(pcap_t *handle)
 {
 #ifdef HAVE_PF_PACKET_SOCKETS
-	int			sock_fd = -1, arptype;
+	int			sock_fd = -1, arptype, val;
 	int			err = 0;
 	struct packet_mreq	mr;
 	const char* device = handle->opt.source;
@@ -1802,6 +1841,20 @@ activate_new(pcap_t *handle)
 		}
 	}
 
+	/* Enable auxillary data if supported and reserve room for
+	 * reconstructing VLAN headers. */
+#ifdef HAVE_PACKET_AUXDATA
+	val = 1;
+	if (setsockopt(sock_fd, SOL_PACKET, PACKET_AUXDATA, &val,
+		       sizeof(val)) == -1 && errno != ENOPROTOOPT) {
+		snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
+			 "setsockopt: %s", pcap_strerror(errno));
+		close(sock_fd);
+		return PCAP_ERROR;
+	}
+	handle->offset += VLAN_TAG_LEN;
+#endif /* HAVE_PACKET_AUXDATA */
+
 	/*
 	 * This is a 2.2[.x] or later kernel (we know that
 	 * because we're not using a SOCK_PACKET socket -
diff --git a/pcap/vlan.h b/pcap/vlan.h
new file mode 100644
index 0000000000..41aa8c7862
--- /dev/null
+++ b/pcap/vlan.h
@@ -0,0 +1,46 @@
+/*-
+ * Copyright (c) 1990, 1991, 1992, 1993, 1994, 1995, 1996, 1997
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *      This product includes software developed by the University of
+ *      California, Berkeley and its contributors.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * @(#) $Header: /tcpdump/master/libpcap/pcap/vlan.h,v 1.1 2008-08-06 07:45:00 guy Exp $
+ */
+
+#ifndef lib_pcap_vlan_h
+#define lib_pcap_vlan_h
+
+struct vlan_tag {
+	u_int16_t	vlan_tpid;		/* ETH_P_8021Q */
+	u_int16_t	vlan_tci;		/* VLAN TCI */
+};
+
+#define VLAN_TAG_LEN	4
+
+#endif