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