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