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