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