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