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