net/wireless/lib80211_crypt_tkip.c

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