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         return keyidx;
 506 }
 507
 508 static int michael_mic(struct crypto_hash *tfm_michael, u8 * key, u8 * hdr,
 509                        u8 * data, size_t data_len, u8 * mic)
 510 {
 511         struct hash_desc desc;
 512         struct scatterlist sg[2];
 513
 514         if (tfm_michael == NULL) {
 515                 printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
 516                 return -1;
 517         }
 518
 519         sg_init_table(sg, 2);
 520         sg_set_buf(&sg[0], hdr, 16);
 521         sg_set_buf(&sg[1], data, data_len);
 522
 523         if (crypto_hash_setkey(tfm_michael, key, 8))
 524                 return -1;
 525
 526         desc.tfm = tfm_michael;
 527         desc.flags = 0;
 528         return crypto_hash_digest(&desc, sg, data_len + 16, mic);
 529 }
 530
 531 static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
 532 {
 533         struct ieee80211_hdr_4addr *hdr11;
 534
 535         hdr11 = (struct ieee80211_hdr_4addr *) skb->data;
 536         switch (le16_to_cpu(hdr11->frame_ctl) &
 537                 (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 538         case IEEE80211_FCTL_TODS:
 539                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 540                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 541                 break;
 542         case IEEE80211_FCTL_FROMDS:
 543                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 544                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 545                 break;
 546         case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
 547                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 548                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 549                 break;
 550         case 0:
 551                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 552                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 553                 break;
 554         }
 555
 556         hdr[12] = 0; /* priority */
 557
 558         hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
 559 }
 560
 561
 562 static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
 563 {
 564         struct ieee80211_tkip_data *tkey = priv;
 565         u8 *pos;
 566         struct ieee80211_hdr_4addr *hdr;
 567
 568         hdr = (struct ieee80211_hdr_4addr *) skb->data;
 569
 570         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 571                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 572                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 573                        skb_tailroom(skb), hdr_len, skb->len);
 574                 return -1;
 575         }
 576
 577         michael_mic_hdr(skb, tkey->tx_hdr);
 578
 579         // { david, 2006.9.1
 580         // fix the wpa process with wmm enabled.
 581         if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
 582                 tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 583         }
 584         // }
 585         pos = skb_put(skb, 8);
 586
 587         if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
 588                         skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
 589                 return -1;
 590
 591         return 0;
 592 }
 593
 594 static void ieee80211_michael_mic_failure(struct net_device *dev,
 595                                        struct ieee80211_hdr_4addr *hdr,
 596                                        int keyidx)
 597 {
 598         union iwreq_data wrqu;
 599         struct iw_michaelmicfailure ev;
 600
 601         /* TODO: needed parameters: count, keyid, key type, TSC */
 602         memset(&ev, 0, sizeof(ev));
 603         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 604         if (hdr->addr1[0] & 0x01)
 605                 ev.flags |= IW_MICFAILURE_GROUP;
 606         else
 607                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 608         ev.src_addr.sa_family = ARPHRD_ETHER;
 609         memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
 610         memset(&wrqu, 0, sizeof(wrqu));
 611         wrqu.data.length = sizeof(ev);
 612         wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
 613 }
 614
 615 static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 616                                      int hdr_len, void *priv)
 617 {
 618         struct ieee80211_tkip_data *tkey = priv;
 619         u8 mic[8];
 620         struct ieee80211_hdr_4addr *hdr;
 621
 622         hdr = (struct ieee80211_hdr_4addr *) skb->data;
 623
 624         if (!tkey->key_set)
 625                 return -1;
 626
 627         michael_mic_hdr(skb, tkey->rx_hdr);
 628         // { david, 2006.9.1
 629         // fix the wpa process with wmm enabled.
 630         if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
 631                 tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 632         }
 633         // }
 634
 635         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 636                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 637                 return -1;
 638         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 639                 struct ieee80211_hdr_4addr *hdr;
 640                 hdr = (struct ieee80211_hdr_4addr *) skb->data;
 641                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 642                        "MSDU from " MAC_FMT " keyidx=%d\n",
 643                        skb->dev ? skb->dev->name : "N/A", MAC_ARG(hdr->addr2),
 644                        keyidx);
 645                 if (skb->dev)
 646                         ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
 647                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 648                 return -1;
 649         }
 650
 651         /* Update TSC counters for RX now that the packet verification has
 652          * completed. */
 653         tkey->rx_iv32 = tkey->rx_iv32_new;
 654         tkey->rx_iv16 = tkey->rx_iv16_new;
 655
 656         skb_trim(skb, skb->len - 8);
 657
 658         return 0;
 659 }
 660
 661
 662 static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
 663 {
 664         struct ieee80211_tkip_data *tkey = priv;
 665         int keyidx;
 666         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 667         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 668         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 669         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 670
 671         keyidx = tkey->key_idx;
 672         memset(tkey, 0, sizeof(*tkey));
 673         tkey->key_idx = keyidx;
 674         tkey->tx_tfm_michael = tfm;
 675         tkey->tx_tfm_arc4 = tfm2;
 676         tkey->rx_tfm_michael = tfm3;
 677         tkey->rx_tfm_arc4 = tfm4;
 678
 679         if (len == TKIP_KEY_LEN) {
 680                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 681                 tkey->key_set = 1;
 682                 tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
 683                 if (seq) {
 684                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 685                                 (seq[3] << 8) | seq[2];
 686                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 687                 }
 688         } else if (len == 0)
 689                 tkey->key_set = 0;
 690         else
 691                 return -1;
 692
 693         return 0;
 694 }
 695
 696
 697 static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 698 {
 699         struct ieee80211_tkip_data *tkey = priv;
 700
 701         if (len < TKIP_KEY_LEN)
 702                 return -1;
 703
 704         if (!tkey->key_set)
 705                 return 0;
 706         memcpy(key, tkey->key, TKIP_KEY_LEN);
 707
 708         if (seq) {
 709                 /* Return the sequence number of the last transmitted frame. */
 710                 u16 iv16 = tkey->tx_iv16;
 711                 u32 iv32 = tkey->tx_iv32;
 712                 if (iv16 == 0)
 713                         iv32--;
 714                 iv16--;
 715                 seq[0] = tkey->tx_iv16;
 716                 seq[1] = tkey->tx_iv16 >> 8;
 717                 seq[2] = tkey->tx_iv32;
 718                 seq[3] = tkey->tx_iv32 >> 8;
 719                 seq[4] = tkey->tx_iv32 >> 16;
 720                 seq[5] = tkey->tx_iv32 >> 24;
 721         }
 722
 723         return TKIP_KEY_LEN;
 724 }
 725
 726
 727 static char * ieee80211_tkip_print_stats(char *p, void *priv)
 728 {
 729         struct ieee80211_tkip_data *tkip = priv;
 730         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 731                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 732                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 733                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 734                      tkip->key_idx, tkip->key_set,
 735                      (tkip->tx_iv32 >> 24) & 0xff,
 736                      (tkip->tx_iv32 >> 16) & 0xff,
 737                      (tkip->tx_iv32 >> 8) & 0xff,
 738                      tkip->tx_iv32 & 0xff,
 739                      (tkip->tx_iv16 >> 8) & 0xff,
 740                      tkip->tx_iv16 & 0xff,
 741                      (tkip->rx_iv32 >> 24) & 0xff,
 742                      (tkip->rx_iv32 >> 16) & 0xff,
 743                      (tkip->rx_iv32 >> 8) & 0xff,
 744                      tkip->rx_iv32 & 0xff,
 745                      (tkip->rx_iv16 >> 8) & 0xff,
 746                      tkip->rx_iv16 & 0xff,
 747                      tkip->dot11RSNAStatsTKIPReplays,
 748                      tkip->dot11RSNAStatsTKIPICVErrors,
 749                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 750         return p;
 751 }
 752
 753
 754 static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
 755         .name                   = "TKIP",
 756         .init                   = ieee80211_tkip_init,
 757         .deinit                 = ieee80211_tkip_deinit,
 758         .encrypt_mpdu           = ieee80211_tkip_encrypt,
 759         .decrypt_mpdu           = ieee80211_tkip_decrypt,
 760         .encrypt_msdu           = ieee80211_michael_mic_add,
 761         .decrypt_msdu           = ieee80211_michael_mic_verify,
 762         .set_key                = ieee80211_tkip_set_key,
 763         .get_key                = ieee80211_tkip_get_key,
 764         .print_stats            = ieee80211_tkip_print_stats,
 765         .extra_prefix_len       = 4 + 4, /* IV + ExtIV */
 766         .extra_postfix_len      = 8 + 4, /* MIC + ICV */
 767         .owner                  = THIS_MODULE,
 768 };
 769
 770 int __init ieee80211_crypto_tkip_init(void)
 771 {
 772         return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
 773 }
 774
 775 void __exit ieee80211_crypto_tkip_exit(void)
 776 {
 777         ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
 778 }
 779
 780 void ieee80211_tkip_null(void)
 781 {
 782 //    printk("============>%s()\n", __FUNCTION__);
 783         return;
 784 }