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