net/wireless/lib80211_crypt_tkip.c

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