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