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/skbuff.h>
  18 #include <linux/netdevice.h>
  19 #include <linux/mm.h>
  20 #include <linux/if_ether.h>
  21 #include <linux/if_arp.h>
  22 #include <asm/string.h>
  23
  24 #include <net/ieee80211.h>
  25
  26 #include <linux/crypto.h>
  27 #include <asm/scatterlist.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_page(&sg[0], virt_to_page(hdr));
 540         sg[0].offset = offset_in_page(hdr);
 541         sg[0].length = 16;
 542
 543         sg_set_page(&sg[1], virt_to_page(data));
 544         sg[1].offset = offset_in_page(data);
 545         sg[1].length = data_len;
 546
 547         if (crypto_hash_setkey(tfm_michael, key, 8))
 548                 return -1;
 549
 550         desc.tfm = tfm_michael;
 551         desc.flags = 0;
 552         return crypto_hash_digest(&desc, sg, data_len + 16, mic);
 553 }
 554
 555 static void michael_mic_hdr(struct sk_buff *skb, u8 * hdr)
 556 {
 557         struct ieee80211_hdr_4addr *hdr11;
 558         u16 stype;
 559
 560         hdr11 = (struct ieee80211_hdr_4addr *)skb->data;
 561         stype  = WLAN_FC_GET_STYPE(le16_to_cpu(hdr11->frame_ctl));
 562
 563         switch (le16_to_cpu(hdr11->frame_ctl) &
 564                 (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 565         case IEEE80211_FCTL_TODS:
 566                 memcpy(hdr, hdr11->addr3, ETH_ALEN);    /* DA */
 567                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 568                 break;
 569         case IEEE80211_FCTL_FROMDS:
 570                 memcpy(hdr, hdr11->addr1, ETH_ALEN);    /* DA */
 571                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 572                 break;
 573         case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
 574                 memcpy(hdr, hdr11->addr3, ETH_ALEN);    /* DA */
 575                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 576                 break;
 577         case 0:
 578                 memcpy(hdr, hdr11->addr1, ETH_ALEN);    /* DA */
 579                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 580                 break;
 581         }
 582
 583         if (stype & IEEE80211_STYPE_QOS_DATA) {
 584                 const struct ieee80211_hdr_3addrqos *qoshdr =
 585                         (struct ieee80211_hdr_3addrqos *)skb->data;
 586                 hdr[12] = le16_to_cpu(qoshdr->qos_ctl) & IEEE80211_QCTL_TID;
 587         } else
 588                 hdr[12] = 0;            /* priority */
 589
 590         hdr[13] = hdr[14] = hdr[15] = 0;        /* reserved */
 591 }
 592
 593 static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len,
 594                                      void *priv)
 595 {
 596         struct ieee80211_tkip_data *tkey = priv;
 597         u8 *pos;
 598
 599         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 600                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 601                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 602                        skb_tailroom(skb), hdr_len, skb->len);
 603                 return -1;
 604         }
 605
 606         michael_mic_hdr(skb, tkey->tx_hdr);
 607         pos = skb_put(skb, 8);
 608         if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
 609                         skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
 610                 return -1;
 611
 612         return 0;
 613 }
 614
 615 static void ieee80211_michael_mic_failure(struct net_device *dev,
 616                                           struct ieee80211_hdr_4addr *hdr,
 617                                           int keyidx)
 618 {
 619         union iwreq_data wrqu;
 620         struct iw_michaelmicfailure ev;
 621
 622         /* TODO: needed parameters: count, keyid, key type, TSC */
 623         memset(&ev, 0, sizeof(ev));
 624         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 625         if (hdr->addr1[0] & 0x01)
 626                 ev.flags |= IW_MICFAILURE_GROUP;
 627         else
 628                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 629         ev.src_addr.sa_family = ARPHRD_ETHER;
 630         memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
 631         memset(&wrqu, 0, sizeof(wrqu));
 632         wrqu.data.length = sizeof(ev);
 633         wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
 634 }
 635
 636 static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 637                                         int hdr_len, void *priv)
 638 {
 639         struct ieee80211_tkip_data *tkey = priv;
 640         u8 mic[8];
 641         DECLARE_MAC_BUF(mac);
 642
 643         if (!tkey->key_set)
 644                 return -1;
 645
 646         michael_mic_hdr(skb, tkey->rx_hdr);
 647         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 648                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 649                 return -1;
 650         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 651                 struct ieee80211_hdr_4addr *hdr;
 652                 hdr = (struct ieee80211_hdr_4addr *)skb->data;
 653                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 654                        "MSDU from %s keyidx=%d\n",
 655                        skb->dev ? skb->dev->name : "N/A", print_mac(mac, hdr->addr2),
 656                        keyidx);
 657                 if (skb->dev)
 658                         ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
 659                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 660                 return -1;
 661         }
 662
 663         /* Update TSC counters for RX now that the packet verification has
 664          * completed. */
 665         tkey->rx_iv32 = tkey->rx_iv32_new;
 666         tkey->rx_iv16 = tkey->rx_iv16_new;
 667
 668         skb_trim(skb, skb->len - 8);
 669
 670         return 0;
 671 }
 672
 673 static int ieee80211_tkip_set_key(void *key, int len, u8 * seq, void *priv)
 674 {
 675         struct ieee80211_tkip_data *tkey = priv;
 676         int keyidx;
 677         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 678         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 679         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 680         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 681
 682         keyidx = tkey->key_idx;
 683         memset(tkey, 0, sizeof(*tkey));
 684         tkey->key_idx = keyidx;
 685         tkey->tx_tfm_michael = tfm;
 686         tkey->tx_tfm_arc4 = tfm2;
 687         tkey->rx_tfm_michael = tfm3;
 688         tkey->rx_tfm_arc4 = tfm4;
 689         if (len == TKIP_KEY_LEN) {
 690                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 691                 tkey->key_set = 1;
 692                 tkey->tx_iv16 = 1;      /* TSC is initialized to 1 */
 693                 if (seq) {
 694                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 695                             (seq[3] << 8) | seq[2];
 696                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 697                 }
 698         } else if (len == 0)
 699                 tkey->key_set = 0;
 700         else
 701                 return -1;
 702
 703         return 0;
 704 }
 705
 706 static int ieee80211_tkip_get_key(void *key, int len, u8 * seq, void *priv)
 707 {
 708         struct ieee80211_tkip_data *tkey = priv;
 709
 710         if (len < TKIP_KEY_LEN)
 711                 return -1;
 712
 713         if (!tkey->key_set)
 714                 return 0;
 715         memcpy(key, tkey->key, TKIP_KEY_LEN);
 716
 717         if (seq) {
 718                 /* Return the sequence number of the last transmitted frame. */
 719                 u16 iv16 = tkey->tx_iv16;
 720                 u32 iv32 = tkey->tx_iv32;
 721                 if (iv16 == 0)
 722                         iv32--;
 723                 iv16--;
 724                 seq[0] = tkey->tx_iv16;
 725                 seq[1] = tkey->tx_iv16 >> 8;
 726                 seq[2] = tkey->tx_iv32;
 727                 seq[3] = tkey->tx_iv32 >> 8;
 728                 seq[4] = tkey->tx_iv32 >> 16;
 729                 seq[5] = tkey->tx_iv32 >> 24;
 730         }
 731
 732         return TKIP_KEY_LEN;
 733 }
 734
 735 static char *ieee80211_tkip_print_stats(char *p, void *priv)
 736 {
 737         struct ieee80211_tkip_data *tkip = priv;
 738         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 739                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 740                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 741                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 742                      tkip->key_idx, tkip->key_set,
 743                      (tkip->tx_iv32 >> 24) & 0xff,
 744                      (tkip->tx_iv32 >> 16) & 0xff,
 745                      (tkip->tx_iv32 >> 8) & 0xff,
 746                      tkip->tx_iv32 & 0xff,
 747                      (tkip->tx_iv16 >> 8) & 0xff,
 748                      tkip->tx_iv16 & 0xff,
 749                      (tkip->rx_iv32 >> 24) & 0xff,
 750                      (tkip->rx_iv32 >> 16) & 0xff,
 751                      (tkip->rx_iv32 >> 8) & 0xff,
 752                      tkip->rx_iv32 & 0xff,
 753                      (tkip->rx_iv16 >> 8) & 0xff,
 754                      tkip->rx_iv16 & 0xff,
 755                      tkip->dot11RSNAStatsTKIPReplays,
 756                      tkip->dot11RSNAStatsTKIPICVErrors,
 757                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 758         return p;
 759 }
 760
 761 static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
 762         .name = "TKIP",
 763         .init = ieee80211_tkip_init,
 764         .deinit = ieee80211_tkip_deinit,
 765         .build_iv = ieee80211_tkip_hdr,
 766         .encrypt_mpdu = ieee80211_tkip_encrypt,
 767         .decrypt_mpdu = ieee80211_tkip_decrypt,
 768         .encrypt_msdu = ieee80211_michael_mic_add,
 769         .decrypt_msdu = ieee80211_michael_mic_verify,
 770         .set_key = ieee80211_tkip_set_key,
 771         .get_key = ieee80211_tkip_get_key,
 772         .print_stats = ieee80211_tkip_print_stats,
 773         .extra_mpdu_prefix_len = 4 + 4, /* IV + ExtIV */
 774         .extra_mpdu_postfix_len = 4,    /* ICV */
 775         .extra_msdu_postfix_len = 8,    /* MIC */
 776         .get_flags = ieee80211_tkip_get_flags,
 777         .set_flags = ieee80211_tkip_set_flags,
 778         .owner = THIS_MODULE,
 779 };
 780
 781 static int __init ieee80211_crypto_tkip_init(void)
 782 {
 783         return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
 784 }
 785
 786 static void __exit ieee80211_crypto_tkip_exit(void)
 787 {
 788         ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
 789 }
 790
 791 module_init(ieee80211_crypto_tkip_init);
 792 module_exit(ieee80211_crypto_tkip_exit);