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/version.h>
  13 #include <linux/module.h>
  14 #include <linux/init.h>
  15 #include <linux/slab.h>
  16 #include <linux/random.h>
  17 #include <linux/skbuff.h>
  18 #include <linux/netdevice.h>
  19 #include <linux/if_ether.h>
  20 #include <linux/if_arp.h>
  21 #include <linux/string.h>
  22 #include <linux/crypto.h>
  23 #include <linux/scatterlist.h>
  24 #include <linux/crc32.h>
  25
  26 #include "rtllib.h"
  27
  28 struct rtllib_tkip_data {
  29 #define TKIP_KEY_LEN 32
  30         u8 key[TKIP_KEY_LEN];
  31         int key_set;
  32
  33         u32 tx_iv32;
  34         u16 tx_iv16;
  35         u16 tx_ttak[5];
  36         int tx_phase1_done;
  37
  38         u32 rx_iv32;
  39         u16 rx_iv16;
  40         bool initialized;
  41         u16 rx_ttak[5];
  42         int rx_phase1_done;
  43         u32 rx_iv32_new;
  44         u16 rx_iv16_new;
  45
  46         u32 dot11RSNAStatsTKIPReplays;
  47         u32 dot11RSNAStatsTKIPICVErrors;
  48         u32 dot11RSNAStatsTKIPLocalMICFailures;
  49
  50         int key_idx;
  51         struct crypto_blkcipher *rx_tfm_arc4;
  52         struct crypto_hash *rx_tfm_michael;
  53         struct crypto_blkcipher *tx_tfm_arc4;
  54         struct crypto_hash *tx_tfm_michael;
  55         /* scratch buffers for virt_to_page() (crypto API) */
  56         u8 rx_hdr[16], tx_hdr[16];
  57 };
  58
  59 static void *rtllib_tkip_init(int key_idx)
  60 {
  61         struct rtllib_tkip_data *priv;
  62
  63         priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
  64         if (priv == NULL)
  65                 goto fail;
  66         memset(priv, 0, sizeof(*priv));
  67         priv->key_idx = key_idx;
  68         priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
  69                         CRYPTO_ALG_ASYNC);
  70         if (IS_ERR(priv->tx_tfm_arc4)) {
  71                 printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
  72                                 "crypto API arc4\n");
  73                 priv->tx_tfm_arc4 = NULL;
  74                 goto fail;
  75         }
  76
  77         priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
  78                         CRYPTO_ALG_ASYNC);
  79         if (IS_ERR(priv->tx_tfm_michael)) {
  80                 printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
  81                                 "crypto API michael_mic\n");
  82                 priv->tx_tfm_michael = NULL;
  83                 goto fail;
  84         }
  85
  86         priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
  87                         CRYPTO_ALG_ASYNC);
  88         if (IS_ERR(priv->rx_tfm_arc4)) {
  89                 printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
  90                                 "crypto API arc4\n");
  91                 priv->rx_tfm_arc4 = NULL;
  92                 goto fail;
  93         }
  94
  95         priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
  96                         CRYPTO_ALG_ASYNC);
  97         if (IS_ERR(priv->rx_tfm_michael)) {
  98                 printk(KERN_DEBUG "rtllib_crypt_tkip: could not allocate "
  99                                 "crypto API michael_mic\n");
 100                 priv->rx_tfm_michael = NULL;
 101                 goto fail;
 102         }
 103         return priv;
 104
 105 fail:
 106         if (priv) {
 107                 if (priv->tx_tfm_michael)
 108                         crypto_free_hash(priv->tx_tfm_michael);
 109                 if (priv->tx_tfm_arc4)
 110                         crypto_free_blkcipher(priv->tx_tfm_arc4);
 111                 if (priv->rx_tfm_michael)
 112                         crypto_free_hash(priv->rx_tfm_michael);
 113                 if (priv->rx_tfm_arc4)
 114                         crypto_free_blkcipher(priv->rx_tfm_arc4);
 115                 kfree(priv);
 116         }
 117
 118         return NULL;
 119 }
 120
 121
 122 static void rtllib_tkip_deinit(void *priv)
 123 {
 124         struct rtllib_tkip_data *_priv = priv;
 125
 126         if (_priv) {
 127                 if (_priv->tx_tfm_michael)
 128                         crypto_free_hash(_priv->tx_tfm_michael);
 129                 if (_priv->tx_tfm_arc4)
 130                         crypto_free_blkcipher(_priv->tx_tfm_arc4);
 131                 if (_priv->rx_tfm_michael)
 132                         crypto_free_hash(_priv->rx_tfm_michael);
 133                 if (_priv->rx_tfm_arc4)
 134                         crypto_free_blkcipher(_priv->rx_tfm_arc4);
 135         }
 136         kfree(priv);
 137 }
 138
 139
 140 static inline u16 RotR1(u16 val)
 141 {
 142         return (val >> 1) | (val << 15);
 143 }
 144
 145
 146 static inline u8 Lo8(u16 val)
 147 {
 148         return val & 0xff;
 149 }
 150
 151
 152 static inline u8 Hi8(u16 val)
 153 {
 154         return val >> 8;
 155 }
 156
 157
 158 static inline u16 Lo16(u32 val)
 159 {
 160         return val & 0xffff;
 161 }
 162
 163
 164 static inline u16 Hi16(u32 val)
 165 {
 166         return val >> 16;
 167 }
 168
 169
 170 static inline u16 Mk16(u8 hi, u8 lo)
 171 {
 172         return lo | (((u16) hi) << 8);
 173 }
 174
 175
 176 static inline u16 Mk16_le(u16 *v)
 177 {
 178         return le16_to_cpu(*v);
 179 }
 180
 181
 182 static const u16 Sbox[256] = {
 183         0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
 184         0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
 185         0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
 186         0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
 187         0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
 188         0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
 189         0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
 190         0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
 191         0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
 192         0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
 193         0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
 194         0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
 195         0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
 196         0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
 197         0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
 198         0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
 199         0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
 200         0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
 201         0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
 202         0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
 203         0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
 204         0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
 205         0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
 206         0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
 207         0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
 208         0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
 209         0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
 210         0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
 211         0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
 212         0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
 213         0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
 214         0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
 215 };
 216
 217
 218 static inline u16 _S_(u16 v)
 219 {
 220         u16 t = Sbox[Hi8(v)];
 221         return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
 222 }
 223
 224
 225 #define PHASE1_LOOP_COUNT 8
 226
 227
 228 static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
 229 {
 230         int i, j;
 231
 232         /* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
 233         TTAK[0] = Lo16(IV32);
 234         TTAK[1] = Hi16(IV32);
 235         TTAK[2] = Mk16(TA[1], TA[0]);
 236         TTAK[3] = Mk16(TA[3], TA[2]);
 237         TTAK[4] = Mk16(TA[5], TA[4]);
 238
 239         for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
 240                 j = 2 * (i & 1);
 241                 TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
 242                 TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
 243                 TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
 244                 TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
 245                 TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
 246         }
 247 }
 248
 249
 250 static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 251                                u16 IV16)
 252 {
 253         /* Make temporary area overlap WEP seed so that the final copy can be
 254          * avoided on little endian hosts. */
 255         u16 *PPK = (u16 *) &WEPSeed[4];
 256
 257         /* Step 1 - make copy of TTAK and bring in TSC */
 258         PPK[0] = TTAK[0];
 259         PPK[1] = TTAK[1];
 260         PPK[2] = TTAK[2];
 261         PPK[3] = TTAK[3];
 262         PPK[4] = TTAK[4];
 263         PPK[5] = TTAK[4] + IV16;
 264
 265         /* Step 2 - 96-bit bijective mixing using S-box */
 266         PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) &TK[0]));
 267         PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) &TK[2]));
 268         PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) &TK[4]));
 269         PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) &TK[6]));
 270         PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) &TK[8]));
 271         PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) &TK[10]));
 272
 273         PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) &TK[12]));
 274         PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) &TK[14]));
 275         PPK[2] += RotR1(PPK[1]);
 276         PPK[3] += RotR1(PPK[2]);
 277         PPK[4] += RotR1(PPK[3]);
 278         PPK[5] += RotR1(PPK[4]);
 279
 280         /* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
 281          * WEPSeed[0..2] is transmitted as WEP IV */
 282         WEPSeed[0] = Hi8(IV16);
 283         WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
 284         WEPSeed[2] = Lo8(IV16);
 285         WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) &TK[0])) >> 1);
 286
 287 #ifdef __BIG_ENDIAN
 288         {
 289                 int i;
 290                 for (i = 0; i < 6; i++)
 291                         PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 292         }
 293 #endif
 294 }
 295
 296
 297 static int rtllib_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 298 {
 299         struct rtllib_tkip_data *tkey = priv;
 300                 int len;
 301         u8 *pos;
 302         struct rtllib_hdr_4addr *hdr;
 303         struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
 304                                     MAX_DEV_ADDR_SIZE);
 305         struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
 306         int ret = 0;
 307         u8 rc4key[16],  *icv;
 308         u32 crc;
 309         struct scatterlist sg;
 310
 311         if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
 312             skb->len < hdr_len)
 313                 return -1;
 314
 315         hdr = (struct rtllib_hdr_4addr *) skb->data;
 316
 317         if (!tcb_desc->bHwSec) {
 318                 if (!tkey->tx_phase1_done) {
 319                         tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
 320                                         tkey->tx_iv32);
 321                         tkey->tx_phase1_done = 1;
 322                 }
 323                 tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak,
 324                                    tkey->tx_iv16);
 325         } else
 326         tkey->tx_phase1_done = 1;
 327
 328
 329         len = skb->len - hdr_len;
 330         pos = skb_push(skb, 8);
 331         memmove(pos, pos + 8, hdr_len);
 332         pos += hdr_len;
 333
 334         if (tcb_desc->bHwSec) {
 335                 *pos++ = Hi8(tkey->tx_iv16);
 336                 *pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
 337                 *pos++ = Lo8(tkey->tx_iv16);
 338         } else {
 339                 *pos++ = rc4key[0];
 340                 *pos++ = rc4key[1];
 341                 *pos++ = rc4key[2];
 342         }
 343
 344         *pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
 345         *pos++ = tkey->tx_iv32 & 0xff;
 346         *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
 347         *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
 348         *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
 349
 350         if (!tcb_desc->bHwSec) {
 351                 icv = skb_put(skb, 4);
 352                 crc = ~crc32_le(~0, pos, len);
 353                 icv[0] = crc;
 354                 icv[1] = crc >> 8;
 355                 icv[2] = crc >> 16;
 356                 icv[3] = crc >> 24;
 357
 358                 sg_init_one(&sg, pos, len+4);
 359
 360
 361                 crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
 362                 ret = crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
 363         }
 364
 365         tkey->tx_iv16++;
 366         if (tkey->tx_iv16 == 0) {
 367                 tkey->tx_phase1_done = 0;
 368                 tkey->tx_iv32++;
 369         }
 370
 371         if (!tcb_desc->bHwSec)
 372                 return ret;
 373         else
 374                 return 0;
 375
 376
 377 }
 378
 379 static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 380 {
 381         struct rtllib_tkip_data *tkey = priv;
 382         u8 keyidx, *pos;
 383         u32 iv32;
 384         u16 iv16;
 385         struct rtllib_hdr_4addr *hdr;
 386         struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
 387                                     MAX_DEV_ADDR_SIZE);
 388         struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
 389         u8 rc4key[16];
 390         u8 icv[4];
 391         u32 crc;
 392         struct scatterlist sg;
 393         int plen;
 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                                      struct rtllib_device *ieee)
 603 {
 604         struct rtllib_tkip_data *tkey = priv;
 605         u8 mic[8];
 606         struct rtllib_hdr_4addr *hdr;
 607
 608         hdr = (struct rtllib_hdr_4addr *) skb->data;
 609
 610         if (!tkey->key_set)
 611                 return -1;
 612
 613         michael_mic_hdr(skb, tkey->rx_hdr);
 614         if (RTLLIB_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
 615                 tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 616
 617         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 618                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 619                 return -1;
 620
 621         if ((memcmp(mic, skb->data + skb->len - 8, 8) != 0) ||
 622            (ieee->force_mic_error)) {
 623                 struct rtllib_hdr_4addr *hdr;
 624                 hdr = (struct rtllib_hdr_4addr *) skb->data;
 625                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 626                        "MSDU from %pM keyidx=%d\n",
 627                        skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 628                        keyidx);
 629                 printk(KERN_DEBUG "%d, force_mic_error = %d\n",
 630                        (memcmp(mic, skb->data + skb->len - 8, 8) != 0),\
 631                         ieee->force_mic_error);
 632                 if (skb->dev) {
 633                         printk(KERN_INFO "skb->dev != NULL\n");
 634                         rtllib_michael_mic_failure(skb->dev, hdr, keyidx);
 635                 }
 636                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 637                 ieee->force_mic_error = false;
 638                 return -1;
 639         }
 640
 641         /* Update TSC counters for RX now that the packet verification has
 642          * completed. */
 643         tkey->rx_iv32 = tkey->rx_iv32_new;
 644         tkey->rx_iv16 = tkey->rx_iv16_new;
 645
 646         skb_trim(skb, skb->len - 8);
 647
 648         return 0;
 649 }
 650
 651
 652 static int rtllib_tkip_set_key(void *key, int len, u8 *seq, void *priv)
 653 {
 654         struct rtllib_tkip_data *tkey = priv;
 655         int keyidx;
 656         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 657         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 658         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 659         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 660
 661         keyidx = tkey->key_idx;
 662         memset(tkey, 0, sizeof(*tkey));
 663         tkey->key_idx = keyidx;
 664         tkey->tx_tfm_michael = tfm;
 665         tkey->tx_tfm_arc4 = tfm2;
 666         tkey->rx_tfm_michael = tfm3;
 667         tkey->rx_tfm_arc4 = tfm4;
 668
 669         if (len == TKIP_KEY_LEN) {
 670                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 671                 tkey->key_set = 1;
 672                 tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
 673                 if (seq) {
 674                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 675                                 (seq[3] << 8) | seq[2];
 676                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 677                 }
 678         } else if (len == 0)
 679                 tkey->key_set = 0;
 680         else
 681                 return -1;
 682
 683         return 0;
 684 }
 685
 686
 687 static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 688 {
 689         struct rtllib_tkip_data *tkey = priv;
 690
 691         if (len < TKIP_KEY_LEN)
 692                 return -1;
 693
 694         if (!tkey->key_set)
 695                 return 0;
 696         memcpy(key, tkey->key, TKIP_KEY_LEN);
 697
 698         if (seq) {
 699                 /* Return the sequence number of the last transmitted frame. */
 700                 u16 iv16 = tkey->tx_iv16;
 701                 u32 iv32 = tkey->tx_iv32;
 702                 if (iv16 == 0)
 703                         iv32--;
 704                 iv16--;
 705                 seq[0] = tkey->tx_iv16;
 706                 seq[1] = tkey->tx_iv16 >> 8;
 707                 seq[2] = tkey->tx_iv32;
 708                 seq[3] = tkey->tx_iv32 >> 8;
 709                 seq[4] = tkey->tx_iv32 >> 16;
 710                 seq[5] = tkey->tx_iv32 >> 24;
 711         }
 712
 713         return TKIP_KEY_LEN;
 714 }
 715
 716
 717 static char *rtllib_tkip_print_stats(char *p, void *priv)
 718 {
 719         struct rtllib_tkip_data *tkip = priv;
 720         p += sprintf(p, "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         return p;
 741 }
 742
 743
 744 static struct rtllib_crypto_ops rtllib_crypt_tkip = {
 745         .name                   = "TKIP",
 746         .init                   = rtllib_tkip_init,
 747         .deinit                 = rtllib_tkip_deinit,
 748         .encrypt_mpdu           = rtllib_tkip_encrypt,
 749         .decrypt_mpdu           = rtllib_tkip_decrypt,
 750         .encrypt_msdu           = rtllib_michael_mic_add,
 751         .decrypt_msdu           = rtllib_michael_mic_verify,
 752         .set_key                = rtllib_tkip_set_key,
 753         .get_key                = rtllib_tkip_get_key,
 754         .print_stats            = rtllib_tkip_print_stats,
 755         .extra_prefix_len       = 4 + 4, /* IV + ExtIV */
 756         .extra_postfix_len      = 8 + 4, /* MIC + ICV */
 757         .owner                  = THIS_MODULE,
 758 };
 759
 760
 761 int __init rtllib_crypto_tkip_init(void)
 762 {
 763         return rtllib_register_crypto_ops(&rtllib_crypt_tkip);
 764 }
 765
 766
 767 void __exit rtllib_crypto_tkip_exit(void)
 768 {
 769         rtllib_unregister_crypto_ops(&rtllib_crypt_tkip);
 770 }
 771
 772 void rtllib_tkip_null(void)
 773 {
 774         return;
 775 }