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                                      struct rtllib_device *ieee)
 601 {
 602         struct rtllib_tkip_data *tkey = priv;
 603         u8 mic[8];
 604         struct rtllib_hdr_4addr *hdr;
 605
 606         hdr = (struct rtllib_hdr_4addr *) skb->data;
 607
 608         if (!tkey->key_set)
 609                 return -1;
 610
 611         michael_mic_hdr(skb, tkey->rx_hdr);
 612         if (RTLLIB_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
 613                 tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 614
 615         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 616                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 617                 return -1;
 618
 619         if ((memcmp(mic, skb->data + skb->len - 8, 8) != 0) ||
 620            (ieee->force_mic_error)) {
 621                 struct rtllib_hdr_4addr *hdr;
 622                 hdr = (struct rtllib_hdr_4addr *) skb->data;
 623                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 624                        "MSDU from %pM keyidx=%d\n",
 625                        skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 626                        keyidx);
 627                 printk(KERN_DEBUG "%d, force_mic_error = %d\n",
 628                        (memcmp(mic, skb->data + skb->len - 8, 8) != 0),\
 629                         ieee->force_mic_error);
 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                 ieee->force_mic_error = false;
 636                 return -1;
 637         }
 638
 639         /* Update TSC counters for RX now that the packet verification has
 640          * completed. */
 641         tkey->rx_iv32 = tkey->rx_iv32_new;
 642         tkey->rx_iv16 = tkey->rx_iv16_new;
 643
 644         skb_trim(skb, skb->len - 8);
 645
 646         return 0;
 647 }
 648
 649
 650 static int rtllib_tkip_set_key(void *key, int len, u8 *seq, void *priv)
 651 {
 652         struct rtllib_tkip_data *tkey = priv;
 653         int keyidx;
 654         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 655         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 656         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 657         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 658
 659         keyidx = tkey->key_idx;
 660         memset(tkey, 0, sizeof(*tkey));
 661         tkey->key_idx = keyidx;
 662         tkey->tx_tfm_michael = tfm;
 663         tkey->tx_tfm_arc4 = tfm2;
 664         tkey->rx_tfm_michael = tfm3;
 665         tkey->rx_tfm_arc4 = tfm4;
 666
 667         if (len == TKIP_KEY_LEN) {
 668                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 669                 tkey->key_set = 1;
 670                 tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
 671                 if (seq) {
 672                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 673                                 (seq[3] << 8) | seq[2];
 674                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 675                 }
 676         } else if (len == 0)
 677                 tkey->key_set = 0;
 678         else
 679                 return -1;
 680
 681         return 0;
 682 }
 683
 684
 685 static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 686 {
 687         struct rtllib_tkip_data *tkey = priv;
 688
 689         if (len < TKIP_KEY_LEN)
 690                 return -1;
 691
 692         if (!tkey->key_set)
 693                 return 0;
 694         memcpy(key, tkey->key, TKIP_KEY_LEN);
 695
 696         if (seq) {
 697                 /* Return the sequence number of the last transmitted frame. */
 698                 u16 iv16 = tkey->tx_iv16;
 699                 u32 iv32 = tkey->tx_iv32;
 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 char *rtllib_tkip_print_stats(char *p, void *priv)
 716 {
 717         struct rtllib_tkip_data *tkip = priv;
 718         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 719                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 720                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 721                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 722                      tkip->key_idx, tkip->key_set,
 723                      (tkip->tx_iv32 >> 24) & 0xff,
 724                      (tkip->tx_iv32 >> 16) & 0xff,
 725                      (tkip->tx_iv32 >> 8) & 0xff,
 726                      tkip->tx_iv32 & 0xff,
 727                      (tkip->tx_iv16 >> 8) & 0xff,
 728                      tkip->tx_iv16 & 0xff,
 729                      (tkip->rx_iv32 >> 24) & 0xff,
 730                      (tkip->rx_iv32 >> 16) & 0xff,
 731                      (tkip->rx_iv32 >> 8) & 0xff,
 732                      tkip->rx_iv32 & 0xff,
 733                      (tkip->rx_iv16 >> 8) & 0xff,
 734                      tkip->rx_iv16 & 0xff,
 735                      tkip->dot11RSNAStatsTKIPReplays,
 736                      tkip->dot11RSNAStatsTKIPICVErrors,
 737                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 738         return p;
 739 }
 740
 741 static struct rtllib_crypto_ops rtllib_crypt_tkip = {
 742         .name                   = "TKIP",
 743         .init                   = rtllib_tkip_init,
 744         .deinit                 = rtllib_tkip_deinit,
 745         .encrypt_mpdu           = rtllib_tkip_encrypt,
 746         .decrypt_mpdu           = rtllib_tkip_decrypt,
 747         .encrypt_msdu           = rtllib_michael_mic_add,
 748         .decrypt_msdu           = rtllib_michael_mic_verify,
 749         .set_key                = rtllib_tkip_set_key,
 750         .get_key                = rtllib_tkip_get_key,
 751         .print_stats            = rtllib_tkip_print_stats,
 752         .extra_prefix_len       = 4 + 4, /* IV + ExtIV */
 753         .extra_postfix_len      = 8 + 4, /* MIC + ICV */
 754         .owner                  = THIS_MODULE,
 755 };
 756
 757
 758 int __init rtllib_crypto_tkip_init(void)
 759 {
 760         return rtllib_register_crypto_ops(&rtllib_crypt_tkip);
 761 }
 762
 763
 764 void __exit rtllib_crypto_tkip_exit(void)
 765 {
 766         rtllib_unregister_crypto_ops(&rtllib_crypt_tkip);
 767 }
 768
 769 module_init(rtllib_crypto_tkip_init);
 770 module_exit(rtllib_crypto_tkip_exit);
 771
 772 MODULE_LICENSE("GPL");