netsniff-ng: Add dissector for Linux "cooked" packets

Added dissector_sll.c which uses sockaddr_ll to lookup & print
higher L3 layer protocol.

This dissector is mapped by LINKTYPE_LINUX_SLL link type.

Sample output of dissected Netlink & Ethernet packets.
Truncated manually some longer lines by "...":

> nlmon0 20 1434193547s.717131169ns #6
 [ Linux "cooked" Pkt Type 4 (outgoing), If Type 824 (netlink), Addr Len 0, Src (), Proto 0x0 ]
 [ NLMSG Family 0 (routing), Len 20, Type 0x0003 (DONE)...

> wlp3s0 52 1434194181s.436224709ns #9
 [ Linux "cooked" Pkt Type 4 (outgoing), If Type 1 (ether), Addr Len 6, Src (XX:XX:XX:XX:XX:XX), Proto 0x800 ]
 [ IPv4 Addr (XXX.XXX.XXX.XXX => 212.42.76.253), Proto (6), TTL (64), TOS (0), ...
   ), CSum (0x1ef5) is ok ]
	[ Geo (local => Ukraine) ]
 [ TCP Port (45849 => 443 (https)), SN (0x1744209), AN (0x46ca9611), DataOff (8) ...
 [ Chr .....w.Rj).. ]
 [ Hex  XX XX XX XX XX XX XX XX XX XX XX XX ]

Signed-off-by: Vadim Kochan <vadim4j@gmail.com>
Signed-off-by: Daniel Borkmann <daniel@iogearbox.net>

10 files changed, 153 insertions, 13 deletions

diff --git a/dev.c b/dev.c
index a29b4c1..c28fa0e 100644
--- a/dev.c
+++ b/dev.c
@@ -376,7 +376,7 @@ const char *device_type2str(uint16_t type)
 }
 
 /* Taken from iproute2 ll_addr_n2a func */
