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