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