wlantest: Parse ICMP echo packet and record STA ping success

diff --git a/wlantest/Makefile b/wlantest/Makefile
index f5391d7067391ecb9536a83304fce9b253504994..6d4416a3fd79339e7495d37fefc456502c8205f6 100644 (file)
--- a/wlantest/Makefile
+++ b/wlantest/Makefile
@@ -57,6 +57,7 @@ OBJS += wired.o
 OBJS += rx_mgmt.o
 OBJS += rx_data.o
 OBJS += rx_eapol.o
+OBJS += rx_ip.o
 OBJS += bss.o
 OBJS += sta.o
 OBJS += crc32.o

diff --git a/wlantest/rx_data.c b/wlantest/rx_data.c
index 3c59f1ff5a5d6026b3b74e5873bff3fdbc380cbc..a344054ff4f8186c4736ecec782ad4857b452f9a 100644 (file)
--- a/wlantest/rx_data.c
+++ b/wlantest/rx_data.c
@@ -13,6 +13,7 @@
  */
 
 #include "utils/includes.h"
+#include <linux/if_ether.h>
 
 #include "utils/common.h"
 #include "common/defs.h"
@@ -58,23 +59,32 @@ static const char * data_stype(u16 stype)
 }
 
 
-static void rx_data_eth(struct wlantest *wt, const u8 *dst, const u8 *src,
+static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
+                       const u8 *sta_addr, const u8 *dst, const u8 *src,
                        u16 ethertype, const u8 *data, size_t len, int prot)
 {
-       if (ethertype == ETH_P_PAE)
+       switch (ethertype) {
+       case ETH_P_PAE:
                rx_data_eapol(wt, dst, src, data, len, prot);
+               break;
+       case ETH_P_IP:
+               rx_data_ip(wt, bssid, sta_addr, dst, src, data, len);
+               break;
+       }
 }
 
 
