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