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