net/ieee80211/ieee80211_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/config.h>
  13 #include <linux/module.h>
  14 #include <linux/init.h>
  15 #include <linux/slab.h>
  16 #include <linux/random.h>
  17 #include <linux/skbuff.h>
  18 #include <linux/netdevice.h>
  19 #include <linux/if_ether.h>
  20 #include <linux/if_arp.h>
  21 #include <asm/string.h>
  22
  23 #include <net/ieee80211.h>
  24
  25 #include <linux/crypto.h>
  26 #include <asm/scatterlist.h>
  27 #include <linux/crc32.h>
  28
  29 MODULE_AUTHOR("Jouni Malinen");
  30 MODULE_DESCRIPTION("Host AP crypt: TKIP");
  31 MODULE_LICENSE("GPL");
  32
  33 struct ieee80211_tkip_data {
  34 #define TKIP_KEY_LEN 32
  35         u8 key[TKIP_KEY_LEN];
  36         int key_set;
  37
  38         u32 tx_iv32;
  39         u16 tx_iv16;
  40         u16 tx_ttak[5];
  41         int tx_phase1_done;
  42
  43         u32 rx_iv32;
  44         u16 rx_iv16;
  45         u16 rx_ttak[5];
  46         int rx_phase1_done;
  47         u32 rx_iv32_new;
  48         u16 rx_iv16_new;
  49
  50         u32 dot11RSNAStatsTKIPReplays;
  51         u32 dot11RSNAStatsTKIPICVErrors;
  52         u32 dot11RSNAStatsTKIPLocalMICFailures;
  53
  54         int key_idx;
  55
  56         struct crypto_tfm *tfm_arc4;
  57         struct crypto_tfm *tfm_michael;
  58
  59         /* scratch buffers for virt_to_page() (crypto API) */
  60         u8 rx_hdr[16], tx_hdr[16];
  61
  62         unsigned long flags;
  63 };
  64
  65 static unsigned long ieee80211_tkip_set_flags(unsigned long flags, void *priv)
  66 {
  67         struct ieee80211_tkip_data *_priv = priv;
  68         unsigned long old_flags = _priv->flags;
  69         _priv->flags = flags;
  70         return old_flags;
  71 }
  72
  73 static unsigned long ieee80211_tkip_get_flags(void *priv)
  74 {
  75         struct ieee80211_tkip_data *_priv = priv;
  76         return _priv->flags;
  77 }
  78
  79 static void *ieee80211_tkip_init(int key_idx)
  80 {
  81         struct ieee80211_tkip_data *priv;
  82
  83         priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
  84         if (priv == NULL)
  85                 goto fail;
  86         memset(priv, 0, sizeof(*priv));
  87
  88         priv->key_idx = key_idx;
  89
  90         priv->tfm_arc4 = crypto_alloc_tfm("arc4", 0);
  91         if (priv->tfm_arc4 == NULL) {
  92                 printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
  93                        "crypto API arc4\n");
  94                 goto fail;
  95         }
  96
  97         priv->tfm_michael = crypto_alloc_tfm("michael_mic", 0);
  98         if (priv->tfm_michael == NULL) {
  99                 printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
 100                        "crypto API michael_mic\n");
 101                 goto fail;
 102         }
 103
 104         return priv;
 105
 106       fail:
 107         if (priv) {
 108                 if (priv->tfm_michael)
 109                         crypto_free_tfm(priv->tfm_michael);
 110                 if (priv->tfm_arc4)
 111                         crypto_free_tfm(priv->tfm_arc4);
 112                 kfree(priv);
 113         }
 114
 115         return NULL;
 116 }
 117
 118 static void ieee80211_tkip_deinit(void *priv)
 119 {
 120         struct ieee80211_tkip_data *_priv = priv;
 121         if (_priv && _priv->tfm_michael)
 122                 crypto_free_tfm(_priv->tfm_michael);
 123         if (_priv && _priv->tfm_arc4)
 124                 crypto_free_tfm(_priv->tfm_arc4);
 125         kfree(priv);
 126 }
 127
 128 static inline u16 RotR1(u16 val)
 129 {
 130         return (val >> 1) | (val << 15);
 131 }
 132
 133 static inline u8 Lo8(u16 val)
 134 {
 135         return val & 0xff;
 136 }
 137
 138 static inline u8 Hi8(u16 val)
 139 {
 140         return val >> 8;
 141 }
 142
 143 static inline u16 Lo16(u32 val)
 144 {
 145         return val & 0xffff;
 146 }
 147
 148 static inline u16 Hi16(u32 val)
 149 {
 150         return val >> 16;
 151 }
 152
 153 static inline u16 Mk16(u8 hi, u8 lo)
 154 {
 155         return lo | (((u16) hi) << 8);
 156 }
 157
 158 static inline u16 Mk16_le(u16 * v)
 159 {
 160         return le16_to_cpu(*v);
 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 static inline u16 _S_(u16 v)
 199 {
 200         u16 t = Sbox[Hi8(v)];
 201         return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
 202 }
 203
 204 #define PHASE1_LOOP_COUNT 8
 205
 206 static void tkip_mixing_phase1(u16 * TTAK, const u8 * TK, const u8 * TA,
 207                                u32 IV32)
 208 {
 209         int i, j;
 210
 211         /* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
 212         TTAK[0] = Lo16(IV32);
 213         TTAK[1] = Hi16(IV32);
 214         TTAK[2] = Mk16(TA[1], TA[0]);
 215         TTAK[3] = Mk16(TA[3], TA[2]);
 216         TTAK[4] = Mk16(TA[5], TA[4]);
 217
 218         for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
 219                 j = 2 * (i & 1);
 220                 TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
 221                 TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
 222                 TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
 223                 TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
 224                 TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
 225         }
 226 }
 227
 228 static void tkip_mixing_phase2(u8 * WEPSeed, const u8 * TK, const u16 * TTAK,
 229                                u16 IV16)
 230 {
 231         /* Make temporary area overlap WEP seed so that the final copy can be
 232          * avoided on little endian hosts. */
 233         u16 *PPK = (u16 *) & WEPSeed[4];
 234
 235         /* Step 1 - make copy of TTAK and bring in TSC */
 236         PPK[0] = TTAK[0];
 237         PPK[1] = TTAK[1];
 238         PPK[2] = TTAK[2];
 239         PPK[3] = TTAK[3];
 240         PPK[4] = TTAK[4];
 241         PPK[5] = TTAK[4] + IV16;
 242
 243         /* Step 2 - 96-bit bijective mixing using S-box */
 244         PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) & TK[0]));
 245         PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) & TK[2]));
 246         PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) & TK[4]));
 247         PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) & TK[6]));
 248         PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) & TK[8]));
 249         PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) & TK[10]));
 250
 251         PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) & TK[12]));
 252         PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) & TK[14]));
 253         PPK[2] += RotR1(PPK[1]);
 254         PPK[3] += RotR1(PPK[2]);
 255         PPK[4] += RotR1(PPK[3]);
 256         PPK[5] += RotR1(PPK[4]);
 257
 258         /* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
 259          * WEPSeed[0..2] is transmitted as WEP IV */
 260         WEPSeed[0] = Hi8(IV16);
 261         WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
 262         WEPSeed[2] = Lo8(IV16);
 263         WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) & TK[0])) >> 1);
 264
 265 #ifdef __BIG_ENDIAN
 266         {
 267                 int i;
 268                 for (i = 0; i < 6; i++)
 269                         PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 270         }
 271 #endif
 272 }
 273
 274 static u8 *ieee80211_tkip_hdr(struct sk_buff *skb, int hdr_len, void *priv)
 275 {
 276         struct ieee80211_tkip_data *tkey = priv;
 277         int len;
 278         u8 *rc4key, *pos, *icv;
 279         struct ieee80211_hdr_4addr *hdr;
 280         u32 crc;
 281
 282         hdr = (struct ieee80211_hdr_4addr *)skb->data;
 283
 284         if (skb_headroom(skb) < 8 || skb->len < hdr_len)
 285                 return NULL;
 286
 287         if (!tkey->tx_phase1_done) {
 288                 tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
 289                                    tkey->tx_iv32);
 290                 tkey->tx_phase1_done = 1;
 291         }
 292         rc4key = kmalloc(16, GFP_ATOMIC);
 293         if (!rc4key)
 294                 return NULL;
 295         tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
 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         icv = skb_put(skb, 4);
 302
 303         *pos++ = *rc4key;
 304         *pos++ = *(rc4key + 1);
 305         *pos++ = *(rc4key + 2);
 306         *pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */ ;
 307         *pos++ = tkey->tx_iv32 & 0xff;
 308         *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
 309         *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
 310         *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
 311
 312         crc = ~crc32_le(~0, pos, len);
 313         icv[0] = crc;
 314         icv[1] = crc >> 8;
 315         icv[2] = crc >> 16;
 316         icv[3] = crc >> 24;
 317
 318         return rc4key;
 319 }
 320
 321 static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 322 {
 323         struct ieee80211_tkip_data *tkey = priv;
 324         int len;
 325         const u8 *rc4key;
 326         u8 *pos;
 327         struct scatterlist sg;
 328
 329         if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
 330                 if (net_ratelimit()) {
 331                         struct ieee80211_hdr_4addr *hdr =
 332                             (struct ieee80211_hdr_4addr *)skb->data;
 333                         printk(KERN_DEBUG "TKIP countermeasures: dropped "
 334                                "TX packet to " MAC_FMT "\n",
 335                                MAC_ARG(hdr->addr1));
 336                 }
 337                 return -1;
 338         }
 339
 340         if (skb_tailroom(skb) < 4 || skb->len < hdr_len)
 341                 return -1;
 342
 343         len = skb->len - hdr_len;
 344         pos = skb->data + hdr_len;
 345
 346         rc4key = ieee80211_tkip_hdr(skb, hdr_len, priv);
 347         if (!rc4key)
 348                 return -1;
 349
 350         crypto_cipher_setkey(tkey->tfm_arc4, rc4key, 16);
 351         sg.page = virt_to_page(pos);
 352         sg.offset = offset_in_page(pos);
 353         sg.length = len + 4;
 354         crypto_cipher_encrypt(tkey->tfm_arc4, &sg, &sg, len + 4);
 355
 356         tkey->tx_iv16++;
 357         if (tkey->tx_iv16 == 0) {
 358                 tkey->tx_phase1_done = 0;
 359                 tkey->tx_iv32++;
 360         }
 361
 362         return 0;
 363 }
 364
 365 static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 366 {
 367         struct ieee80211_tkip_data *tkey = priv;
 368         u8 rc4key[16];
 369         u8 keyidx, *pos;
 370         u32 iv32;
 371         u16 iv16;
 372         struct ieee80211_hdr_4addr *hdr;
 373         u8 icv[4];
 374         u32 crc;
 375         struct scatterlist sg;
 376         int plen;
 377
 378         hdr = (struct ieee80211_hdr_4addr *)skb->data;
 379
 380         if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
 381                 if (net_ratelimit()) {
 382                         printk(KERN_DEBUG "TKIP countermeasures: dropped "
 383                                "received packet from " MAC_FMT "\n",
 384                                MAC_ARG(hdr->addr2));
 385                 }
 386                 return -1;
 387         }
 388
 389         if (skb->len < hdr_len + 8 + 4)
 390                 return -1;
 391
 392         pos = skb->data + hdr_len;
 393         keyidx = pos[3];
 394         if (!(keyidx & (1 << 5))) {
 395                 if (net_ratelimit()) {
 396                         printk(KERN_DEBUG "TKIP: received packet without ExtIV"
 397                                " flag from " MAC_FMT "\n", MAC_ARG(hdr->addr2));
 398                 }
 399                 return -2;
 400         }
 401         keyidx >>= 6;
 402         if (tkey->key_idx != keyidx) {
 403                 printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
 404                        "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
 405                 return -6;
 406         }
 407         if (!tkey->key_set) {
 408                 if (net_ratelimit()) {
 409                         printk(KERN_DEBUG "TKIP: received packet from " MAC_FMT
 410                                " with keyid=%d that does not have a configured"
 411                                " key\n", MAC_ARG(hdr->addr2), keyidx);
 412                 }
 413                 return -3;
 414         }
 415         iv16 = (pos[0] << 8) | pos[2];
 416         iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
 417         pos += 8;
 418
 419         if (iv32 < tkey->rx_iv32 ||
 420             (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) {
 421                 if (net_ratelimit()) {
 422                         printk(KERN_DEBUG "TKIP: replay detected: STA=" MAC_FMT
 423                                " previous TSC %08x%04x received TSC "
 424                                "%08x%04x\n", MAC_ARG(hdr->addr2),
 425                                tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
 426                 }
 427                 tkey->dot11RSNAStatsTKIPReplays++;
 428                 return -4;
 429         }
 430
 431         if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
 432                 tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
 433                 tkey->rx_phase1_done = 1;
 434         }
 435         tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
 436
 437         plen = skb->len - hdr_len - 12;
 438
 439         crypto_cipher_setkey(tkey->tfm_arc4, rc4key, 16);
 440         sg.page = virt_to_page(pos);
 441         sg.offset = offset_in_page(pos);
 442         sg.length = plen + 4;
 443         crypto_cipher_decrypt(tkey->tfm_arc4, &sg, &sg, plen + 4);
 444
 445         crc = ~crc32_le(~0, pos, plen);
 446         icv[0] = crc;
 447         icv[1] = crc >> 8;
 448         icv[2] = crc >> 16;
 449         icv[3] = crc >> 24;
 450         if (memcmp(icv, pos + plen, 4) != 0) {
 451                 if (iv32 != tkey->rx_iv32) {
 452                         /* Previously cached Phase1 result was already lost, so
 453                          * it needs to be recalculated for the next packet. */
 454                         tkey->rx_phase1_done = 0;
 455                 }
 456                 if (net_ratelimit()) {
 457                         printk(KERN_DEBUG "TKIP: ICV error detected: STA="
 458                                MAC_FMT "\n", MAC_ARG(hdr->addr2));
 459                 }
 460                 tkey->dot11RSNAStatsTKIPICVErrors++;
 461                 return -5;
 462         }
 463
 464         /* Update real counters only after Michael MIC verification has
 465          * completed */
 466         tkey->rx_iv32_new = iv32;
 467         tkey->rx_iv16_new = iv16;
 468
 469         /* Remove IV and ICV */
 470         memmove(skb->data + 8, skb->data, hdr_len);
 471         skb_pull(skb, 8);
 472         skb_trim(skb, skb->len - 4);
 473
 474         return keyidx;
 475 }
 476
 477 static int michael_mic(struct ieee80211_tkip_data *tkey, u8 * key, u8 * hdr,
 478                        u8 * data, size_t data_len, u8 * mic)
 479 {
 480         struct scatterlist sg[2];
 481
 482         if (tkey->tfm_michael == NULL) {
 483                 printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
 484                 return -1;
 485         }
 486         sg[0].page = virt_to_page(hdr);
 487         sg[0].offset = offset_in_page(hdr);
 488         sg[0].length = 16;
 489
 490         sg[1].page = virt_to_page(data);
 491         sg[1].offset = offset_in_page(data);
 492         sg[1].length = data_len;
 493
 494         crypto_digest_init(tkey->tfm_michael);
 495         crypto_digest_setkey(tkey->tfm_michael, key, 8);
 496         crypto_digest_update(tkey->tfm_michael, sg, 2);
 497         crypto_digest_final(tkey->tfm_michael, mic);
 498
 499         return 0;
 500 }
 501
 502 static void michael_mic_hdr(struct sk_buff *skb, u8 * hdr)
 503 {
 504         struct ieee80211_hdr_4addr *hdr11;
 505
 506         hdr11 = (struct ieee80211_hdr_4addr *)skb->data;
 507         switch (le16_to_cpu(hdr11->frame_ctl) &
 508                 (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 509         case IEEE80211_FCTL_TODS:
 510                 memcpy(hdr, hdr11->addr3, ETH_ALEN);    /* DA */
 511                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 512                 break;
 513         case IEEE80211_FCTL_FROMDS:
 514                 memcpy(hdr, hdr11->addr1, ETH_ALEN);    /* DA */
 515                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 516                 break;
 517         case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
 518                 memcpy(hdr, hdr11->addr3, ETH_ALEN);    /* DA */
 519                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 520                 break;
 521         case 0:
 522                 memcpy(hdr, hdr11->addr1, ETH_ALEN);    /* DA */
 523                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 524                 break;
 525         }
 526
 527         hdr[12] = 0;            /* priority */
 528         hdr[13] = hdr[14] = hdr[15] = 0;        /* reserved */
 529 }
 530
 531 static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len,
 532                                      void *priv)
 533 {
 534         struct ieee80211_tkip_data *tkey = priv;
 535         u8 *pos;
 536
 537         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 538                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 539                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 540                        skb_tailroom(skb), hdr_len, skb->len);
 541                 return -1;
 542         }
 543
 544         michael_mic_hdr(skb, tkey->tx_hdr);
 545         pos = skb_put(skb, 8);
 546         if (michael_mic(tkey, &tkey->key[16], tkey->tx_hdr,
 547                         skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
 548                 return -1;
 549
 550         return 0;
 551 }
 552
 553 static void ieee80211_michael_mic_failure(struct net_device *dev,
 554                                           struct ieee80211_hdr_4addr *hdr,
 555                                           int keyidx)
 556 {
 557         union iwreq_data wrqu;
 558         struct iw_michaelmicfailure ev;
 559
 560         /* TODO: needed parameters: count, keyid, key type, TSC */
 561         memset(&ev, 0, sizeof(ev));
 562         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 563         if (hdr->addr1[0] & 0x01)
 564                 ev.flags |= IW_MICFAILURE_GROUP;
 565         else
 566                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 567         ev.src_addr.sa_family = ARPHRD_ETHER;
 568         memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
 569         memset(&wrqu, 0, sizeof(wrqu));
 570         wrqu.data.length = sizeof(ev);
 571         wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
 572 }
 573
 574 static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 575                                         int hdr_len, void *priv)
 576 {
 577         struct ieee80211_tkip_data *tkey = priv;
 578         u8 mic[8];
 579
 580         if (!tkey->key_set)
 581                 return -1;
 582
 583         michael_mic_hdr(skb, tkey->rx_hdr);
 584         if (michael_mic(tkey, &tkey->key[24], tkey->rx_hdr,
 585                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 586                 return -1;
 587         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 588                 struct ieee80211_hdr_4addr *hdr;
 589                 hdr = (struct ieee80211_hdr_4addr *)skb->data;
 590                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 591                        "MSDU from " MAC_FMT " keyidx=%d\n",
 592                        skb->dev ? skb->dev->name : "N/A", MAC_ARG(hdr->addr2),
 593                        keyidx);
 594                 if (skb->dev)
 595                         ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
 596                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 597                 return -1;
 598         }
 599
 600         /* Update TSC counters for RX now that the packet verification has
 601          * completed. */
 602         tkey->rx_iv32 = tkey->rx_iv32_new;
 603         tkey->rx_iv16 = tkey->rx_iv16_new;
 604
 605         skb_trim(skb, skb->len - 8);
 606
 607         return 0;
 608 }
 609
 610 static int ieee80211_tkip_set_key(void *key, int len, u8 * seq, void *priv)
 611 {
 612         struct ieee80211_tkip_data *tkey = priv;
 613         int keyidx;
 614         struct crypto_tfm *tfm = tkey->tfm_michael;
 615         struct crypto_tfm *tfm2 = tkey->tfm_arc4;
 616
 617         keyidx = tkey->key_idx;
 618         memset(tkey, 0, sizeof(*tkey));
 619         tkey->key_idx = keyidx;
 620         tkey->tfm_michael = tfm;
 621         tkey->tfm_arc4 = tfm2;
 622         if (len == TKIP_KEY_LEN) {
 623                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 624                 tkey->key_set = 1;
 625                 tkey->tx_iv16 = 1;      /* TSC is initialized to 1 */
 626                 if (seq) {
 627                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 628                             (seq[3] << 8) | seq[2];
 629                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 630                 }
 631         } else if (len == 0)
 632                 tkey->key_set = 0;
 633         else
 634                 return -1;
 635
 636         return 0;
 637 }
 638
 639 static int ieee80211_tkip_get_key(void *key, int len, u8 * seq, void *priv)
 640 {
 641         struct ieee80211_tkip_data *tkey = priv;
 642
 643         if (len < TKIP_KEY_LEN)
 644                 return -1;
 645
 646         if (!tkey->key_set)
 647                 return 0;
 648         memcpy(key, tkey->key, TKIP_KEY_LEN);
 649
 650         if (seq) {
 651                 /* Return the sequence number of the last transmitted frame. */
 652                 u16 iv16 = tkey->tx_iv16;
 653                 u32 iv32 = tkey->tx_iv32;
 654                 if (iv16 == 0)
 655                         iv32--;
 656                 iv16--;
 657                 seq[0] = tkey->tx_iv16;
 658                 seq[1] = tkey->tx_iv16 >> 8;
 659                 seq[2] = tkey->tx_iv32;
 660                 seq[3] = tkey->tx_iv32 >> 8;
 661                 seq[4] = tkey->tx_iv32 >> 16;
 662                 seq[5] = tkey->tx_iv32 >> 24;
 663         }
 664
 665         return TKIP_KEY_LEN;
 666 }
 667
 668 static char *ieee80211_tkip_print_stats(char *p, void *priv)
 669 {
 670         struct ieee80211_tkip_data *tkip = priv;
 671         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 672                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 673                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 674                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 675                      tkip->key_idx, tkip->key_set,
 676                      (tkip->tx_iv32 >> 24) & 0xff,
 677                      (tkip->tx_iv32 >> 16) & 0xff,
 678                      (tkip->tx_iv32 >> 8) & 0xff,
 679                      tkip->tx_iv32 & 0xff,
 680                      (tkip->tx_iv16 >> 8) & 0xff,
 681                      tkip->tx_iv16 & 0xff,
 682                      (tkip->rx_iv32 >> 24) & 0xff,
 683                      (tkip->rx_iv32 >> 16) & 0xff,
 684                      (tkip->rx_iv32 >> 8) & 0xff,
 685                      tkip->rx_iv32 & 0xff,
 686                      (tkip->rx_iv16 >> 8) & 0xff,
 687                      tkip->rx_iv16 & 0xff,
 688                      tkip->dot11RSNAStatsTKIPReplays,
 689                      tkip->dot11RSNAStatsTKIPICVErrors,
 690                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 691         return p;
 692 }
 693
 694 static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
 695         .name = "TKIP",
 696         .init = ieee80211_tkip_init,
 697         .deinit = ieee80211_tkip_deinit,
 698         .encrypt_mpdu = ieee80211_tkip_encrypt,
 699         .decrypt_mpdu = ieee80211_tkip_decrypt,
 700         .encrypt_msdu = ieee80211_michael_mic_add,
 701         .decrypt_msdu = ieee80211_michael_mic_verify,
 702         .set_key = ieee80211_tkip_set_key,
 703         .get_key = ieee80211_tkip_get_key,
 704         .print_stats = ieee80211_tkip_print_stats,
 705         .extra_mpdu_prefix_len = 4 + 4, /* IV + ExtIV */
 706         .extra_mpdu_postfix_len = 4,    /* ICV */
 707         .extra_msdu_postfix_len = 8,    /* MIC */
 708         .get_flags = ieee80211_tkip_get_flags,
 709         .set_flags = ieee80211_tkip_set_flags,
 710         .owner = THIS_MODULE,
 711 };
 712
 713 static int __init ieee80211_crypto_tkip_init(void)
 714 {
 715         return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
 716 }
 717
 718 static void __exit ieee80211_crypto_tkip_exit(void)
 719 {
 720         ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
 721 }
 722
 723 module_init(ieee80211_crypto_tkip_init);
 724 module_exit(ieee80211_crypto_tkip_exit);