[Listener] Fix reading of QoS Data frames

The IEEE 802.11 header size is 2 bytes bigger when a Data frame has QoS
information.

infrastructure-centred/owlps-listener/TODO

@@ -1,4 +1,9 @@
+* read_packet(): use ieee80211_header_size for all implicit packets
+   (currently the size is corrected only for data packets).
 * Fix segfault when rtap_iface is not in monitor mode (?) on Foneras.
 * Fusionner Makefile et Makefile_atheros.
-* Éventuellement remplacer les options positives (USE_PTHREAD) par des options négatives (NO_USE_PTHREAD), en fonction des valeurs par défaut.
-* Permettre d'utiliser un nom d'hôte putôt qu'une IP pour le serveur d'agrégation.
+* Éventuellement remplacer les options positives (USE_PTHREAD) par des
+   options négatives (NO_USE_PTHREAD), en fonction des valeurs par
+   défaut.
+* Permettre d'utiliser un nom d'hôte putôt qu'une IP pour le serveur
+   d'agrégation.

infrastructure-centred/owlps-listener/owlps-listenerd.c

@@ -484,6 +484,8 @@ void read_packet(u_char *args, const struct pcap_pkthdr *header,
   unsigned char raw_packet_fc1 ; // First byte of the received frame's FC
   unsigned char raw_packet_fc2 ; // Second byte of the received frame's FC
   unsigned char raw_packet_flags ; // IEEE 802.11 header flags
+  // Size of the IEEE 802.11 header:
+  int ieee80211_header_size = IEEE80211_HEADER_SIZE_DATA ;
   unsigned short llc_packet_type = 0 ;
   // Pointer to the (possible) IP header of the packet:
   struct iphdr *packet_ip_header = NULL ;
@@ -509,28 +511,33 @@ void read_packet(u_char *args, const struct pcap_pkthdr *header,
   // the type of the packet (Management, Control or Data) and its subtype
   // (QoS, etc.):
   raw_packet_fc1 = data[rtap_bytes] ;
-  if (! IS_DATA_FRAME(raw_packet_fc1)) // Data packet?
+
+  if (! IS_DATA_FRAME(raw_packet_fc1)) // Data frame?
     goto not_explicit_packet ;
 
+  if (DATA_FRAME_IS_QOS(raw_packet_fc1)) // QoS Data frame?
+    ieee80211_header_size += 2 ; // 2 bytes of QoS information
+
   // The second byte of the FC field contains the frame flags. The two
   // first bits indicate the frame source and destination types: the
   // first bit is "To DS" and the second is "From DS", so if the second
   // bit is 0 the frame comes from a STA. That's what we want for an
   // explicit packet:
   raw_packet_fc2 = data[rtap_bytes+1] ;
+
   if (! IS_FRAME_FROM_STA(raw_packet_fc2))
     goto not_explicit_packet ;
 
   // Get the packet type (protocol, 2 bytes) from the LLC header:
   memcpy((unsigned char*) &llc_packet_type,
-         &data[rtap_bytes + IEEE80211_HEADER_SIZE + 6], 2) ;
+         &data[rtap_bytes + ieee80211_header_size + 6], 2) ;
   llc_packet_type = ntohs(llc_packet_type) ;
 
   if (llc_packet_type != ETH_P_IP) // IP packet?
     goto not_explicit_packet ;
 
   packet_ip_header = (struct iphdr *)
-    &data[rtap_bytes + IEEE80211_HEADER_SIZE + LLC_HEADER_SIZE] ;
+    &data[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE] ;
   // Get the source IP:
   memcpy(couple.mobile_ip_addr_bytes, &packet_ip_header->saddr, 4) ;
 
@@ -542,7 +549,7 @@ void read_packet(u_char *args, const struct pcap_pkthdr *header,
 
       // Check destination port:
       packet_udp_header = (struct udphdr *)
-        &data[rtap_bytes + IEEE80211_HEADER_SIZE +
+        &data[rtap_bytes + ieee80211_header_size +
               LLC_HEADER_SIZE + sizeof(struct iphdr)] ;
       if (ntohs(packet_udp_header->dest) != GET_LISTENING_PORT())
         goto not_explicit_packet ;
@@ -587,7 +594,7 @@ void read_packet(u_char *args, const struct pcap_pkthdr *header,
       && ! IS_RETRY(raw_packet_flags))
     {
       packet_type =
-        data[rtap_bytes + IEEE80211_HEADER_SIZE + LLC_HEADER_SIZE
+        data[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
              + sizeof(struct iphdr) + sizeof(struct udphdr)] ;
       switch(packet_type)
 	{
@@ -600,18 +607,18 @@ void read_packet(u_char *args, const struct pcap_pkthdr *header,
 	  if (GET_VERBOSE())
 	    printf("\nExplicit calibration packet received.\n") ;
 	  couple.direction =
-            data[rtap_bytes + IEEE80211_HEADER_SIZE + LLC_HEADER_SIZE
+            data[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
                  + sizeof(struct iphdr) + sizeof(struct udphdr) + 9];
 	  memcpy(&couple.x_position,
-                 &data[rtap_bytes + IEEE80211_HEADER_SIZE
+                 &data[rtap_bytes + ieee80211_header_size
                        + LLC_HEADER_SIZE + sizeof(struct iphdr)
                        + sizeof(struct udphdr) + 10], sizeof(float)) ;
 	  memcpy(&couple.y_position,
-                 &data[rtap_bytes + IEEE80211_HEADER_SIZE
+                 &data[rtap_bytes + ieee80211_header_size
                        + LLC_HEADER_SIZE + sizeof(struct iphdr)
                        + sizeof(struct udphdr) + 14], sizeof(float)) ;
 	  memcpy(&couple.z_position,
-                 &data[rtap_bytes + IEEE80211_HEADER_SIZE
+                 &data[rtap_bytes + ieee80211_header_size
                        + LLC_HEADER_SIZE + sizeof(struct iphdr)
                        + sizeof(struct udphdr) + 18], sizeof(float)) ;
 	  break ;
@@ -639,7 +646,7 @@ void read_packet(u_char *args, const struct pcap_pkthdr *header,
         }
       else
         memcpy(&couple.request_time,
-               &data[rtap_bytes + IEEE80211_HEADER_SIZE + LLC_HEADER_SIZE
+               &data[rtap_bytes + ieee80211_header_size + LLC_HEADER_SIZE
                      + sizeof(struct iphdr) + sizeof(struct udphdr) + 1],
                sizeof(struct timeval)) ;
     }

libowlps/owlps.h

@@ -122,15 +122,24 @@ typedef struct _autocalibration_order
 
 
 /* Taille des en-têtes des paquets (en octets) */
-#define IEEE80211_HEADER_SIZE 24
+#define IEEE80211_HEADER_SIZE_DATA 24 // Header size for a Data frame
 #define LLC_HEADER_SIZE 8
 
 /* Types des paquets capturés (en-tête IEEE 802.11) */
+// Beacon (TODO: convert to mask)
 #define RAW_PACKET_TYPE_BEACON 0x80
+
+// Data frame
 #define FRAME_TYPE_DATA_MASK 0x08
-#define IS_DATA_FRAME(FCS1) (((FCS1) & FRAME_TYPE_DATA_MASK) == FRAME_TYPE_DATA_MASK)
+#define IS_DATA_FRAME(FC1) (((FC1) & FRAME_TYPE_DATA_MASK) == FRAME_TYPE_DATA_MASK)
+
+// QoS Data frame
+#define FRAME_SUBTYPE_QOS_MASK 0x80
+#define DATA_FRAME_IS_QOS(FC1) (((FC1) & FRAME_SUBTYPE_QOS_MASK) == FRAME_SUBTYPE_QOS_MASK)
+
+// To/From DS
 #define FRAME_FROM_STA_MASK 0x02
-#define IS_FRAME_FROM_STA(FCS2) (((FCS2) & FRAME_FROM_STA_MASK) != FRAME_FROM_STA_MASK)
+#define IS_FRAME_FROM_STA(FC2) (((FC2) & FRAME_FROM_STA_MASK) != FRAME_FROM_STA_MASK)
 
 
 /* Position des champs fixes de l'en-tête radiotap (octets) */