process.c 8.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387
  1. /*
  2. * Received frame processing
  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 "utils/common.h"
  16. #include "utils/radiotap.h"
  17. #include "utils/radiotap_iter.h"
  18. #include "common/ieee802_11_defs.h"
  19. #include "wlantest.h"
  20. static struct wlantest_sta * rx_get_sta(struct wlantest *wt,
  21. const struct ieee80211_hdr *hdr,
  22. size_t len, int *to_ap)
  23. {
  24. u16 fc;
  25. const u8 *sta_addr, *bssid;
  26. struct wlantest_bss *bss;
  27. *to_ap = 0;
  28. if (hdr->addr1[0] & 0x01)
  29. return NULL; /* Ignore group addressed frames */
  30. fc = le_to_host16(hdr->frame_control);
  31. switch (WLAN_FC_GET_TYPE(fc)) {
  32. case WLAN_FC_TYPE_MGMT:
  33. if (len < 24)
  34. return NULL;
  35. bssid = hdr->addr3;
  36. if (os_memcmp(bssid, hdr->addr2, ETH_ALEN) == 0) {
  37. sta_addr = hdr->addr1;
  38. *to_ap = 0;
  39. } else {
  40. if (os_memcmp(bssid, hdr->addr1, ETH_ALEN) != 0)
  41. return NULL; /* Unsupported STA-to-STA frame */
  42. sta_addr = hdr->addr2;
  43. *to_ap = 1;
  44. }
  45. break;
  46. case WLAN_FC_TYPE_DATA:
  47. if (len < 24)
  48. return NULL;
  49. switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
  50. case 0:
  51. return NULL; /* IBSS not supported */
  52. case WLAN_FC_FROMDS:
  53. sta_addr = hdr->addr1;
  54. bssid = hdr->addr2;
  55. *to_ap = 0;
  56. break;
  57. case WLAN_FC_TODS:
  58. sta_addr = hdr->addr2;
  59. bssid = hdr->addr1;
  60. *to_ap = 1;
  61. break;
  62. case WLAN_FC_TODS | WLAN_FC_FROMDS:
  63. return NULL; /* WDS not supported */
  64. default:
  65. return NULL;
  66. }
  67. break;
  68. case WLAN_FC_TYPE_CTRL:
  69. if (WLAN_FC_GET_STYPE(fc) == WLAN_FC_STYPE_PSPOLL &&
  70. len >= 16) {
  71. sta_addr = hdr->addr2;
  72. bssid = hdr->addr1;
  73. *to_ap = 1;
  74. } else
  75. return NULL;
  76. break;
  77. default:
  78. return NULL;
  79. }
  80. bss = bss_find(wt, bssid);
  81. if (bss == NULL)
  82. return NULL;
  83. return sta_find(bss, sta_addr);
  84. }
  85. static void rx_update_ps(struct wlantest *wt, const struct ieee80211_hdr *hdr,
  86. size_t len, struct wlantest_sta *sta, int to_ap)
  87. {
  88. u16 fc, type, stype;
  89. if (sta == NULL)
  90. return;
  91. fc = le_to_host16(hdr->frame_control);
  92. type = WLAN_FC_GET_TYPE(fc);
  93. stype = WLAN_FC_GET_STYPE(fc);
  94. if (!to_ap) {
  95. if (sta->pwrmgt && !sta->pspoll) {
  96. u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
  97. wpa_printf(MSG_DEBUG, "AP " MACSTR " sent a frame "
  98. "(%u:%u) to a sleeping STA " MACSTR
  99. " (seq=%u)",
  100. MAC2STR(sta->bss->bssid),
  101. type, stype, MAC2STR(sta->addr),
  102. WLAN_GET_SEQ_SEQ(seq_ctrl));
  103. } else
  104. sta->pspoll = 0;
  105. return;
  106. }
  107. sta->pspoll = 0;
  108. if (type == WLAN_FC_TYPE_DATA || type == WLAN_FC_TYPE_MGMT ||
  109. (type == WLAN_FC_TYPE_CTRL && stype == WLAN_FC_STYPE_PSPOLL)) {
  110. /*
  111. * In theory, the PS state changes only at the end of the frame
  112. * exchange that is ACKed by the AP. However, most cases are
  113. * handled with this simpler implementation that does not
  114. * maintain state through the frame exchange.
  115. */
  116. if (sta->pwrmgt && !(fc & WLAN_FC_PWRMGT)) {
  117. wpa_printf(MSG_DEBUG, "STA " MACSTR " woke up from "
  118. "sleep", MAC2STR(sta->addr));
  119. sta->pwrmgt = 0;
  120. } else if (!sta->pwrmgt && (fc & WLAN_FC_PWRMGT)) {
  121. wpa_printf(MSG_DEBUG, "STA " MACSTR " went to sleep",
  122. MAC2STR(sta->addr));
  123. sta->pwrmgt = 1;
  124. }
  125. }
  126. if (type == WLAN_FC_TYPE_CTRL && stype == WLAN_FC_STYPE_PSPOLL)
  127. sta->pspoll = 1;
  128. }
  129. static int rx_duplicate(struct wlantest *wt, const struct ieee80211_hdr *hdr,
  130. size_t len, struct wlantest_sta *sta, int to_ap)
  131. {
  132. u16 fc;
  133. int tid = 16;
  134. le16 *seq_ctrl;
  135. if (sta == NULL)
  136. return 0;
  137. fc = le_to_host16(hdr->frame_control);
  138. if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
  139. (WLAN_FC_GET_STYPE(fc) & 0x08) && len >= 26) {
  140. const u8 *qos = ((const u8 *) hdr) + 24;
  141. tid = qos[0] & 0x0f;
  142. }
  143. if (to_ap)
  144. seq_ctrl = &sta->seq_ctrl_to_ap[tid];
  145. else
  146. seq_ctrl = &sta->seq_ctrl_to_sta[tid];
  147. if ((fc & WLAN_FC_RETRY) && hdr->seq_ctrl == *seq_ctrl) {
  148. u16 s = le_to_host16(hdr->seq_ctrl);
  149. wpa_printf(MSG_MSGDUMP, "Ignore duplicated frame (seq=%u "
  150. "frag=%u A1=" MACSTR " A2=" MACSTR ")",
  151. WLAN_GET_SEQ_SEQ(s), WLAN_GET_SEQ_FRAG(s),
  152. MAC2STR(hdr->addr1), MAC2STR(hdr->addr2));
  153. return 1;
  154. }
  155. *seq_ctrl = hdr->seq_ctrl;
  156. return 0;
  157. }
  158. static void rx_ack(struct wlantest *wt, const struct ieee80211_hdr *hdr)
  159. {
  160. struct ieee80211_hdr *last = (struct ieee80211_hdr *) wt->last_hdr;
  161. u16 fc;
  162. if (wt->last_len < 24 || (last->addr1[0] & 0x01) ||
  163. os_memcmp(hdr->addr1, last->addr2, ETH_ALEN) != 0) {
  164. wpa_printf(MSG_MSGDUMP, "Unknown Ack frame (previous frame "
  165. "not seen)");
  166. return;
  167. }
  168. /* Ack to the previous frame */
  169. fc = le_to_host16(last->frame_control);
  170. if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
  171. rx_mgmt_ack(wt, last);
  172. }
  173. static void rx_frame(struct wlantest *wt, const u8 *data, size_t len)
  174. {
  175. const struct ieee80211_hdr *hdr;
  176. u16 fc;
  177. struct wlantest_sta *sta;
  178. int to_ap;
  179. wpa_hexdump(MSG_EXCESSIVE, "RX frame", data, len);
  180. if (len < 2)
  181. return;
  182. hdr = (const struct ieee80211_hdr *) data;
  183. fc = le_to_host16(hdr->frame_control);
  184. if (fc & WLAN_FC_PVER) {
  185. wpa_printf(MSG_DEBUG, "Drop RX frame with unexpected pver=%d",
  186. fc & WLAN_FC_PVER);
  187. return;
  188. }
  189. sta = rx_get_sta(wt, hdr, len, &to_ap);
  190. switch (WLAN_FC_GET_TYPE(fc)) {
  191. case WLAN_FC_TYPE_MGMT:
  192. if (len < 24)
  193. break;
  194. if (rx_duplicate(wt, hdr, len, sta, to_ap))
  195. break;
  196. rx_update_ps(wt, hdr, len, sta, to_ap);
  197. rx_mgmt(wt, data, len);
  198. break;
  199. case WLAN_FC_TYPE_CTRL:
  200. if (len < 10)
  201. break;
  202. wt->rx_ctrl++;
  203. rx_update_ps(wt, hdr, len, sta, to_ap);
  204. if (WLAN_FC_GET_STYPE(fc) == WLAN_FC_STYPE_ACK)
  205. rx_ack(wt, hdr);
  206. break;
  207. case WLAN_FC_TYPE_DATA:
  208. if (len < 24)
  209. break;
  210. if (rx_duplicate(wt, hdr, len, sta, to_ap))
  211. break;
  212. rx_update_ps(wt, hdr, len, sta, to_ap);
  213. rx_data(wt, data, len);
  214. break;
  215. default:
  216. wpa_printf(MSG_DEBUG, "Drop RX frame with unexpected type %d",
  217. WLAN_FC_GET_TYPE(fc));
  218. break;
  219. }
  220. os_memcpy(wt->last_hdr, data, len > sizeof(wt->last_hdr) ?
  221. sizeof(wt->last_hdr) : len);
  222. wt->last_len = len;
  223. }
  224. static void tx_status(struct wlantest *wt, const u8 *data, size_t len, int ack)
  225. {
  226. wpa_printf(MSG_DEBUG, "TX status: ack=%d", ack);
  227. wpa_hexdump(MSG_EXCESSIVE, "TX status frame", data, len);
  228. }
  229. static int check_fcs(const u8 *frame, size_t frame_len, const u8 *fcs)
  230. {
  231. if (WPA_GET_LE32(fcs) != crc32(frame, frame_len))
  232. return -1;
  233. return 0;
  234. }
  235. void wlantest_process(struct wlantest *wt, const u8 *data, size_t len)
  236. {
  237. struct ieee80211_radiotap_iterator iter;
  238. int ret;
  239. int rxflags = 0, txflags = 0, failed = 0, fcs = 0;
  240. const u8 *frame, *fcspos;
  241. size_t frame_len;
  242. wpa_hexdump(MSG_EXCESSIVE, "Process data", data, len);
  243. if (ieee80211_radiotap_iterator_init(&iter, (void *) data, len)) {
  244. wpa_printf(MSG_INFO, "Invalid radiotap frame");
  245. return;
  246. }
  247. for (;;) {
  248. ret = ieee80211_radiotap_iterator_next(&iter);
  249. wpa_printf(MSG_EXCESSIVE, "radiotap iter: %d "
  250. "this_arg_index=%d", ret, iter.this_arg_index);
  251. if (ret == -ENOENT)
  252. break;
  253. if (ret) {
  254. wpa_printf(MSG_INFO, "Invalid radiotap header: %d",
  255. ret);
  256. return;
  257. }
  258. switch (iter.this_arg_index) {
  259. case IEEE80211_RADIOTAP_FLAGS:
  260. if (*iter.this_arg & IEEE80211_RADIOTAP_F_FCS)
  261. fcs = 1;
  262. break;
  263. case IEEE80211_RADIOTAP_RX_FLAGS:
  264. rxflags = 1;
  265. break;
  266. case IEEE80211_RADIOTAP_TX_FLAGS:
  267. txflags = 1;
  268. failed = le_to_host16((*(u16 *) iter.this_arg)) &
  269. IEEE80211_RADIOTAP_F_TX_FAIL;
  270. break;
  271. }
  272. }
  273. if (iter.max_length == 8) {
  274. wpa_printf(MSG_DEBUG, "Skip frame inserted by wlantest");
  275. return;
  276. }
  277. frame = data + iter.max_length;
  278. frame_len = len - iter.max_length;
  279. if (fcs && frame_len >= 4) {
  280. frame_len -= 4;
  281. fcspos = frame + frame_len;
  282. if (check_fcs(frame, frame_len, fcspos) < 0) {
  283. wpa_printf(MSG_EXCESSIVE, "Drop RX frame with invalid "
  284. "FCS");
  285. wt->fcs_error++;
  286. return;
  287. }
  288. }
  289. if (rxflags && txflags)
  290. return;
  291. if (!txflags)
  292. rx_frame(wt, frame, frame_len);
  293. else
  294. tx_status(wt, frame, frame_len, !failed);
  295. }
  296. void wlantest_process_prism(struct wlantest *wt, const u8 *data, size_t len)
  297. {
  298. int fcs = 0;
  299. const u8 *frame, *fcspos;
  300. size_t frame_len;
  301. u32 hdrlen;
  302. wpa_hexdump(MSG_EXCESSIVE, "Process data", data, len);
  303. if (len < 8)
  304. return;
  305. hdrlen = WPA_GET_LE32(data + 4);
  306. if (len < hdrlen) {
  307. wpa_printf(MSG_INFO, "Too short frame to include prism "
  308. "header");
  309. return;
  310. }
  311. frame = data + hdrlen;
  312. frame_len = len - hdrlen;
  313. fcs = 1;
  314. if (fcs && frame_len >= 4) {
  315. frame_len -= 4;
  316. fcspos = frame + frame_len;
  317. if (check_fcs(frame, frame_len, fcspos) < 0) {
  318. wpa_printf(MSG_EXCESSIVE, "Drop RX frame with invalid "
  319. "FCS");
  320. wt->fcs_error++;
  321. return;
  322. }
  323. }
  324. rx_frame(wt, frame, frame_len);
  325. }
  326. void wlantest_process_80211(struct wlantest *wt, const u8 *data, size_t len)
  327. {
  328. wpa_hexdump(MSG_EXCESSIVE, "Process data", data, len);
  329. rx_frame(wt, data, len);
  330. }