drivers/staging/rtl8192e/rtllib_crypt_tkip.c

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