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