net/ieee80211/ieee80211_crypt_tkip.c

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