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