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