kernel/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
 301                 for (i = 0; i < 6; i++)
 302                         PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 303         }
 304 #endif
 305 }
 306
 307
 308 static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 309 {
 310         struct ieee80211_tkip_data *tkey = priv;
 311         int len;
 312         u8 *pos;
 313         struct rtl_80211_hdr_4addr *hdr;
 314         cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 315         struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
 316         int ret = 0;
 317         u8 rc4key[16],  *icv;
 318         u32 crc;
 319         struct scatterlist sg;
 320
 321         if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
 322             skb->len < hdr_len)
 323                 return -1;
 324
 325         hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 326
 327         if (!tcb_desc->bHwSec) {
 328                 if (!tkey->tx_phase1_done) {
 329                         tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
 330                                            tkey->tx_iv32);
 331                         tkey->tx_phase1_done = 1;
 332                 }
 333                 tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
 334         } 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                 *pos++ = Hi8(tkey->tx_iv16);
 345                 *pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
 346                 *pos++ = Lo8(tkey->tx_iv16);
 347         } else {
 348                 *pos++ = rc4key[0];
 349                 *pos++ = rc4key[1];
 350                 *pos++ = rc4key[2];
 351         }
 352
 353         *pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
 354         *pos++ = tkey->tx_iv32 & 0xff;
 355         *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
 356         *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
 357         *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
 358
 359         if (!tcb_desc->bHwSec) {
 360                 icv = skb_put(skb, 4);
 361                 crc = ~crc32_le(~0, pos, len);
 362                 icv[0] = crc;
 363                 icv[1] = crc >> 8;
 364                 icv[2] = crc >> 16;
 365                 icv[3] = crc >> 24;
 366                 crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
 367                 sg_init_one(&sg, pos, len+4);
 368                 ret = crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
 369         }
 370
 371         tkey->tx_iv16++;
 372         if (tkey->tx_iv16 == 0) {
 373                 tkey->tx_phase1_done = 0;
 374                 tkey->tx_iv32++;
 375         }
 376
 377         if (!tcb_desc->bHwSec)
 378                 return ret;
 379         else
 380                 return 0;
 381
 382
 383 }
 384
 385 static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 386 {
 387         struct ieee80211_tkip_data *tkey = priv;
 388         u8 keyidx, *pos;
 389         u32 iv32;
 390         u16 iv16;
 391         struct rtl_80211_hdr_4addr *hdr;
 392         cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 393         struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
 394         u8 rc4key[16];
 395         u8 icv[4];
 396         u32 crc;
 397         struct scatterlist sg;
 398         int plen;
 399
 400         if (skb->len < hdr_len + 8 + 4)
 401                 return -1;
 402
 403         hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 404         pos = skb->data + hdr_len;
 405         keyidx = pos[3];
 406         if (!(keyidx & (1 << 5))) {
 407                 if (net_ratelimit()) {
 408                         printk(KERN_DEBUG "TKIP: received packet without ExtIV"
 409                                " flag from %pM\n", hdr->addr2);
 410                 }
 411                 return -2;
 412         }
 413         keyidx >>= 6;
 414         if (tkey->key_idx != keyidx) {
 415                 printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
 416                        "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
 417                 return -6;
 418         }
 419         if (!tkey->key_set) {
 420                 if (net_ratelimit()) {
 421                         printk(KERN_DEBUG "TKIP: received packet from %pM"
 422                                " with keyid=%d that does not have a configured"
 423                                " key\n", hdr->addr2, keyidx);
 424                 }
 425                 return -3;
 426         }
 427         iv16 = (pos[0] << 8) | pos[2];
 428         iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
 429         pos += 8;
 430
 431         if (!tcb_desc->bHwSec) {
 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_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
 453                 sg_init_one(&sg, pos, plen+4);
 454
 455                 if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
 456                         if (net_ratelimit()) {
 457                                 printk(KERN_DEBUG ": TKIP: failed to decrypt "
 458                                                 "received packet from %pM\n",
 459                                                 hdr->addr2);
 460                         }
 461                         return -7;
 462                 }
 463
 464                 crc = ~crc32_le(~0, pos, plen);
 465                 icv[0] = crc;
 466                 icv[1] = crc >> 8;
 467                 icv[2] = crc >> 16;
 468                 icv[3] = crc >> 24;
 469
 470                 if (memcmp(icv, pos + plen, 4) != 0) {
 471                         if (iv32 != tkey->rx_iv32) {
 472                                 /*
 473                                  * Previously cached Phase1 result was already
 474                                  * lost, so it needs to be recalculated for the
 475                                  * next packet.
 476                                  */
 477                                 tkey->rx_phase1_done = 0;
 478                         }
 479                         if (net_ratelimit()) {
 480                                 printk(KERN_DEBUG "TKIP: ICV error detected: STA="
 481                                 "%pM\n", hdr->addr2);
 482                         }
 483                         tkey->dot11RSNAStatsTKIPICVErrors++;
 484                         return -5;
 485                 }
 486
 487         }
 488
 489         /*
 490          * Update real counters only after Michael MIC verification has
 491          * completed.
 492          */
 493         tkey->rx_iv32_new = iv32;
 494         tkey->rx_iv16_new = iv16;
 495
 496         /* Remove IV and ICV */
 497         memmove(skb->data + 8, skb->data, hdr_len);
 498         skb_pull(skb, 8);
 499         skb_trim(skb, skb->len - 4);
 500
 501         return keyidx;
 502 }
 503
 504 static int michael_mic(struct crypto_hash *tfm_michael, u8 *key, u8 *hdr,
 505                        u8 *data, size_t data_len, u8 *mic)
 506 {
 507         struct hash_desc desc;
 508         struct scatterlist sg[2];
 509
 510         if (tfm_michael == NULL) {
 511                 printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
 512                 return -1;
 513         }
 514
 515         sg_init_table(sg, 2);
 516         sg_set_buf(&sg[0], hdr, 16);
 517         sg_set_buf(&sg[1], data, data_len);
 518
 519         if (crypto_hash_setkey(tfm_michael, key, 8))
 520                 return -1;
 521
 522         desc.tfm = tfm_michael;
 523         desc.flags = 0;
 524         return crypto_hash_digest(&desc, sg, data_len + 16, mic);
 525 }
 526
 527 static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
 528 {
 529         struct rtl_80211_hdr_4addr *hdr11;
 530
 531         hdr11 = (struct rtl_80211_hdr_4addr *) skb->data;
 532         switch (le16_to_cpu(hdr11->frame_ctl) &
 533                 (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 534         case IEEE80211_FCTL_TODS:
 535                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 536                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 537                 break;
 538         case IEEE80211_FCTL_FROMDS:
 539                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 540                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 541                 break;
 542         case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
 543                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 544                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 545                 break;
 546         case 0:
 547                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 548                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 549                 break;
 550         }
 551
 552         hdr[12] = 0; /* priority */
 553
 554         hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
 555 }
 556
 557
 558 static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
 559 {
 560         struct ieee80211_tkip_data *tkey = priv;
 561         u8 *pos;
 562         struct rtl_80211_hdr_4addr *hdr;
 563
 564         hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 565
 566         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 567                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 568                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 569                        skb_tailroom(skb), hdr_len, skb->len);
 570                 return -1;
 571         }
 572
 573         michael_mic_hdr(skb, tkey->tx_hdr);
 574
 575         // { david, 2006.9.1
 576         // fix the wpa process with wmm enabled.
 577         if (IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
 578                 tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 579         // }
 580         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 rtl_80211_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 rtl_80211_hdr_4addr *hdr;
 616
 617         hdr = (struct rtl_80211_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         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 630                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 631                 return -1;
 632         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 633                 struct rtl_80211_hdr_4addr *hdr;
 634                 hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 635
 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         /*
 647          * Update TSC counters for RX now that the packet verification has
 648          * completed.
 649          */
 650         tkey->rx_iv32 = tkey->rx_iv32_new;
 651         tkey->rx_iv16 = tkey->rx_iv16_new;
 652
 653         skb_trim(skb, skb->len - 8);
 654
 655         return 0;
 656 }
 657
 658
 659 static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
 660 {
 661         struct ieee80211_tkip_data *tkey = priv;
 662         int keyidx;
 663         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 664         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 665         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 666         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 667
 668         keyidx = tkey->key_idx;
 669         memset(tkey, 0, sizeof(*tkey));
 670         tkey->key_idx = keyidx;
 671         tkey->tx_tfm_michael = tfm;
 672         tkey->tx_tfm_arc4 = tfm2;
 673         tkey->rx_tfm_michael = tfm3;
 674         tkey->rx_tfm_arc4 = tfm4;
 675
 676         if (len == TKIP_KEY_LEN) {
 677                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 678                 tkey->key_set = 1;
 679                 tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
 680                 if (seq) {
 681                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 682                                 (seq[3] << 8) | seq[2];
 683                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 684                 }
 685         } else if (len == 0)
 686                 tkey->key_set = 0;
 687         else
 688                 return -1;
 689
 690         return 0;
 691 }
 692
 693
 694 static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 695 {
 696         struct ieee80211_tkip_data *tkey = priv;
 697
 698         if (len < TKIP_KEY_LEN)
 699                 return -1;
 700
 701         if (!tkey->key_set)
 702                 return 0;
 703         memcpy(key, tkey->key, TKIP_KEY_LEN);
 704
 705         if (seq) {
 706                 /* Return the sequence number of the last transmitted frame. */
 707                 u16 iv16 = tkey->tx_iv16;
 708                 u32 iv32 = tkey->tx_iv32;
 709
 710                 if (iv16 == 0)
 711                         iv32--;
 712                 iv16--;
 713                 seq[0] = tkey->tx_iv16;
 714                 seq[1] = tkey->tx_iv16 >> 8;
 715                 seq[2] = tkey->tx_iv32;
 716                 seq[3] = tkey->tx_iv32 >> 8;
 717                 seq[4] = tkey->tx_iv32 >> 16;
 718                 seq[5] = tkey->tx_iv32 >> 24;
 719         }
 720
 721         return TKIP_KEY_LEN;
 722 }
 723
 724
 725 static char *ieee80211_tkip_print_stats(char *p, void *priv)
 726 {
 727         struct ieee80211_tkip_data *tkip = priv;
 728
 729         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 730                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 731                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 732                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 733                      tkip->key_idx, tkip->key_set,
 734                      (tkip->tx_iv32 >> 24) & 0xff,
 735                      (tkip->tx_iv32 >> 16) & 0xff,
 736                      (tkip->tx_iv32 >> 8) & 0xff,
 737                      tkip->tx_iv32 & 0xff,
 738                      (tkip->tx_iv16 >> 8) & 0xff,
 739                      tkip->tx_iv16 & 0xff,
 740                      (tkip->rx_iv32 >> 24) & 0xff,
 741                      (tkip->rx_iv32 >> 16) & 0xff,
 742                      (tkip->rx_iv32 >> 8) & 0xff,
 743                      tkip->rx_iv32 & 0xff,
 744                      (tkip->rx_iv16 >> 8) & 0xff,
 745                      tkip->rx_iv16 & 0xff,
 746                      tkip->dot11RSNAStatsTKIPReplays,
 747                      tkip->dot11RSNAStatsTKIPICVErrors,
 748                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 749         return p;
 750 }
 751
 752
 753 static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
 754         .name                   = "TKIP",
 755         .init                   = ieee80211_tkip_init,
 756         .deinit                 = ieee80211_tkip_deinit,
 757         .encrypt_mpdu           = ieee80211_tkip_encrypt,
 758         .decrypt_mpdu           = ieee80211_tkip_decrypt,
 759         .encrypt_msdu           = ieee80211_michael_mic_add,
 760         .decrypt_msdu           = ieee80211_michael_mic_verify,
 761         .set_key                = ieee80211_tkip_set_key,
 762         .get_key                = ieee80211_tkip_get_key,
 763         .print_stats            = ieee80211_tkip_print_stats,
 764         .extra_prefix_len       = 4 + 4, /* IV + ExtIV */
 765         .extra_postfix_len      = 8 + 4, /* MIC + ICV */
 766         .owner                  = THIS_MODULE,
 767 };
 768
 769 int __init ieee80211_crypto_tkip_init(void)
 770 {
 771         return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
 772 }
 773
 774 void __exit ieee80211_crypto_tkip_exit(void)
 775 {
 776         ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
 777 }
 778
 779 void ieee80211_tkip_null(void)
 780 {
 781 //    printk("============>%s()\n", __func__);
 782         return;
 783 }