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