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