net/wireless/lib80211_crypt_tkip.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * lib80211 crypt: host-based TKIP encryption implementation for lib80211
   4  *
   5  * Copyright (c) 2003-2004, Jouni Malinen <j@w1.fi>
   6  * Copyright (c) 2008, John W. Linville <linville@tuxdriver.com>
   7  */
   8
   9 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  10
  11 #include <linux/err.h>
  12 #include <linux/fips.h>
  13 #include <linux/module.h>
  14 #include <linux/init.h>
  15 #include <linux/slab.h>
  16 #include <linux/random.h>
  17 #include <linux/scatterlist.h>
  18 #include <linux/skbuff.h>
  19 #include <linux/netdevice.h>
  20 #include <linux/mm.h>
  21 #include <linux/if_ether.h>
  22 #include <linux/if_arp.h>
  23 #include <asm/string.h>
  24
  25 #include <linux/wireless.h>
  26 #include <linux/ieee80211.h>
  27 #include <net/iw_handler.h>
  28
  29 #include <crypto/arc4.h>
  30 #include <crypto/hash.h>
  31 #include <linux/crypto.h>
  32 #include <linux/crc32.h>
  33
  34 #include <net/lib80211.h>
  35
  36 MODULE_AUTHOR("Jouni Malinen");
  37 MODULE_DESCRIPTION("lib80211 crypt: TKIP");
  38 MODULE_LICENSE("GPL");
  39
  40 #define TKIP_HDR_LEN 8
  41
  42 struct lib80211_tkip_data {
  43 #define TKIP_KEY_LEN 32
  44         u8 key[TKIP_KEY_LEN];
  45         int key_set;
  46
  47         u32 tx_iv32;
  48         u16 tx_iv16;
  49         u16 tx_ttak[5];
  50         int tx_phase1_done;
  51
  52         u32 rx_iv32;
  53         u16 rx_iv16;
  54         u16 rx_ttak[5];
  55         int rx_phase1_done;
  56         u32 rx_iv32_new;
  57         u16 rx_iv16_new;
  58
  59         u32 dot11RSNAStatsTKIPReplays;
  60         u32 dot11RSNAStatsTKIPICVErrors;
  61         u32 dot11RSNAStatsTKIPLocalMICFailures;
  62
  63         int key_idx;
  64
  65         struct arc4_ctx rx_ctx_arc4;
  66         struct arc4_ctx tx_ctx_arc4;
  67         struct crypto_shash *rx_tfm_michael;
  68         struct crypto_shash *tx_tfm_michael;
  69
  70         /* scratch buffers for virt_to_page() (crypto API) */
  71         u8 rx_hdr[16], tx_hdr[16];
  72
  73         unsigned long flags;
  74 };
  75
  76 static unsigned long lib80211_tkip_set_flags(unsigned long flags, void *priv)
  77 {
  78         struct lib80211_tkip_data *_priv = priv;
  79         unsigned long old_flags = _priv->flags;
  80         _priv->flags = flags;
  81         return old_flags;
  82 }
  83
  84 static unsigned long lib80211_tkip_get_flags(void *priv)
  85 {
  86         struct lib80211_tkip_data *_priv = priv;
  87         return _priv->flags;
  88 }
  89
  90 static void *lib80211_tkip_init(int key_idx)
  91 {
  92         struct lib80211_tkip_data *priv;
  93
  94         if (fips_enabled)
  95                 return NULL;
  96
  97         priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
  98         if (priv == NULL)
  99                 goto fail;
 100
 101         priv->key_idx = key_idx;
 102
 103         priv->tx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
 104         if (IS_ERR(priv->tx_tfm_michael)) {
 105                 priv->tx_tfm_michael = NULL;
 106                 goto fail;
 107         }
 108
 109         priv->rx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
 110         if (IS_ERR(priv->rx_tfm_michael)) {
 111                 priv->rx_tfm_michael = NULL;
 112                 goto fail;
 113         }
 114
 115         return priv;
 116
 117       fail:
 118         if (priv) {
 119                 crypto_free_shash(priv->tx_tfm_michael);
 120                 crypto_free_shash(priv->rx_tfm_michael);
 121                 kfree(priv);
 122         }
 123
 124         return NULL;
 125 }
 126
 127 static void lib80211_tkip_deinit(void *priv)
 128 {
 129         struct lib80211_tkip_data *_priv = priv;
 130         if (_priv) {
 131                 crypto_free_shash(_priv->tx_tfm_michael);
 132                 crypto_free_shash(_priv->rx_tfm_michael);
 133         }
 134         kzfree(priv);
 135 }
 136
 137 static inline u16 RotR1(u16 val)
 138 {
 139         return (val >> 1) | (val << 15);
 140 }
 141
 142 static inline u8 Lo8(u16 val)
 143 {
 144         return val & 0xff;
 145 }
 146
 147 static inline u8 Hi8(u16 val)
 148 {
 149         return val >> 8;
 150 }
 151
 152 static inline u16 Lo16(u32 val)
 153 {
 154         return val & 0xffff;
 155 }
 156
 157 static inline u16 Hi16(u32 val)
 158 {
 159         return val >> 16;
 160 }
 161
 162 static inline u16 Mk16(u8 hi, u8 lo)
 163 {
 164         return lo | (((u16) hi) << 8);
 165 }
 166
 167 static inline u16 Mk16_le(__le16 * v)
 168 {
 169         return le16_to_cpu(*v);
 170 }
 171
 172 static const u16 Sbox[256] = {
 173         0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
 174         0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
 175         0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
 176         0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
 177         0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
 178         0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
 179         0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
 180         0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
 181         0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
 182         0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
 183         0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
 184         0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
 185         0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
 186         0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
 187         0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
 188         0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
 189         0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
 190         0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
 191         0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
 192         0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
 193         0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
 194         0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
 195         0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
 196         0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
 197         0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
 198         0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
 199         0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
 200         0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
 201         0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
 202         0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
 203         0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
 204         0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
 205 };
 206
 207 static inline u16 _S_(u16 v)
 208 {
 209         u16 t = Sbox[Hi8(v)];
 210         return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
 211 }
 212
 213 #define PHASE1_LOOP_COUNT 8
 214
 215 static void tkip_mixing_phase1(u16 * TTAK, const u8 * TK, const u8 * TA,
 216                                u32 IV32)
 217 {
 218         int i, j;
 219
 220         /* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
 221         TTAK[0] = Lo16(IV32);
 222         TTAK[1] = Hi16(IV32);
 223         TTAK[2] = Mk16(TA[1], TA[0]);
 224         TTAK[3] = Mk16(TA[3], TA[2]);
 225         TTAK[4] = Mk16(TA[5], TA[4]);
 226
 227         for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
 228                 j = 2 * (i & 1);
 229                 TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
 230                 TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
 231                 TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
 232                 TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
 233                 TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
 234         }
 235 }
 236
 237 static void tkip_mixing_phase2(u8 * WEPSeed, const u8 * TK, const u16 * TTAK,
 238                                u16 IV16)
 239 {
 240         /* Make temporary area overlap WEP seed so that the final copy can be
 241          * avoided on little endian hosts. */
 242         u16 *PPK = (u16 *) & WEPSeed[4];
 243
 244         /* Step 1 - make copy of TTAK and bring in TSC */
 245         PPK[0] = TTAK[0];
 246         PPK[1] = TTAK[1];
 247         PPK[2] = TTAK[2];
 248         PPK[3] = TTAK[3];
 249         PPK[4] = TTAK[4];
 250         PPK[5] = TTAK[4] + IV16;
 251
 252         /* Step 2 - 96-bit bijective mixing using S-box */
 253         PPK[0] += _S_(PPK[5] ^ Mk16_le((__le16 *) & TK[0]));
 254         PPK[1] += _S_(PPK[0] ^ Mk16_le((__le16 *) & TK[2]));
 255         PPK[2] += _S_(PPK[1] ^ Mk16_le((__le16 *) & TK[4]));
 256         PPK[3] += _S_(PPK[2] ^ Mk16_le((__le16 *) & TK[6]));
 257         PPK[4] += _S_(PPK[3] ^ Mk16_le((__le16 *) & TK[8]));
 258         PPK[5] += _S_(PPK[4] ^ Mk16_le((__le16 *) & TK[10]));
 259
 260         PPK[0] += RotR1(PPK[5] ^ Mk16_le((__le16 *) & TK[12]));
 261         PPK[1] += RotR1(PPK[0] ^ Mk16_le((__le16 *) & TK[14]));
 262         PPK[2] += RotR1(PPK[1]);
 263         PPK[3] += RotR1(PPK[2]);
 264         PPK[4] += RotR1(PPK[3]);
 265         PPK[5] += RotR1(PPK[4]);
 266
 267         /* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
 268          * WEPSeed[0..2] is transmitted as WEP IV */
 269         WEPSeed[0] = Hi8(IV16);
 270         WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
 271         WEPSeed[2] = Lo8(IV16);
 272         WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((__le16 *) & TK[0])) >> 1);
 273
 274 #ifdef __BIG_ENDIAN
 275         {
 276                 int i;
 277                 for (i = 0; i < 6; i++)
 278                         PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 279         }
 280 #endif
 281 }
 282
 283 static int lib80211_tkip_hdr(struct sk_buff *skb, int hdr_len,
 284                               u8 * rc4key, int keylen, void *priv)
 285 {
 286         struct lib80211_tkip_data *tkey = priv;
 287         u8 *pos;
 288         struct ieee80211_hdr *hdr;
 289
 290         hdr = (struct ieee80211_hdr *)skb->data;
 291
 292         if (skb_headroom(skb) < TKIP_HDR_LEN || skb->len < hdr_len)
 293                 return -1;
 294
 295         if (rc4key == NULL || keylen < 16)
 296                 return -1;
 297
 298         if (!tkey->tx_phase1_done) {
 299                 tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
 300                                    tkey->tx_iv32);
 301                 tkey->tx_phase1_done = 1;
 302         }
 303         tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
 304
 305         pos = skb_push(skb, TKIP_HDR_LEN);
 306         memmove(pos, pos + TKIP_HDR_LEN, hdr_len);
 307         pos += hdr_len;
 308
 309         *pos++ = *rc4key;
 310         *pos++ = *(rc4key + 1);
 311         *pos++ = *(rc4key + 2);
 312         *pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */ ;
 313         *pos++ = tkey->tx_iv32 & 0xff;
 314         *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
 315         *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
 316         *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
 317
 318         tkey->tx_iv16++;
 319         if (tkey->tx_iv16 == 0) {
 320                 tkey->tx_phase1_done = 0;
 321                 tkey->tx_iv32++;
 322         }
 323
 324         return TKIP_HDR_LEN;
 325 }
 326
 327 static int lib80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 328 {
 329         struct lib80211_tkip_data *tkey = priv;
 330         int len;
 331         u8 rc4key[16], *pos, *icv;
 332         u32 crc;
 333
 334         if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
 335                 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 336                 net_dbg_ratelimited("TKIP countermeasures: dropped TX packet to %pM\n",
 337                                     hdr->addr1);
 338                 return -1;
 339         }
 340
 341         if (skb_tailroom(skb) < 4 || skb->len < hdr_len)
 342                 return -1;
 343
 344         len = skb->len - hdr_len;
 345         pos = skb->data + hdr_len;
 346
 347         if ((lib80211_tkip_hdr(skb, hdr_len, rc4key, 16, priv)) < 0)
 348                 return -1;
 349
 350         crc = ~crc32_le(~0, pos, len);
 351         icv = skb_put(skb, 4);
 352         icv[0] = crc;
 353         icv[1] = crc >> 8;
 354         icv[2] = crc >> 16;
 355         icv[3] = crc >> 24;
 356
 357         arc4_setkey(&tkey->tx_ctx_arc4, rc4key, 16);
 358         arc4_crypt(&tkey->tx_ctx_arc4, pos, pos, len + 4);
 359
 360         return 0;
 361 }
 362
 363 /*
 364  * deal with seq counter wrapping correctly.
 365  * refer to timer_after() for jiffies wrapping handling
 366  */
 367 static inline int tkip_replay_check(u32 iv32_n, u16 iv16_n,
 368                                     u32 iv32_o, u16 iv16_o)
 369 {
 370         if ((s32)iv32_n - (s32)iv32_o < 0 ||
 371             (iv32_n == iv32_o && iv16_n <= iv16_o))
 372                 return 1;
 373         return 0;
 374 }
 375
 376 static int lib80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 377 {
 378         struct lib80211_tkip_data *tkey = priv;
 379         u8 rc4key[16];
 380         u8 keyidx, *pos;
 381         u32 iv32;
 382         u16 iv16;
 383         struct ieee80211_hdr *hdr;
 384         u8 icv[4];
 385         u32 crc;
 386         int plen;
 387
 388         hdr = (struct ieee80211_hdr *)skb->data;
 389
 390         if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
 391                 net_dbg_ratelimited("TKIP countermeasures: dropped received packet from %pM\n",
 392                                     hdr->addr2);
 393                 return -1;
 394         }
 395
 396         if (skb->len < hdr_len + TKIP_HDR_LEN + 4)
 397                 return -1;
 398
 399         pos = skb->data + hdr_len;
 400         keyidx = pos[3];
 401         if (!(keyidx & (1 << 5))) {
 402                 net_dbg_ratelimited("TKIP: received packet without ExtIV flag from %pM\n",
 403                                     hdr->addr2);
 404                 return -2;
 405         }
 406         keyidx >>= 6;
 407         if (tkey->key_idx != keyidx) {
 408                 net_dbg_ratelimited("TKIP: RX tkey->key_idx=%d frame keyidx=%d\n",
 409                                     tkey->key_idx, keyidx);
 410                 return -6;
 411         }
 412         if (!tkey->key_set) {
 413                 net_dbg_ratelimited("TKIP: received packet from %pM with keyid=%d that does not have a configured key\n",
 414                                     hdr->addr2, keyidx);
 415                 return -3;
 416         }
 417         iv16 = (pos[0] << 8) | pos[2];
 418         iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
 419         pos += TKIP_HDR_LEN;
 420
 421         if (tkip_replay_check(iv32, iv16, tkey->rx_iv32, tkey->rx_iv16)) {
 422 #ifdef CONFIG_LIB80211_DEBUG
 423                 net_dbg_ratelimited("TKIP: replay detected: STA=%pM previous TSC %08x%04x received TSC %08x%04x\n",
 424                                     hdr->addr2, tkey->rx_iv32, tkey->rx_iv16,
 425                                     iv32, iv16);
 426 #endif
 427                 tkey->dot11RSNAStatsTKIPReplays++;
 428                 return -4;
 429         }
 430
 431         if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
 432                 tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
 433                 tkey->rx_phase1_done = 1;
 434         }
 435         tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
 436
 437         plen = skb->len - hdr_len - 12;
 438
 439         arc4_setkey(&tkey->rx_ctx_arc4, rc4key, 16);
 440         arc4_crypt(&tkey->rx_ctx_arc4, pos, pos, plen + 4);
 441
 442         crc = ~crc32_le(~0, pos, plen);
 443         icv[0] = crc;
 444         icv[1] = crc >> 8;
 445         icv[2] = crc >> 16;
 446         icv[3] = crc >> 24;
 447         if (memcmp(icv, pos + plen, 4) != 0) {
 448                 if (iv32 != tkey->rx_iv32) {
 449                         /* Previously cached Phase1 result was already lost, so
 450                          * it needs to be recalculated for the next packet. */
 451                         tkey->rx_phase1_done = 0;
 452                 }
 453 #ifdef CONFIG_LIB80211_DEBUG
 454                 net_dbg_ratelimited("TKIP: ICV error detected: STA=%pM\n",
 455                                     hdr->addr2);
 456 #endif
 457                 tkey->dot11RSNAStatsTKIPICVErrors++;
 458                 return -5;
 459         }
 460
 461         /* Update real counters only after Michael MIC verification has
 462          * completed */
 463         tkey->rx_iv32_new = iv32;
 464         tkey->rx_iv16_new = iv16;
 465
 466         /* Remove IV and ICV */
 467         memmove(skb->data + TKIP_HDR_LEN, skb->data, hdr_len);
 468         skb_pull(skb, TKIP_HDR_LEN);
 469         skb_trim(skb, skb->len - 4);
 470
 471         return keyidx;
 472 }
 473
 474 static int michael_mic(struct crypto_shash *tfm_michael, u8 *key, u8 *hdr,
 475                        u8 *data, size_t data_len, u8 *mic)
 476 {
 477         SHASH_DESC_ON_STACK(desc, tfm_michael);
 478         int err;
 479
 480         if (tfm_michael == NULL) {
 481                 pr_warn("%s(): tfm_michael == NULL\n", __func__);
 482                 return -1;
 483         }
 484
 485         desc->tfm = tfm_michael;
 486
 487         if (crypto_shash_setkey(tfm_michael, key, 8))
 488                 return -1;
 489
 490         err = crypto_shash_init(desc);
 491         if (err)
 492                 goto out;
 493         err = crypto_shash_update(desc, hdr, 16);
 494         if (err)
 495                 goto out;
 496         err = crypto_shash_update(desc, data, data_len);
 497         if (err)
 498                 goto out;
 499         err = crypto_shash_final(desc, mic);
 500
 501 out:
 502         shash_desc_zero(desc);
 503         return err;
 504 }
 505
 506 static void michael_mic_hdr(struct sk_buff *skb, u8 * hdr)
 507 {
 508         struct ieee80211_hdr *hdr11;
 509
 510         hdr11 = (struct ieee80211_hdr *)skb->data;
 511
 512         switch (le16_to_cpu(hdr11->frame_control) &
 513                 (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 514         case IEEE80211_FCTL_TODS:
 515                 memcpy(hdr, hdr11->addr3, ETH_ALEN);    /* DA */
 516                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 517                 break;
 518         case IEEE80211_FCTL_FROMDS:
 519                 memcpy(hdr, hdr11->addr1, ETH_ALEN);    /* DA */
 520                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 521                 break;
 522         case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
 523                 memcpy(hdr, hdr11->addr3, ETH_ALEN);    /* DA */
 524                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 525                 break;
 526         default:
 527                 memcpy(hdr, hdr11->addr1, ETH_ALEN);    /* DA */
 528                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 529                 break;
 530         }
 531
 532         if (ieee80211_is_data_qos(hdr11->frame_control)) {
 533                 hdr[12] = le16_to_cpu(*((__le16 *)ieee80211_get_qos_ctl(hdr11)))
 534                         & IEEE80211_QOS_CTL_TID_MASK;
 535         } else
 536                 hdr[12] = 0;            /* priority */
 537
 538         hdr[13] = hdr[14] = hdr[15] = 0;        /* reserved */
 539 }
 540
 541 static int lib80211_michael_mic_add(struct sk_buff *skb, int hdr_len,
 542                                      void *priv)
 543 {
 544         struct lib80211_tkip_data *tkey = priv;
 545         u8 *pos;
 546
 547         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 548                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 549                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 550                        skb_tailroom(skb), hdr_len, skb->len);
 551                 return -1;
 552         }
 553
 554         michael_mic_hdr(skb, tkey->tx_hdr);
 555         pos = skb_put(skb, 8);
 556         if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
 557                         skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
 558                 return -1;
 559
 560         return 0;
 561 }
 562
 563 static void lib80211_michael_mic_failure(struct net_device *dev,
 564                                           struct ieee80211_hdr *hdr,
 565                                           int keyidx)
 566 {
 567         union iwreq_data wrqu;
 568         struct iw_michaelmicfailure ev;
 569
 570         /* TODO: needed parameters: count, keyid, key type, TSC */
 571         memset(&ev, 0, sizeof(ev));
 572         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 573         if (hdr->addr1[0] & 0x01)
 574                 ev.flags |= IW_MICFAILURE_GROUP;
 575         else
 576                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 577         ev.src_addr.sa_family = ARPHRD_ETHER;
 578         memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
 579         memset(&wrqu, 0, sizeof(wrqu));
 580         wrqu.data.length = sizeof(ev);
 581         wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
 582 }
 583
 584 static int lib80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 585                                         int hdr_len, void *priv)
 586 {
 587         struct lib80211_tkip_data *tkey = priv;
 588         u8 mic[8];
 589
 590         if (!tkey->key_set)
 591                 return -1;
 592
 593         michael_mic_hdr(skb, tkey->rx_hdr);
 594         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 595                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 596                 return -1;
 597         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 598                 struct ieee80211_hdr *hdr;
 599                 hdr = (struct ieee80211_hdr *)skb->data;
 600                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 601                        "MSDU from %pM keyidx=%d\n",
 602                        skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 603                        keyidx);
 604                 if (skb->dev)
 605                         lib80211_michael_mic_failure(skb->dev, hdr, keyidx);
 606                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 607                 return -1;
 608         }
 609
 610         /* Update TSC counters for RX now that the packet verification has
 611          * completed. */
 612         tkey->rx_iv32 = tkey->rx_iv32_new;
 613         tkey->rx_iv16 = tkey->rx_iv16_new;
 614
 615         skb_trim(skb, skb->len - 8);
 616
 617         return 0;
 618 }
 619
 620 static int lib80211_tkip_set_key(void *key, int len, u8 * seq, void *priv)
 621 {
 622         struct lib80211_tkip_data *tkey = priv;
 623         int keyidx;
 624         struct crypto_shash *tfm = tkey->tx_tfm_michael;
 625         struct arc4_ctx *tfm2 = &tkey->tx_ctx_arc4;
 626         struct crypto_shash *tfm3 = tkey->rx_tfm_michael;
 627         struct arc4_ctx *tfm4 = &tkey->rx_ctx_arc4;
 628
 629         keyidx = tkey->key_idx;
 630         memset(tkey, 0, sizeof(*tkey));
 631         tkey->key_idx = keyidx;
 632         tkey->tx_tfm_michael = tfm;
 633         tkey->tx_ctx_arc4 = *tfm2;
 634         tkey->rx_tfm_michael = tfm3;
 635         tkey->rx_ctx_arc4 = *tfm4;
 636         if (len == TKIP_KEY_LEN) {
 637                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 638                 tkey->key_set = 1;
 639                 tkey->tx_iv16 = 1;      /* TSC is initialized to 1 */
 640                 if (seq) {
 641                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 642                             (seq[3] << 8) | seq[2];
 643                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 644                 }
 645         } else if (len == 0)
 646                 tkey->key_set = 0;
 647         else
 648                 return -1;
 649
 650         return 0;
 651 }
 652
 653 static int lib80211_tkip_get_key(void *key, int len, u8 * seq, void *priv)
 654 {
 655         struct lib80211_tkip_data *tkey = priv;
 656
 657         if (len < TKIP_KEY_LEN)
 658                 return -1;
 659
 660         if (!tkey->key_set)
 661                 return 0;
 662         memcpy(key, tkey->key, TKIP_KEY_LEN);
 663
 664         if (seq) {
 665                 /* Return the sequence number of the last transmitted frame. */
 666                 u16 iv16 = tkey->tx_iv16;
 667                 u32 iv32 = tkey->tx_iv32;
 668                 if (iv16 == 0)
 669                         iv32--;
 670                 iv16--;
 671                 seq[0] = tkey->tx_iv16;
 672                 seq[1] = tkey->tx_iv16 >> 8;
 673                 seq[2] = tkey->tx_iv32;
 674                 seq[3] = tkey->tx_iv32 >> 8;
 675                 seq[4] = tkey->tx_iv32 >> 16;
 676                 seq[5] = tkey->tx_iv32 >> 24;
 677         }
 678
 679         return TKIP_KEY_LEN;
 680 }
 681
 682 static void lib80211_tkip_print_stats(struct seq_file *m, void *priv)
 683 {
 684         struct lib80211_tkip_data *tkip = priv;
 685         seq_printf(m,
 686                    "key[%d] alg=TKIP key_set=%d "
 687                    "tx_pn=%02x%02x%02x%02x%02x%02x "
 688                    "rx_pn=%02x%02x%02x%02x%02x%02x "
 689                    "replays=%d icv_errors=%d local_mic_failures=%d\n",
 690                    tkip->key_idx, tkip->key_set,
 691                    (tkip->tx_iv32 >> 24) & 0xff,
 692                    (tkip->tx_iv32 >> 16) & 0xff,
 693                    (tkip->tx_iv32 >> 8) & 0xff,
 694                    tkip->tx_iv32 & 0xff,
 695                    (tkip->tx_iv16 >> 8) & 0xff,
 696                    tkip->tx_iv16 & 0xff,
 697                    (tkip->rx_iv32 >> 24) & 0xff,
 698                    (tkip->rx_iv32 >> 16) & 0xff,
 699                    (tkip->rx_iv32 >> 8) & 0xff,
 700                    tkip->rx_iv32 & 0xff,
 701                    (tkip->rx_iv16 >> 8) & 0xff,
 702                    tkip->rx_iv16 & 0xff,
 703                    tkip->dot11RSNAStatsTKIPReplays,
 704                    tkip->dot11RSNAStatsTKIPICVErrors,
 705                    tkip->dot11RSNAStatsTKIPLocalMICFailures);
 706 }
 707
 708 static struct lib80211_crypto_ops lib80211_crypt_tkip = {
 709         .name = "TKIP",
 710         .init = lib80211_tkip_init,
 711         .deinit = lib80211_tkip_deinit,
 712         .encrypt_mpdu = lib80211_tkip_encrypt,
 713         .decrypt_mpdu = lib80211_tkip_decrypt,
 714         .encrypt_msdu = lib80211_michael_mic_add,
 715         .decrypt_msdu = lib80211_michael_mic_verify,
 716         .set_key = lib80211_tkip_set_key,
 717         .get_key = lib80211_tkip_get_key,
 718         .print_stats = lib80211_tkip_print_stats,
 719         .extra_mpdu_prefix_len = 4 + 4, /* IV + ExtIV */
 720         .extra_mpdu_postfix_len = 4,    /* ICV */
 721         .extra_msdu_postfix_len = 8,    /* MIC */
 722         .get_flags = lib80211_tkip_get_flags,
 723         .set_flags = lib80211_tkip_set_flags,
 724         .owner = THIS_MODULE,
 725 };
 726
 727 static int __init lib80211_crypto_tkip_init(void)
 728 {
 729         return lib80211_register_crypto_ops(&lib80211_crypt_tkip);
 730 }
 731
 732 static void __exit lib80211_crypto_tkip_exit(void)
 733 {
 734         lib80211_unregister_crypto_ops(&lib80211_crypt_tkip);
 735 }
 736
 737 module_init(lib80211_crypto_tkip_init);
 738 module_exit(lib80211_crypto_tkip_exit);