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