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