netsniff-ng: Consider radiotap header of monitor dev

netsniff-ng does not check if monitor device includes radiotap
header which leads to the wrong 802.11 frame parsing.

Tested if the .pcap file is understandable by wireshark and if
dump info is basically correct, but did not test the case when xmit
packets from .pcap file to the output device and from the input device
to the output device.

Signed-off-by: Vadim Kochan <vadim4j@gmail.com>
[tklauser: whitespace changes]
Signed-off-by: Tobias Klauser <tklauser@distanz.ch>

6 files changed, 36 insertions, 15 deletions

diff --git a/dissector.c b/dissector.c
index ccc9b3c..7c8ba39 100644
--- a/dissector.c
+++ b/dissector.c
@@ -69,6 +69,7 @@ void dissector_entry_point(uint8_t *packet, size_t len, int linktype, int mode)
 		return;
 
 	pkt = pkt_alloc(packet, len);
+	pkt->link_type = linktype;
 
 	switch (linktype) {
 	case LINKTYPE_EN10MB:
@@ -76,6 +77,8 @@ void dissector_entry_point(uint8_t *packet, size_t len, int linktype, int mode)
 		proto_start = dissector_get_ethernet_entry_point();
 		proto_end = dissector_get_ethernet_exit_point();
 		break;
+	case LINKTYPE_IEEE802_11_RADIOTAP:
+	case ___constant_swab32(LINKTYPE_IEEE802_11_RADIOTAP):
 	case LINKTYPE_IEEE802_11:
 	case ___constant_swab32(LINKTYPE_IEEE802_11):
 		proto_start = dissector_get_ieee80211_entry_point();
diff --git a/linktype.h b/linktype.h
index 8caa697..a78f8be 100644
--- a/linktype.h
+++ b/linktype.h
@@ -22,6 +22,7 @@
 #define LINKTYPE_IEEE802_11			105
 #define LINKTYPE_FRELAY				107
 #define LINKTYPE_ECONET				115
+#define LINKTYPE_IEEE802_11_RADIOTAP		127
 #define LINKTYPE_ARCNET_LINUX			129
 #define LINKTYPE_LINUX_IRDA			144
 #define LINKTYPE_CAN20B				190
diff --git a/netsniff-ng.c b/netsniff-ng.c
index 8c7a105..dfb99bb 100644
--- a/netsniff-ng.c
+++ b/netsniff-ng.c
@@ -224,7 +224,8 @@ static void pcap_to_xmit(struct ctx *ctx)
 		xfree(ctx->device_out);
 
 		enter_rfmon_mac80211(ctx->device_trans, &ctx->device_out);
-		if (ctx->link_type != LINKTYPE_IEEE802_11)
+		if (ctx->link_type != LINKTYPE_IEEE802_11 &&
+				ctx->link_type != LINKTYPE_IEEE802_11_RADIOTAP)
 			panic("Wrong linktype of pcap!\n");
 	}
 
@@ -907,14 +908,6 @@ static void recv_only_or_dump(struct ctx *ctx)
 
 	sock = pf_socket();
 
-	if (ctx->rfraw) {
-		ctx->device_trans = xstrdup(ctx->device_in);
-		xfree(ctx->device_in);
-
-		enter_rfmon_mac80211(ctx->device_trans, &ctx->device_in);
-		ctx->link_type = LINKTYPE_IEEE802_11;
-	}
-
 	ifindex = device_ifindex(ctx->device_in);
 
 	size = ring_size(ctx->device_in, ctx->reserve_size);
@@ -1198,7 +1191,6 @@ int main(int argc, char **argv)
 			ctx.prefix = xstrdup(optarg);
 			break;
 		case 'R':
-			ctx.link_type = LINKTYPE_IEEE802_11;
 			ctx.rfraw = 1;
 			break;
 		case 'r':
