net/mac80211/wpa.c

   1 /*
   2  * Copyright 2002-2004, Instant802 Networks, Inc.
   3  * Copyright 2008, Jouni Malinen <j@w1.fi>
   4  * Copyright (C) 2016 Intel Deutschland GmbH
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10
  11 #include <linux/netdevice.h>
  12 #include <linux/types.h>
  13 #include <linux/skbuff.h>
  14 #include <linux/compiler.h>
  15 #include <linux/ieee80211.h>
  16 #include <linux/gfp.h>
  17 #include <asm/unaligned.h>
  18 #include <net/mac80211.h>
  19 #include <crypto/aes.h>
  20
  21 #include "ieee80211_i.h"
  22 #include "michael.h"
  23 #include "tkip.h"
  24 #include "aes_ccm.h"
  25 #include "aes_cmac.h"
  26 #include "aes_gmac.h"
  27 #include "aes_gcm.h"
  28 #include "wpa.h"
  29
  30 ieee80211_tx_result
  31 ieee80211_tx_h_michael_mic_add(struct ieee80211_tx_data *tx)
  32 {
  33         u8 *data, *key, *mic;
  34         size_t data_len;
  35         unsigned int hdrlen;
  36         struct ieee80211_hdr *hdr;
  37         struct sk_buff *skb = tx->skb;
  38         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
  39         int tail;
  40
  41         hdr = (struct ieee80211_hdr *)skb->data;
  42         if (!tx->key || tx->key->conf.cipher != WLAN_CIPHER_SUITE_TKIP ||
  43             skb->len < 24 || !ieee80211_is_data_present(hdr->frame_control))
  44                 return TX_CONTINUE;
  45
  46         hdrlen = ieee80211_hdrlen(hdr->frame_control);
  47         if (skb->len < hdrlen)
  48                 return TX_DROP;
  49
  50         data = skb->data + hdrlen;
  51         data_len = skb->len - hdrlen;
  52
  53         if (unlikely(info->flags & IEEE80211_TX_INTFL_TKIP_MIC_FAILURE)) {
  54                 /* Need to use software crypto for the test */
  55                 info->control.hw_key = NULL;
  56         }
  57
  58         if (info->control.hw_key &&
  59             (info->flags & IEEE80211_TX_CTL_DONTFRAG ||
  60              ieee80211_hw_check(&tx->local->hw, SUPPORTS_TX_FRAG)) &&
  61             !(tx->key->conf.flags & IEEE80211_KEY_FLAG_GENERATE_MMIC)) {
  62                 /* hwaccel - with no need for SW-generated MMIC */
  63                 return TX_CONTINUE;
  64         }
  65
  66         tail = MICHAEL_MIC_LEN;
  67         if (!info->control.hw_key)
  68                 tail += IEEE80211_TKIP_ICV_LEN;
  69
  70         if (WARN(skb_tailroom(skb) < tail ||
  71                  skb_headroom(skb) < IEEE80211_TKIP_IV_LEN,
  72                  "mmic: not enough head/tail (%d/%d,%d/%d)\n",
  73                  skb_headroom(skb), IEEE80211_TKIP_IV_LEN,
  74                  skb_tailroom(skb), tail))
  75                 return TX_DROP;
  76
  77         key = &tx->key->conf.key[NL80211_TKIP_DATA_OFFSET_TX_MIC_KEY];
  78         mic = skb_put(skb, MICHAEL_MIC_LEN);
  79         michael_mic(key, hdr, data, data_len, mic);
  80         if (unlikely(info->flags & IEEE80211_TX_INTFL_TKIP_MIC_FAILURE))
  81                 mic[0]++;
  82
  83         return TX_CONTINUE;
  84 }
  85
  86
  87 ieee80211_rx_result
  88 ieee80211_rx_h_michael_mic_verify(struct ieee80211_rx_data *rx)
  89 {
  90         u8 *data, *key = NULL;
  91         size_t data_len;
  92         unsigned int hdrlen;
  93         u8 mic[MICHAEL_MIC_LEN];
  94         struct sk_buff *skb = rx->skb;
  95         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
  96         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
  97
  98         /*
  99          * it makes no sense to check for MIC errors on anything other
 100          * than data frames.
 101          */
 102         if (!ieee80211_is_data_present(hdr->frame_control))
 103                 return RX_CONTINUE;
 104
 105         /*
 106          * No way to verify the MIC if the hardware stripped it or
 107          * the IV with the key index. In this case we have solely rely
 108          * on the driver to set RX_FLAG_MMIC_ERROR in the event of a
 109          * MIC failure report.
 110          */
 111         if (status->flag & (RX_FLAG_MMIC_STRIPPED | RX_FLAG_IV_STRIPPED)) {
 112                 if (status->flag & RX_FLAG_MMIC_ERROR)
 113                         goto mic_fail_no_key;
 114
 115                 if (!(status->flag & RX_FLAG_IV_STRIPPED) && rx->key &&
 116                     rx->key->conf.cipher == WLAN_CIPHER_SUITE_TKIP)
 117                         goto update_iv;
 118
 119                 return RX_CONTINUE;
 120         }
 121
 122         /*
 123          * Some hardware seems to generate Michael MIC failure reports; even
 124          * though, the frame was not encrypted with TKIP and therefore has no
 125          * MIC. Ignore the flag them to avoid triggering countermeasures.
 126          */
 127         if (!rx->key || rx->key->conf.cipher != WLAN_CIPHER_SUITE_TKIP ||
 128             !(status->flag & RX_FLAG_DECRYPTED))
 129                 return RX_CONTINUE;
 130
 131         if (rx->sdata->vif.type == NL80211_IFTYPE_AP && rx->key->conf.keyidx) {
 132                 /*
 133                  * APs with pairwise keys should never receive Michael MIC
 134                  * errors for non-zero keyidx because these are reserved for
 135                  * group keys and only the AP is sending real multicast
 136                  * frames in the BSS.
 137                  */
 138                 return RX_DROP_UNUSABLE;
 139         }
 140
 141         if (status->flag & RX_FLAG_MMIC_ERROR)
 142                 goto mic_fail;
 143
 144         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 145         if (skb->len < hdrlen + MICHAEL_MIC_LEN)
 146                 return RX_DROP_UNUSABLE;
 147
 148         if (skb_linearize(rx->skb))
 149                 return RX_DROP_UNUSABLE;
 150         hdr = (void *)skb->data;
 151
 152         data = skb->data + hdrlen;
 153         data_len = skb->len - hdrlen - MICHAEL_MIC_LEN;
 154         key = &rx->key->conf.key[NL80211_TKIP_DATA_OFFSET_RX_MIC_KEY];
 155         michael_mic(key, hdr, data, data_len, mic);
 156         if (memcmp(mic, data + data_len, MICHAEL_MIC_LEN) != 0)
 157                 goto mic_fail;
 158
 159         /* remove Michael MIC from payload */
 160         skb_trim(skb, skb->len - MICHAEL_MIC_LEN);
 161
 162 update_iv:
 163         /* update IV in key information to be able to detect replays */
 164         rx->key->u.tkip.rx[rx->security_idx].iv32 = rx->tkip_iv32;
 165         rx->key->u.tkip.rx[rx->security_idx].iv16 = rx->tkip_iv16;
 166
 167         return RX_CONTINUE;
 168
 169 mic_fail:
 170         rx->key->u.tkip.mic_failures++;
 171
 172 mic_fail_no_key:
 173         /*
 174          * In some cases the key can be unset - e.g. a multicast packet, in
 175          * a driver that supports HW encryption. Send up the key idx only if
 176          * the key is set.
 177          */
 178         cfg80211_michael_mic_failure(rx->sdata->dev, hdr->addr2,
 179                                      is_multicast_ether_addr(hdr->addr1) ?
 180                                      NL80211_KEYTYPE_GROUP :
 181                                      NL80211_KEYTYPE_PAIRWISE,
 182                                      rx->key ? rx->key->conf.keyidx : -1,
 183                                      NULL, GFP_ATOMIC);
 184         return RX_DROP_UNUSABLE;
 185 }
 186
 187 static int tkip_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
 188 {
 189         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 190         struct ieee80211_key *key = tx->key;
 191         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 192         unsigned int hdrlen;
 193         int len, tail;
 194         u64 pn;
 195         u8 *pos;
 196
 197         if (info->control.hw_key &&
 198             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV) &&
 199             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE)) {
 200                 /* hwaccel - with no need for software-generated IV */
 201                 return 0;
 202         }
 203
 204         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 205         len = skb->len - hdrlen;
 206
 207         if (info->control.hw_key)
 208                 tail = 0;
 209         else
 210                 tail = IEEE80211_TKIP_ICV_LEN;
 211
 212         if (WARN_ON(skb_tailroom(skb) < tail ||
 213                     skb_headroom(skb) < IEEE80211_TKIP_IV_LEN))
 214                 return -1;
 215
 216         pos = skb_push(skb, IEEE80211_TKIP_IV_LEN);
 217         memmove(pos, pos + IEEE80211_TKIP_IV_LEN, hdrlen);
 218         pos += hdrlen;
 219
 220         /* the HW only needs room for the IV, but not the actual IV */
 221         if (info->control.hw_key &&
 222             (info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE))
 223                 return 0;
 224
 225         /* Increase IV for the frame */
 226         pn = atomic64_inc_return(&key->conf.tx_pn);
 227         pos = ieee80211_tkip_add_iv(pos, &key->conf, pn);
 228
 229         /* hwaccel - with software IV */
 230         if (info->control.hw_key)
 231                 return 0;
 232
 233         /* Add room for ICV */
 234         skb_put(skb, IEEE80211_TKIP_ICV_LEN);
 235
 236         return ieee80211_tkip_encrypt_data(tx->local->wep_tx_tfm,
 237                                            key, skb, pos, len);
 238 }
 239
 240
 241 ieee80211_tx_result
 242 ieee80211_crypto_tkip_encrypt(struct ieee80211_tx_data *tx)
 243 {
 244         struct sk_buff *skb;
 245
 246         ieee80211_tx_set_protected(tx);
 247
 248         skb_queue_walk(&tx->skbs, skb) {
 249                 if (tkip_encrypt_skb(tx, skb) < 0)
 250                         return TX_DROP;
 251         }
 252
 253         return TX_CONTINUE;
 254 }
 255
 256
 257 ieee80211_rx_result
 258 ieee80211_crypto_tkip_decrypt(struct ieee80211_rx_data *rx)
 259 {
 260         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) rx->skb->data;
 261         int hdrlen, res, hwaccel = 0;
 262         struct ieee80211_key *key = rx->key;
 263         struct sk_buff *skb = rx->skb;
 264         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
 265
 266         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 267
 268         if (!ieee80211_is_data(hdr->frame_control))
 269                 return RX_CONTINUE;
 270
 271         if (!rx->sta || skb->len - hdrlen < 12)
 272                 return RX_DROP_UNUSABLE;
 273
 274         /* it may be possible to optimize this a bit more */
 275         if (skb_linearize(rx->skb))
 276                 return RX_DROP_UNUSABLE;
 277         hdr = (void *)skb->data;
 278
 279         /*
 280          * Let TKIP code verify IV, but skip decryption.
 281          * In the case where hardware checks the IV as well,
 282          * we don't even get here, see ieee80211_rx_h_decrypt()
 283          */
 284         if (status->flag & RX_FLAG_DECRYPTED)
 285                 hwaccel = 1;
 286
 287         res = ieee80211_tkip_decrypt_data(rx->local->wep_rx_tfm,
 288                                           key, skb->data + hdrlen,
 289                                           skb->len - hdrlen, rx->sta->sta.addr,
 290                                           hdr->addr1, hwaccel, rx->security_idx,
 291                                           &rx->tkip_iv32,
 292                                           &rx->tkip_iv16);
 293         if (res != TKIP_DECRYPT_OK)
 294                 return RX_DROP_UNUSABLE;
 295
 296         /* Trim ICV */
 297         if (!(status->flag & RX_FLAG_ICV_STRIPPED))
 298                 skb_trim(skb, skb->len - IEEE80211_TKIP_ICV_LEN);
 299
 300         /* Remove IV */
 301         memmove(skb->data + IEEE80211_TKIP_IV_LEN, skb->data, hdrlen);
 302         skb_pull(skb, IEEE80211_TKIP_IV_LEN);
 303
 304         return RX_CONTINUE;
 305 }
 306
 307
 308 static void ccmp_special_blocks(struct sk_buff *skb, u8 *pn, u8 *b_0, u8 *aad)
 309 {
 310         __le16 mask_fc;
 311         int a4_included, mgmt;
 312         u8 qos_tid;
 313         u16 len_a;
 314         unsigned int hdrlen;
 315         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 316
 317         /*
 318          * Mask FC: zero subtype b4 b5 b6 (if not mgmt)
 319          * Retry, PwrMgt, MoreData; set Protected
 320          */
 321         mgmt = ieee80211_is_mgmt(hdr->frame_control);
 322         mask_fc = hdr->frame_control;
 323         mask_fc &= ~cpu_to_le16(IEEE80211_FCTL_RETRY |
 324                                 IEEE80211_FCTL_PM | IEEE80211_FCTL_MOREDATA);
 325         if (!mgmt)
 326                 mask_fc &= ~cpu_to_le16(0x0070);
 327         mask_fc |= cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 328
 329         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 330         len_a = hdrlen - 2;
 331         a4_included = ieee80211_has_a4(hdr->frame_control);
 332
 333         if (ieee80211_is_data_qos(hdr->frame_control))
 334                 qos_tid = *ieee80211_get_qos_ctl(hdr) & IEEE80211_QOS_CTL_TID_MASK;
 335         else
 336                 qos_tid = 0;
 337
 338         /* In CCM, the initial vectors (IV) used for CTR mode encryption and CBC
 339          * mode authentication are not allowed to collide, yet both are derived
 340          * from this vector b_0. We only set L := 1 here to indicate that the
 341          * data size can be represented in (L+1) bytes. The CCM layer will take
 342          * care of storing the data length in the top (L+1) bytes and setting
 343          * and clearing the other bits as is required to derive the two IVs.
 344          */
 345         b_0[0] = 0x1;
 346
 347         /* Nonce: Nonce Flags | A2 | PN
 348          * Nonce Flags: Priority (b0..b3) | Management (b4) | Reserved (b5..b7)
 349          */
 350         b_0[1] = qos_tid | (mgmt << 4);
 351         memcpy(&b_0[2], hdr->addr2, ETH_ALEN);
 352         memcpy(&b_0[8], pn, IEEE80211_CCMP_PN_LEN);
 353
 354         /* AAD (extra authenticate-only data) / masked 802.11 header
 355          * FC | A1 | A2 | A3 | SC | [A4] | [QC] */
 356         put_unaligned_be16(len_a, &aad[0]);
 357         put_unaligned(mask_fc, (__le16 *)&aad[2]);
 358         memcpy(&aad[4], &hdr->addr1, 3 * ETH_ALEN);
 359
 360         /* Mask Seq#, leave Frag# */
 361         aad[22] = *((u8 *) &hdr->seq_ctrl) & 0x0f;
 362         aad[23] = 0;
 363
 364         if (a4_included) {
 365                 memcpy(&aad[24], hdr->addr4, ETH_ALEN);
 366                 aad[30] = qos_tid;
 367                 aad[31] = 0;
 368         } else {
 369                 memset(&aad[24], 0, ETH_ALEN + IEEE80211_QOS_CTL_LEN);
 370                 aad[24] = qos_tid;
 371         }
 372 }
 373
 374
 375 static inline void ccmp_pn2hdr(u8 *hdr, u8 *pn, int key_id)
 376 {
 377         hdr[0] = pn[5];
 378         hdr[1] = pn[4];
 379         hdr[2] = 0;
 380         hdr[3] = 0x20 | (key_id << 6);
 381         hdr[4] = pn[3];
 382         hdr[5] = pn[2];
 383         hdr[6] = pn[1];
 384         hdr[7] = pn[0];
 385 }
 386
 387
 388 static inline void ccmp_hdr2pn(u8 *pn, u8 *hdr)
 389 {
 390         pn[0] = hdr[7];
 391         pn[1] = hdr[6];
 392         pn[2] = hdr[5];
 393         pn[3] = hdr[4];
 394         pn[4] = hdr[1];
 395         pn[5] = hdr[0];
 396 }
 397
 398
 399 static int ccmp_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb,
 400                             unsigned int mic_len)
 401 {
 402         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 403         struct ieee80211_key *key = tx->key;
 404         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 405         int hdrlen, len, tail;
 406         u8 *pos;
 407         u8 pn[6];
 408         u64 pn64;
 409         u8 aad[CCM_AAD_LEN];
 410         u8 b_0[AES_BLOCK_SIZE];
 411
 412         if (info->control.hw_key &&
 413             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV) &&
 414             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE) &&
 415             !((info->control.hw_key->flags &
 416                IEEE80211_KEY_FLAG_GENERATE_IV_MGMT) &&
 417               ieee80211_is_mgmt(hdr->frame_control))) {
 418                 /*
 419                  * hwaccel has no need for preallocated room for CCMP
 420                  * header or MIC fields
 421                  */
 422                 return 0;
 423         }
 424
 425         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 426         len = skb->len - hdrlen;
 427
 428         if (info->control.hw_key)
 429                 tail = 0;
 430         else
 431                 tail = mic_len;
 432
 433         if (WARN_ON(skb_tailroom(skb) < tail ||
 434                     skb_headroom(skb) < IEEE80211_CCMP_HDR_LEN))
 435                 return -1;
 436
 437         pos = skb_push(skb, IEEE80211_CCMP_HDR_LEN);
 438         memmove(pos, pos + IEEE80211_CCMP_HDR_LEN, hdrlen);
 439
 440         /* the HW only needs room for the IV, but not the actual IV */
 441         if (info->control.hw_key &&
 442             (info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE))
 443                 return 0;
 444
 445         hdr = (struct ieee80211_hdr *) pos;
 446         pos += hdrlen;
 447
 448         pn64 = atomic64_inc_return(&key->conf.tx_pn);
 449
 450         pn[5] = pn64;
 451         pn[4] = pn64 >> 8;
 452         pn[3] = pn64 >> 16;
 453         pn[2] = pn64 >> 24;
 454         pn[1] = pn64 >> 32;
 455         pn[0] = pn64 >> 40;
 456
 457         ccmp_pn2hdr(pos, pn, key->conf.keyidx);
 458
 459         /* hwaccel - with software CCMP header */
 460         if (info->control.hw_key)
 461                 return 0;
 462
 463         pos += IEEE80211_CCMP_HDR_LEN;
 464         ccmp_special_blocks(skb, pn, b_0, aad);
 465         return ieee80211_aes_ccm_encrypt(key->u.ccmp.tfm, b_0, aad, pos, len,
 466                                          skb_put(skb, mic_len), mic_len);
 467 }
 468
 469
 470 ieee80211_tx_result
 471 ieee80211_crypto_ccmp_encrypt(struct ieee80211_tx_data *tx,
 472                               unsigned int mic_len)
 473 {
 474         struct sk_buff *skb;
 475
 476         ieee80211_tx_set_protected(tx);
 477
 478         skb_queue_walk(&tx->skbs, skb) {
 479                 if (ccmp_encrypt_skb(tx, skb, mic_len) < 0)
 480                         return TX_DROP;
 481         }
 482
 483         return TX_CONTINUE;
 484 }
 485
 486
 487 ieee80211_rx_result
 488 ieee80211_crypto_ccmp_decrypt(struct ieee80211_rx_data *rx,
 489                               unsigned int mic_len)
 490 {
 491         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
 492         int hdrlen;
 493         struct ieee80211_key *key = rx->key;
 494         struct sk_buff *skb = rx->skb;
 495         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
 496         u8 pn[IEEE80211_CCMP_PN_LEN];
 497         int data_len;
 498         int queue;
 499
 500         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 501
 502         if (!ieee80211_is_data(hdr->frame_control) &&
 503             !ieee80211_is_robust_mgmt_frame(skb))
 504                 return RX_CONTINUE;
 505
 506         if (status->flag & RX_FLAG_DECRYPTED) {
 507                 if (!pskb_may_pull(rx->skb, hdrlen + IEEE80211_CCMP_HDR_LEN))
 508                         return RX_DROP_UNUSABLE;
 509                 if (status->flag & RX_FLAG_MIC_STRIPPED)
 510                         mic_len = 0;
 511         } else {
 512                 if (skb_linearize(rx->skb))
 513                         return RX_DROP_UNUSABLE;
 514         }
 515
 516         data_len = skb->len - hdrlen - IEEE80211_CCMP_HDR_LEN - mic_len;
 517         if (!rx->sta || data_len < 0)
 518                 return RX_DROP_UNUSABLE;
 519
 520         if (!(status->flag & RX_FLAG_PN_VALIDATED)) {
 521                 int res;
 522
 523                 ccmp_hdr2pn(pn, skb->data + hdrlen);
 524
 525                 queue = rx->security_idx;
 526
 527                 res = memcmp(pn, key->u.ccmp.rx_pn[queue],
 528                              IEEE80211_CCMP_PN_LEN);
 529                 if (res < 0 ||
 530                     (!res && !(status->flag & RX_FLAG_ALLOW_SAME_PN))) {
 531                         key->u.ccmp.replays++;
 532                         return RX_DROP_UNUSABLE;
 533                 }
 534
 535                 if (!(status->flag & RX_FLAG_DECRYPTED)) {
 536                         u8 aad[2 * AES_BLOCK_SIZE];
 537                         u8 b_0[AES_BLOCK_SIZE];
 538                         /* hardware didn't decrypt/verify MIC */
 539                         ccmp_special_blocks(skb, pn, b_0, aad);
 540
 541                         if (ieee80211_aes_ccm_decrypt(
 542                                     key->u.ccmp.tfm, b_0, aad,
 543                                     skb->data + hdrlen + IEEE80211_CCMP_HDR_LEN,
 544                                     data_len,
 545                                     skb->data + skb->len - mic_len, mic_len))
 546                                 return RX_DROP_UNUSABLE;
 547                 }
 548
 549                 memcpy(key->u.ccmp.rx_pn[queue], pn, IEEE80211_CCMP_PN_LEN);
 550         }
 551
 552         /* Remove CCMP header and MIC */
 553         if (pskb_trim(skb, skb->len - mic_len))
 554                 return RX_DROP_UNUSABLE;
 555         memmove(skb->data + IEEE80211_CCMP_HDR_LEN, skb->data, hdrlen);
 556         skb_pull(skb, IEEE80211_CCMP_HDR_LEN);
 557
 558         return RX_CONTINUE;
 559 }
 560
 561 static void gcmp_special_blocks(struct sk_buff *skb, u8 *pn, u8 *j_0, u8 *aad)
 562 {
 563         __le16 mask_fc;
 564         u8 qos_tid;
 565         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 566
 567         memcpy(j_0, hdr->addr2, ETH_ALEN);
 568         memcpy(&j_0[ETH_ALEN], pn, IEEE80211_GCMP_PN_LEN);
 569         j_0[13] = 0;
 570         j_0[14] = 0;
 571         j_0[AES_BLOCK_SIZE - 1] = 0x01;
 572
 573         /* AAD (extra authenticate-only data) / masked 802.11 header
 574          * FC | A1 | A2 | A3 | SC | [A4] | [QC]
 575          */
 576         put_unaligned_be16(ieee80211_hdrlen(hdr->frame_control) - 2, &aad[0]);
 577         /* Mask FC: zero subtype b4 b5 b6 (if not mgmt)
 578          * Retry, PwrMgt, MoreData; set Protected
 579          */
 580         mask_fc = hdr->frame_control;
 581         mask_fc &= ~cpu_to_le16(IEEE80211_FCTL_RETRY |
 582                                 IEEE80211_FCTL_PM | IEEE80211_FCTL_MOREDATA);
 583         if (!ieee80211_is_mgmt(hdr->frame_control))
 584                 mask_fc &= ~cpu_to_le16(0x0070);
 585         mask_fc |= cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 586
 587         put_unaligned(mask_fc, (__le16 *)&aad[2]);
 588         memcpy(&aad[4], &hdr->addr1, 3 * ETH_ALEN);
 589
 590         /* Mask Seq#, leave Frag# */
 591         aad[22] = *((u8 *)&hdr->seq_ctrl) & 0x0f;
 592         aad[23] = 0;
 593
 594         if (ieee80211_is_data_qos(hdr->frame_control))
 595                 qos_tid = *ieee80211_get_qos_ctl(hdr) &
 596                         IEEE80211_QOS_CTL_TID_MASK;
 597         else
 598                 qos_tid = 0;
 599
 600         if (ieee80211_has_a4(hdr->frame_control)) {
 601                 memcpy(&aad[24], hdr->addr4, ETH_ALEN);
 602                 aad[30] = qos_tid;
 603                 aad[31] = 0;
 604         } else {
 605                 memset(&aad[24], 0, ETH_ALEN + IEEE80211_QOS_CTL_LEN);
 606                 aad[24] = qos_tid;
 607         }
 608 }
 609
 610 static inline void gcmp_pn2hdr(u8 *hdr, const u8 *pn, int key_id)
 611 {
 612         hdr[0] = pn[5];
 613         hdr[1] = pn[4];
 614         hdr[2] = 0;
 615         hdr[3] = 0x20 | (key_id << 6);
 616         hdr[4] = pn[3];
 617         hdr[5] = pn[2];
 618         hdr[6] = pn[1];
 619         hdr[7] = pn[0];
 620 }
 621
 622 static inline void gcmp_hdr2pn(u8 *pn, const u8 *hdr)
 623 {
 624         pn[0] = hdr[7];
 625         pn[1] = hdr[6];
 626         pn[2] = hdr[5];
 627         pn[3] = hdr[4];
 628         pn[4] = hdr[1];
 629         pn[5] = hdr[0];
 630 }
 631
 632 static int gcmp_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
 633 {
 634         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 635         struct ieee80211_key *key = tx->key;
 636         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 637         int hdrlen, len, tail;
 638         u8 *pos;
 639         u8 pn[6];
 640         u64 pn64;
 641         u8 aad[GCM_AAD_LEN];
 642         u8 j_0[AES_BLOCK_SIZE];
 643
 644         if (info->control.hw_key &&
 645             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV) &&
 646             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE) &&
 647             !((info->control.hw_key->flags &
 648                IEEE80211_KEY_FLAG_GENERATE_IV_MGMT) &&
 649               ieee80211_is_mgmt(hdr->frame_control))) {
 650                 /* hwaccel has no need for preallocated room for GCMP
 651                  * header or MIC fields
 652                  */
 653                 return 0;
 654         }
 655
 656         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 657         len = skb->len - hdrlen;
 658
 659         if (info->control.hw_key)
 660                 tail = 0;
 661         else
 662                 tail = IEEE80211_GCMP_MIC_LEN;
 663
 664         if (WARN_ON(skb_tailroom(skb) < tail ||
 665                     skb_headroom(skb) < IEEE80211_GCMP_HDR_LEN))
 666                 return -1;
 667
 668         pos = skb_push(skb, IEEE80211_GCMP_HDR_LEN);
 669         memmove(pos, pos + IEEE80211_GCMP_HDR_LEN, hdrlen);
 670         skb_set_network_header(skb, skb_network_offset(skb) +
 671                                     IEEE80211_GCMP_HDR_LEN);
 672
 673         /* the HW only needs room for the IV, but not the actual IV */
 674         if (info->control.hw_key &&
 675             (info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE))
 676                 return 0;
 677
 678         hdr = (struct ieee80211_hdr *)pos;
 679         pos += hdrlen;
 680
 681         pn64 = atomic64_inc_return(&key->conf.tx_pn);
 682
 683         pn[5] = pn64;
 684         pn[4] = pn64 >> 8;
 685         pn[3] = pn64 >> 16;
 686         pn[2] = pn64 >> 24;
 687         pn[1] = pn64 >> 32;
 688         pn[0] = pn64 >> 40;
 689
 690         gcmp_pn2hdr(pos, pn, key->conf.keyidx);
 691
 692         /* hwaccel - with software GCMP header */
 693         if (info->control.hw_key)
 694                 return 0;
 695
 696         pos += IEEE80211_GCMP_HDR_LEN;
 697         gcmp_special_blocks(skb, pn, j_0, aad);
 698         return ieee80211_aes_gcm_encrypt(key->u.gcmp.tfm, j_0, aad, pos, len,
 699                                          skb_put(skb, IEEE80211_GCMP_MIC_LEN));
 700 }
 701
 702 ieee80211_tx_result
 703 ieee80211_crypto_gcmp_encrypt(struct ieee80211_tx_data *tx)
 704 {
 705         struct sk_buff *skb;
 706
 707         ieee80211_tx_set_protected(tx);
 708
 709         skb_queue_walk(&tx->skbs, skb) {
 710                 if (gcmp_encrypt_skb(tx, skb) < 0)
 711                         return TX_DROP;
 712         }
 713
 714         return TX_CONTINUE;
 715 }
 716
 717 ieee80211_rx_result
 718 ieee80211_crypto_gcmp_decrypt(struct ieee80211_rx_data *rx)
 719 {
 720         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
 721         int hdrlen;
 722         struct ieee80211_key *key = rx->key;
 723         struct sk_buff *skb = rx->skb;
 724         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
 725         u8 pn[IEEE80211_GCMP_PN_LEN];
 726         int data_len, queue, mic_len = IEEE80211_GCMP_MIC_LEN;
 727
 728         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 729
 730         if (!ieee80211_is_data(hdr->frame_control) &&
 731             !ieee80211_is_robust_mgmt_frame(skb))
 732                 return RX_CONTINUE;
 733
 734         if (status->flag & RX_FLAG_DECRYPTED) {
 735                 if (!pskb_may_pull(rx->skb, hdrlen + IEEE80211_GCMP_HDR_LEN))
 736                         return RX_DROP_UNUSABLE;
 737                 if (status->flag & RX_FLAG_MIC_STRIPPED)
 738                         mic_len = 0;
 739         } else {
 740                 if (skb_linearize(rx->skb))
 741                         return RX_DROP_UNUSABLE;
 742         }
 743
 744         data_len = skb->len - hdrlen - IEEE80211_GCMP_HDR_LEN - mic_len;
 745         if (!rx->sta || data_len < 0)
 746                 return RX_DROP_UNUSABLE;
 747
 748         if (!(status->flag & RX_FLAG_PN_VALIDATED)) {
 749                 int res;
 750
 751                 gcmp_hdr2pn(pn, skb->data + hdrlen);
 752
 753                 queue = rx->security_idx;
 754
 755                 res = memcmp(pn, key->u.gcmp.rx_pn[queue],
 756                              IEEE80211_GCMP_PN_LEN);
 757                 if (res < 0 ||
 758                     (!res && !(status->flag & RX_FLAG_ALLOW_SAME_PN))) {
 759                         key->u.gcmp.replays++;
 760                         return RX_DROP_UNUSABLE;
 761                 }
 762
 763                 if (!(status->flag & RX_FLAG_DECRYPTED)) {
 764                         u8 aad[2 * AES_BLOCK_SIZE];
 765                         u8 j_0[AES_BLOCK_SIZE];
 766                         /* hardware didn't decrypt/verify MIC */
 767                         gcmp_special_blocks(skb, pn, j_0, aad);
 768
 769                         if (ieee80211_aes_gcm_decrypt(
 770                                     key->u.gcmp.tfm, j_0, aad,
 771                                     skb->data + hdrlen + IEEE80211_GCMP_HDR_LEN,
 772                                     data_len,
 773                                     skb->data + skb->len -
 774                                     IEEE80211_GCMP_MIC_LEN))
 775                                 return RX_DROP_UNUSABLE;
 776                 }
 777
 778                 memcpy(key->u.gcmp.rx_pn[queue], pn, IEEE80211_GCMP_PN_LEN);
 779         }
 780
 781         /* Remove GCMP header and MIC */
 782         if (pskb_trim(skb, skb->len - mic_len))
 783                 return RX_DROP_UNUSABLE;
 784         memmove(skb->data + IEEE80211_GCMP_HDR_LEN, skb->data, hdrlen);
 785         skb_pull(skb, IEEE80211_GCMP_HDR_LEN);
 786
 787         return RX_CONTINUE;
 788 }
 789
 790 static ieee80211_tx_result
 791 ieee80211_crypto_cs_encrypt(struct ieee80211_tx_data *tx,
 792                             struct sk_buff *skb)
 793 {
 794         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 795         struct ieee80211_key *key = tx->key;
 796         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 797         int hdrlen;
 798         u8 *pos, iv_len = key->conf.iv_len;
 799
 800         if (info->control.hw_key &&
 801             !(info->control.hw_key->flags & IEEE80211_KEY_FLAG_PUT_IV_SPACE)) {
 802                 /* hwaccel has no need for preallocated head room */
 803                 return TX_CONTINUE;
 804         }
 805
 806         if (unlikely(skb_headroom(skb) < iv_len &&
 807                      pskb_expand_head(skb, iv_len, 0, GFP_ATOMIC)))
 808                 return TX_DROP;
 809
 810         hdrlen = ieee80211_hdrlen(hdr->frame_control);
 811
 812         pos = skb_push(skb, iv_len);
 813         memmove(pos, pos + iv_len, hdrlen);
 814
 815         return TX_CONTINUE;
 816 }
 817
 818 static inline int ieee80211_crypto_cs_pn_compare(u8 *pn1, u8 *pn2, int len)
 819 {
 820         int i;
 821
 822         /* pn is little endian */
 823         for (i = len - 1; i >= 0; i--) {
 824                 if (pn1[i] < pn2[i])
 825                         return -1;
 826                 else if (pn1[i] > pn2[i])
 827                         return 1;
 828         }
 829
 830         return 0;
 831 }
 832
 833 static ieee80211_rx_result
 834 ieee80211_crypto_cs_decrypt(struct ieee80211_rx_data *rx)
 835 {
 836         struct ieee80211_key *key = rx->key;
 837         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
 838         const struct ieee80211_cipher_scheme *cs = NULL;
 839         int hdrlen = ieee80211_hdrlen(hdr->frame_control);
 840         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(rx->skb);
 841         int data_len;
 842         u8 *rx_pn;
 843         u8 *skb_pn;
 844         u8 qos_tid;
 845
 846         if (!rx->sta || !rx->sta->cipher_scheme ||
 847             !(status->flag & RX_FLAG_DECRYPTED))
 848                 return RX_DROP_UNUSABLE;
 849
 850         if (!ieee80211_is_data(hdr->frame_control))
 851                 return RX_CONTINUE;
 852
 853         cs = rx->sta->cipher_scheme;
 854
 855         data_len = rx->skb->len - hdrlen - cs->hdr_len;
 856
 857         if (data_len < 0)
 858                 return RX_DROP_UNUSABLE;
 859
 860         if (ieee80211_is_data_qos(hdr->frame_control))
 861                 qos_tid = *ieee80211_get_qos_ctl(hdr) &
 862                                 IEEE80211_QOS_CTL_TID_MASK;
 863         else
 864                 qos_tid = 0;
 865
 866         if (skb_linearize(rx->skb))
 867                 return RX_DROP_UNUSABLE;
 868
 869         hdr = (struct ieee80211_hdr *)rx->skb->data;
 870
 871         rx_pn = key->u.gen.rx_pn[qos_tid];
 872         skb_pn = rx->skb->data + hdrlen + cs->pn_off;
 873
 874         if (ieee80211_crypto_cs_pn_compare(skb_pn, rx_pn, cs->pn_len) <= 0)
 875                 return RX_DROP_UNUSABLE;
 876
 877         memcpy(rx_pn, skb_pn, cs->pn_len);
 878
 879         /* remove security header and MIC */
 880         if (pskb_trim(rx->skb, rx->skb->len - cs->mic_len))
 881                 return RX_DROP_UNUSABLE;
 882
 883         memmove(rx->skb->data + cs->hdr_len, rx->skb->data, hdrlen);
 884         skb_pull(rx->skb, cs->hdr_len);
 885
 886         return RX_CONTINUE;
 887 }
 888
 889 static void bip_aad(struct sk_buff *skb, u8 *aad)
 890 {
 891         __le16 mask_fc;
 892         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 893
 894         /* BIP AAD: FC(masked) || A1 || A2 || A3 */
 895
 896         /* FC type/subtype */
 897         /* Mask FC Retry, PwrMgt, MoreData flags to zero */
 898         mask_fc = hdr->frame_control;
 899         mask_fc &= ~cpu_to_le16(IEEE80211_FCTL_RETRY | IEEE80211_FCTL_PM |
 900                                 IEEE80211_FCTL_MOREDATA);
 901         put_unaligned(mask_fc, (__le16 *) &aad[0]);
 902         /* A1 || A2 || A3 */
 903         memcpy(aad + 2, &hdr->addr1, 3 * ETH_ALEN);
 904 }
 905
 906
 907 static inline void bip_ipn_set64(u8 *d, u64 pn)
 908 {
 909         *d++ = pn;
 910         *d++ = pn >> 8;
 911         *d++ = pn >> 16;
 912         *d++ = pn >> 24;
 913         *d++ = pn >> 32;
 914         *d = pn >> 40;
 915 }
 916
 917 static inline void bip_ipn_swap(u8 *d, const u8 *s)
 918 {
 919         *d++ = s[5];
 920         *d++ = s[4];
 921         *d++ = s[3];
 922         *d++ = s[2];
 923         *d++ = s[1];
 924         *d = s[0];
 925 }
 926
 927
 928 ieee80211_tx_result
 929 ieee80211_crypto_aes_cmac_encrypt(struct ieee80211_tx_data *tx)
 930 {
 931         struct sk_buff *skb;
 932         struct ieee80211_tx_info *info;
 933         struct ieee80211_key *key = tx->key;
 934         struct ieee80211_mmie *mmie;
 935         u8 aad[20];
 936         u64 pn64;
 937
 938         if (WARN_ON(skb_queue_len(&tx->skbs) != 1))
 939                 return TX_DROP;
 940
 941         skb = skb_peek(&tx->skbs);
 942
 943         info = IEEE80211_SKB_CB(skb);
 944
 945         if (info->control.hw_key)
 946                 return TX_CONTINUE;
 947
 948         if (WARN_ON(skb_tailroom(skb) < sizeof(*mmie)))
 949                 return TX_DROP;
 950
 951         mmie = (struct ieee80211_mmie *) skb_put(skb, sizeof(*mmie));
 952         mmie->element_id = WLAN_EID_MMIE;
 953         mmie->length = sizeof(*mmie) - 2;
 954         mmie->key_id = cpu_to_le16(key->conf.keyidx);
 955
 956         /* PN = PN + 1 */
 957         pn64 = atomic64_inc_return(&key->conf.tx_pn);
 958
 959         bip_ipn_set64(mmie->sequence_number, pn64);
 960
 961         bip_aad(skb, aad);
 962
 963         /*
 964          * MIC = AES-128-CMAC(IGTK, AAD || Management Frame Body || MMIE, 64)
 965          */
 966         ieee80211_aes_cmac(key->u.aes_cmac.tfm, aad,
 967                            skb->data + 24, skb->len - 24, mmie->mic);
 968
 969         return TX_CONTINUE;
 970 }
 971
 972 ieee80211_tx_result
 973 ieee80211_crypto_aes_cmac_256_encrypt(struct ieee80211_tx_data *tx)
 974 {
 975         struct sk_buff *skb;
 976         struct ieee80211_tx_info *info;
 977         struct ieee80211_key *key = tx->key;
 978         struct ieee80211_mmie_16 *mmie;
 979         u8 aad[20];
 980         u64 pn64;
 981
 982         if (WARN_ON(skb_queue_len(&tx->skbs) != 1))
 983                 return TX_DROP;
 984
 985         skb = skb_peek(&tx->skbs);
 986
 987         info = IEEE80211_SKB_CB(skb);
 988
 989         if (info->control.hw_key)
 990                 return TX_CONTINUE;
 991
 992         if (WARN_ON(skb_tailroom(skb) < sizeof(*mmie)))
 993                 return TX_DROP;
 994
 995         mmie = (struct ieee80211_mmie_16 *)skb_put(skb, sizeof(*mmie));
 996         mmie->element_id = WLAN_EID_MMIE;
 997         mmie->length = sizeof(*mmie) - 2;
 998         mmie->key_id = cpu_to_le16(key->conf.keyidx);
 999
1000         /* PN = PN + 1 */
1001         pn64 = atomic64_inc_return(&key->conf.tx_pn);
1002
1003         bip_ipn_set64(mmie->sequence_number, pn64);
1004
1005         bip_aad(skb, aad);
1006
1007         /* MIC = AES-256-CMAC(IGTK, AAD || Management Frame Body || MMIE, 128)
1008          */
1009         ieee80211_aes_cmac_256(key->u.aes_cmac.tfm, aad,
1010                                skb->data + 24, skb->len - 24, mmie->mic);
1011
1012         return TX_CONTINUE;
1013 }
1014
1015 ieee80211_rx_result
1016 ieee80211_crypto_aes_cmac_decrypt(struct ieee80211_rx_data *rx)
1017 {
1018         struct sk_buff *skb = rx->skb;
1019         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
1020         struct ieee80211_key *key = rx->key;
1021         struct ieee80211_mmie *mmie;
1022         u8 aad[20], mic[8], ipn[6];
1023         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
1024
1025         if (!ieee80211_is_mgmt(hdr->frame_control))
1026                 return RX_CONTINUE;
1027
1028         /* management frames are already linear */
1029
1030         if (skb->len < 24 + sizeof(*mmie))
1031                 return RX_DROP_UNUSABLE;
1032
1033         mmie = (struct ieee80211_mmie *)
1034                 (skb->data + skb->len - sizeof(*mmie));
1035         if (mmie->element_id != WLAN_EID_MMIE ||
1036             mmie->length != sizeof(*mmie) - 2)
1037                 return RX_DROP_UNUSABLE; /* Invalid MMIE */
1038
1039         bip_ipn_swap(ipn, mmie->sequence_number);
1040
1041         if (memcmp(ipn, key->u.aes_cmac.rx_pn, 6) <= 0) {
1042                 key->u.aes_cmac.replays++;
1043                 return RX_DROP_UNUSABLE;
1044         }
1045
1046         if (!(status->flag & RX_FLAG_DECRYPTED)) {
1047                 /* hardware didn't decrypt/verify MIC */
1048                 bip_aad(skb, aad);
1049                 ieee80211_aes_cmac(key->u.aes_cmac.tfm, aad,
1050                                    skb->data + 24, skb->len - 24, mic);
1051                 if (memcmp(mic, mmie->mic, sizeof(mmie->mic)) != 0) {
1052                         key->u.aes_cmac.icverrors++;
1053                         return RX_DROP_UNUSABLE;
1054                 }
1055         }
1056
1057         memcpy(key->u.aes_cmac.rx_pn, ipn, 6);
1058
1059         /* Remove MMIE */
1060         skb_trim(skb, skb->len - sizeof(*mmie));
1061
1062         return RX_CONTINUE;
1063 }
1064
1065 ieee80211_rx_result
1066 ieee80211_crypto_aes_cmac_256_decrypt(struct ieee80211_rx_data *rx)
1067 {
1068         struct sk_buff *skb = rx->skb;
1069         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
1070         struct ieee80211_key *key = rx->key;
1071         struct ieee80211_mmie_16 *mmie;
1072         u8 aad[20], mic[16], ipn[6];
1073         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
1074
1075         if (!ieee80211_is_mgmt(hdr->frame_control))
1076                 return RX_CONTINUE;
1077
1078         /* management frames are already linear */
1079
1080         if (skb->len < 24 + sizeof(*mmie))
1081                 return RX_DROP_UNUSABLE;
1082
1083         mmie = (struct ieee80211_mmie_16 *)
1084                 (skb->data + skb->len - sizeof(*mmie));
1085         if (mmie->element_id != WLAN_EID_MMIE ||
1086             mmie->length != sizeof(*mmie) - 2)
1087                 return RX_DROP_UNUSABLE; /* Invalid MMIE */
1088
1089         bip_ipn_swap(ipn, mmie->sequence_number);
1090
1091         if (memcmp(ipn, key->u.aes_cmac.rx_pn, 6) <= 0) {
1092                 key->u.aes_cmac.replays++;
1093                 return RX_DROP_UNUSABLE;
1094         }
1095
1096         if (!(status->flag & RX_FLAG_DECRYPTED)) {
1097                 /* hardware didn't decrypt/verify MIC */
1098                 bip_aad(skb, aad);
1099                 ieee80211_aes_cmac_256(key->u.aes_cmac.tfm, aad,
1100                                        skb->data + 24, skb->len - 24, mic);
1101                 if (memcmp(mic, mmie->mic, sizeof(mmie->mic)) != 0) {
1102                         key->u.aes_cmac.icverrors++;
1103                         return RX_DROP_UNUSABLE;
1104                 }
1105         }
1106
1107         memcpy(key->u.aes_cmac.rx_pn, ipn, 6);
1108
1109         /* Remove MMIE */
1110         skb_trim(skb, skb->len - sizeof(*mmie));
1111
1112         return RX_CONTINUE;
1113 }
1114
1115 ieee80211_tx_result
1116 ieee80211_crypto_aes_gmac_encrypt(struct ieee80211_tx_data *tx)
1117 {
1118         struct sk_buff *skb;
1119         struct ieee80211_tx_info *info;
1120         struct ieee80211_key *key = tx->key;
1121         struct ieee80211_mmie_16 *mmie;
1122         struct ieee80211_hdr *hdr;
1123         u8 aad[GMAC_AAD_LEN];
1124         u64 pn64;
1125         u8 nonce[GMAC_NONCE_LEN];
1126
1127         if (WARN_ON(skb_queue_len(&tx->skbs) != 1))
1128                 return TX_DROP;
1129
1130         skb = skb_peek(&tx->skbs);
1131
1132         info = IEEE80211_SKB_CB(skb);
1133
1134         if (info->control.hw_key)
1135                 return TX_CONTINUE;
1136
1137         if (WARN_ON(skb_tailroom(skb) < sizeof(*mmie)))
1138                 return TX_DROP;
1139
1140         mmie = (struct ieee80211_mmie_16 *)skb_put(skb, sizeof(*mmie));
1141         mmie->element_id = WLAN_EID_MMIE;
1142         mmie->length = sizeof(*mmie) - 2;
1143         mmie->key_id = cpu_to_le16(key->conf.keyidx);
1144
1145         /* PN = PN + 1 */
1146         pn64 = atomic64_inc_return(&key->conf.tx_pn);
1147
1148         bip_ipn_set64(mmie->sequence_number, pn64);
1149
1150         bip_aad(skb, aad);
1151
1152         hdr = (struct ieee80211_hdr *)skb->data;
1153         memcpy(nonce, hdr->addr2, ETH_ALEN);
1154         bip_ipn_swap(nonce + ETH_ALEN, mmie->sequence_number);
1155
1156         /* MIC = AES-GMAC(IGTK, AAD || Management Frame Body || MMIE, 128) */
1157         if (ieee80211_aes_gmac(key->u.aes_gmac.tfm, aad, nonce,
1158                                skb->data + 24, skb->len - 24, mmie->mic) < 0)
1159                 return TX_DROP;
1160
1161         return TX_CONTINUE;
1162 }
1163
1164 ieee80211_rx_result
1165 ieee80211_crypto_aes_gmac_decrypt(struct ieee80211_rx_data *rx)
1166 {
1167         struct sk_buff *skb = rx->skb;
1168         struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
1169         struct ieee80211_key *key = rx->key;
1170         struct ieee80211_mmie_16 *mmie;
1171         u8 aad[GMAC_AAD_LEN], mic[GMAC_MIC_LEN], ipn[6], nonce[GMAC_NONCE_LEN];
1172         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
1173
1174         if (!ieee80211_is_mgmt(hdr->frame_control))
1175                 return RX_CONTINUE;
1176
1177         /* management frames are already linear */
1178
1179         if (skb->len < 24 + sizeof(*mmie))
1180                 return RX_DROP_UNUSABLE;
1181
1182         mmie = (struct ieee80211_mmie_16 *)
1183                 (skb->data + skb->len - sizeof(*mmie));
1184         if (mmie->element_id != WLAN_EID_MMIE ||
1185             mmie->length != sizeof(*mmie) - 2)
1186                 return RX_DROP_UNUSABLE; /* Invalid MMIE */
1187
1188         bip_ipn_swap(ipn, mmie->sequence_number);
1189
1190         if (memcmp(ipn, key->u.aes_gmac.rx_pn, 6) <= 0) {
1191                 key->u.aes_gmac.replays++;
1192                 return RX_DROP_UNUSABLE;
1193         }
1194
1195         if (!(status->flag & RX_FLAG_DECRYPTED)) {
1196                 /* hardware didn't decrypt/verify MIC */
1197                 bip_aad(skb, aad);
1198
1199                 memcpy(nonce, hdr->addr2, ETH_ALEN);
1200                 memcpy(nonce + ETH_ALEN, ipn, 6);
1201
1202                 if (ieee80211_aes_gmac(key->u.aes_gmac.tfm, aad, nonce,
1203                                        skb->data + 24, skb->len - 24,
1204                                        mic) < 0 ||
1205                     memcmp(mic, mmie->mic, sizeof(mmie->mic)) != 0) {
1206                         key->u.aes_gmac.icverrors++;
1207                         return RX_DROP_UNUSABLE;
1208                 }
1209         }
1210
1211         memcpy(key->u.aes_gmac.rx_pn, ipn, 6);
1212
1213         /* Remove MMIE */
1214         skb_trim(skb, skb->len - sizeof(*mmie));
1215
1216         return RX_CONTINUE;
1217 }
1218
1219 ieee80211_tx_result
1220 ieee80211_crypto_hw_encrypt(struct ieee80211_tx_data *tx)
1221 {
1222         struct sk_buff *skb;
1223         struct ieee80211_tx_info *info = NULL;
1224         ieee80211_tx_result res;
1225
1226         skb_queue_walk(&tx->skbs, skb) {
1227                 info  = IEEE80211_SKB_CB(skb);
1228
1229                 /* handle hw-only algorithm */
1230                 if (!info->control.hw_key)
1231                         return TX_DROP;
1232
1233                 if (tx->key->flags & KEY_FLAG_CIPHER_SCHEME) {
1234                         res = ieee80211_crypto_cs_encrypt(tx, skb);
1235                         if (res != TX_CONTINUE)
1236                                 return res;
1237                 }
1238         }
1239
1240         ieee80211_tx_set_protected(tx);
1241
1242         return TX_CONTINUE;
1243 }
1244
1245 ieee80211_rx_result
1246 ieee80211_crypto_hw_decrypt(struct ieee80211_rx_data *rx)
1247 {
1248         if (rx->sta && rx->sta->cipher_scheme)
1249                 return ieee80211_crypto_cs_decrypt(rx);
1250
1251         return RX_DROP_UNUSABLE;
1252 }