From 75849f8fe245b245573f874eb370449086c71923 Mon Sep 17 00:00:00 2001
From: Jan Grashoefer <jan.grashoefer@gmail.com>
Date: Tue, 14 Jun 2016 17:52:34 +0200
Subject: [PATCH] Improved handling of 802.11 headers.

Frame types except data and frames subtypes without payload are skipped.
Header length is determined based on presence of QoS and flags
indicating the use of the 4th address field. Handling of aggregated
MSDUs is explicitly prevented.
---
 src/iosource/Packet.cc | 84 +++++++++++++++++++++---------------------
 1 file changed, 43 insertions(+), 41 deletions(-)

diff --git a/src/iosource/Packet.cc b/src/iosource/Packet.cc
index b5a16fa69d..88c1aeee79 100644
--- a/src/iosource/Packet.cc
+++ b/src/iosource/Packet.cc
@@ -276,21 +276,33 @@ void Packet::ProcessLayer2()
 			}
 		pdata += rtheader_len;
 
-		u_char len_80211 = 0;
-		int type_80211 = pdata[0];
+		u_char len_80211 = 0; // Frame header length
+		u_char fc_80211 = pdata[0]; // Frame Control field
 
-		if ( (type_80211 >> 4) & 0x04 )
-			{
-			//identified a null frame (we ignore for now).  no weird.
+		// Skip non data frame types (management & control).
+		if ( !((fc_80211 >> 2) & 0x02) )
 			return;
-			}
+
+		len_80211 = 24; // minimal length of data frames
+
+		// Skip subtypes without data.
+		if ( (fc_80211 >> 4) & 0x04 )
+			return;
+
+		// 'To DS' and 'From DS' flags set indicate
+		// use of the 4th address field
+		if ( (pdata[1] & 0x03) == 0x03 )
+			len_80211 += l2_addr_len;
 
 		// Look for the QoS indicator bit.
-
-		if ( (type_80211 >> 4) & 0x08 )
-			len_80211 = 26;
-		else
-			len_80211 = 24;
+		if ( (fc_80211 >> 4) & 0x08 )
+			{
+			// Skip in case of A-MSDU subframes
+			// indicated by QoS control field
+			if ( pdata[len_80211] & 0x80)
+				return;
+			len_80211 += 2;
+			}
 
 		if ( pdata + len_80211 >= end_of_data )
 			{
@@ -298,39 +310,29 @@ void Packet::ProcessLayer2()
 			return;
 			}
 
-		// Look for data frames
-		if ( type_80211 & 0x08 )
-			{
-			// Determine link-layer addresses based
-			// on 'To DS' and 'From DS' flags
-			switch ( pdata[1] & 0x03 ) {
-				case 0x00:
-					l2_dst = pdata + 4;
-					l2_src = pdata + 10;
-					break;
+		// Determine link-layer addresses based
+		// on 'To DS' and 'From DS' flags
+		switch ( pdata[1] & 0x03 ) {
+			case 0x00:
+				l2_src = pdata + 10;
+				l2_dst = pdata + 4;
+				break;
 
-				case 0x01:
-					l2_src = pdata + 10;
-					l2_dst = pdata + 16;
-					break;
+			case 0x01:
+				l2_src = pdata + 10;
+				l2_dst = pdata + 16;
+				break;
 
-				case 0x02:
-					l2_src = pdata + 16;
-					l2_dst = pdata + 4;
-					break;
+			case 0x02:
+				l2_src = pdata + 16;
+				l2_dst = pdata + 4;
+				break;
 
-				case 0x03:
-					// TODO: We should integrate this
-					// test into the length check above.
-					if ( pdata + 24 + l2_addr_len >= end_of_data )
-						{
-						l2_dst = pdata + 16;
-						l2_src = pdata + 24;
-						}
-
-					break;
-			}
-			}
+			case 0x03:
+				l2_src = pdata + 24;
+				l2_dst = pdata + 16;
+				break;
+		}
 
 		// skip 802.11 data header
 		pdata += len_80211;