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 le16_to_cpu(*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                 for (i = 0; i < 6; i++)
 289                         PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 290         }
 291 #endif
 292 }
 293
 294
 295 static int rtllib_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 296 {
 297         struct rtllib_tkip_data *tkey = priv;
 298                 int len;
 299         u8 *pos;
 300         struct rtllib_hdr_4addr *hdr;
 301         struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
 302                                     MAX_DEV_ADDR_SIZE);
 303         struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
 304         int ret = 0;
 305         u8 rc4key[16],  *icv;
 306         u32 crc;
 307         struct scatterlist sg;
 308
 309         if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
 310             skb->len < hdr_len)
 311                 return -1;
 312
 313         hdr = (struct rtllib_hdr_4addr *) skb->data;
 314
 315         if (!tcb_desc->bHwSec) {
 316                 if (!tkey->tx_phase1_done) {
 317                         tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
 318                                         tkey->tx_iv32);
 319                         tkey->tx_phase1_done = 1;
 320                 }
 321                 tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak,
 322                                    tkey->tx_iv16);
 323         } else
 324         tkey->tx_phase1_done = 1;
 325
 326
 327         len = skb->len - hdr_len;
 328         pos = skb_push(skb, 8);
 329         memmove(pos, pos + 8, hdr_len);
 330         pos += hdr_len;
 331
 332         if (tcb_desc->bHwSec) {
 333                 *pos++ = Hi8(tkey->tx_iv16);
 334                 *pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
 335                 *pos++ = Lo8(tkey->tx_iv16);
 336         } else {
 337                 *pos++ = rc4key[0];
 338                 *pos++ = rc4key[1];
 339                 *pos++ = rc4key[2];
 340         }
 341
 342         *pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
 343         *pos++ = tkey->tx_iv32 & 0xff;
 344         *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
 345         *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
 346         *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
 347
 348         if (!tcb_desc->bHwSec) {
 349                 icv = skb_put(skb, 4);
 350                 crc = ~crc32_le(~0, pos, len);
 351                 icv[0] = crc;
 352                 icv[1] = crc >> 8;
 353                 icv[2] = crc >> 16;
 354                 icv[3] = crc >> 24;
 355
 356                 sg_init_one(&sg, pos, len+4);
 357
 358
 359                 crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
 360                 ret = crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
 361         }
 362
 363         tkey->tx_iv16++;
 364         if (tkey->tx_iv16 == 0) {
 365                 tkey->tx_phase1_done = 0;
 366                 tkey->tx_iv32++;
 367         }
 368
 369         if (!tcb_desc->bHwSec)
 370                 return ret;
 371         else
 372                 return 0;
 373
 374
 375 }
 376
 377 static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 378 {
 379         struct rtllib_tkip_data *tkey = priv;
 380         u8 keyidx, *pos;
 381         u32 iv32;
 382         u16 iv16;
 383         struct rtllib_hdr_4addr *hdr;
 384         struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
 385                                     MAX_DEV_ADDR_SIZE);
 386         struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
 387         u8 rc4key[16];
 388         u8 icv[4];
 389         u32 crc;
 390         struct scatterlist sg;
 391         int plen;
 392         if (skb->len < hdr_len + 8 + 4)
 393                 return -1;
 394
 395         hdr = (struct rtllib_hdr_4addr *) skb->data;
 396         pos = skb->data + hdr_len;
 397         keyidx = pos[3];
 398         if (!(keyidx & (1 << 5))) {
 399                 if (net_ratelimit()) {
 400                         printk(KERN_DEBUG "TKIP: received packet without ExtIV"
 401                                " flag from %pM\n", hdr->addr2);
 402                 }
 403                 return -2;
 404         }
 405         keyidx >>= 6;
 406         if (tkey->key_idx != keyidx) {
 407                 printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
 408                        "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
 409                 return -6;
 410         }
 411         if (!tkey->key_set) {
 412                 if (net_ratelimit()) {
 413                         printk(KERN_DEBUG "TKIP: received packet from %pM"
 414                                " with keyid=%d that does not have a configured"
 415                                " key\n", hdr->addr2, keyidx);
 416                 }
 417                 return -3;
 418         }
 419         iv16 = (pos[0] << 8) | pos[2];
 420         iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
 421         pos += 8;
 422
 423         if (!tcb_desc->bHwSec || (skb->cb[0] == 1)) {
 424                 if ((iv32 < tkey->rx_iv32 ||
 425                     (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) &&
 426                     tkey->initialized) {
 427                         if (net_ratelimit()) {
 428                                 printk(KERN_DEBUG "TKIP: replay detected: STA="
 429                                        " %pM previous TSC %08x%04x received "
 430                                       "TSC %08x%04x\n",hdr->addr2,
 431                                       tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
 432                         }
 433                         tkey->dot11RSNAStatsTKIPReplays++;
 434                         return -4;
 435                 }
 436                 tkey->initialized = true;
 437
 438                 if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
 439                         tkip_mixing_phase1(tkey->rx_ttak, tkey->key,
 440                                            hdr->addr2, iv32);
 441                         tkey->rx_phase1_done = 1;
 442                 }
 443                 tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
 444
 445                 plen = skb->len - hdr_len - 12;
 446
 447                 sg_init_one(&sg, pos, plen+4);
 448
 449                 crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
 450                 if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
 451                         if (net_ratelimit()) {
 452                                 printk(KERN_DEBUG ": TKIP: failed to decrypt "
 453                                        "received packet from %pM\n",
 454                                        hdr->addr2);
 455                         }
 456                         return -7;
 457                 }
 458
 459                 crc = ~crc32_le(~0, pos, plen);
 460                 icv[0] = crc;
 461                 icv[1] = crc >> 8;
 462                 icv[2] = crc >> 16;
 463                 icv[3] = crc >> 24;
 464
 465                 if (memcmp(icv, pos + plen, 4) != 0) {
 466                         if (iv32 != tkey->rx_iv32) {
 467                                 /* Previously cached Phase1 result was already
 468                                  * lost, so it needs to be recalculated for the
 469                                  * next packet. */
 470                                 tkey->rx_phase1_done = 0;
 471                         }
 472                         if (net_ratelimit()) {
 473                                 printk(KERN_DEBUG "TKIP: ICV error detected: STA="
 474                                 " %pM\n", hdr->addr2);
 475                         }
 476                         tkey->dot11RSNAStatsTKIPICVErrors++;
 477                         return -5;
 478                 }
 479
 480         }
 481
 482         /* Update real counters only after Michael MIC verification has
 483          * completed */
 484         tkey->rx_iv32_new = iv32;
 485         tkey->rx_iv16_new = iv16;
 486
 487         /* Remove IV and ICV */
 488         memmove(skb->data + 8, skb->data, hdr_len);
 489         skb_pull(skb, 8);
 490         skb_trim(skb, skb->len - 4);
 491
 492         return keyidx;
 493 }
 494
 495
 496 static int michael_mic(struct crypto_hash *tfm_michael, u8 *key, u8 *hdr,
 497                        u8 *data, size_t data_len, u8 *mic)
 498 {
 499         struct hash_desc desc;
 500         struct scatterlist sg[2];
 501
 502         if (tfm_michael == NULL) {
 503                 printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
 504                 return -1;
 505         }
 506         sg_init_table(sg, 2);
 507         sg_set_buf(&sg[0], hdr, 16);
 508         sg_set_buf(&sg[1], data, data_len);
 509
 510         if (crypto_hash_setkey(tfm_michael, key, 8))
 511                 return -1;
 512
 513         desc.tfm = tfm_michael;
 514         desc.flags = 0;
 515         return crypto_hash_digest(&desc, sg, data_len + 16, mic);
 516 }
 517
 518 static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
 519 {
 520         struct rtllib_hdr_4addr *hdr11;
 521
 522         hdr11 = (struct rtllib_hdr_4addr *) skb->data;
 523         switch (le16_to_cpu(hdr11->frame_ctl) &
 524                 (RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS)) {
 525         case RTLLIB_FCTL_TODS:
 526                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 527                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 528                 break;
 529         case RTLLIB_FCTL_FROMDS:
 530                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 531                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 532                 break;
 533         case RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS:
 534                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 535                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 536                 break;
 537         case 0:
 538                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 539                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 540                 break;
 541         }
 542
 543         hdr[12] = 0; /* priority */
 544
 545         hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
 546 }
 547
 548
 549 static int rtllib_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
 550 {
 551         struct rtllib_tkip_data *tkey = priv;
 552         u8 *pos;
 553         struct rtllib_hdr_4addr *hdr;
 554
 555         hdr = (struct rtllib_hdr_4addr *) skb->data;
 556
 557         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 558                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 559                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 560                        skb_tailroom(skb), hdr_len, skb->len);
 561                 return -1;
 562         }
 563
 564         michael_mic_hdr(skb, tkey->tx_hdr);
 565
 566         if (RTLLIB_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
 567                 tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 568         pos = skb_put(skb, 8);
 569         if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
 570             skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
 571                 return -1;
 572
 573         return 0;
 574 }
 575
 576
 577 static void rtllib_michael_mic_failure(struct net_device *dev,
 578                                        struct rtllib_hdr_4addr *hdr,
 579                                        int keyidx)
 580 {
 581         union iwreq_data wrqu;
 582         struct iw_michaelmicfailure ev;
 583
 584         /* TODO: needed parameters: count, keyid, key type, TSC */
 585         memset(&ev, 0, sizeof(ev));
 586         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 587         if (hdr->addr1[0] & 0x01)
 588                 ev.flags |= IW_MICFAILURE_GROUP;
 589         else
 590                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 591         ev.src_addr.sa_family = ARPHRD_ETHER;
 592         memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
 593         memset(&wrqu, 0, sizeof(wrqu));
 594         wrqu.data.length = sizeof(ev);
 595         wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
 596 }
 597
 598 static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
 599                                      int hdr_len, void *priv)
 600 {
 601         struct rtllib_tkip_data *tkey = priv;
 602         u8 mic[8];
 603         struct rtllib_hdr_4addr *hdr;
 604
 605         hdr = (struct rtllib_hdr_4addr *) skb->data;
 606
 607         if (!tkey->key_set)
 608                 return -1;
 609
 610         michael_mic_hdr(skb, tkey->rx_hdr);
 611         if (RTLLIB_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
 612                 tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 613
 614         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 615                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 616                 return -1;
 617
 618         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 619                 struct rtllib_hdr_4addr *hdr;
 620                 hdr = (struct rtllib_hdr_4addr *) skb->data;
 621                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 622                        "MSDU from %pM keyidx=%d\n",
 623                        skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 624                        keyidx);
 625                 printk(KERN_DEBUG "%d\n",
 626                        memcmp(mic, skb->data + skb->len - 8, 8) != 0);
 627                 if (skb->dev) {
 628                         printk(KERN_INFO "skb->dev != NULL\n");
 629                         rtllib_michael_mic_failure(skb->dev, hdr, keyidx);
 630                 }
 631                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 632                 return -1;
 633         }
 634
 635         /* Update TSC counters for RX now that the packet verification has
 636          * completed. */
 637         tkey->rx_iv32 = tkey->rx_iv32_new;
 638         tkey->rx_iv16 = tkey->rx_iv16_new;
 639
 640         skb_trim(skb, skb->len - 8);
 641
 642         return 0;
 643 }
 644
 645
 646 static int rtllib_tkip_set_key(void *key, int len, u8 *seq, void *priv)
 647 {
 648         struct rtllib_tkip_data *tkey = priv;
 649         int keyidx;
 650         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 651         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 652         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 653         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 654
 655         keyidx = tkey->key_idx;
 656         memset(tkey, 0, sizeof(*tkey));
 657         tkey->key_idx = keyidx;
 658         tkey->tx_tfm_michael = tfm;
 659         tkey->tx_tfm_arc4 = tfm2;
 660         tkey->rx_tfm_michael = tfm3;
 661         tkey->rx_tfm_arc4 = tfm4;
 662
 663         if (len == TKIP_KEY_LEN) {
 664                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 665                 tkey->key_set = 1;
 666                 tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
 667                 if (seq) {
 668                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 669                                 (seq[3] << 8) | seq[2];
 670                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 671                 }
 672         } else if (len == 0)
 673                 tkey->key_set = 0;
 674         else
 675                 return -1;
 676
 677         return 0;
 678 }
 679
 680
 681 static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 682 {
 683         struct rtllib_tkip_data *tkey = priv;
 684
 685         if (len < TKIP_KEY_LEN)
 686                 return -1;
 687
 688         if (!tkey->key_set)
 689                 return 0;
 690         memcpy(key, tkey->key, TKIP_KEY_LEN);
 691
 692         if (seq) {
 693                 /* Return the sequence number of the last transmitted frame. */
 694                 u16 iv16 = tkey->tx_iv16;
 695                 u32 iv32 = tkey->tx_iv32;
 696                 if (iv16 == 0)
 697                         iv32--;
 698                 iv16--;
 699                 seq[0] = tkey->tx_iv16;
 700                 seq[1] = tkey->tx_iv16 >> 8;
 701                 seq[2] = tkey->tx_iv32;
 702                 seq[3] = tkey->tx_iv32 >> 8;
 703                 seq[4] = tkey->tx_iv32 >> 16;
 704                 seq[5] = tkey->tx_iv32 >> 24;
 705         }
 706
 707         return TKIP_KEY_LEN;
 708 }
 709
 710
 711 static char *rtllib_tkip_print_stats(char *p, void *priv)
 712 {
 713         struct rtllib_tkip_data *tkip = priv;
 714         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 715                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 716                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 717                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 718                      tkip->key_idx, tkip->key_set,
 719                      (tkip->tx_iv32 >> 24) & 0xff,
 720                      (tkip->tx_iv32 >> 16) & 0xff,
 721                      (tkip->tx_iv32 >> 8) & 0xff,
 722                      tkip->tx_iv32 & 0xff,
 723                      (tkip->tx_iv16 >> 8) & 0xff,
 724                      tkip->tx_iv16 & 0xff,
 725                      (tkip->rx_iv32 >> 24) & 0xff,
 726                      (tkip->rx_iv32 >> 16) & 0xff,
 727                      (tkip->rx_iv32 >> 8) & 0xff,
 728                      tkip->rx_iv32 & 0xff,
 729                      (tkip->rx_iv16 >> 8) & 0xff,
 730                      tkip->rx_iv16 & 0xff,
 731                      tkip->dot11RSNAStatsTKIPReplays,
 732                      tkip->dot11RSNAStatsTKIPICVErrors,
 733                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 734         return p;
 735 }
 736
 737 static struct lib80211_crypto_ops rtllib_crypt_tkip = {
 738         .name                   = "R-TKIP",
 739         .init                   = rtllib_tkip_init,
 740         .deinit                 = rtllib_tkip_deinit,
 741         .encrypt_mpdu           = rtllib_tkip_encrypt,
 742         .decrypt_mpdu           = rtllib_tkip_decrypt,
 743         .encrypt_msdu           = rtllib_michael_mic_add,
 744         .decrypt_msdu           = rtllib_michael_mic_verify,
 745         .set_key                = rtllib_tkip_set_key,
 746         .get_key                = rtllib_tkip_get_key,
 747         .print_stats            = rtllib_tkip_print_stats,
 748         .extra_mpdu_prefix_len = 4 + 4, /* IV + ExtIV */
 749         .extra_mpdu_postfix_len = 4,    /* ICV */
 750         .extra_msdu_postfix_len = 8,    /* MIC */
 751         .owner                  = THIS_MODULE,
 752 };
 753
 754
 755 int __init rtllib_crypto_tkip_init(void)
 756 {
 757         return lib80211_register_crypto_ops(&rtllib_crypt_tkip);
 758 }
 759
 760
 761 void __exit rtllib_crypto_tkip_exit(void)
 762 {
 763         lib80211_unregister_crypto_ops(&rtllib_crypt_tkip);
 764 }
 765
 766 module_init(rtllib_crypto_tkip_init);
 767 module_exit(rtllib_crypto_tkip_exit);
 768
 769 MODULE_LICENSE("GPL");