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