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