net/wireless/lib80211_crypt_tkip.c

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