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