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