net/mac80211/wpa.c

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