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