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