[PATCH 02/04]: pcap-linux: reconstruct VLAN header from PACKET_AUXDATA

[Patrick McHardy <kaber () trash net>]

pcap-linux: reconstruct VLAN header from PACKET_AUXDATA

From: Patrick McHardy <kaber () trash net>

VLAN packets sent over devices supporting VLAN tagging/stripping in
hardwaredon'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.
---

 pcap-linux.c |   59 +++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 pcap/vlan.h  |   11 +++++++++++
 2 files changed, 67 insertions(+), 3 deletions(-)
 create mode 100644 pcap/vlan.h


diff --git a/pcap-linux.c b/pcap-linux.c
index 8d9d302..fd8eefa 100644
--- a/pcap-linux.c
+++ b/pcap-linux.c
@@ -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 0000000..2a47ca2
--- /dev/null
+++ b/pcap/vlan.h
@@ -0,0 +1,11 @@
+#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

-
This is the tcpdump-workers list.
Visit https://cod.sandelman.ca/ to unsubscribe.