rx_ip.c 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163
  1. /*
  2. * Received Data frame processing for IPv4 packets
  3. * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
  4. *
  5. * This program is free software; you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License version 2 as
  7. * published by the Free Software Foundation.
  8. *
  9. * Alternatively, this software may be distributed under the terms of BSD
  10. * license.
  11. *
  12. * See README and COPYING for more details.
  13. */
  14. #include "utils/includes.h"
  15. #include <netinet/ip.h>
  16. #include <netinet/ip_icmp.h>
  17. #include "utils/common.h"
  18. #include "wlantest.h"
  19. static void ping_update(struct wlantest_sta *sta, int req, u32 src, u32 dst,
  20. u16 id, u16 seq)
  21. {
  22. if (req) {
  23. sta->icmp_echo_req_src = src;
  24. sta->icmp_echo_req_dst = dst;
  25. sta->icmp_echo_req_id = id;
  26. sta->icmp_echo_req_seq = seq;
  27. return;
  28. }
  29. if (sta->icmp_echo_req_src == dst &&
  30. sta->icmp_echo_req_dst == src &&
  31. sta->icmp_echo_req_id == id &&
  32. sta->icmp_echo_req_seq == seq) {
  33. sta->counters[WLANTEST_STA_COUNTER_PING_OK]++;
  34. if (sta->counters[WLANTEST_STA_COUNTER_ASSOCREQ_TX] == 0 &&
  35. sta->counters[WLANTEST_STA_COUNTER_REASSOCREQ_TX] == 0)
  36. sta->counters[
  37. WLANTEST_STA_COUNTER_PING_OK_FIRST_ASSOC]++;
  38. wpa_printf(MSG_DEBUG, "ICMP echo (ping) match for STA " MACSTR,
  39. MAC2STR(sta->addr));
  40. }
  41. }
  42. static void rx_data_icmp(struct wlantest *wt, const u8 *bssid,
  43. const u8 *sta_addr, u32 dst, u32 src,
  44. const u8 *data, size_t len, const u8 *peer_addr)
  45. {
  46. struct in_addr addr;
  47. char buf[20];
  48. const struct icmphdr *hdr;
  49. u16 id, seq;
  50. struct wlantest_bss *bss;
  51. struct wlantest_sta *sta;
  52. hdr = (const struct icmphdr *) data;
  53. if (len < 4)
  54. return;
  55. /* TODO: check hdr->checksum */
  56. if (hdr->type != ICMP_ECHOREPLY && hdr->type != ICMP_ECHO)
  57. return;
  58. if (len < 8)
  59. return;
  60. id = ntohs(hdr->un.echo.id);
  61. seq = ntohs(hdr->un.echo.sequence);
  62. addr.s_addr = dst;
  63. snprintf(buf, sizeof(buf), "%s", inet_ntoa(addr));
  64. addr.s_addr = src;
  65. wpa_printf(MSG_DEBUG, "ICMP echo %s %s -> %s id=%04x seq=%u len=%u%s",
  66. hdr->type == ICMP_ECHO ? "request" : "response",
  67. inet_ntoa(addr), buf, id, seq, (unsigned) len - 8,
  68. peer_addr ? " [DL]" : "");
  69. bss = bss_find(wt, bssid);
  70. if (bss == NULL) {
  71. wpa_printf(MSG_INFO, "No BSS " MACSTR " known for ICMP packet",
  72. MAC2STR(bssid));
  73. return;
  74. }
  75. if (sta_addr == NULL)
  76. return; /* FromDS broadcast ping */
  77. sta = sta_find(bss, sta_addr);
  78. if (sta == NULL) {
  79. wpa_printf(MSG_INFO, "No STA " MACSTR " known for ICMP packet",
  80. MAC2STR(sta_addr));
  81. return;
  82. }
  83. ping_update(sta, hdr->type == ICMP_ECHO, src, dst, id, seq);
  84. if (peer_addr && (sta = sta_find(bss, peer_addr)))
  85. ping_update(sta, hdr->type == ICMP_ECHO, src, dst, id, seq);
  86. }
  87. void rx_data_ip(struct wlantest *wt, const u8 *bssid, const u8 *sta_addr,
  88. const u8 *dst, const u8 *src, const u8 *data, size_t len,
  89. const u8 *peer_addr)
  90. {
  91. const struct iphdr *ip;
  92. const u8 *payload;
  93. size_t plen;
  94. u16 frag_off, tot_len;
  95. ip = (const struct iphdr *) data;
  96. if (len < sizeof(*ip))
  97. return;
  98. if (ip->version != 4) {
  99. wpa_printf(MSG_DEBUG, "Unexpected IP protocol version %u in "
  100. "IPv4 packet (bssid=" MACSTR " str=" MACSTR
  101. " dst=" MACSTR ")", ip->version, MAC2STR(bssid),
  102. MAC2STR(src), MAC2STR(dst));
  103. return;
  104. }
  105. if (ip->ihl * 4 < sizeof(*ip)) {
  106. wpa_printf(MSG_DEBUG, "Unexpected IP header length %u in "
  107. "IPv4 packet (bssid=" MACSTR " str=" MACSTR
  108. " dst=" MACSTR ")", ip->ihl, MAC2STR(bssid),
  109. MAC2STR(src), MAC2STR(dst));
  110. return;
  111. }
  112. if (ip->ihl * 4 > len) {
  113. wpa_printf(MSG_DEBUG, "Truncated IP header (ihl=%u len=%u) in "
  114. "IPv4 packet (bssid=" MACSTR " str=" MACSTR
  115. " dst=" MACSTR ")", ip->ihl, (unsigned) len,
  116. MAC2STR(bssid), MAC2STR(src), MAC2STR(dst));
  117. return;
  118. }
  119. /* TODO: check header checksum in ip->check */
  120. frag_off = be_to_host16(ip->frag_off);
  121. if (frag_off & 0x1fff) {
  122. wpa_printf(MSG_EXCESSIVE, "IP fragment reassembly not yet "
  123. "supported");
  124. return;
  125. }
  126. tot_len = be_to_host16(ip->tot_len);
  127. if (tot_len > len)
  128. return;
  129. if (tot_len < len)
  130. len = tot_len;
  131. payload = data + 4 * ip->ihl;
  132. plen = len - 4 * ip->ihl;
  133. switch (ip->protocol) {
  134. case IPPROTO_ICMP:
  135. rx_data_icmp(wt, bssid, sta_addr, ip->daddr, ip->saddr,
  136. payload, plen, peer_addr);
  137. break;
  138. }
  139. }