Merge 841d8ed82d into 271cabb86a

libdnet-stripped/include/dnet/eth.h

@@ -46,6 +46,7 @@ struct eth_hdr {
 #define ETH_TYPE_MPLS_MCAST	0x8848	/* MPLS Multicast */
 #define ETH_TYPE_PPPOEDISC	0x8863	/* PPP Over Ethernet Discovery Stage */
 #define ETH_TYPE_PPPOE	0x8864		/* PPP Over Ethernet Session Stage */
+#define ETH_TYPE_8021AD	0x88A8		/* IEEE 802.1ad VLAN tagging */
 #define ETH_TYPE_LOOPBACK	0x9000	/* used to test interfaces */
 
 #define ETH_IS_MULTICAST(ea)	(*(ea) & 0x01) /* is address mcast/bcast? */

libnetutil/netutil.cc

@@ -3291,6 +3291,8 @@ int read_reply_pcap(pcap_t *pd, long to_usec,
   int badcounter = 0;
   struct timeval tv_start, tv_end;
   int ioffset;
+  size_t l2len;
+  unsigned ethertype;
 
   if (!pd)
     netutil_fatal("NULL packet device passed to %s", __func__);
@@ -3354,10 +3356,13 @@ int read_reply_pcap(pcap_t *pd, long to_usec,
 
     if (pcap_status == 1 && *p != NULL) {
       /* Offset may be different in the case of 802.1q */
-      if (*datalink == DLT_EN10MB
-          && (*head)->caplen >= sizeof(struct eth_hdr)
-          && 0 == memcmp((*p) + offsetof(struct eth_hdr, eth_type), "\x81\x00", 2)) {
-        *offset += 4;
+      if (*datalink == DLT_EN10MB) {
+        /* Skip over 802.1Q tags; 802.1ad allows more than one */
+        for (l2len = *offset; l2len <= (*head)->caplen; l2len += 4) {
+          ethertype = ntohs(*(uint16_t *)(*p + l2len - 2));
+          if (ethertype != ETH_TYPE_8021Q && ethertype != ETH_TYPE_8021AD) break;
+        }
+        *offset = l2len;
       }
       if (accept_callback(*p, *head, *datalink, *offset)) {
         break;

nselib/packet.lua

@@ -136,22 +136,34 @@ ND_OPT_RTR_ADV_INTERVAL = 7
 ND_OPT_HOME_AGENT_INFO = 8
 
 ETHER_TYPE_IPV4 = 0x0800
+ETHER_TYPE_8021Q = 0x8100
 ETHER_TYPE_IPV6 = 0x86dd
 ETHER_TYPE_PPPOE_DISCOVERY = 0x8863
 ETHER_TYPE_PPPOE_SESSION = 0x8864
 ETHER_TYPE_EAPOL = 0x888e
 ETHER_TYPE_PROFINET = 0x8892
 ETHER_TYPE_ATAOE = 0x88a2
+ETHER_TYPE_8021AD = 0x88a8
 
 ----------------------------------------------------------------------------------------------------------------
 -- Frame is a class
 Frame = {}
 
 function Frame:new(frame, force_continue)
-  local mac_dst, mac_src, ether_type, packet
+  local mac_dst, mac_src, ether_type, vlans, packet
   if frame and #frame >= 14 then
     local pos
     mac_dst, mac_src, ether_type, pos = ("c6c6>I2"):unpack(frame)
+    vlans = {}
+    while pos < #frame + 2 and (ether_type == ETHER_TYPE_8021Q or ether_type == ETHER_TYPE_8021AD) do
+      local vlan = {tpid=ether_type}
+      local tci
+      tci, ether_type, pos = (">I2I2"):unpack(frame, pos)
+      vlan.pcp = 0x0007 & (tci >> 13)
+      vlan.dei = 0x0001 & (tci >> 12)
+      vlan.vid = 0x0FFF & tci
+      table.insert(vlans, vlan)
+    end
     packet = frame:sub(pos, -1)
     if #packet == 0 then packet = nil end
   end
@@ -162,6 +174,7 @@ function Frame:new(frame, force_continue)
   o.mac_dst = mac_dst
   o.mac_src = mac_src
   o.ether_type = ether_type
+  o.vlans = vlans
   return o
 end
 --- Build an Ethernet frame.
@@ -169,16 +182,29 @@ end
 -- @param mac_src six-byte string of the source MAC address.
 -- @param ether_type IEEE 802 ethertype as a 16-bit integer (0x0800 for IPv4)
 -- @param packet string of the payload.
+-- @param vlans list of VLAN tags. Each tag is a table of TPID and
+--              TCI fields PCP, DEI, and VID
 -- @return frame string of the Ether frame.
-function Frame:build_ether_frame(mac_dst, mac_src, ether_type, packet)
+function Frame:build_ether_frame(mac_dst, mac_src, ether_type, packet, vlans)
   self.mac_dst = mac_dst or self.mac_dst
   self.mac_src = mac_src or self.mac_src
   self.ether_type = ether_type or self.ether_type
+  self.vlans = vlans or self.vlans
   self.buf = packet or self.buf
   if not self.ether_type then
     return nil, "Unknown packet type."
   end
-  self.frame_buf = self.mac_dst..self.mac_src..(">I2"):pack(self.ether_type)..self.buf
+  local chunks = {self.mac_dst, self.mac_src}
+  for idx, vlan in ipairs(self.vlans or {}) do
+    local tpid = 0xFFFF & (vlan.tpid or idx < #vlans and ETHER_TYPE_8021AD or ETHER_TYPE_8021Q)
+    local tci = (0x0007 & (vlan.pcp or 0)) << 13 |
+                (0x0001 & (vlan.dei or 0)) << 12 |
+                (0x0FFF & (vlan.vid or 0))
+    table.insert(chunks, (">I2I2"):pack(tpid, tci))
+  end
+  table.insert(chunks, (">I2"):pack(self.ether_type))
+  table.insert(chunks, self.buf)
+  self.frame_buf = table.concat(chunks)
 end
 
 ----------------------------------------------------------------------------------------------------------------

nsock/src/nsock_pcap.c

@@ -466,6 +466,7 @@ void nse_readpcap(nsock_event nsev, const unsigned char **l2_data, size_t *l2_le
   nsock_pcap *n;
   size_t l2l;
   size_t l3l;
+  unsigned ethertype;
 
   n = (nsock_pcap *)fs_str(&(nse->iobuf));
   if (fs_length(&(nse->iobuf)) < sizeof(nsock_pcap)) {
@@ -482,15 +483,18 @@ void nse_readpcap(nsock_event nsev, const unsigned char **l2_data, size_t *l2_le
     return;
   }
 
-  l2l = mp->l3_offset;
-  if (mp->datalink == DLT_EN10MB
-      && n->caplen >= 14 /* size of ethernet header */
-      && 0 == memcmp(n->packet + 12 /* offset of eth_type */, "\x81\x00", 2)) {
-    l2l += 4;
+  l2l = MIN(mp->l3_offset, n->caplen);
+
+  if (mp->datalink == DLT_EN10MB) {
+    /* Skip over 802.1Q tags; 802.1ad allows more than one */
+    while (l2l < n->caplen) {
+      ethertype = ntohs(*(uint16_t *)(n->packet + l2l - 2));
+      if (ethertype != ETHERTYPE_8021Q && ethertype != ETHERTYPE_8021AD) break;
+      l2l = MIN(l2l + 4, n->caplen);
+    }
   }
-  if (l2l > n->caplen)
-    l2l = n->caplen;
-  l3l = MAX(0, n->caplen - l2l);
+
+  l3l = n->caplen - l2l;
 
   if (l2_data)
     *l2_data = n->packet;

nsock/src/nsock_pcap.h

@@ -58,6 +58,7 @@
 #ifdef HAVE_PCAP
 
 #include "pcap.h"
+#include "ethertype.h"
 
 #include <string.h>
 #include <stdarg.h>