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