openwrt/target/linux/package/ieee80211-dscape/src/wpa.c

   1 /*
   2  * Copyright 2002-2004, Instant802 Networks, Inc.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2 as
   6  * published by the Free Software Foundation.
   7  */
   8
   9 #include <linux/config.h>
  10 #include <linux/version.h>
  11 #include <linux/module.h>
  12 #include <linux/netdevice.h>
  13 #include <linux/types.h>
  14 #include <linux/slab.h>
  15 #include <linux/skbuff.h>
  16 #include <linux/compiler.h>
  17 #include <linux/wireless.h>
  18 #include <net/iw_handler.h>
  19
  20 #include <net/ieee80211.h>
  21 #include <net/ieee80211_common.h>
  22 #include "ieee80211_i.h"
  23 #include "michael.h"
  24 #include "tkip.h"
  25 #include "aes_ccm.h"
  26 #include "wpa.h"
  27 #ifdef CONFIG_HOSTAPD_WPA_TESTING
  28 #include "hostapd_ioctl.h"
  29 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
  30
  31
  32 #define MICHAEL_MIC_HWACCEL
  33
  34
  35 int ieee80211_get_hdr_info(const struct sk_buff *skb, u8 **sa, u8 **da,
  36                            u8 *qos_tid, u8 **data, size_t *data_len)
  37 {
  38         struct ieee80211_hdr *hdr;
  39         size_t hdrlen;
  40         u16 fc;
  41         int a4_included;
  42         u8 *pos;
  43
  44         hdr = (struct ieee80211_hdr *) skb->data;
  45         fc = le16_to_cpu(hdr->frame_control);
  46
  47         hdrlen = 24;
  48         if ((fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)) ==
  49             (WLAN_FC_FROMDS | WLAN_FC_TODS)) {
  50                 hdrlen += ETH_ALEN;
  51                 *sa = hdr->addr4;
  52                 *da = hdr->addr3;
  53         } else if (fc & WLAN_FC_FROMDS) {
  54                 *sa = hdr->addr3;
  55                 *da = hdr->addr1;
  56         } else if (fc & WLAN_FC_TODS) {
  57                 *sa = hdr->addr2;
  58                 *da = hdr->addr3;
  59         } else {
  60                 *sa = hdr->addr2;
  61                 *da = hdr->addr1;
  62         }
  63
  64         if (fc & 0x80)
  65                 hdrlen += 2;
  66
  67         *data = skb->data + hdrlen;
  68         *data_len = skb->len - hdrlen;
  69
  70         a4_included = (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
  71                 (WLAN_FC_TODS | WLAN_FC_FROMDS);
  72         if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
  73             WLAN_FC_GET_STYPE(fc) & 0x08) {
  74                 pos = (u8 *) &hdr->addr4;
  75                 if (a4_included)
  76                         pos += 6;
  77                 *qos_tid = pos[0] & 0x0f;
  78                 *qos_tid |= 0x80; /* qos_included flag */
  79         } else
  80                 *qos_tid = 0;
  81
  82         return skb->len < hdrlen ? -1 : 0;
  83 }
  84
  85
  86 ieee80211_txrx_result
  87 ieee80211_tx_h_michael_mic_add(struct ieee80211_txrx_data *tx)
  88 {
  89         u8 *data, *sa, *da, *key, *mic, qos_tid;
  90         size_t data_len;
  91         u16 fc;
  92         struct sk_buff *skb = tx->skb;
  93         int authenticator;
  94 #if defined(CONFIG_HOSTAPD_WPA_TESTING) || defined(MICHAEL_MIC_HWACCEL)
  95         int wpa_test = 0;
  96 #endif
  97
  98         fc = tx->fc;
  99
 100         if (!tx->key || tx->key->alg != ALG_TKIP || skb->len < 24 ||
 101             !WLAN_FC_DATA_PRESENT(fc))
 102                 return TXRX_CONTINUE;
 103
 104         if (ieee80211_get_hdr_info(skb, &sa, &da, &qos_tid, &data, &data_len))
 105                 return TXRX_DROP;
 106
 107 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 108         if ((tx->sta && tx->sta->wpa_trigger & WPA_TRIGGER_FAIL_TX_MIC) ||
 109             (!tx->u.tx.unicast &&
 110              tx->local->wpa_trigger & WPA_TRIGGER_FAIL_TX_MIC)) {
 111                 wpa_test = 1;
 112         }
 113 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 114
 115 #ifdef MICHAEL_MIC_HWACCEL
 116         if (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt &&
 117             !tx->fragmented && !wpa_test) {
 118                 /* hwaccel - with no need for preallocated room for Michael MIC
 119                  */
 120                 return TXRX_CONTINUE;
 121         }
 122 #endif /* MICHAEL_MIC_HWACCEL */
 123
 124         if (skb_tailroom(skb) < MICHAEL_MIC_LEN) {
 125                 I802_DEBUG_INC(tx->local->tx_expand_skb_head);
 126                 if (unlikely(pskb_expand_head(skb, TKIP_IV_LEN,
 127                                               MICHAEL_MIC_LEN + TKIP_ICV_LEN,
 128                                               GFP_ATOMIC))) {
 129                         printk(KERN_DEBUG "%s: failed to allocate more memory "
 130                                "for Michael MIC\n", tx->dev->name);
 131                         return TXRX_DROP;
 132                 }
 133         }
 134
 135 #if 0
 136         authenticator = fc & WLAN_FC_FROMDS; /* FIX */
 137 #else
 138         authenticator = 1;
 139 #endif
 140         key = &tx->key->key[authenticator ? ALG_TKIP_TEMP_AUTH_TX_MIC_KEY :
 141                             ALG_TKIP_TEMP_AUTH_RX_MIC_KEY];
 142         mic = skb_put(skb, MICHAEL_MIC_LEN);
 143         michael_mic(key, da, sa, qos_tid & 0x0f, data, data_len, mic);
 144
 145 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 146         if (tx->sta && tx->sta->wpa_trigger & WPA_TRIGGER_FAIL_TX_MIC) {
 147                 printk(KERN_INFO "%s: WPA testing - corrupting TX Michael MIC "
 148                        "for STA " MACSTR "\n",
 149                        tx->dev->name, MAC2STR(tx->sta->addr));
 150                 tx->u.tx.control->key_idx = HW_KEY_IDX_INVALID;
 151                 tx->sta->wpa_trigger &= ~WPA_TRIGGER_FAIL_TX_MIC;
 152                 tx->wpa_test = 1;
 153                 mic[0]++;
 154         } else if (!tx->u.tx.unicast &&
 155                    tx->local->wpa_trigger & WPA_TRIGGER_FAIL_TX_MIC) {
 156                 printk(KERN_INFO "%s: WPA testing - corrupting TX Michael MIC "
 157                        "for Group Key\n", tx->dev->name);
 158                 tx->u.tx.control->key_idx = HW_KEY_IDX_INVALID;
 159                 tx->local->wpa_trigger &= ~WPA_TRIGGER_FAIL_TX_MIC;
 160                 tx->wpa_test = 1;
 161                 mic[0]++;
 162         }
 163 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 164
 165         return TXRX_CONTINUE;
 166 }
 167
 168
 169 ieee80211_txrx_result
 170 ieee80211_rx_h_michael_mic_verify(struct ieee80211_txrx_data *rx)
 171 {
 172         u8 *data, *sa, *da, *key = NULL, qos_tid;
 173         size_t data_len;
 174         u16 fc;
 175         u8 mic[MICHAEL_MIC_LEN];
 176         struct sk_buff *skb = rx->skb;
 177         int authenticator = 1, wpa_test = 0;
 178
 179         fc = rx->fc;
 180
 181         /* If device handles decryption totally, skip this check */
 182         if (rx->local->hw->device_hides_wep ||
 183             rx->local->hw->device_strips_mic)
 184                 return TXRX_CONTINUE;
 185
 186         if (!rx->key || rx->key->alg != ALG_TKIP ||
 187             !(rx->fc & WLAN_FC_ISWEP) || !WLAN_FC_DATA_PRESENT(fc))
 188                 return TXRX_CONTINUE;
 189
 190 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 191         if (rx->sta && rx->sta->wpa_trigger & WPA_TRIGGER_FAIL_RX_MIC) {
 192                 wpa_test = 1;
 193         }
 194 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 195
 196 #ifdef MICHAEL_MIC_HWACCEL
 197         if ((rx->u.rx.status->flag & RX_FLAG_DECRYPTED) &&
 198             !rx->key->force_sw_encrypt && !rx->local->conf.sw_decrypt) {
 199                 if (rx->local->hw->wep_include_iv) {
 200                         if (skb->len < MICHAEL_MIC_LEN)
 201                                 return TXRX_DROP;
 202                 }
 203                 /* Need to verify Michael MIC sometimes in software even when
 204                  * hwaccel is used. Atheros ar5212: fragmented frames and QoS
 205                  * frames. */
 206                 if (!rx->fragmented && !wpa_test)
 207                         goto remove_mic;
 208         }
 209 #endif /* MICHAEL_MIC_HWACCEL */
 210
 211         if (ieee80211_get_hdr_info(skb, &sa, &da, &qos_tid, &data, &data_len)
 212             || data_len < MICHAEL_MIC_LEN)
 213                 return TXRX_DROP;
 214
 215         data_len -= MICHAEL_MIC_LEN;
 216
 217 #if 0
 218         authenticator = fc & WLAN_FC_TODS; /* FIX */
 219 #else
 220         authenticator = 1;
 221 #endif
 222         key = &rx->key->key[authenticator ? ALG_TKIP_TEMP_AUTH_RX_MIC_KEY :
 223                             ALG_TKIP_TEMP_AUTH_TX_MIC_KEY];
 224         michael_mic(key, da, sa, qos_tid & 0x0f, data, data_len, mic);
 225 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 226         if (rx->sta && rx->sta->wpa_trigger & WPA_TRIGGER_FAIL_RX_MIC) {
 227                 printk(KERN_INFO "%s: WPA testing - corrupting RX Michael MIC "
 228                        "for STA " MACSTR "\n",
 229                        rx->dev->name, MAC2STR(rx->sta->addr));
 230                 rx->sta->wpa_trigger &= ~WPA_TRIGGER_FAIL_RX_MIC;
 231                 mic[0]++;
 232         }
 233 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 234         if (memcmp(mic, data + data_len, MICHAEL_MIC_LEN) != 0 || wpa_test) {
 235 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 236                 int i;
 237 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 238                 printk(KERN_DEBUG "%s: invalid Michael MIC in data frame from "
 239                        MACSTR "\n", rx->dev->name, MAC2STR(sa));
 240 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 241                 printk(KERN_DEBUG "   received");
 242                 for (i = 0; i < MICHAEL_MIC_LEN; i++)
 243                         printk(" %02x", data[data_len + i]);
 244                 printk(" expected");
 245                 for (i = 0; i < MICHAEL_MIC_LEN; i++)
 246                         printk(" %02x", mic[i]);
 247                 printk("\n");
 248                 printk(KERN_DEBUG "   SA=" MACSTR " DA=" MACSTR " key",
 249                        MAC2STR(sa), MAC2STR(da));
 250                 for (i = 0; i < 8; i++)
 251                         printk(" %02x", key[i]);
 252                 printk(" (%d)\n", authenticator);
 253 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 254
 255                 do {
 256                         struct ieee80211_hdr *hdr;
 257                         union iwreq_data wrqu;
 258                         char *buf = kmalloc(128, GFP_ATOMIC);
 259                         if (buf == NULL)
 260                                 break;
 261
 262                         /* TODO: needed parameters: count, key type, TSC */
 263                         hdr = (struct ieee80211_hdr *) skb->data;
 264                         sprintf(buf, "MLME-MICHAELMICFAILURE.indication("
 265                                 "keyid=%d %scast addr=" MACSTR ")",
 266                                 rx->key->keyidx,
 267                                 hdr->addr1[0] & 0x01 ? "broad" : "uni",
 268                                 MAC2STR(hdr->addr2));
 269                         memset(&wrqu, 0, sizeof(wrqu));
 270                         wrqu.data.length = strlen(buf);
 271                         wireless_send_event(rx->dev, IWEVCUSTOM, &wrqu, buf);
 272                         kfree(buf);
 273                 } while (0);
 274
 275                 ieee80211_rx_mgmt(rx->dev, rx->skb, rx->u.rx.status,
 276                                   ieee80211_msg_michael_mic_failure);
 277
 278                 return TXRX_QUEUED;
 279         }
 280
 281 #ifdef MICHAEL_MIC_HWACCEL
 282  remove_mic:
 283 #endif /* MICHAEL_MIC_HWACCEL */
 284         /* remove Michael MIC from payload */
 285         skb_trim(skb, skb->len - MICHAEL_MIC_LEN);
 286
 287         return TXRX_CONTINUE;
 288 }
 289
 290
 291 static int tkip_encrypt_skb(struct ieee80211_txrx_data *tx,
 292                             struct sk_buff *skb, int test)
 293 {
 294         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 295         struct ieee80211_key *key = tx->key;
 296         int hdrlen, len, tailneed;
 297         u16 fc;
 298         u8 *pos;
 299
 300         fc = le16_to_cpu(hdr->frame_control);
 301         hdrlen = ieee80211_get_hdrlen(fc);
 302         len = skb->len - hdrlen;
 303
 304         tailneed = (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt)
 305                 ? 0 : TKIP_ICV_LEN;
 306         if ((skb_headroom(skb) < TKIP_IV_LEN ||
 307              skb_tailroom(skb) < tailneed)) {
 308                 I802_DEBUG_INC(tx->local->tx_expand_skb_head);
 309                 if (unlikely(pskb_expand_head(skb, TKIP_IV_LEN, tailneed,
 310                                               GFP_ATOMIC)))
 311                         return -1;
 312         }
 313
 314         pos = skb_push(skb, TKIP_IV_LEN);
 315         memmove(pos, pos + TKIP_IV_LEN, hdrlen);
 316         pos += hdrlen;
 317
 318 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 319         if (test & WPA_TRIGGER_TX_REPLAY)
 320                 goto skip_iv_inc;
 321 iv_inc:
 322 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 323
 324         /* Increase IV for the frame */
 325         key->u.tkip.iv16++;
 326         if (key->u.tkip.iv16 == 0)
 327                 key->u.tkip.iv32++;
 328
 329 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 330         if (test & WPA_TRIGGER_TX_SKIP_SEQ) {
 331                 test = 0;
 332                 goto iv_inc;
 333         }
 334 skip_iv_inc:
 335 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 336
 337         if (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt
 338 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 339             && !tx->wpa_test
 340 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 341                 ) {
 342                 /* hwaccel - with preallocated room for IV */
 343
 344                 ieee80211_tkip_add_iv(pos, key,
 345                                       (u8) (key->u.tkip.iv16 >> 8),
 346                                       (u8) (((key->u.tkip.iv16 >> 8) | 0x20) &
 347                                             0x7f),
 348                                       (u8) key->u.tkip.iv16);
 349
 350                 tx->u.tx.control->key_idx = tx->key->hw_key_idx;
 351                 return 0;
 352         }
 353
 354         /* Add room for ICV */
 355         skb_put(skb, TKIP_ICV_LEN);
 356
 357         hdr = (struct ieee80211_hdr *) skb->data;
 358         ieee80211_tkip_encrypt_data(key, pos, len, hdr->addr2);
 359         return 0;
 360 }
 361
 362
 363 ieee80211_txrx_result
 364 ieee80211_tx_h_tkip_encrypt(struct ieee80211_txrx_data *tx)
 365 {
 366         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) tx->skb->data;
 367         u16 fc;
 368         struct ieee80211_key *key = tx->key;
 369         struct sk_buff *skb = tx->skb;
 370         int wpa_test = 0, test = 0;
 371
 372         fc = le16_to_cpu(hdr->frame_control);
 373
 374         if (!key || key->alg != ALG_TKIP || !WLAN_FC_DATA_PRESENT(fc))
 375                 return TXRX_CONTINUE;
 376
 377         tx->u.tx.control->icv_len = TKIP_ICV_LEN;
 378         tx->u.tx.control->iv_len = TKIP_IV_LEN;
 379         ieee80211_tx_set_iswep(tx);
 380
 381 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 382         if ((tx->sta && tx->sta->wpa_trigger & WPA_TRIGGER_FAIL_TX_ICV) ||
 383             (!tx->u.tx.unicast &&
 384              tx->local->wpa_trigger & WPA_TRIGGER_FAIL_TX_ICV)) {
 385                 wpa_test = 1;
 386         }
 387
 388         if (tx->sta) {
 389                 test = tx->sta->wpa_trigger;
 390                 tx->sta->wpa_trigger &=
 391                         ~(WPA_TRIGGER_TX_REPLAY | WPA_TRIGGER_TX_REPLAY_FRAG |
 392                           WPA_TRIGGER_TX_SKIP_SEQ);
 393         } else {
 394                 test = tx->local->wpa_trigger;
 395                 tx->local->wpa_trigger &=
 396                         ~(WPA_TRIGGER_TX_REPLAY | WPA_TRIGGER_TX_REPLAY_FRAG |
 397                           WPA_TRIGGER_TX_SKIP_SEQ);
 398         }
 399         if (test &
 400             (WPA_TRIGGER_TX_REPLAY | WPA_TRIGGER_TX_REPLAY_FRAG |
 401              WPA_TRIGGER_TX_SKIP_SEQ)) {
 402                 printk(KERN_INFO "%s: WPA testing - TKIP TX packet number "
 403                        "%s%s%s%s\n", tx->dev->name,
 404                        tx->sta ? "[UNICAST]" : "[MULTICAST]",
 405                        test & WPA_TRIGGER_TX_REPLAY ? "[REPLAY]" : "",
 406                        test & WPA_TRIGGER_TX_REPLAY_FRAG ?
 407                        "[REPLAY FRAG]" : "",
 408                        test & WPA_TRIGGER_TX_SKIP_SEQ ? "[SKIP SEQ]" : "");
 409         }
 410 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 411
 412         if (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt &&
 413             !tx->local->hw->wep_include_iv && !wpa_test) {
 414                 /* hwaccel - with no need for preallocated room for IV/ICV */
 415                 tx->u.tx.control->key_idx = tx->key->hw_key_idx;
 416                 return TXRX_CONTINUE;
 417         }
 418
 419         if (tkip_encrypt_skb(tx, skb, test) < 0)
 420                 return TXRX_DROP;
 421
 422         if (tx->u.tx.extra_frag) {
 423                 int i;
 424 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 425                 if (test & WPA_TRIGGER_TX_REPLAY_FRAG)
 426                         test |= WPA_TRIGGER_TX_REPLAY;
 427 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 428                 for (i = 0; i < tx->u.tx.num_extra_frag; i++) {
 429                         if (tkip_encrypt_skb(tx, tx->u.tx.extra_frag[i], test)
 430                             < 0)
 431                                 return TXRX_DROP;
 432                 }
 433         }
 434
 435 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 436         if (tx->sta && tx->sta->wpa_trigger & WPA_TRIGGER_FAIL_TX_ICV) {
 437                 printk(KERN_INFO "%s: WPA testing - corrupting TX TKIP ICV "
 438                        "for STA " MACSTR "\n",
 439                        tx->dev->name, MAC2STR(tx->sta->addr));
 440                 tx->u.tx.control->key_idx = HW_KEY_IDX_INVALID;
 441                 tx->sta->wpa_trigger &= ~WPA_TRIGGER_FAIL_TX_ICV;
 442                 skb->data[skb->len - 1]++;
 443         } else if (!tx->u.tx.unicast &&
 444                    tx->local->wpa_trigger & WPA_TRIGGER_FAIL_TX_ICV) {
 445                 printk(KERN_INFO "%s: WPA testing - corrupting TX TKIP ICV "
 446                        "for Group Key\n",
 447                        tx->dev->name);
 448                 tx->u.tx.control->key_idx = HW_KEY_IDX_INVALID;
 449                 tx->local->wpa_trigger &= ~WPA_TRIGGER_FAIL_TX_ICV;
 450                 skb->data[skb->len - 1]++;
 451         }
 452 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 453
 454         return TXRX_CONTINUE;
 455 }
 456
 457
 458 ieee80211_txrx_result
 459 ieee80211_rx_h_tkip_decrypt(struct ieee80211_txrx_data *rx)
 460 {
 461         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) rx->skb->data;
 462         u16 fc;
 463         int hdrlen, res, hwaccel = 0, wpa_test = 0;
 464         struct ieee80211_key *key = rx->key;
 465         struct sk_buff *skb = rx->skb;
 466
 467         fc = le16_to_cpu(hdr->frame_control);
 468         hdrlen = ieee80211_get_hdrlen(fc);
 469
 470         if (!rx->key || rx->key->alg != ALG_TKIP ||
 471             !(rx->fc & WLAN_FC_ISWEP) ||
 472             WLAN_FC_GET_TYPE(rx->fc) != WLAN_FC_TYPE_DATA)
 473                 return TXRX_CONTINUE;
 474
 475         if (!rx->sta || skb->len - hdrlen < 12)
 476                 return TXRX_DROP;
 477
 478 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 479         if (rx->sta && rx->sta->wpa_trigger & WPA_TRIGGER_FAIL_RX_ICV) {
 480                 printk(KERN_INFO "%s: WPA testing - corrupting RX TKIP ICV "
 481                        "for STA " MACSTR "\n",
 482                        rx->dev->name, MAC2STR(rx->sta->addr));
 483                 rx->sta->wpa_trigger &= ~WPA_TRIGGER_FAIL_RX_ICV;
 484                 skb->data[skb->len - 1]++;
 485                 wpa_test = 1;
 486         }
 487 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 488
 489         if ((rx->u.rx.status->flag & RX_FLAG_DECRYPTED) &&
 490             !rx->key->force_sw_encrypt && !rx->local->conf.sw_decrypt) {
 491                 if (!rx->local->hw->wep_include_iv) {
 492                         /* Hardware takes care of all processing, including
 493                          * replay protection, so no need to continue here. */
 494                         return TXRX_CONTINUE;
 495                 }
 496
 497                 /* let TKIP code verify IV, but skip decryption */
 498                 hwaccel = 1;
 499         }
 500
 501         res = ieee80211_tkip_decrypt_data(key, skb->data + hdrlen,
 502                                           skb->len - hdrlen, rx->sta->addr,
 503                                           hwaccel, rx->u.rx.queue);
 504         if (res != TKIP_DECRYPT_OK || wpa_test) {
 505                 printk(KERN_DEBUG "%s: TKIP decrypt failed for RX frame from "
 506                        MACSTR " (res=%d)\n",
 507                        rx->dev->name, MAC2STR(rx->sta->addr), res);
 508                 return TXRX_DROP;
 509         }
 510
 511         /* Trim ICV */
 512         skb_trim(skb, skb->len - TKIP_ICV_LEN);
 513
 514         /* Remove IV */
 515         memmove(skb->data + TKIP_IV_LEN, skb->data, hdrlen);
 516         skb_pull(skb, TKIP_IV_LEN);
 517
 518         return TXRX_CONTINUE;
 519 }
 520
 521
 522 static void ccmp_special_blocks(struct sk_buff *skb, u8 *pn, u8 *b_0, u8 *aad,
 523                                 int encrypted)
 524 {
 525         u16 fc;
 526         int a4_included, qos_included;
 527         u8 qos_tid, *fc_pos, *data, *sa, *da;
 528         int len_a;
 529         size_t data_len;
 530         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 531
 532         fc_pos = (u8 *) &hdr->frame_control;
 533         fc = fc_pos[0] ^ (fc_pos[1] << 8);
 534         a4_included = (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
 535                 (WLAN_FC_TODS | WLAN_FC_FROMDS);
 536
 537         ieee80211_get_hdr_info(skb, &sa, &da, &qos_tid, &data, &data_len);
 538         data_len -= CCMP_HDR_LEN + (encrypted ? CCMP_MIC_LEN : 0);
 539         if (qos_tid & 0x80) {
 540                 qos_included = 1;
 541                 qos_tid &= 0x0f;
 542         } else
 543                 qos_included = 0;
 544         /* First block, b_0 */
 545
 546         b_0[0] = 0x59; /* flags: Adata: 1, M: 011, L: 001 */
 547         /* Nonce: QoS Priority | A2 | PN */
 548         b_0[1] = qos_tid;
 549         memcpy(&b_0[2], hdr->addr2, 6);
 550         memcpy(&b_0[8], pn, CCMP_PN_LEN);
 551         /* l(m) */
 552         b_0[14] = (data_len >> 8) & 0xff;
 553         b_0[15] = data_len & 0xff;
 554
 555
 556         /* AAD (extra authenticate-only data) / masked 802.11 header
 557          * FC | A1 | A2 | A3 | SC | [A4] | [QC] */
 558
 559         len_a = a4_included ? 28 : 22;
 560         if (qos_included)
 561                 len_a += 2;
 562
 563         aad[0] = 0; /* (len_a >> 8) & 0xff; */
 564         aad[1] = len_a & 0xff;
 565         /* Mask FC: zero subtype b4 b5 b6 */
 566         aad[2] = fc_pos[0] & ~(BIT(4) | BIT(5) | BIT(6));
 567         /* Retry, PwrMgt, MoreData; set Protected */
 568         aad[3] = (fc_pos[1] & ~(BIT(3) | BIT(4) | BIT(5))) | BIT(6);
 569         memcpy(&aad[4], &hdr->addr1, 18);
 570
 571         /* Mask Seq#, leave Frag# */
 572         aad[22] = *((u8 *) &hdr->seq_ctrl) & 0x0f;
 573         aad[23] = 0;
 574         if (a4_included) {
 575                 memcpy(&aad[24], hdr->addr4, 6);
 576                 aad[30] = 0;
 577                 aad[31] = 0;
 578         } else
 579                 memset(&aad[24], 0, 8);
 580         if (qos_included) {
 581                 u8 *dpos = &aad[a4_included ? 30 : 24];
 582
 583                 /* Mask QoS Control field */
 584                 dpos[0] = qos_tid;
 585                 dpos[1] = 0;
 586         }
 587 }
 588
 589
 590 static inline void ccmp_pn2hdr(u8 *hdr, u8 *pn, int key_id)
 591 {
 592         hdr[0] = pn[5];
 593         hdr[1] = pn[4];
 594         hdr[2] = 0;
 595         hdr[3] = 0x20 | (key_id << 6);
 596         hdr[4] = pn[3];
 597         hdr[5] = pn[2];
 598         hdr[6] = pn[1];
 599         hdr[7] = pn[0];
 600 }
 601
 602
 603 static inline int ccmp_hdr2pn(u8 *pn, u8 *hdr)
 604 {
 605         pn[0] = hdr[7];
 606         pn[1] = hdr[6];
 607         pn[2] = hdr[5];
 608         pn[3] = hdr[4];
 609         pn[4] = hdr[1];
 610         pn[5] = hdr[0];
 611         return (hdr[3] >> 6) & 0x03;
 612 }
 613
 614
 615 static int ccmp_encrypt_skb(struct ieee80211_txrx_data *tx,
 616                             struct sk_buff *skb, int test)
 617 {
 618         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 619         struct ieee80211_key *key = tx->key;
 620         int hdrlen, len, tailneed;
 621         u16 fc;
 622         u8 *pos, *pn;
 623         u8 b_0[AES_BLOCK_LEN], aad[2 * AES_BLOCK_LEN];
 624         int i;
 625
 626         fc = le16_to_cpu(hdr->frame_control);
 627         hdrlen = ieee80211_get_hdrlen(fc);
 628         len = skb->len - hdrlen;
 629
 630         tailneed = (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt)
 631                 ? 0 : CCMP_MIC_LEN;
 632
 633         if ((skb_headroom(skb) < CCMP_HDR_LEN ||
 634              skb_tailroom(skb) < tailneed)) {
 635                 I802_DEBUG_INC(tx->local->tx_expand_skb_head);
 636                 if (unlikely(pskb_expand_head(skb, CCMP_HDR_LEN, tailneed,
 637                                               GFP_ATOMIC)))
 638                         return -1;
 639         }
 640
 641         pos = skb_push(skb, CCMP_HDR_LEN);
 642         memmove(pos, pos + CCMP_HDR_LEN, hdrlen);
 643         hdr = (struct ieee80211_hdr *) pos;
 644         pos += hdrlen;
 645
 646         /* PN = PN + 1 */
 647         pn = key->u.ccmp.tx_pn;
 648
 649 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 650         if (test & WPA_TRIGGER_TX_REPLAY)
 651                 goto skip_pn_inc;
 652 pn_inc:
 653 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 654
 655         for (i = CCMP_PN_LEN - 1; i >= 0; i--) {
 656                 pn[i]++;
 657                 if (pn[i])
 658                         break;
 659         }
 660
 661 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 662         if (test & WPA_TRIGGER_TX_SKIP_SEQ) {
 663                 test = 0;
 664                 goto pn_inc;
 665         }
 666 skip_pn_inc:
 667 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 668
 669         ccmp_pn2hdr(pos, pn, key->keyidx);
 670
 671         if (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt) {
 672                 /* hwaccel - with preallocated room for CCMP header */
 673                 tx->u.tx.control->key_idx = tx->key->hw_key_idx;
 674                 return 0;
 675         }
 676
 677         pos += CCMP_HDR_LEN;
 678         ccmp_special_blocks(skb, pn, b_0, aad, 0);
 679         ieee80211_aes_ccm_encrypt(key->u.ccmp.aes_state, b_0, aad, pos, len,
 680                                   pos, skb_put(skb, CCMP_MIC_LEN));
 681
 682         return 0;
 683 }
 684
 685
 686 ieee80211_txrx_result
 687 ieee80211_tx_h_ccmp_encrypt(struct ieee80211_txrx_data *tx)
 688 {
 689         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) tx->skb->data;
 690         struct ieee80211_key *key = tx->key;
 691         u16 fc;
 692         struct sk_buff *skb = tx->skb;
 693         int test = 0;
 694
 695         fc = le16_to_cpu(hdr->frame_control);
 696
 697         if (!key || key->alg != ALG_CCMP || !WLAN_FC_DATA_PRESENT(fc))
 698                 return TXRX_CONTINUE;
 699
 700 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 701         if (tx->sta) {
 702                 test = tx->sta->wpa_trigger;
 703                 tx->sta->wpa_trigger = 0;
 704         } else {
 705                 test = tx->local->wpa_trigger;
 706                 tx->local->wpa_trigger = 0;
 707         }
 708         if (test &
 709             (WPA_TRIGGER_TX_REPLAY | WPA_TRIGGER_TX_REPLAY_FRAG |
 710              WPA_TRIGGER_TX_SKIP_SEQ)) {
 711                 printk(KERN_INFO "%s: WPA testing - CCMP TX packet number "
 712                        "%s%s%s%s\n", tx->dev->name,
 713                        tx->sta ? "[UNICAST]" : "[MULTICAST]",
 714                        test & WPA_TRIGGER_TX_REPLAY ? "[REPLAY]" : "",
 715                        test & WPA_TRIGGER_TX_REPLAY_FRAG ?
 716                        "[REPLAY FRAG]" : "",
 717                        test & WPA_TRIGGER_TX_SKIP_SEQ ? "[SKIP SEQ]" : "");
 718         }
 719 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 720
 721         tx->u.tx.control->icv_len = CCMP_MIC_LEN;
 722         tx->u.tx.control->iv_len = CCMP_HDR_LEN;
 723         ieee80211_tx_set_iswep(tx);
 724
 725         if (!tx->key->force_sw_encrypt && !tx->local->conf.sw_decrypt &&
 726             !tx->local->hw->wep_include_iv) {
 727                 /* hwaccel - with no need for preallocated room for CCMP "
 728                  * header or MIC fields */
 729                 tx->u.tx.control->key_idx = tx->key->hw_key_idx;
 730                 return TXRX_CONTINUE;
 731         }
 732
 733         if (ccmp_encrypt_skb(tx, skb, test) < 0)
 734                 return TXRX_DROP;
 735
 736         if (tx->u.tx.extra_frag) {
 737                 int i;
 738 #ifdef CONFIG_HOSTAPD_WPA_TESTING
 739                 if (test & WPA_TRIGGER_TX_REPLAY_FRAG)
 740                         test |= WPA_TRIGGER_TX_REPLAY;
 741 #endif /* CONFIG_HOSTAPD_WPA_TESTING */
 742                 for (i = 0; i < tx->u.tx.num_extra_frag; i++) {
 743                         if (ccmp_encrypt_skb(tx, tx->u.tx.extra_frag[i], test)
 744                             < 0)
 745                                 return TXRX_DROP;
 746                 }
 747         }
 748
 749         return TXRX_CONTINUE;
 750 }
 751
 752
 753 ieee80211_txrx_result
 754 ieee80211_rx_h_ccmp_decrypt(struct ieee80211_txrx_data *rx)
 755 {
 756         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) rx->skb->data;
 757         u16 fc;
 758         int hdrlen;
 759         struct ieee80211_key *key = rx->key;
 760         struct sk_buff *skb = rx->skb;
 761         u8 b_0[AES_BLOCK_LEN], aad[2 * AES_BLOCK_LEN];
 762         u8 pn[CCMP_PN_LEN];
 763         int data_len;
 764
 765         fc = le16_to_cpu(hdr->frame_control);
 766         hdrlen = ieee80211_get_hdrlen(fc);
 767
 768         if (!rx->key || rx->key->alg != ALG_CCMP ||
 769             !(rx->fc & WLAN_FC_ISWEP) ||
 770             WLAN_FC_GET_TYPE(rx->fc) != WLAN_FC_TYPE_DATA)
 771                 return TXRX_CONTINUE;
 772
 773         data_len = skb->len - hdrlen - CCMP_HDR_LEN - CCMP_MIC_LEN;
 774         if (!rx->sta || data_len < 0)
 775                 return TXRX_DROP;
 776
 777         if ((rx->u.rx.status->flag & RX_FLAG_DECRYPTED) &&
 778             !rx->key->force_sw_encrypt && !rx->local->conf.sw_decrypt &&
 779             !rx->local->hw->wep_include_iv)
 780                 return TXRX_CONTINUE;
 781
 782         (void) ccmp_hdr2pn(pn, skb->data + hdrlen);
 783
 784         if (memcmp(pn, key->u.ccmp.rx_pn[rx->u.rx.queue], CCMP_PN_LEN) <= 0) {
 785 #ifdef CONFIG_IEEE80211_DEBUG
 786                 u8 *ppn = key->u.ccmp.rx_pn[rx->u.rx.queue];
 787                 printk(KERN_DEBUG "%s: CCMP replay detected for RX frame from "
 788                        MACSTR " (RX PN %02x%02x%02x%02x%02x%02x <= prev. PN "
 789                        "%02x%02x%02x%02x%02x%02x)\n", rx->dev->name,
 790                        MAC2STR(rx->sta->addr),
 791                        pn[0], pn[1], pn[2], pn[3], pn[4], pn[5],
 792                        ppn[0], ppn[1], ppn[2], ppn[3], ppn[4], ppn[5]);
 793 #endif /* CONFIG_IEEE80211_DEBUG */
 794                 key->u.ccmp.replays++;
 795                 return TXRX_DROP;
 796         }
 797
 798         if ((rx->u.rx.status->flag & RX_FLAG_DECRYPTED) &&
 799             !rx->key->force_sw_encrypt && !rx->local->conf.sw_decrypt) {
 800                 /* hwaccel has already decrypted frame and verified MIC */
 801         } else {
 802                 ccmp_special_blocks(skb, pn, b_0, aad, 1);
 803
 804                 if (ieee80211_aes_ccm_decrypt(
 805                             key->u.ccmp.aes_state, b_0, aad,
 806                             skb->data + hdrlen + CCMP_HDR_LEN, data_len,
 807                             skb->data + skb->len - CCMP_MIC_LEN,
 808                             skb->data + hdrlen + CCMP_HDR_LEN)) {
 809                         printk(KERN_DEBUG "%s: CCMP decrypt failed for RX "
 810                                "frame from " MACSTR "\n", rx->dev->name,
 811                                MAC2STR(rx->sta->addr));
 812                         return TXRX_DROP;
 813                 }
 814         }
 815
 816         memcpy(key->u.ccmp.rx_pn[rx->u.rx.queue], pn, CCMP_PN_LEN);
 817
 818         /* Remove CCMP header and MIC */
 819         skb_trim(skb, skb->len - CCMP_MIC_LEN);
 820         memmove(skb->data + CCMP_HDR_LEN, skb->data, hdrlen);
 821         skb_pull(skb, CCMP_HDR_LEN);
 822
 823         return TXRX_CONTINUE;
 824 }
 825