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