@@ -1428,8 +1420,15 @@ int main(int argc, char **argv)
 	}
 
 	if (device_mtu(ctx.device_in) || !strncmp("any", ctx.device_in, strlen(ctx.device_in))) {
-		if (!ctx.rfraw)
-			ctx.link_type = pcap_devtype_to_linktype(ctx.device_in);
+		if (ctx.rfraw) {
+			ctx.device_trans = xstrdup(ctx.device_in);
+			xfree(ctx.device_in);
+
+			enter_rfmon_mac80211(ctx.device_trans, &ctx.device_in);
+		}
+
+		ctx.link_type = pcap_devtype_to_linktype(ctx.device_in);
+
 		if (!ctx.device_out) {
 			ctx.dump = 0;
 			main_loop = recv_only_or_dump;
diff --git a/pcap_io.h b/pcap_io.h
index 529cde0..c3fc951 100644
--- a/pcap_io.h
+++ b/pcap_io.h
@@ -161,8 +161,8 @@ static inline int pcap_devtype_to_linktype(const char *ifname)
 	case ARPHRD_IPGRE:
 	case ARPHRD_IP6GRE:
 	case ARPHRD_ETHER:	return LINKTYPE_EN10MB;
+	case ARPHRD_IEEE80211_RADIOTAP: return LINKTYPE_IEEE802_11_RADIOTAP;
 	case ARPHRD_IEEE80211_PRISM:
-	case ARPHRD_IEEE80211_RADIOTAP:
 	case ARPHRD_IEEE80211:	return LINKTYPE_IEEE802_11;
 	case ARPHRD_NETLINK:	return LINKTYPE_NETLINK;
 	case ARPHRD_EETHER:	return LINKTYPE_EN3MB;
@@ -650,6 +650,7 @@ static const bool pcap_supported_linktypes[LINKTYPE_MAX] __maybe_unused = {
 	[LINKTYPE_ATM_CLIP] = true,
 	[LINKTYPE_C_HDLC] = true,
 	[LINKTYPE_IEEE802_11] = true,
+	[LINKTYPE_IEEE802_11_RADIOTAP] = true,
 	[LINKTYPE_FRELAY] = true,
 	[LINKTYPE_ECONET] = true,
 	[LINKTYPE_ARCNET_LINUX] = true,
diff --git a/pkt_buff.h b/pkt_buff.h
index 3376107..1350388 100644
--- a/pkt_buff.h
+++ b/pkt_buff.h
@@ -20,6 +20,7 @@ struct pkt_buff {
 	unsigned int  size;
 
 	struct protocol *proto;
+	int link_type;
 };
 
 static inline struct pkt_buff *pkt_alloc(uint8_t *packet, unsigned int len)
diff --git a/proto_80211_mac_hdr.c b/proto_80211_mac_hdr.c
index 0be749f..9bd6ee1 100644
--- a/proto_80211_mac_hdr.c
+++ b/proto_80211_mac_hdr.c
@@ -22,6 +22,7 @@
 #include "built_in.h"
 #include "pkt_buff.h"
 #include "oui.h"
+#include "linktype.h"
 
 #define	TU		0.001024
 
@@ -772,6 +773,13 @@ struct element_vend_spec {
 	u8 specific[0];
 } __packed;
 
+struct ieee80211_radiotap_header {
+	u8 version;	/* set to 0 */
+	u8 pad;
+	u16 len;	/* entire length */
+	u32 present;	/* fields present */
+} __packed;
+
 static int8_t len_neq_error(u8 len, u8 intended)
 {
 	if(intended != len) {
@@ -3141,8 +3149,16 @@ static void ieee80211(struct pkt_buff *pkt)
 	const char *subtype = NULL;
 	struct ieee80211_frm_ctrl *frm_ctrl;
 
-	frm_ctrl = (struct ieee80211_frm_ctrl *)
-			pkt_pull(pkt, sizeof(*frm_ctrl));
+	if (pkt->link_type == LINKTYPE_IEEE802_11_RADIOTAP) {
+		struct ieee80211_radiotap_header *rtap;
+
+		rtap = (struct ieee80211_radiotap_header *)pkt_pull(pkt,
+				sizeof(*rtap));
+
+		pkt_pull(pkt, le16_to_cpu(rtap->len) - sizeof(*rtap));
+	}
+
+	frm_ctrl = (struct ieee80211_frm_ctrl *)pkt_pull(pkt, sizeof(*frm_ctrl));
 	if (frm_ctrl == NULL)
 		return;