-static void rx_data_process(struct wlantest *wt, const u8 *dst, const u8 *src,
+static void rx_data_process(struct wlantest *wt, const u8 *bssid,
+                           const u8 *sta_addr,
+                           const u8 *dst, const u8 *src,
                            const u8 *data, size_t len, int prot)
 {
        if (len == 0)
                return;
 
        if (len >= 8 && os_memcmp(data, "\xaa\xaa\x03\x00\x00\x00", 6) == 0) {
-               rx_data_eth(wt, dst, src, WPA_GET_BE16(data + 6),
-                           data + 8, len - 8, prot);
+               rx_data_eth(wt, bssid, sta_addr, dst, src,
+                           WPA_GET_BE16(data + 6), data + 8, len - 8, prot);
                return;
        }
 
@@ -156,7 +166,8 @@ static void rx_data_bss_prot_group(struct wlantest *wt,
                decrypted = ccmp_decrypt(bss->gtk[keyid], hdr, data, len,
                                         &dlen);
        if (decrypted) {
-               rx_data_process(wt, dst, src, decrypted, dlen, 1);
+               rx_data_process(wt, bss->bssid, NULL, dst, src, decrypted,
+                               dlen, 1);
                os_memcpy(bss->rsc[keyid], pn, 6);
                write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
                                     decrypted, dlen);
@@ -267,7 +278,8 @@ static void rx_data_bss_prot(struct wlantest *wt,
        else
                decrypted = ccmp_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
        if (decrypted) {
-               rx_data_process(wt, dst, src, decrypted, dlen, 1);
+               rx_data_process(wt, bss->bssid, sta->addr, dst, src, decrypted,
+                               dlen, 1);
                os_memcpy(rsc, pn, 6);
                write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
                                     decrypted, dlen);
@@ -302,8 +314,17 @@ static void rx_data_bss(struct wlantest *wt, const struct ieee80211_hdr *hdr,
 
        if (prot)
                rx_data_bss_prot(wt, hdr, qos, dst, src, data, len);
-       else
-               rx_data_process(wt, dst, src, data, len, 0);
+       else {
+               const u8 *bssid, *sta_addr;
+               if (fc & WLAN_FC_TODS) {
+                       bssid = hdr->addr1;
+                       sta_addr = hdr->addr2;
+               } else {
+                       bssid = hdr->addr2;
+                       sta_addr = hdr->addr1;
+               }
+               rx_data_process(wt, bssid, sta_addr, dst, src, data, len, 0);
+       }
 }
 
 

diff --git a/wlantest/rx_ip.c b/wlantest/rx_ip.c
new file mode 100644 (file)
index 0000000..29874e6
--- /dev/null
+++ b/wlantest/rx_ip.c
@@ -0,0 +1,148 @@
+/*
+ * Received Data frame processing for IPv4 packets
+ * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Alternatively, this software may be distributed under the terms of BSD
+ * license.
+ *
+ * See README and COPYING for more details.
+ */
+
+#include "utils/includes.h"
+#include <netinet/ip.h>
+#include <netinet/ip_icmp.h>
+
+#include "utils/common.h"
+#include "wlantest.h"
+
+
+static void rx_data_icmp(struct wlantest *wt, const u8 *bssid,
+                        const u8 *sta_addr, u32 dst, u32 src,
+                        const u8 *data, size_t len)
+{
+       struct in_addr addr;
+       char buf[20];
+       const struct icmphdr *hdr;
+       u16 id, seq;
+       struct wlantest_bss *bss;
+       struct wlantest_sta *sta;
+
+       hdr = (const struct icmphdr *) data;
+       if (len < 4)
+               return;
+
+       /* TODO: check hdr->checksum */
+
+       if (hdr->type != ICMP_ECHOREPLY && hdr->type != ICMP_ECHO)
+               return;
+       if (len < 8)
+               return;
+
+       id = ntohs(hdr->un.echo.id);
+       seq = ntohs(hdr->un.echo.sequence);
+
+       addr.s_addr = dst;
+       snprintf(buf, sizeof(buf), "%s", inet_ntoa(addr));
+       addr.s_addr = src;
+       wpa_printf(MSG_DEBUG, "ICMP echo %s %s -> %s id=%04x seq=%u len=%u",
+                  hdr->type == ICMP_ECHO ? "request" : "response",
+                  inet_ntoa(addr), buf, id, seq, (unsigned) len - 8);
+
+       bss = bss_find(wt, bssid);
+       if (bss == NULL) {
+               wpa_printf(MSG_INFO, "No BSS " MACSTR " known for ICMP packet",
+                          MAC2STR(bssid));
+               return;
+       }
+
+       if (sta_addr == NULL)
+               return; /* FromDS broadcast ping */
+
+       sta = sta_find(bss, sta_addr);
+       if (sta == NULL) {
+               wpa_printf(MSG_INFO, "No STA " MACSTR " known for ICMP packet",
+                          MAC2STR(sta_addr));
+               return;
+       }
+
+       if (hdr->type == ICMP_ECHO) {
+               sta->icmp_echo_req_src = src;
+               sta->icmp_echo_req_dst = dst;
+               sta->icmp_echo_req_id = id;
+               sta->icmp_echo_req_seq = seq;
+               return;
+       }
+
+       if (sta->icmp_echo_req_src == dst &&
+           sta->icmp_echo_req_dst == src &&
+           sta->icmp_echo_req_id == id &&
+           sta->icmp_echo_req_seq == seq) {
+               sta->counters[WLANTEST_STA_COUNTER_PING_OK]++;
+               wpa_printf(MSG_DEBUG, "ICMP echo (ping) match for STA " MACSTR,
+                          MAC2STR(sta->addr));
+       }
+}
+
+
+void rx_data_ip(struct wlantest *wt, const u8 *bssid, const u8 *sta_addr,
+               const u8 *dst, const u8 *src, const u8 *data, size_t len)
+{
+       const struct iphdr *ip;
+       const u8 *payload;
+       size_t plen;
+       u16 frag_off, tot_len;
+
+       ip = (const struct iphdr *) data;
+       if (len < sizeof(*ip))
+               return;
+       if (ip->version != 4) {
+               wpa_printf(MSG_DEBUG, "Unexpected IP protocol version %u in "
+                          "IPv4 packet (bssid=" MACSTR " str=" MACSTR
+                          " dst=" MACSTR ")", ip->version, MAC2STR(bssid),
+                          MAC2STR(src), MAC2STR(dst));
+               return;
+       }
+       if (ip->ihl * 4 < sizeof(*ip)) {
+               wpa_printf(MSG_DEBUG, "Unexpected IP header length %u in "
+                          "IPv4 packet (bssid=" MACSTR " str=" MACSTR
+                          " dst=" MACSTR ")", ip->ihl, MAC2STR(bssid),
+                          MAC2STR(src), MAC2STR(dst));
+               return;
+       }
+       if (ip->ihl * 4 > len) {
+               wpa_printf(MSG_DEBUG, "Truncated IP header (ihl=%u len=%u) in "
+                          "IPv4 packet (bssid=" MACSTR " str=" MACSTR
+                          " dst=" MACSTR ")", ip->ihl, (unsigned) len,
+                          MAC2STR(bssid), MAC2STR(src), MAC2STR(dst));
+               return;
+       }
+
+       /* TODO: check header checksum in ip->check */
+
+       frag_off = be_to_host16(ip->frag_off);
+       if (frag_off & 0x1fff) {
+               wpa_printf(MSG_EXCESSIVE, "IP fragment reassembly not yet "
+                          "supported");
+               return;
+       }
+
+       tot_len = be_to_host16(ip->tot_len);
+       if (tot_len > len)
+               return;
+       if (tot_len < len)
+               len = tot_len;
+
+       payload = data + 4 * ip->ihl;
+       plen = len - 4 * ip->ihl;
+
+       switch (ip->protocol) {
+       case IPPROTO_ICMP:
+               rx_data_icmp(wt, bssid, sta_addr, ip->daddr, ip->saddr,
+                            payload, plen);
+               break;
+       }
+}

diff --git a/wlantest/wlantest.h b/wlantest/wlantest.h
index 6d422a9d49f20b8c999a9eab3a618691f08426ea..444855836f048857e9e1bf7f62a5e2e3aa7e3be3 100644 (file)
--- a/wlantest/wlantest.h
+++ b/wlantest/wlantest.h
@@ -73,6 +73,12 @@ struct wlantest_sta {
        u16 assocreq_listen_int;
        u8 *assocreq_ies;
        size_t assocreq_ies_len;
+
+       /* Last ICMP Echo request information */
+       u32 icmp_echo_req_src;
+       u32 icmp_echo_req_dst;
+       u16 icmp_echo_req_id;
+       u16 icmp_echo_req_seq;
 };
 
 struct wlantest_bss {
@@ -154,6 +160,8 @@ void rx_mgmt(struct wlantest *wt, const u8 *data, size_t len);
 void rx_data(struct wlantest *wt, const u8 *data, size_t len);
 void rx_data_eapol(struct wlantest *wt, const u8 *dst, const u8 *src,
                   const u8 *data, size_t len, int prot);
+void rx_data_ip(struct wlantest *wt, const u8 *bssid, const u8 *sta_addr,
+               const u8 *dst, const u8 *src, const u8 *data, size_t len);
 
 struct wlantest_bss * bss_find(struct wlantest *wt, const u8 *bssid);
 struct wlantest_bss * bss_get(struct wlantest *wt, const u8 *bssid);

diff --git a/wlantest/wlantest_cli.c b/wlantest/wlantest_cli.c
index 8191bdaa4cf60b58e2da96e6819e41973a44fa42..df96bd3516645be00954484296d856e0617f64ab 100644 (file)
--- a/wlantest/wlantest_cli.c
+++ b/wlantest/wlantest_cli.c
@@ -461,6 +461,7 @@ static const struct sta_counters sta_counters[] = {
          WLANTEST_STA_COUNTER_INVALID_SAQUERYRESP_TX },
        { "invalid_saqueryresp_rx",
          WLANTEST_STA_COUNTER_INVALID_SAQUERYRESP_RX },
+       { "ping_ok", WLANTEST_STA_COUNTER_PING_OK },
        { NULL, 0 }
 };
 

diff --git a/wlantest/wlantest_ctrl.h b/wlantest/wlantest_ctrl.h
index 73451c439c7abc2dd37bcd5cf3d089161df5cbee..a84cf9cc1a3cca9bfef07a1afbd1aa17c8adb211 100644 (file)
--- a/wlantest/wlantest_ctrl.h
+++ b/wlantest/wlantest_ctrl.h
@@ -80,6 +80,7 @@ enum wlantest_sta_counter {
        WLANTEST_STA_COUNTER_VALID_SAQUERYRESP_RX,
        WLANTEST_STA_COUNTER_INVALID_SAQUERYRESP_TX,
        WLANTEST_STA_COUNTER_INVALID_SAQUERYRESP_RX,
+       WLANTEST_STA_COUNTER_PING_OK,
        NUM_WLANTEST_STA_COUNTER
 };