1 /*
2  * lib80211 crypt: host-based TKIP encryption implementation for lib80211
3  *
4  * Copyright (c) 2003-2004, Jouni Malinen <j@w1.fi>
5  * Copyright (c) 2008, John W. Linville <linville@tuxdriver.com>
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License version 2 as
9  * published by the Free Software Foundation. See README and COPYING for
10  * more details.
11  */
12 
13 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
14 
15 #include <linux/err.h>
16 #include <linux/module.h>
17 #include <linux/init.h>
18 #include <linux/slab.h>
19 #include <linux/random.h>
20 #include <linux/scatterlist.h>
21 #include <linux/skbuff.h>
22 #include <linux/netdevice.h>
23 #include <linux/mm.h>
24 #include <linux/if_ether.h>
25 #include <linux/if_arp.h>
26 #include <asm/string.h>
27 
28 #include <linux/wireless.h>
29 #include <linux/ieee80211.h>
30 #include <net/iw_handler.h>
31 
32 #include <crypto/hash.h>
33 #include <linux/crypto.h>
34 #include <linux/crc32.h>
35 
36 #include <net/lib80211.h>
37 
38 MODULE_AUTHOR("Jouni Malinen");
39 MODULE_DESCRIPTION("lib80211 crypt: TKIP");
40 MODULE_LICENSE("GPL");
41 
42 #define TKIP_HDR_LEN 8
43 
44 struct lib80211_tkip_data {
45 #define TKIP_KEY_LEN 32
46 	u8 key[TKIP_KEY_LEN];
47 	int key_set;
48 
49 	u32 tx_iv32;
50 	u16 tx_iv16;
51 	u16 tx_ttak[5];
52 	int tx_phase1_done;
53 
54 	u32 rx_iv32;
55 	u16 rx_iv16;
56 	u16 rx_ttak[5];
57 	int rx_phase1_done;
58 	u32 rx_iv32_new;
59 	u16 rx_iv16_new;
60 
61 	u32 dot11RSNAStatsTKIPReplays;
62 	u32 dot11RSNAStatsTKIPICVErrors;
63 	u32 dot11RSNAStatsTKIPLocalMICFailures;
64 
65 	int key_idx;
66 
67 	struct crypto_cipher *rx_tfm_arc4;
68 	struct crypto_shash *rx_tfm_michael;
69 	struct crypto_cipher *tx_tfm_arc4;
70 	struct crypto_shash *tx_tfm_michael;
71 
72 	/* scratch buffers for virt_to_page() (crypto API) */
73 	u8 rx_hdr[16], tx_hdr[16];
74 
75 	unsigned long flags;
76 };
77 
78 static unsigned long lib80211_tkip_set_flags(unsigned long flags, void *priv)
79 {
80 	struct lib80211_tkip_data *_priv = priv;
81 	unsigned long old_flags = _priv->flags;
82 	_priv->flags = flags;
83 	return old_flags;
84 }
85 
86 static unsigned long lib80211_tkip_get_flags(void *priv)
87 {
88 	struct lib80211_tkip_data *_priv = priv;
89 	return _priv->flags;
90 }
91 
92 static void *lib80211_tkip_init(int key_idx)
93 {
94 	struct lib80211_tkip_data *priv;
95 
96 	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
97 	if (priv == NULL)
98 		goto fail;
99 
100 	priv->key_idx = key_idx;
101 
102 	priv->tx_tfm_arc4 = crypto_alloc_cipher("arc4", 0, 0);
103 	if (IS_ERR(priv->tx_tfm_arc4)) {
104 		priv->tx_tfm_arc4 = NULL;
105 		goto fail;
106 	}
107 
108 	priv->tx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
109 	if (IS_ERR(priv->tx_tfm_michael)) {
110 		priv->tx_tfm_michael = NULL;
111 		goto fail;
112 	}
113 
114 	priv->rx_tfm_arc4 = crypto_alloc_cipher("arc4", 0, 0);
115 	if (IS_ERR(priv->rx_tfm_arc4)) {
116 		priv->rx_tfm_arc4 = NULL;
117 		goto fail;
118 	}
119 
120 	priv->rx_tfm_michael = crypto_alloc_shash("michael_mic", 0, 0);
121 	if (IS_ERR(priv->rx_tfm_michael)) {
122 		priv->rx_tfm_michael = NULL;
123 		goto fail;
124 	}
125 
126 	return priv;
127 
128       fail:
129 	if (priv) {
130 		crypto_free_shash(priv->tx_tfm_michael);
131 		crypto_free_cipher(priv->tx_tfm_arc4);
132 		crypto_free_shash(priv->rx_tfm_michael);
133 		crypto_free_cipher(priv->rx_tfm_arc4);
134 		kfree(priv);
135 	}
136 
137 	return NULL;
138 }
139 
140 static void lib80211_tkip_deinit(void *priv)
141 {
142 	struct lib80211_tkip_data *_priv = priv;
143 	if (_priv) {
144 		crypto_free_shash(_priv->tx_tfm_michael);
145 		crypto_free_cipher(_priv->tx_tfm_arc4);
146 		crypto_free_shash(_priv->rx_tfm_michael);
147 		crypto_free_cipher(_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(__le16 * 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((__le16 *) & TK[0]));
269 	PPK[1] += _S_(PPK[0] ^ Mk16_le((__le16 *) & TK[2]));
270 	PPK[2] += _S_(PPK[1] ^ Mk16_le((__le16 *) & TK[4]));
271 	PPK[3] += _S_(PPK[2] ^ Mk16_le((__le16 *) & TK[6]));
272 	PPK[4] += _S_(PPK[3] ^ Mk16_le((__le16 *) & TK[8]));
273 	PPK[5] += _S_(PPK[4] ^ Mk16_le((__le16 *) & TK[10]));
274 
275 	PPK[0] += RotR1(PPK[5] ^ Mk16_le((__le16 *) & TK[12]));
276 	PPK[1] += RotR1(PPK[0] ^ Mk16_le((__le16 *) & 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((__le16 *) & 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 lib80211_tkip_hdr(struct sk_buff *skb, int hdr_len,
299 			      u8 * rc4key, int keylen, void *priv)
300 {
301 	struct lib80211_tkip_data *tkey = priv;
302 	u8 *pos;
303 	struct ieee80211_hdr *hdr;
304 
305 	hdr = (struct ieee80211_hdr *)skb->data;
306 
307 	if (skb_headroom(skb) < TKIP_HDR_LEN || skb->len < hdr_len)
308 		return -1;
309 
310 	if (rc4key == NULL || keylen < 16)
311 		return -1;
312 
313 	if (!tkey->tx_phase1_done) {
314 		tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
315 				   tkey->tx_iv32);
316 		tkey->tx_phase1_done = 1;
317 	}
318 	tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
319 
320 	pos = skb_push(skb, TKIP_HDR_LEN);
321 	memmove(pos, pos + TKIP_HDR_LEN, hdr_len);
322 	pos += hdr_len;
323 
324 	*pos++ = *rc4key;
325 	*pos++ = *(rc4key + 1);
326 	*pos++ = *(rc4key + 2);
327 	*pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */ ;
328 	*pos++ = tkey->tx_iv32 & 0xff;
329 	*pos++ = (tkey->tx_iv32 >> 8) & 0xff;
330 	*pos++ = (tkey->tx_iv32 >> 16) & 0xff;
331 	*pos++ = (tkey->tx_iv32 >> 24) & 0xff;
332 
333 	tkey->tx_iv16++;
334 	if (tkey->tx_iv16 == 0) {
335 		tkey->tx_phase1_done = 0;
336 		tkey->tx_iv32++;
337 	}
338 
339 	return TKIP_HDR_LEN;
340 }
341 
342 static int lib80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
343 {
344 	struct lib80211_tkip_data *tkey = priv;
345 	int len;
346 	u8 rc4key[16], *pos, *icv;
347 	u32 crc;
348 	int i;
349 
350 	if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
351 		struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
352 		net_dbg_ratelimited("TKIP countermeasures: dropped TX packet to %pM\n",
353 				    hdr->addr1);
354 		return -1;
355 	}
356 
357 	if (skb_tailroom(skb) < 4 || skb->len < hdr_len)
358 		return -1;
359 
360 	len = skb->len - hdr_len;
361 	pos = skb->data + hdr_len;
362 
363 	if ((lib80211_tkip_hdr(skb, hdr_len, rc4key, 16, priv)) < 0)
364 		return -1;
365 
366 	crc = ~crc32_le(~0, pos, len);
367 	icv = skb_put(skb, 4);
368 	icv[0] = crc;
369 	icv[1] = crc >> 8;
370 	icv[2] = crc >> 16;
371 	icv[3] = crc >> 24;
372 
373 	crypto_cipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
374 	for (i = 0; i < len + 4; i++)
375 		crypto_cipher_encrypt_one(tkey->tx_tfm_arc4, pos + i, pos + i);
376 	return 0;
377 }
378 
379 /*
380  * deal with seq counter wrapping correctly.
381  * refer to timer_after() for jiffies wrapping handling
382  */
383 static inline int tkip_replay_check(u32 iv32_n, u16 iv16_n,
384 				    u32 iv32_o, u16 iv16_o)
385 {
386 	if ((s32)iv32_n - (s32)iv32_o < 0 ||
387 	    (iv32_n == iv32_o && iv16_n <= iv16_o))
388 		return 1;
389 	return 0;
390 }
391 
392 static int lib80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
393 {
394 	struct lib80211_tkip_data *tkey = priv;
395 	u8 rc4key[16];
396 	u8 keyidx, *pos;
397 	u32 iv32;
398 	u16 iv16;
399 	struct ieee80211_hdr *hdr;
400 	u8 icv[4];
401 	u32 crc;
402 	int plen;
403 	int i;
404 
405 	hdr = (struct ieee80211_hdr *)skb->data;
406 
407 	if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
408 		net_dbg_ratelimited("TKIP countermeasures: dropped received packet from %pM\n",
409 				    hdr->addr2);
410 		return -1;
411 	}
412 
413 	if (skb->len < hdr_len + TKIP_HDR_LEN + 4)
414 		return -1;
415 
416 	pos = skb->data + hdr_len;
417 	keyidx = pos[3];
418 	if (!(keyidx & (1 << 5))) {
419 		net_dbg_ratelimited("TKIP: received packet without ExtIV flag from %pM\n",
420 				    hdr->addr2);
421 		return -2;
422 	}
423 	keyidx >>= 6;
424 	if (tkey->key_idx != keyidx) {
425 		net_dbg_ratelimited("TKIP: RX tkey->key_idx=%d frame keyidx=%d\n",
426 				    tkey->key_idx, keyidx);
427 		return -6;
428 	}
429 	if (!tkey->key_set) {
430 		net_dbg_ratelimited("TKIP: received packet from %pM with keyid=%d that does not have a configured key\n",
431 				    hdr->addr2, keyidx);
432 		return -3;
433 	}
434 	iv16 = (pos[0] << 8) | pos[2];
435 	iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
436 	pos += TKIP_HDR_LEN;
437 
438 	if (tkip_replay_check(iv32, iv16, tkey->rx_iv32, tkey->rx_iv16)) {
439 #ifdef CONFIG_LIB80211_DEBUG
440 		net_dbg_ratelimited("TKIP: replay detected: STA=%pM previous TSC %08x%04x received TSC %08x%04x\n",
441 				    hdr->addr2, tkey->rx_iv32, tkey->rx_iv16,
442 				    iv32, iv16);
443 #endif
444 		tkey->dot11RSNAStatsTKIPReplays++;
445 		return -4;
446 	}
447 
448 	if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
449 		tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
450 		tkey->rx_phase1_done = 1;
451 	}
452 	tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
453 
454 	plen = skb->len - hdr_len - 12;
455 
456 	crypto_cipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
457 	for (i = 0; i < plen + 4; i++)
458 		crypto_cipher_decrypt_one(tkey->rx_tfm_arc4, pos + i, pos + i);
459 
460 	crc = ~crc32_le(~0, pos, plen);
461 	icv[0] = crc;
462 	icv[1] = crc >> 8;
463 	icv[2] = crc >> 16;
464 	icv[3] = crc >> 24;
465 	if (memcmp(icv, pos + plen, 4) != 0) {
466 		if (iv32 != tkey->rx_iv32) {
467 			/* Previously cached Phase1 result was already lost, so
468 			 * it needs to be recalculated for the next packet. */
469 			tkey->rx_phase1_done = 0;
470 		}
471 #ifdef CONFIG_LIB80211_DEBUG
472 		net_dbg_ratelimited("TKIP: ICV error detected: STA=%pM\n",
473 				    hdr->addr2);
474 #endif
475 		tkey->dot11RSNAStatsTKIPICVErrors++;
476 		return -5;
477 	}
478 
479 	/* Update real counters only after Michael MIC verification has
480 	 * completed */
481 	tkey->rx_iv32_new = iv32;
482 	tkey->rx_iv16_new = iv16;
483 
484 	/* Remove IV and ICV */
485 	memmove(skb->data + TKIP_HDR_LEN, skb->data, hdr_len);
486 	skb_pull(skb, TKIP_HDR_LEN);
487 	skb_trim(skb, skb->len - 4);
488 
489 	return keyidx;
490 }
491 
492 static int michael_mic(struct crypto_shash *tfm_michael, u8 *key, u8 *hdr,
493 		       u8 *data, size_t data_len, u8 *mic)
494 {
495 	SHASH_DESC_ON_STACK(desc, tfm_michael);
496 	int err;
497 
498 	if (tfm_michael == NULL) {
499 		pr_warn("%s(): tfm_michael == NULL\n", __func__);
500 		return -1;
501 	}
502 
503 	desc->tfm = tfm_michael;
504 
505 	if (crypto_shash_setkey(tfm_michael, key, 8))
506 		return -1;
507 
508 	err = crypto_shash_init(desc);
509 	if (err)
510 		goto out;
511 	err = crypto_shash_update(desc, hdr, 16);
512 	if (err)
513 		goto out;
514 	err = crypto_shash_update(desc, data, data_len);
515 	if (err)
516 		goto out;
517 	err = crypto_shash_final(desc, mic);
518 
519 out:
520 	shash_desc_zero(desc);
521 	return err;
522 }
523 
524 static void michael_mic_hdr(struct sk_buff *skb, u8 * hdr)
525 {
526 	struct ieee80211_hdr *hdr11;
527 
528 	hdr11 = (struct ieee80211_hdr *)skb->data;
529 
530 	switch (le16_to_cpu(hdr11->frame_control) &
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 	default:
545 		memcpy(hdr, hdr11->addr1, ETH_ALEN);	/* DA */
546 		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN);	/* SA */
547 		break;
548 	}
549 
550 	if (ieee80211_is_data_qos(hdr11->frame_control)) {
551 		hdr[12] = le16_to_cpu(*((__le16 *)ieee80211_get_qos_ctl(hdr11)))
552 			& IEEE80211_QOS_CTL_TID_MASK;
553 	} else
554 		hdr[12] = 0;		/* priority */
555 
556 	hdr[13] = hdr[14] = hdr[15] = 0;	/* reserved */
557 }
558 
559 static int lib80211_michael_mic_add(struct sk_buff *skb, int hdr_len,
560 				     void *priv)
561 {
562 	struct lib80211_tkip_data *tkey = priv;
563 	u8 *pos;
564 
565 	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
566 		printk(KERN_DEBUG "Invalid packet for Michael MIC add "
567 		       "(tailroom=%d hdr_len=%d skb->len=%d)\n",
568 		       skb_tailroom(skb), hdr_len, skb->len);
569 		return -1;
570 	}
571 
572 	michael_mic_hdr(skb, tkey->tx_hdr);
573 	pos = skb_put(skb, 8);
574 	if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
575 			skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
576 		return -1;
577 
578 	return 0;
579 }
580 
581 static void lib80211_michael_mic_failure(struct net_device *dev,
582 					  struct ieee80211_hdr *hdr,
583 					  int keyidx)
584 {
585 	union iwreq_data wrqu;
586 	struct iw_michaelmicfailure ev;
587 
588 	/* TODO: needed parameters: count, keyid, key type, TSC */
589 	memset(&ev, 0, sizeof(ev));
590 	ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
591 	if (hdr->addr1[0] & 0x01)
592 		ev.flags |= IW_MICFAILURE_GROUP;
593 	else
594 		ev.flags |= IW_MICFAILURE_PAIRWISE;
595 	ev.src_addr.sa_family = ARPHRD_ETHER;
596 	memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
597 	memset(&wrqu, 0, sizeof(wrqu));
598 	wrqu.data.length = sizeof(ev);
599 	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
600 }
601 
602 static int lib80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
603 					int hdr_len, void *priv)
604 {
605 	struct lib80211_tkip_data *tkey = priv;
606 	u8 mic[8];
607 
608 	if (!tkey->key_set)
609 		return -1;
610 
611 	michael_mic_hdr(skb, tkey->rx_hdr);
612 	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
613 			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
614 		return -1;
615 	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
616 		struct ieee80211_hdr *hdr;
617 		hdr = (struct ieee80211_hdr *)skb->data;
618 		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
619 		       "MSDU from %pM keyidx=%d\n",
620 		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
621 		       keyidx);
622 		if (skb->dev)
623 			lib80211_michael_mic_failure(skb->dev, hdr, keyidx);
624 		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
625 		return -1;
626 	}
627 
628 	/* Update TSC counters for RX now that the packet verification has
629 	 * completed. */
630 	tkey->rx_iv32 = tkey->rx_iv32_new;
631 	tkey->rx_iv16 = tkey->rx_iv16_new;
632 
633 	skb_trim(skb, skb->len - 8);
634 
635 	return 0;
636 }
637 
638 static int lib80211_tkip_set_key(void *key, int len, u8 * seq, void *priv)
639 {
640 	struct lib80211_tkip_data *tkey = priv;
641 	int keyidx;
642 	struct crypto_shash *tfm = tkey->tx_tfm_michael;
643 	struct crypto_cipher *tfm2 = tkey->tx_tfm_arc4;
644 	struct crypto_shash *tfm3 = tkey->rx_tfm_michael;
645 	struct crypto_cipher *tfm4 = tkey->rx_tfm_arc4;
646 
647 	keyidx = tkey->key_idx;
648 	memset(tkey, 0, sizeof(*tkey));
649 	tkey->key_idx = keyidx;
650 	tkey->tx_tfm_michael = tfm;
651 	tkey->tx_tfm_arc4 = tfm2;
652 	tkey->rx_tfm_michael = tfm3;
653 	tkey->rx_tfm_arc4 = tfm4;
654 	if (len == TKIP_KEY_LEN) {
655 		memcpy(tkey->key, key, TKIP_KEY_LEN);
656 		tkey->key_set = 1;
657 		tkey->tx_iv16 = 1;	/* TSC is initialized to 1 */
658 		if (seq) {
659 			tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
660 			    (seq[3] << 8) | seq[2];
661 			tkey->rx_iv16 = (seq[1] << 8) | seq[0];
662 		}
663 	} else if (len == 0)
664 		tkey->key_set = 0;
665 	else
666 		return -1;
667 
668 	return 0;
669 }
670 
671 static int lib80211_tkip_get_key(void *key, int len, u8 * seq, void *priv)
672 {
673 	struct lib80211_tkip_data *tkey = priv;
674 
675 	if (len < TKIP_KEY_LEN)
676 		return -1;
677 
678 	if (!tkey->key_set)
679 		return 0;
680 	memcpy(key, tkey->key, TKIP_KEY_LEN);
681 
682 	if (seq) {
683 		/* Return the sequence number of the last transmitted frame. */
684 		u16 iv16 = tkey->tx_iv16;
685 		u32 iv32 = tkey->tx_iv32;
686 		if (iv16 == 0)
687 			iv32--;
688 		iv16--;
689 		seq[0] = tkey->tx_iv16;
690 		seq[1] = tkey->tx_iv16 >> 8;
691 		seq[2] = tkey->tx_iv32;
692 		seq[3] = tkey->tx_iv32 >> 8;
693 		seq[4] = tkey->tx_iv32 >> 16;
694 		seq[5] = tkey->tx_iv32 >> 24;
695 	}
696 
697 	return TKIP_KEY_LEN;
698 }
699 
700 static void lib80211_tkip_print_stats(struct seq_file *m, void *priv)
701 {
702 	struct lib80211_tkip_data *tkip = priv;
703 	seq_printf(m,
704 		   "key[%d] alg=TKIP key_set=%d "
705 		   "tx_pn=%02x%02x%02x%02x%02x%02x "
706 		   "rx_pn=%02x%02x%02x%02x%02x%02x "
707 		   "replays=%d icv_errors=%d local_mic_failures=%d\n",
708 		   tkip->key_idx, tkip->key_set,
709 		   (tkip->tx_iv32 >> 24) & 0xff,
710 		   (tkip->tx_iv32 >> 16) & 0xff,
711 		   (tkip->tx_iv32 >> 8) & 0xff,
712 		   tkip->tx_iv32 & 0xff,
713 		   (tkip->tx_iv16 >> 8) & 0xff,
714 		   tkip->tx_iv16 & 0xff,
715 		   (tkip->rx_iv32 >> 24) & 0xff,
716 		   (tkip->rx_iv32 >> 16) & 0xff,
717 		   (tkip->rx_iv32 >> 8) & 0xff,
718 		   tkip->rx_iv32 & 0xff,
719 		   (tkip->rx_iv16 >> 8) & 0xff,
720 		   tkip->rx_iv16 & 0xff,
721 		   tkip->dot11RSNAStatsTKIPReplays,
722 		   tkip->dot11RSNAStatsTKIPICVErrors,
723 		   tkip->dot11RSNAStatsTKIPLocalMICFailures);
724 }
725 
726 static struct lib80211_crypto_ops lib80211_crypt_tkip = {
727 	.name = "TKIP",
728 	.init = lib80211_tkip_init,
729 	.deinit = lib80211_tkip_deinit,
730 	.encrypt_mpdu = lib80211_tkip_encrypt,
731 	.decrypt_mpdu = lib80211_tkip_decrypt,
732 	.encrypt_msdu = lib80211_michael_mic_add,
733 	.decrypt_msdu = lib80211_michael_mic_verify,
734 	.set_key = lib80211_tkip_set_key,
735 	.get_key = lib80211_tkip_get_key,
736 	.print_stats = lib80211_tkip_print_stats,
737 	.extra_mpdu_prefix_len = 4 + 4,	/* IV + ExtIV */
738 	.extra_mpdu_postfix_len = 4,	/* ICV */
739 	.extra_msdu_postfix_len = 8,	/* MIC */
740 	.get_flags = lib80211_tkip_get_flags,
741 	.set_flags = lib80211_tkip_set_flags,
742 	.owner = THIS_MODULE,
743 };
744 
745 static int __init lib80211_crypto_tkip_init(void)
746 {
747 	return lib80211_register_crypto_ops(&lib80211_crypt_tkip);
748 }
749 
750 static void __exit lib80211_crypto_tkip_exit(void)
751 {
752 	lib80211_unregister_crypto_ops(&lib80211_crypt_tkip);
753 }
754 
755 module_init(lib80211_crypto_tkip_init);
756 module_exit(lib80211_crypto_tkip_exit);
757