wlantest: Select correct TDLS context if multiple exists
[mech_eap.git] / wlantest / rx_data.c
1 /*
2  * Received Data 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 #include <linux/if_ether.h>
11
12 #include "utils/common.h"
13 #include "common/defs.h"
14 #include "common/ieee802_11_defs.h"
15 #include "wlantest.h"
16
17
18 static const char * data_stype(u16 stype)
19 {
20         switch (stype) {
21         case WLAN_FC_STYPE_DATA:
22                 return "DATA";
23         case WLAN_FC_STYPE_DATA_CFACK:
24                 return "DATA-CFACK";
25         case WLAN_FC_STYPE_DATA_CFPOLL:
26                 return "DATA-CFPOLL";
27         case WLAN_FC_STYPE_DATA_CFACKPOLL:
28                 return "DATA-CFACKPOLL";
29         case WLAN_FC_STYPE_NULLFUNC:
30                 return "NULLFUNC";
31         case WLAN_FC_STYPE_CFACK:
32                 return "CFACK";
33         case WLAN_FC_STYPE_CFPOLL:
34                 return "CFPOLL";
35         case WLAN_FC_STYPE_CFACKPOLL:
36                 return "CFACKPOLL";
37         case WLAN_FC_STYPE_QOS_DATA:
38                 return "QOSDATA";
39         case WLAN_FC_STYPE_QOS_DATA_CFACK:
40                 return "QOSDATA-CFACK";
41         case WLAN_FC_STYPE_QOS_DATA_CFPOLL:
42                 return "QOSDATA-CFPOLL";
43         case WLAN_FC_STYPE_QOS_DATA_CFACKPOLL:
44                 return "QOSDATA-CFACKPOLL";
45         case WLAN_FC_STYPE_QOS_NULL:
46                 return "QOS-NULL";
47         case WLAN_FC_STYPE_QOS_CFPOLL:
48                 return "QOS-CFPOLL";
49         case WLAN_FC_STYPE_QOS_CFACKPOLL:
50                 return "QOS-CFACKPOLL";
51         }
52         return "??";
53 }
54
55
56 static void rx_data_eth(struct wlantest *wt, const u8 *bssid,
57                         const u8 *sta_addr, const u8 *dst, const u8 *src,
58                         u16 ethertype, const u8 *data, size_t len, int prot,
59                         const u8 *peer_addr)
60 {
61         switch (ethertype) {
62         case ETH_P_PAE:
63                 rx_data_eapol(wt, dst, src, data, len, prot);
64                 break;
65         case ETH_P_IP:
66                 rx_data_ip(wt, bssid, sta_addr, dst, src, data, len,
67                            peer_addr);
68                 break;
69         case 0x890d:
70                 rx_data_80211_encap(wt, bssid, sta_addr, dst, src, data, len);
71                 break;
72         }
73 }
74
75
76 static void rx_data_process(struct wlantest *wt, const u8 *bssid,
77                             const u8 *sta_addr,
78                             const u8 *dst, const u8 *src,
79                             const u8 *data, size_t len, int prot,
80                             const u8 *peer_addr)
81 {
82         if (len == 0)
83                 return;
84
85         if (len >= 8 && os_memcmp(data, "\xaa\xaa\x03\x00\x00\x00", 6) == 0) {
86                 rx_data_eth(wt, bssid, sta_addr, dst, src,
87                             WPA_GET_BE16(data + 6), data + 8, len - 8, prot,
88                             peer_addr);
89                 return;
90         }
91
92         wpa_hexdump(MSG_DEBUG, "Unrecognized LLC", data, len > 8 ? 8 : len);
93 }
94
95
96 static void rx_data_bss_prot_group(struct wlantest *wt,
97                                    const struct ieee80211_hdr *hdr,
98                                    const u8 *qos, const u8 *dst, const u8 *src,
99                                    const u8 *data, size_t len)
100 {
101         struct wlantest_bss *bss;
102         int keyid;
103         u8 *decrypted;
104         size_t dlen;
105         u8 pn[6];
106
107         bss = bss_get(wt, hdr->addr2);
108         if (bss == NULL)
109                 return;
110         if (len < 4) {
111                 add_note(wt, MSG_INFO, "Too short group addressed data frame");
112                 return;
113         }
114
115         if (bss->group_cipher & (WPA_CIPHER_TKIP | WPA_CIPHER_CCMP) &&
116             !(data[3] & 0x20)) {
117                 add_note(wt, MSG_INFO, "Expected TKIP/CCMP frame from "
118                          MACSTR " did not have ExtIV bit set to 1",
119                          MAC2STR(bss->bssid));
120                 return;
121         }
122
123         if (bss->group_cipher == WPA_CIPHER_TKIP) {
124                 if (data[3] & 0x1f) {
125                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
126                                  " used non-zero reserved bit",
127                                  MAC2STR(bss->bssid));
128                 }
129                 if (data[1] != ((data[0] | 0x20) & 0x7f)) {
130                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
131                                  " used incorrect WEPSeed[1] (was 0x%x, "
132                                  "expected 0x%x)",
133                                  MAC2STR(bss->bssid), data[1],
134                                  (data[0] | 0x20) & 0x7f);
135                 }
136         } else if (bss->group_cipher == WPA_CIPHER_CCMP) {
137                 if (data[2] != 0 || (data[3] & 0x1f) != 0) {
138                         add_note(wt, MSG_INFO, "CCMP frame from " MACSTR
139                                  " used non-zero reserved bit",
140                                  MAC2STR(bss->bssid));
141                 }
142         }
143
144         keyid = data[3] >> 6;
145         if (bss->gtk_len[keyid] == 0 && bss->group_cipher != WPA_CIPHER_WEP40)
146         {
147                 add_note(wt, MSG_MSGDUMP, "No GTK known to decrypt the frame "
148                          "(A2=" MACSTR " KeyID=%d)",
149                          MAC2STR(hdr->addr2), keyid);
150                 return;
151         }
152
153         if (bss->group_cipher == WPA_CIPHER_TKIP)
154                 tkip_get_pn(pn, data);
155         else if (bss->group_cipher == WPA_CIPHER_WEP40)
156                 goto skip_replay_det;
157         else
158                 ccmp_get_pn(pn, data);
159         if (os_memcmp(pn, bss->rsc[keyid], 6) <= 0) {
160                 u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
161                 add_note(wt, MSG_INFO, "CCMP/TKIP replay detected: A1=" MACSTR
162                          " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
163                          MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
164                          MAC2STR(hdr->addr3),
165                          WLAN_GET_SEQ_SEQ(seq_ctrl),
166                          WLAN_GET_SEQ_FRAG(seq_ctrl));
167                 wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
168                 wpa_hexdump(MSG_INFO, "RSC", bss->rsc[keyid], 6);
169         }
170
171 skip_replay_det:
172         if (bss->group_cipher == WPA_CIPHER_TKIP)
173                 decrypted = tkip_decrypt(bss->gtk[keyid], hdr, data, len,
174                                          &dlen);
175         else if (bss->group_cipher == WPA_CIPHER_WEP40)
176                 decrypted = wep_decrypt(wt, hdr, data, len, &dlen);
177         else
178                 decrypted = ccmp_decrypt(bss->gtk[keyid], hdr, data, len,
179                                          &dlen);
180         if (decrypted) {
181                 rx_data_process(wt, bss->bssid, NULL, dst, src, decrypted,
182                                 dlen, 1, NULL);
183                 os_memcpy(bss->rsc[keyid], pn, 6);
184                 write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
185                                      decrypted, dlen);
186         } else
187                 add_note(wt, MSG_DEBUG, "Failed to decrypt frame");
188         os_free(decrypted);
189 }
190
191
192 static void rx_data_bss_prot(struct wlantest *wt,
193                              const struct ieee80211_hdr *hdr, const u8 *qos,
194                              const u8 *dst, const u8 *src, const u8 *data,
195                              size_t len)
196 {
197         struct wlantest_bss *bss;
198         struct wlantest_sta *sta, *sta2;
199         int keyid;
200         u16 fc = le_to_host16(hdr->frame_control);
201         u8 *decrypted;
202         size_t dlen;
203         int tid;
204         u8 pn[6], *rsc;
205         struct wlantest_tdls *tdls = NULL, *found;
206         const u8 *tk = NULL;
207
208         if (hdr->addr1[0] & 0x01) {
209                 rx_data_bss_prot_group(wt, hdr, qos, dst, src, data, len);
210                 return;
211         }
212
213         if (fc & WLAN_FC_TODS) {
214                 bss = bss_get(wt, hdr->addr1);
215                 if (bss == NULL)
216                         return;
217                 sta = sta_get(bss, hdr->addr2);
218                 if (sta)
219                         sta->counters[WLANTEST_STA_COUNTER_PROT_DATA_TX]++;
220         } else if (fc & WLAN_FC_FROMDS) {
221                 bss = bss_get(wt, hdr->addr2);
222                 if (bss == NULL)
223                         return;
224                 sta = sta_get(bss, hdr->addr1);
225         } else {
226                 bss = bss_get(wt, hdr->addr3);
227                 if (bss == NULL)
228                         return;
229                 sta = sta_find(bss, hdr->addr2);
230                 sta2 = sta_find(bss, hdr->addr1);
231                 if (sta == NULL || sta2 == NULL)
232                         return;
233                 found = NULL;
234                 dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list)
235                 {
236                         if ((tdls->init == sta && tdls->resp == sta2) ||
237                             (tdls->init == sta2 && tdls->resp == sta)) {
238                                 found = tdls;
239                                 if (tdls->link_up)
240                                         break;
241                         }
242                 }
243                 if (found) {
244                         if (!found->link_up)
245                                 add_note(wt, MSG_DEBUG,
246                                          "TDLS: Link not up, but Data "
247                                          "frame seen");
248                         tk = found->tpk.tk;
249                         tdls = found;
250                 }
251         }
252         if ((sta == NULL ||
253              (!sta->ptk_set && sta->pairwise_cipher != WPA_CIPHER_WEP40)) &&
254             tk == NULL) {
255                 add_note(wt, MSG_MSGDUMP, "No PTK known to decrypt the frame");
256                 return;
257         }
258
259         if (len < 4) {
260                 add_note(wt, MSG_INFO, "Too short encrypted data frame");
261                 return;
262         }
263
264         if (sta->pairwise_cipher & (WPA_CIPHER_TKIP | WPA_CIPHER_CCMP) &&
265             !(data[3] & 0x20)) {
266                 add_note(wt, MSG_INFO, "Expected TKIP/CCMP frame from "
267                          MACSTR " did not have ExtIV bit set to 1",
268                          MAC2STR(src));
269                 return;
270         }
271
272         if (tk == NULL && sta->pairwise_cipher == WPA_CIPHER_TKIP) {
273                 if (data[3] & 0x1f) {
274                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
275                                  " used non-zero reserved bit",
276                                  MAC2STR(hdr->addr2));
277                 }
278                 if (data[1] != ((data[0] | 0x20) & 0x7f)) {
279                         add_note(wt, MSG_INFO, "TKIP frame from " MACSTR
280                                  " used incorrect WEPSeed[1] (was 0x%x, "
281                                  "expected 0x%x)",
282                                  MAC2STR(hdr->addr2), data[1],
283                                  (data[0] | 0x20) & 0x7f);
284                 }
285         } else if (tk || sta->pairwise_cipher == WPA_CIPHER_CCMP) {
286                 if (data[2] != 0 || (data[3] & 0x1f) != 0) {
287                         add_note(wt, MSG_INFO, "CCMP frame from " MACSTR
288                                  " used non-zero reserved bit",
289                                  MAC2STR(hdr->addr2));
290                 }
291         }
292
293         keyid = data[3] >> 6;
294         if (keyid != 0) {
295                 add_note(wt, MSG_INFO, "Unexpected non-zero KeyID %d in "
296                          "individually addressed Data frame from " MACSTR,
297                          keyid, MAC2STR(hdr->addr2));
298         }
299
300         if (qos)
301                 tid = qos[0] & 0x0f;
302         else
303                 tid = 0;
304         if (tk) {
305                 if (os_memcmp(hdr->addr2, tdls->init->addr, ETH_ALEN) == 0)
306                         rsc = tdls->rsc_init[tid];
307                 else
308                         rsc = tdls->rsc_resp[tid];
309         } else if (fc & WLAN_FC_TODS)
310                 rsc = sta->rsc_tods[tid];
311         else
312                 rsc = sta->rsc_fromds[tid];
313
314
315         if (tk == NULL && sta->pairwise_cipher == WPA_CIPHER_TKIP)
316                 tkip_get_pn(pn, data);
317         else if (sta->pairwise_cipher == WPA_CIPHER_WEP40)
318                 goto skip_replay_det;
319         else
320                 ccmp_get_pn(pn, data);
321         if (os_memcmp(pn, rsc, 6) <= 0) {
322                 u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
323                 add_note(wt, MSG_INFO, "CCMP/TKIP replay detected: A1=" MACSTR
324                          " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
325                          MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
326                          MAC2STR(hdr->addr3),
327                          WLAN_GET_SEQ_SEQ(seq_ctrl),
328                          WLAN_GET_SEQ_FRAG(seq_ctrl));
329                 wpa_hexdump(MSG_INFO, "RX PN", pn, 6);
330                 wpa_hexdump(MSG_INFO, "RSC", rsc, 6);
331         }
332
333 skip_replay_det:
334         if (tk)
335                 decrypted = ccmp_decrypt(tk, hdr, data, len, &dlen);
336         else if (sta->pairwise_cipher == WPA_CIPHER_TKIP)
337                 decrypted = tkip_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
338         else if (sta->pairwise_cipher == WPA_CIPHER_WEP40)
339                 decrypted = wep_decrypt(wt, hdr, data, len, &dlen);
340         else
341                 decrypted = ccmp_decrypt(sta->ptk.tk1, hdr, data, len, &dlen);
342         if (decrypted) {
343                 u16 fc = le_to_host16(hdr->frame_control);
344                 const u8 *peer_addr = NULL;
345                 if (!(fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)))
346                         peer_addr = hdr->addr1;
347                 os_memcpy(rsc, pn, 6);
348                 rx_data_process(wt, bss->bssid, sta->addr, dst, src, decrypted,
349                                 dlen, 1, peer_addr);
350                 write_pcap_decrypted(wt, (const u8 *) hdr, 24 + (qos ? 2 : 0),
351                                      decrypted, dlen);
352         } else
353                 add_note(wt, MSG_DEBUG, "Failed to decrypt frame");
354         os_free(decrypted);
355 }
356
357
358 static void rx_data_bss(struct wlantest *wt, const struct ieee80211_hdr *hdr,
359                         const u8 *qos, const u8 *dst, const u8 *src,
360                         const u8 *data, size_t len)
361 {
362         u16 fc = le_to_host16(hdr->frame_control);
363         int prot = !!(fc & WLAN_FC_ISWEP);
364
365         if (qos) {
366                 u8 ack = (qos[0] & 0x60) >> 5;
367                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
368                            " len=%u%s tid=%u%s%s",
369                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
370                            prot ? " Prot" : "", qos[0] & 0x0f,
371                            (qos[0] & 0x10) ? " EOSP" : "",
372                            ack == 0 ? "" :
373                            (ack == 1 ? " NoAck" :
374                             (ack == 2 ? " NoExpAck" : " BA")));
375         } else {
376                 wpa_printf(MSG_MSGDUMP, "BSS DATA: " MACSTR " -> " MACSTR
377                            " len=%u%s",
378                            MAC2STR(src), MAC2STR(dst), (unsigned int) len,
379                            prot ? " Prot" : "");
380         }
381
382         if (prot)
383                 rx_data_bss_prot(wt, hdr, qos, dst, src, data, len);
384         else {
385                 const u8 *bssid, *sta_addr, *peer_addr;
386                 if (fc & WLAN_FC_TODS) {
387                         bssid = hdr->addr1;
388                         sta_addr = hdr->addr2;
389                         peer_addr = NULL;
390                 } else if (fc & WLAN_FC_FROMDS) {
391                         bssid = hdr->addr2;
392                         sta_addr = hdr->addr1;
393                         peer_addr = NULL;
394                 } else {
395                         bssid = hdr->addr3;
396                         sta_addr = hdr->addr2;
397                         peer_addr = hdr->addr1;
398                 }
399                 rx_data_process(wt, bssid, sta_addr, dst, src, data, len, 0,
400                                 peer_addr);
401         }
402 }
403
404
405 static struct wlantest_tdls * get_tdls(struct wlantest *wt, const u8 *bssid,
406                                        const u8 *sta1_addr,
407                                        const u8 *sta2_addr)
408 {
409         struct wlantest_bss *bss;
410         struct wlantest_sta *sta1, *sta2;
411         struct wlantest_tdls *tdls, *found = NULL;
412
413         bss = bss_find(wt, bssid);
414         if (bss == NULL)
415                 return NULL;
416         sta1 = sta_find(bss, sta1_addr);
417         if (sta1 == NULL)
418                 return NULL;
419         sta2 = sta_find(bss, sta2_addr);
420         if (sta2 == NULL)
421                 return NULL;
422
423         dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
424                 if ((tdls->init == sta1 && tdls->resp == sta2) ||
425                     (tdls->init == sta2 && tdls->resp == sta1)) {
426                         found = tdls;
427                         if (tdls->link_up)
428                                 break;
429                 }
430         }
431
432         return found;
433 }
434
435
436 static void add_direct_link(struct wlantest *wt, const u8 *bssid,
437                             const u8 *sta1_addr, const u8 *sta2_addr)
438 {
439         struct wlantest_tdls *tdls;
440
441         tdls = get_tdls(wt, bssid, sta1_addr, sta2_addr);
442         if (tdls == NULL)
443                 return;
444
445         if (tdls->link_up)
446                 tdls->counters[WLANTEST_TDLS_COUNTER_VALID_DIRECT_LINK]++;
447         else
448                 tdls->counters[WLANTEST_TDLS_COUNTER_INVALID_DIRECT_LINK]++;
449 }
450
451
452 static void add_ap_path(struct wlantest *wt, const u8 *bssid,
453                         const u8 *sta1_addr, const u8 *sta2_addr)
454 {
455         struct wlantest_tdls *tdls;
456
457         tdls = get_tdls(wt, bssid, sta1_addr, sta2_addr);
458         if (tdls == NULL)
459                 return;
460
461         if (tdls->link_up)
462                 tdls->counters[WLANTEST_TDLS_COUNTER_INVALID_AP_PATH]++;
463         else
464                 tdls->counters[WLANTEST_TDLS_COUNTER_VALID_AP_PATH]++;
465 }
466
467
468 void rx_data(struct wlantest *wt, const u8 *data, size_t len)
469 {
470         const struct ieee80211_hdr *hdr;
471         u16 fc, stype;
472         size_t hdrlen;
473         const u8 *qos = NULL;
474
475         if (len < 24)
476                 return;
477
478         hdr = (const struct ieee80211_hdr *) data;
479         fc = le_to_host16(hdr->frame_control);
480         stype = WLAN_FC_GET_STYPE(fc);
481         hdrlen = 24;
482         if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
483             (WLAN_FC_TODS | WLAN_FC_FROMDS))
484                 hdrlen += ETH_ALEN;
485         if (stype & 0x08) {
486                 qos = data + hdrlen;
487                 hdrlen += 2;
488         }
489         if (len < hdrlen)
490                 return;
491         wt->rx_data++;
492
493         switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
494         case 0:
495                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s IBSS DA=" MACSTR " SA="
496                            MACSTR " BSSID=" MACSTR,
497                            data_stype(WLAN_FC_GET_STYPE(fc)),
498                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
499                            fc & WLAN_FC_ISWEP ? " Prot" : "",
500                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
501                            MAC2STR(hdr->addr3));
502                 add_direct_link(wt, hdr->addr3, hdr->addr1, hdr->addr2);
503                 rx_data_bss(wt, hdr, qos, hdr->addr1, hdr->addr2,
504                             data + hdrlen, len - hdrlen);
505                 break;
506         case WLAN_FC_FROMDS:
507                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s FromDS DA=" MACSTR
508                            " BSSID=" MACSTR " SA=" MACSTR,
509                            data_stype(WLAN_FC_GET_STYPE(fc)),
510                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
511                            fc & WLAN_FC_ISWEP ? " Prot" : "",
512                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
513                            MAC2STR(hdr->addr3));
514                 add_ap_path(wt, hdr->addr2, hdr->addr1, hdr->addr3);
515                 rx_data_bss(wt, hdr, qos, hdr->addr1, hdr->addr3,
516                             data + hdrlen, len - hdrlen);
517                 break;
518         case WLAN_FC_TODS:
519                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s ToDS BSSID=" MACSTR
520                            " SA=" MACSTR " DA=" MACSTR,
521                            data_stype(WLAN_FC_GET_STYPE(fc)),
522                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
523                            fc & WLAN_FC_ISWEP ? " Prot" : "",
524                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
525                            MAC2STR(hdr->addr3));
526                 add_ap_path(wt, hdr->addr1, hdr->addr3, hdr->addr2);
527                 rx_data_bss(wt, hdr, qos, hdr->addr3, hdr->addr2,
528                             data + hdrlen, len - hdrlen);
529                 break;
530         case WLAN_FC_TODS | WLAN_FC_FROMDS:
531                 wpa_printf(MSG_EXCESSIVE, "DATA %s%s%s WDS RA=" MACSTR " TA="
532                            MACSTR " DA=" MACSTR " SA=" MACSTR,
533                            data_stype(WLAN_FC_GET_STYPE(fc)),
534                            fc & WLAN_FC_PWRMGT ? " PwrMgt" : "",
535                            fc & WLAN_FC_ISWEP ? " Prot" : "",
536                            MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
537                            MAC2STR(hdr->addr3),
538                            MAC2STR((const u8 *) (hdr + 1)));
539                 break;
540         }
541 }