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