wlantest/process.c

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