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