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