netsniff-ng: vlan: Show vlan info from tpacket v3

Show vlan info (vid, prio & proto) from tpacket struct, in separated
line. It might be useful to sniff it in case if vlan reordering is on
(which is by default) and physical (vlan underlying) device supports
vlan offloading.

Meanwhile it uses only v3 tpacket info as location of vlan fields are
different between v2 & v3 (v1 does not have it at all), but current code
only has possibility to check if v3 is used which is not enough.

Signed-off-by: Vadim Kochan <vadim4j@gmail.com>
[ tk: make print format consistent with VLAN dissector ]
Signed-off-by: Tobias Klauser <tklauser@distanz.ch>

2 files changed, 36 insertions, 0 deletions

diff --git a/dissector.h b/dissector.h
index 5580110..226d2bc 100644
--- a/dissector.h
+++ b/dissector.h
@@ -17,6 +17,7 @@
 #include "ring.h"
 #include "tprintf.h"
 #include "linktype.h"
+#include "proto_vlan.h"
 
 #define PRINT_NORM		0
 #define PRINT_LESS		1
@@ -91,6 +92,17 @@ static inline void __show_frame_hdr(uint8_t *packet, size_t len, int linktype,
 			tpacket_uhdr(hdr, tp_nsec, v3),
 			count,
 			v3 ? "" : __show_ts_source(hdr.h2->tp_status));
+
+		if (tpacket_has_vlan_info(&hdr)) {
+			uint16_t tci = tpacket_uhdr_vlan_tci(&hdr, v3);
+
+			tprintf(" [ tpacketv3 VLAN ");
+			tprintf("Prio (%u), ", vlan_tci2prio(tci));
+			tprintf("CFI (%u), ", vlan_tci2cfi(tci));
+			tprintf("ID (%u), ", vlan_tci2vid(tci));
+			tprintf("Proto (0x%.4x)", tpacket_uhdr_vlan_proto(&hdr, v3));
+			tprintf(" ]\n");
+		}
 		break;
 	}
 }
diff --git a/ring.h b/ring.h
index e9f1b18..c8562b8 100644
--- a/ring.h
+++ b/ring.h
@@ -47,6 +47,30 @@ union tpacket_uhdr {
 	(((hdr).h2)->member)
 #endif /* HAVE_TPACKET3 */
 
+static inline uint16_t tpacket_uhdr_vlan_tci(union tpacket_uhdr *hdr, bool v3)
+{
+#ifdef HAVE_TPACKET3
+	if (v3)
+		return hdr->h3->hv1.tp_vlan_tci;
+#endif
+	return 0;
+}
+
+static inline uint16_t tpacket_uhdr_vlan_proto(union tpacket_uhdr *hdr, bool v3)
+{
+#ifdef HAVE_TPACKET3
+	if (v3)
+		return hdr->h3->hv1.tp_vlan_tpid;
+#endif
+	return 0;
+}
+
+static inline bool tpacket_has_vlan_info(union tpacket_uhdr *hdr)
+{
+	return tpacket_uhdr(*hdr, tp_status, true) &
+		(TP_STATUS_VLAN_VALID | TP_STATUS_VLAN_TPID_VALID);
+}
+
 struct frame_map {
 	struct tpacket2_hdr tp_h __aligned_tpacket;
 	struct sockaddr_ll s_ll __align_tpacket(sizeof(struct tpacket2_hdr));