-const char *device_addr2str(const char *addr, int alen, int type,
+const char *device_addr2str(const unsigned char *addr, int alen, int type,
 			    char *buf, int blen)
 {
 	int i, l;
diff --git a/dev.h b/dev.h
index 6aa770d..3c12d8c 100644
--- a/dev.h
+++ b/dev.h
@@ -16,6 +16,6 @@ extern u32 device_bitrate(const char *ifname);
 extern short device_enter_promiscuous_mode(const char *ifname);
 extern void device_leave_promiscuous_mode(const char *ifname, short oldflags);
 extern const char *device_type2str(uint16_t type);
-extern const char *device_addr2str(const char *addr, int alen, int type,
+extern const char *device_addr2str(const unsigned char *addr, int alen, int type,
 				   char *buf, int blen);
 #endif /* DEV_H */
diff --git a/dissector.c b/dissector.c
index 4cad588..6aa253d 100644
--- a/dissector.c
+++ b/dissector.c
@@ -14,6 +14,7 @@
 #include "proto.h"
 #include "dissector.h"
 #include "dissector_eth.h"
+#include "dissector_sll.h"
 #include "dissector_80211.h"
 #include "dissector_netlink.h"
 #include "linktype.h"
@@ -61,7 +62,7 @@ static void dissector_main(struct pkt_buff *pkt, struct protocol *start,
 }
 
 void dissector_entry_point(uint8_t *packet, size_t len, int linktype, int mode,
-			   uint16_t proto)
+			   struct sockaddr_ll *sll)
 {
 	struct protocol *proto_start, *proto_end;
 	struct pkt_buff *pkt;
@@ -71,7 +72,7 @@ void dissector_entry_point(uint8_t *packet, size_t len, int linktype, int mode,
 
 	pkt = pkt_alloc(packet, len);
 	pkt->link_type = linktype;
-	pkt->proto = proto;
+	pkt->sll = sll;
 
 	switch (linktype) {
 	case LINKTYPE_EN10MB:
@@ -91,6 +92,11 @@ void dissector_entry_point(uint8_t *packet, size_t len, int linktype, int mode,
 		proto_start = dissector_get_netlink_entry_point();
 		proto_end = dissector_get_netlink_exit_point();
 		break;
+	case LINKTYPE_LINUX_SLL:
+	case ___constant_swab32(LINKTYPE_LINUX_SLL):
+		proto_start = dissector_get_sll_entry_point();
+		proto_end = dissector_get_sll_exit_point();
+		break;
 	default:
 		proto_start = &none_ops;
 		proto_end = NULL;
@@ -120,6 +126,7 @@ void dissector_init_all(int fnttype)
 	dissector_init_ethernet(fnttype);
 	dissector_init_ieee80211(fnttype);
 	dissector_init_netlink(fnttype);
+	dissector_init_sll(fnttype);
 }
 
 void dissector_cleanup_all(void)
@@ -127,4 +134,5 @@ void dissector_cleanup_all(void)
 	dissector_cleanup_ethernet();
 	dissector_cleanup_ieee80211();
 	dissector_cleanup_netlink();
+	dissector_cleanup_sll();
 }
diff --git a/dissector.h b/dissector.h
index a99442e..5580110 100644
--- a/dissector.h
+++ b/dissector.h
@@ -105,7 +105,7 @@ static inline void show_frame_hdr(uint8_t *packet, size_t len, int linktype,
 
 extern void dissector_init_all(int fnttype);
 extern void dissector_entry_point(uint8_t *packet, size_t len, int linktype,
-				  int mode, uint16_t proto);
+				  int mode, struct sockaddr_ll *sll);
 extern void dissector_cleanup_all(void);
 extern int dissector_set_print_type(void *ptr, int type);
 
diff --git a/dissector_sll.c b/dissector_sll.c
new file mode 100644
index 0000000..e2e5bfa
--- /dev/null
+++ b/dissector_sll.c
@@ -0,0 +1,113 @@
+/*
+ * netsniff-ng - the packet sniffing beast
+ * Subject to the GPL, version 2.
+ */
+
+#include "oui.h"
+#include "protos.h"
+#include "pcap_io.h"
+#include "pkt_buff.h"
+#include "dissector.h"
+#include "dissector_sll.h"
+#include "dissector_eth.h"
+
+static char *pkt_type2str(uint8_t pkttype)
+{
+	switch (pkttype) {
+	case PACKET_HOST:
+		return "host";
+	case PACKET_BROADCAST:
+		return "broadcast";
+	case PACKET_MULTICAST:
+		return "multicast";
+	case PACKET_OTHERHOST:
+		return "other host";
+	case PACKET_OUTGOING:
+		return "outgoing";
+	case PACKET_USER:
+		return "user";
+	case PACKET_KERNEL:
+		return "kernel";
+	}
+
+	return "Unknown";
+}
+
+static void sll_print_full(struct pkt_buff *pkt)
+{
+	struct sockaddr_ll *sll = pkt->sll;
+	char addr_str[40] = {};
+
+	if (!pkt || !sll)
+		return;
+
+	tprintf(" [ Linux \"cooked\"");
+	tprintf(" Pkt Type %d (%s)", sll->sll_pkttype,
+			pkt_type2str(sll->sll_pkttype));
+	tprintf(", If Type %d (%s)", sll->sll_hatype,
+			device_type2str(sll->sll_hatype));
+	tprintf(", Addr Len %d", sll->sll_halen);
+	tprintf(", Src (%s)", device_addr2str(sll->sll_addr, sll->sll_halen,
+			sll->sll_hatype, addr_str, sizeof(addr_str)));
+	tprintf(", Proto 0x%x", ntohs(sll->sll_protocol));
+	tprintf(" ]\n");
+
+	switch (pcap_devtype_to_linktype(sll->sll_hatype)) {
+	case LINKTYPE_EN10MB:
+	case ___constant_swab32(LINKTYPE_EN10MB):
+		pkt_set_dissector(pkt, &eth_lay2, ntohs(sll->sll_protocol));
+		break;
+	case LINKTYPE_NETLINK:
+	case ___constant_swab32(LINKTYPE_NETLINK):
+		pkt->dissector = &nlmsg_ops;
+		break;
+	default:
+		tprintf(" [ Uknown protocol ]\n");
+	}
+}
+
+static void sll_print_less(struct pkt_buff *pkt)
+{
+	struct sockaddr_ll *sll = pkt->sll;
+	char addr_str[40] = {};
+
+	if (!pkt || !sll)
+		return;
+
+	tprintf(" Pkt Type %d (%s)", sll->sll_pkttype,
+			pkt_type2str(sll->sll_pkttype));
+	tprintf(", If Type %d (%s)", sll->sll_hatype,
+			device_type2str(sll->sll_hatype));
+	tprintf(", Addr Len %d", sll->sll_halen);
+	tprintf(", Src (%s)", device_addr2str(sll->sll_addr, sll->sll_halen,
+			sll->sll_hatype, addr_str, sizeof(addr_str)));
+	tprintf(", Proto 0x%x", ntohs(sll->sll_protocol));
+}
+
+struct protocol sll_ops = {
+	.key = 0,
+	.print_full = sll_print_full,
+	.print_less = sll_print_less,
+};
+
+struct protocol *dissector_get_sll_entry_point(void)
+{
+	return &sll_ops;
+}
+
+struct protocol *dissector_get_sll_exit_point(void)
+{
+	return &none_ops;
+}
+
+void dissector_init_sll(int fnttype)
+{
+	dissector_set_print_type(&sll_ops, fnttype);
+	dissector_set_print_type(&none_ops, fnttype);
+	dissector_init_oui();
+}
+
+void dissector_cleanup_sll(void)
+{
+	dissector_cleanup_oui();
+}
diff --git a/dissector_sll.h b/dissector_sll.h
new file mode 100644
index 0000000..2067942
--- /dev/null
+++ b/dissector_sll.h
@@ -0,0 +1,18 @@
+/*
+ * netsniff-ng - the packet sniffing beast
+ * Subject to the GPL, version 2.
+ */
+
+#ifndef DISSECTOR_SLL_H
+#define DISSECTOR_SLL_H
+
+#include "hash.h"
+#include "proto.h"
+
+extern void dissector_init_sll(int fnttype);
+extern void dissector_cleanup_sll(void);
+
+extern struct protocol *dissector_get_sll_entry_point(void);
+extern struct protocol *dissector_get_sll_exit_point(void);
+
+#endif /* DISSECTOR_SLL_H */
diff --git a/netsniff-ng.c b/netsniff-ng.c
index e593b9d..91356ba 100644
--- a/netsniff-ng.c
+++ b/netsniff-ng.c
@@ -318,7 +318,7 @@ static void pcap_to_xmit(struct ctx *ctx)
 
 			dissector_entry_point(out, hdr->tp_h.tp_snaplen,
 					      ctx->link_type, ctx->print_mode,
-					      hdr->s_ll.sll_protocol);
+					      &hdr->s_ll);
 
 			kernel_may_pull_from_tx(&hdr->tp_h);
 
@@ -469,7 +469,7 @@ static void receive_to_xmit(struct ctx *ctx)
 
 			dissector_entry_point(in, hdr_in->tp_h.tp_snaplen,
 					      ctx->link_type, ctx->print_mode,
-					      hdr_in->s_ll.sll_protocol);
+					      &hdr_in->s_ll);
 
 			if (frame_count_max != 0) {
 				if (frame_count >= frame_count_max) {
@@ -662,7 +662,7 @@ static void read_pcap(struct ctx *ctx)
 
 		dissector_entry_point(out, fm.tp_h.tp_snaplen,
 				      ctx->link_type, ctx->print_mode,
-				      fm.s_ll.sll_protocol);
+				      &fm.s_ll);
 
 		if (is_out_pcap) {
 			size_t pcap_len = pcap_get_length(&phdr, ctx->magic);
@@ -929,7 +929,7 @@ static void walk_t3_block(struct block_desc *pbd, struct ctx *ctx,
 				 hdr, ctx->print_mode, true, *frame_count);
 
 		dissector_entry_point(packet, hdr->tp_snaplen, ctx->link_type,
-				      ctx->print_mode, sll->sll_protocol);
+				      ctx->print_mode, sll);
 next:
                 hdr = (void *) ((uint8_t *) hdr + hdr->tp_next_offset);
 		sll = (void *) ((uint8_t *) hdr + TPACKET_ALIGN(sizeof(*hdr)));
@@ -1064,7 +1064,7 @@ static void recv_only_or_dump(struct ctx *ctx)
 
 			dissector_entry_point(packet, hdr->tp_h.tp_snaplen,
 					      ctx->link_type, ctx->print_mode,
-					      hdr->s_ll.sll_protocol);
+					      &hdr->s_ll);
 
 			if (frame_count_max != 0) {
 				if (unlikely(frame_count >= frame_count_max)) {
diff --git a/netsniff-ng/Makefile b/netsniff-ng/Makefile
index 92990ff..33701a4 100644
--- a/netsniff-ng/Makefile
+++ b/netsniff-ng/Makefile
@@ -12,6 +12,7 @@ netsniff-ng-libs +=	-lGeoIP \
 endif
 
 netsniff-ng-objs =	dissector.o \
+			dissector_sll.o \
 			dissector_eth.o \
 			dissector_80211.o \
 			dissector_netlink.o \
diff --git a/pkt_buff.h b/pkt_buff.h
index 28872ef..30b999e 100644
--- a/pkt_buff.h
+++ b/pkt_buff.h
@@ -21,7 +21,7 @@ struct pkt_buff {
 
 	struct protocol *dissector;
 	uint32_t link_type;
-	uint16_t proto;
+	struct sockaddr_ll *sll;
 };
 
 static inline struct pkt_buff *pkt_alloc(uint8_t *packet, unsigned int len)
diff --git a/proto_nlmsg.c b/proto_nlmsg.c
index 5018adf..44ef328 100644
--- a/proto_nlmsg.c
+++ b/proto_nlmsg.c
@@ -740,7 +740,7 @@ static void nlmsg(struct pkt_buff *pkt)
 	struct nlmsghdr *hdr = (struct nlmsghdr *) pkt_pull(pkt, sizeof(*hdr));
 
 	while (hdr) {
-		nlmsg_print(ntohs(pkt->proto), hdr);
+		nlmsg_print(ntohs(pkt->sll->sll_protocol), hdr);
 
 		if (!pkt_pull(pkt, NLMSG_PAYLOAD(hdr, 0)))
 			break;
@@ -755,7 +755,7 @@ static void nlmsg(struct pkt_buff *pkt)
 static void nlmsg_less(struct pkt_buff *pkt)
 {
 	struct nlmsghdr *hdr = (struct nlmsghdr *) pkt_pull(pkt, sizeof(*hdr));
-	uint16_t family = ntohs(pkt->proto);
+	uint16_t family = ntohs(pkt->sll->sll_protocol);
 	char type[32];
 
 	if (hdr == NULL)