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