wlantest/process.c

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