drivers/staging/rtl8192su/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/version.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 "ieee80211.h"
  24
  25 #include <linux/crypto.h>
  26 #include <linux/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         bool initialized;
  46         u16 rx_ttak[5];
  47         int rx_phase1_done;
  48         u32 rx_iv32_new;
  49         u16 rx_iv16_new;
  50
  51         u32 dot11RSNAStatsTKIPReplays;
  52         u32 dot11RSNAStatsTKIPICVErrors;
  53         u32 dot11RSNAStatsTKIPLocalMICFailures;
  54
  55         int key_idx;
  56
  57         struct crypto_blkcipher *rx_tfm_arc4;
  58         struct crypto_hash *rx_tfm_michael;
  59         struct crypto_blkcipher *tx_tfm_arc4;
  60         struct crypto_hash *tx_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
 234 #define PHASE1_LOOP_COUNT 8
 235
 236
 237 static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
 238 {
 239         int i, j;
 240
 241         /* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
 242         TTAK[0] = Lo16(IV32);
 243         TTAK[1] = Hi16(IV32);
 244         TTAK[2] = Mk16(TA[1], TA[0]);
 245         TTAK[3] = Mk16(TA[3], TA[2]);
 246         TTAK[4] = Mk16(TA[5], TA[4]);
 247
 248         for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
 249                 j = 2 * (i & 1);
 250                 TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
 251                 TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
 252                 TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
 253                 TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
 254                 TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
 255         }
 256 }
 257
 258
 259 static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 260                                u16 IV16)
 261 {
 262         /* Make temporary area overlap WEP seed so that the final copy can be
 263          * avoided on little endian hosts. */
 264         u16 *PPK = (u16 *) &WEPSeed[4];
 265
 266         /* Step 1 - make copy of TTAK and bring in TSC */
 267         PPK[0] = TTAK[0];
 268         PPK[1] = TTAK[1];
 269         PPK[2] = TTAK[2];
 270         PPK[3] = TTAK[3];
 271         PPK[4] = TTAK[4];
 272         PPK[5] = TTAK[4] + IV16;
 273
 274         /* Step 2 - 96-bit bijective mixing using S-box */
 275         PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) &TK[0]));
 276         PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) &TK[2]));
 277         PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) &TK[4]));
 278         PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) &TK[6]));
 279         PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) &TK[8]));
 280         PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) &TK[10]));
 281
 282         PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) &TK[12]));
 283         PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) &TK[14]));
 284         PPK[2] += RotR1(PPK[1]);
 285         PPK[3] += RotR1(PPK[2]);
 286         PPK[4] += RotR1(PPK[3]);
 287         PPK[5] += RotR1(PPK[4]);
 288
 289         /* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
 290          * WEPSeed[0..2] is transmitted as WEP IV */
 291         WEPSeed[0] = Hi8(IV16);
 292         WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
 293         WEPSeed[2] = Lo8(IV16);
 294         WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) &TK[0])) >> 1);
 295
 296 #ifdef __BIG_ENDIAN
 297         {
 298                 int i;
 299                 for (i = 0; i < 6; i++)
 300                         PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 301         }
 302 #endif
 303 }
 304
 305
 306 static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 307 {
 308         struct ieee80211_tkip_data *tkey = priv;
 309                 int len;
 310         u8 *pos;
 311         struct ieee80211_hdr_4addr *hdr;
 312         cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 313         struct blkcipher_desc desc = { .tfm = tkey->tx_tfm_arc4 };
 314         int ret = 0;
 315         u8 rc4key[16],  *icv;
 316         u32 crc;
 317         struct scatterlist sg;
 318
 319         if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
 320             skb->len < hdr_len)
 321                 return -1;
 322
 323         hdr = (struct ieee80211_hdr_4addr *) skb->data;
 324
 325         if (!tcb_desc->bHwSec)
 326         {
 327                 if (!tkey->tx_phase1_done) {
 328                         tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
 329                                         tkey->tx_iv32);
 330                         tkey->tx_phase1_done = 1;
 331                 }
 332                 tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
 333         }
 334         else
 335         tkey->tx_phase1_done = 1;
 336
 337
 338         len = skb->len - hdr_len;
 339         pos = skb_push(skb, 8);
 340         memmove(pos, pos + 8, hdr_len);
 341         pos += hdr_len;
 342
 343         if (tcb_desc->bHwSec)
 344         {
 345                 *pos++ = Hi8(tkey->tx_iv16);
 346                 *pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
 347                 *pos++ = Lo8(tkey->tx_iv16);
 348         }
 349         else
 350         {
 351                 *pos++ = rc4key[0];
 352                 *pos++ = rc4key[1];
 353                 *pos++ = rc4key[2];
 354         }
 355
 356         *pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
 357         *pos++ = tkey->tx_iv32 & 0xff;
 358         *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
 359         *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
 360         *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
 361
 362         if (!tcb_desc->bHwSec)
 363         {
 364                 icv = skb_put(skb, 4);
 365                 crc = ~crc32_le(~0, pos, len);
 366                 icv[0] = crc;
 367                 icv[1] = crc >> 8;
 368                 icv[2] = crc >> 16;
 369                 icv[3] = crc >> 24;
 370                 crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
 371                 sg_init_one(&sg, pos, len + 4);
 372                 ret= crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
 373         }
 374
 375         tkey->tx_iv16++;
 376         if (tkey->tx_iv16 == 0) {
 377                 tkey->tx_phase1_done = 0;
 378                 tkey->tx_iv32++;
 379         }
 380
 381         if (!tcb_desc->bHwSec)
 382                 return ret;
 383         else
 384                 return 0;
 385
 386
 387 }
 388
 389 static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 390 {
 391         struct ieee80211_tkip_data *tkey = priv;
 392         u8 keyidx, *pos;
 393         u32 iv32;
 394         u16 iv16;
 395         struct ieee80211_hdr_4addr *hdr;
 396         cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 397         struct blkcipher_desc desc = { .tfm = tkey->rx_tfm_arc4 };
 398         u8 rc4key[16];
 399         u8 icv[4];
 400         u32 crc;
 401         struct scatterlist sg;
 402         int plen;
 403         if (skb->len < hdr_len + 8 + 4)
 404                 return -1;
 405
 406         hdr = (struct ieee80211_hdr_4addr *) skb->data;
 407         pos = skb->data + hdr_len;
 408         keyidx = pos[3];
 409         if (!(keyidx & (1 << 5))) {
 410                 if (net_ratelimit()) {
 411                         printk(KERN_DEBUG "TKIP: received packet without ExtIV"
 412                                " flag from %pM\n", hdr->addr2);
 413                 }
 414                 return -2;
 415         }
 416         keyidx >>= 6;
 417         if (tkey->key_idx != keyidx) {
 418                 printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
 419                        "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
 420                 return -6;
 421         }
 422         if (!tkey->key_set) {
 423                 if (net_ratelimit()) {
 424                         printk(KERN_DEBUG "TKIP: received packet from %pM"
 425                                " with keyid=%d that does not have a configured"
 426                                " key\n", hdr->addr2, keyidx);
 427                 }
 428                 return -3;
 429         }
 430         iv16 = (pos[0] << 8) | pos[2];
 431         iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
 432         pos += 8;
 433
 434         if (!tcb_desc->bHwSec)
 435         {
 436                 if ((iv32 < tkey->rx_iv32 ||
 437                 (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16))&&tkey->initialized) {
 438                         if (net_ratelimit()) {
 439                                 printk(KERN_DEBUG "TKIP: replay detected: STA=%pM"
 440                                 " previous TSC %08x%04x received TSC "
 441                                 "%08x%04x\n", hdr->addr2,
 442                                 tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
 443                         }
 444                         tkey->dot11RSNAStatsTKIPReplays++;
 445                         return -4;
 446                 }
 447                 tkey->initialized = true;
 448
 449                 if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
 450                         tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
 451                         tkey->rx_phase1_done = 1;
 452                 }
 453                 tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
 454
 455                 plen = skb->len - hdr_len - 12;
 456                 sg_init_one(&sg, pos, plen+4);
 457                 crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
 458                 if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
 459                         if (net_ratelimit()) {
 460                                 printk(KERN_DEBUG ": TKIP: failed to decrypt "
 461                                                 "received packet from %pM\n",
 462                                                 hdr->addr2);
 463                         }
 464                         return -7;
 465                 }
 466
 467                 crc = ~crc32_le(~0, pos, plen);
 468                 icv[0] = crc;
 469                 icv[1] = crc >> 8;
 470                 icv[2] = crc >> 16;
 471                 icv[3] = crc >> 24;
 472
 473                 if (memcmp(icv, pos + plen, 4) != 0) {
 474                         if (iv32 != tkey->rx_iv32) {
 475                                 /* Previously cached Phase1 result was already lost, so
 476                                 * it needs to be recalculated for the next packet. */
 477                                 tkey->rx_phase1_done = 0;
 478                         }
 479                         if (net_ratelimit()) {
 480                                 printk(KERN_DEBUG "TKIP: ICV error detected: STA="
 481                                 "%pM\n", hdr->addr2);
 482                         }
 483                         tkey->dot11RSNAStatsTKIPICVErrors++;
 484                         return -5;
 485                 }
 486
 487         }
 488
 489         /* Update real counters only after Michael MIC verification has
 490          * completed */
 491         tkey->rx_iv32_new = iv32;
 492         tkey->rx_iv16_new = iv16;
 493
 494         /* Remove IV and ICV */
 495         memmove(skb->data + 8, skb->data, hdr_len);
 496         skb_pull(skb, 8);
 497         skb_trim(skb, skb->len - 4);
 498
 499         return keyidx;
 500 }
 501
 502 static int michael_mic(struct crypto_hash *tfm_michael, u8 * key, u8 * hdr,
 503                        u8 * data, size_t data_len, u8 * mic)
 504 {
 505         struct hash_desc desc;
 506         struct scatterlist sg[2];
 507
 508         if (tfm_michael == NULL) {
 509                 printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
 510                 return -1;
 511         }
 512
 513         sg_init_table(sg, 2);
 514         sg_set_buf(&sg[0], hdr, 16);
 515         sg_set_buf(&sg[1], data, data_len);
 516
 517         if (crypto_hash_setkey(tfm_michael, key, 8))
 518                 return -1;
 519
 520         desc.tfm = tfm_michael;
 521         desc.flags = 0;
 522         return crypto_hash_digest(&desc, sg, data_len + 16, mic);
 523 }
 524
 525 static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
 526 {
 527         struct ieee80211_hdr_4addr *hdr11;
 528
 529         hdr11 = (struct ieee80211_hdr_4addr *) skb->data;
 530         switch (le16_to_cpu(hdr11->frame_ctl) &
 531                 (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 532         case IEEE80211_FCTL_TODS:
 533                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 534                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 535                 break;
 536         case IEEE80211_FCTL_FROMDS:
 537                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 538                 memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
 539                 break;
 540         case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
 541                 memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
 542                 memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
 543                 break;
 544         case 0:
 545                 memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
 546                 memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
 547                 break;
 548         }
 549
 550         hdr[12] = 0; /* priority */
 551
 552         hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
 553 }
 554
 555
 556 static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
 557 {
 558         struct ieee80211_tkip_data *tkey = priv;
 559         u8 *pos;
 560         struct ieee80211_hdr_4addr *hdr;
 561
 562         hdr = (struct ieee80211_hdr_4addr *) skb->data;
 563
 564         if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 565                 printk(KERN_DEBUG "Invalid packet for Michael MIC add "
 566                        "(tailroom=%d hdr_len=%d skb->len=%d)\n",
 567                        skb_tailroom(skb), hdr_len, skb->len);
 568                 return -1;
 569         }
 570
 571         michael_mic_hdr(skb, tkey->tx_hdr);
 572
 573         if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
 574                 tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 575         }
 576         pos = skb_put(skb, 8);
 577
 578         if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
 579                         skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
 580                 return -1;
 581
 582         return 0;
 583 }
 584
 585 static void ieee80211_michael_mic_failure(struct net_device *dev,
 586                                        struct ieee80211_hdr_4addr *hdr,
 587                                        int keyidx)
 588 {
 589         union iwreq_data wrqu;
 590         struct iw_michaelmicfailure ev;
 591
 592         /* TODO: needed parameters: count, keyid, key type, TSC */
 593         memset(&ev, 0, sizeof(ev));
 594         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 595         if (hdr->addr1[0] & 0x01)
 596                 ev.flags |= IW_MICFAILURE_GROUP;
 597         else
 598                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 599         ev.src_addr.sa_family = ARPHRD_ETHER;
 600         memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
 601         memset(&wrqu, 0, sizeof(wrqu));
 602         wrqu.data.length = sizeof(ev);
 603         wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
 604 }
 605
 606 static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 607                                      int hdr_len, void *priv, struct ieee80211_device* ieee)
 608 {
 609         struct ieee80211_tkip_data *tkey = priv;
 610         u8 mic[8];
 611         struct ieee80211_hdr_4addr *hdr;
 612
 613         hdr = (struct ieee80211_hdr_4addr *) skb->data;
 614
 615         if (!tkey->key_set)
 616                 return -1;
 617
 618         michael_mic_hdr(skb, tkey->rx_hdr);
 619         if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
 620                 tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
 621         }
 622
 623         if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
 624                         skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 625                 return -1;
 626         if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 627                 struct ieee80211_hdr_4addr *hdr;
 628                 hdr = (struct ieee80211_hdr_4addr *) skb->data;
 629                 printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 630                        "MSDU from %pM keyidx=%d\n",
 631                        skb->dev ? skb->dev->name : "N/A", hdr->addr2,
 632                        keyidx);
 633                 printk("%d, force_mic_error = %d\n", (memcmp(mic, skb->data + skb->len - 8, 8) != 0),\
 634                         ieee->force_mic_error);
 635                 if (skb->dev) {
 636                         printk("skb->dev != NULL\n");
 637                         ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
 638                 }
 639                 tkey->dot11RSNAStatsTKIPLocalMICFailures++;
 640                 ieee->force_mic_error = false;
 641                 return -1;
 642         }
 643
 644         /* Update TSC counters for RX now that the packet verification has
 645          * completed. */
 646         tkey->rx_iv32 = tkey->rx_iv32_new;
 647         tkey->rx_iv16 = tkey->rx_iv16_new;
 648
 649         skb_trim(skb, skb->len - 8);
 650
 651         return 0;
 652 }
 653
 654
 655 static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
 656 {
 657         struct ieee80211_tkip_data *tkey = priv;
 658         int keyidx;
 659         struct crypto_hash *tfm = tkey->tx_tfm_michael;
 660         struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
 661         struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
 662         struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
 663
 664         keyidx = tkey->key_idx;
 665         memset(tkey, 0, sizeof(*tkey));
 666         tkey->key_idx = keyidx;
 667         tkey->tx_tfm_michael = tfm;
 668         tkey->tx_tfm_arc4 = tfm2;
 669         tkey->rx_tfm_michael = tfm3;
 670         tkey->rx_tfm_arc4 = tfm4;
 671
 672         if (len == TKIP_KEY_LEN) {
 673                 memcpy(tkey->key, key, TKIP_KEY_LEN);
 674                 tkey->key_set = 1;
 675                 tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
 676                 if (seq) {
 677                         tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
 678                                 (seq[3] << 8) | seq[2];
 679                         tkey->rx_iv16 = (seq[1] << 8) | seq[0];
 680                 }
 681         } else if (len == 0)
 682                 tkey->key_set = 0;
 683         else
 684                 return -1;
 685
 686         return 0;
 687 }
 688
 689
 690 static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 691 {
 692         struct ieee80211_tkip_data *tkey = priv;
 693
 694         if (len < TKIP_KEY_LEN)
 695                 return -1;
 696
 697         if (!tkey->key_set)
 698                 return 0;
 699         memcpy(key, tkey->key, TKIP_KEY_LEN);
 700
 701         if (seq) {
 702                 /* Return the sequence number of the last transmitted frame. */
 703                 u16 iv16 = tkey->tx_iv16;
 704                 u32 iv32 = tkey->tx_iv32;
 705                 if (iv16 == 0)
 706                         iv32--;
 707                 iv16--;
 708                 seq[0] = tkey->tx_iv16;
 709                 seq[1] = tkey->tx_iv16 >> 8;
 710                 seq[2] = tkey->tx_iv32;
 711                 seq[3] = tkey->tx_iv32 >> 8;
 712                 seq[4] = tkey->tx_iv32 >> 16;
 713                 seq[5] = tkey->tx_iv32 >> 24;
 714         }
 715
 716         return TKIP_KEY_LEN;
 717 }
 718
 719
 720 static char * ieee80211_tkip_print_stats(char *p, void *priv)
 721 {
 722         struct ieee80211_tkip_data *tkip = priv;
 723         p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
 724                      "tx_pn=%02x%02x%02x%02x%02x%02x "
 725                      "rx_pn=%02x%02x%02x%02x%02x%02x "
 726                      "replays=%d icv_errors=%d local_mic_failures=%d\n",
 727                      tkip->key_idx, tkip->key_set,
 728                      (tkip->tx_iv32 >> 24) & 0xff,
 729                      (tkip->tx_iv32 >> 16) & 0xff,
 730                      (tkip->tx_iv32 >> 8) & 0xff,
 731                      tkip->tx_iv32 & 0xff,
 732                      (tkip->tx_iv16 >> 8) & 0xff,
 733                      tkip->tx_iv16 & 0xff,
 734                      (tkip->rx_iv32 >> 24) & 0xff,
 735                      (tkip->rx_iv32 >> 16) & 0xff,
 736                      (tkip->rx_iv32 >> 8) & 0xff,
 737                      tkip->rx_iv32 & 0xff,
 738                      (tkip->rx_iv16 >> 8) & 0xff,
 739                      tkip->rx_iv16 & 0xff,
 740                      tkip->dot11RSNAStatsTKIPReplays,
 741                      tkip->dot11RSNAStatsTKIPICVErrors,
 742                      tkip->dot11RSNAStatsTKIPLocalMICFailures);
 743         return p;
 744 }
 745
 746
 747 static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
 748         .name                   = "TKIP",
 749         .init                   = ieee80211_tkip_init,
 750         .deinit                 = ieee80211_tkip_deinit,
 751         .encrypt_mpdu           = ieee80211_tkip_encrypt,
 752         .decrypt_mpdu           = ieee80211_tkip_decrypt,
 753         .encrypt_msdu           = ieee80211_michael_mic_add,
 754         .decrypt_msdu           = ieee80211_michael_mic_verify,
 755         .set_key                = ieee80211_tkip_set_key,
 756         .get_key                = ieee80211_tkip_get_key,
 757         .print_stats            = ieee80211_tkip_print_stats,
 758         .extra_prefix_len       = 4 + 4, /* IV + ExtIV */
 759         .extra_postfix_len      = 8 + 4, /* MIC + ICV */
 760         .owner                  = THIS_MODULE,
 761 };
 762
 763 int ieee80211_crypto_tkip_init(void)
 764 {
 765         return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
 766 }
 767
 768 void ieee80211_crypto_tkip_exit(void)
 769 {
 770         ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
 771 }
 772
 773 void ieee80211_tkip_null(void)
 774 {
 775         return;
 776 }