/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c

https://bitbucket.org/wisechild/galaxy-nexus · C · 779 lines · 618 code · 126 blank · 35 comment · 75 complexity · 6dda0ea0279fe0ae4e818cb34612ab98 MD5 · raw file 

    

  1. /*
    2. 

  2.  * Host AP crypt: host-based TKIP encryption implementation for Host AP driver
    3. 

  3.  *
    4. 

  4.  * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
    5. 

  5.  *
    6. 

  6.  * This program is free software; you can redistribute it and/or modify
    7. 

  7.  * it under the terms of the GNU General Public License version 2 as
    8. 

  8.  * published by the Free Software Foundation. See README and COPYING for
    9. 

  9.  * more details.
    10. 

  10.  */
    11. 

  11. 

  12. //#include <linux/config.h>
    13. 

  13. #include <linux/version.h>
    14. 

  14. #include <linux/module.h>
    15. 

  15. #include <linux/init.h>
    16. 

  16. #include <linux/slab.h>
    17. 

  17. #include <linux/random.h>
    18. 

  18. #include <linux/skbuff.h>
    19. 

  19. #include <linux/netdevice.h>
    20. 

  20. #include <linux/if_ether.h>
    21. 

  21. #include <linux/if_arp.h>
    22. 

  22. #include <asm/string.h>
    23. 

  23. 

  24. #include "ieee80211.h"
    25. 

  25. 

  26. #include <linux/crypto.h>
    27. 

  27. 	#include <linux/scatterlist.h>
    28. 

  28. #include <linux/crc32.h>
    29. 

  29. 

  30. MODULE_AUTHOR("Jouni Malinen");
    31. 

  31. MODULE_DESCRIPTION("Host AP crypt: TKIP");
    32. 

  32. MODULE_LICENSE("GPL");
    33. 

  33. 

  34. struct ieee80211_tkip_data {
    35. 

  35. #define TKIP_KEY_LEN 32
    36. 

  36. 	u8 key[TKIP_KEY_LEN];
    37. 

  37. 	int key_set;
    38. 

  38. 

  39. 	u32 tx_iv32;
    40. 

  40. 	u16 tx_iv16;
    41. 

  41. 	u16 tx_ttak[5];
    42. 

  42. 	int tx_phase1_done;
    43. 

  43. 

  44. 	u32 rx_iv32;
    45. 

  45. 	u16 rx_iv16;
    46. 

  46. 	u16 rx_ttak[5];
    47. 

  47. 	int rx_phase1_done;
    48. 

  48. 	u32 rx_iv32_new;
    49. 

  49. 	u16 rx_iv16_new;
    50. 

  50. 

  51. 	u32 dot11RSNAStatsTKIPReplays;
    52. 

  52. 	u32 dot11RSNAStatsTKIPICVErrors;
    53. 

  53. 	u32 dot11RSNAStatsTKIPLocalMICFailures;
    54. 

  54. 

  55. 	int key_idx;
    56. 

  56. 

  57. 	struct crypto_blkcipher *rx_tfm_arc4;
    58. 

  58. 	struct crypto_hash *rx_tfm_michael;
    59. 

  59. 	struct crypto_blkcipher *tx_tfm_arc4;
    60. 

  60. 	struct crypto_hash *tx_tfm_michael;
    61. 

  61. 

  62. 	/* scratch buffers for virt_to_page() (crypto API) */
    63. 

  63. 	u8 rx_hdr[16], tx_hdr[16];
    64. 

  64. };
    65. 

  65. 

  66. static void * ieee80211_tkip_init(int key_idx)
    67. 

  67. {
    68. 

  68. 	struct ieee80211_tkip_data *priv;
    69. 

  69. 

  70. 	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
    71. 

  71. 	if (priv == NULL)
    72. 

  72. 		goto fail;
    73. 

  73. 	priv->key_idx = key_idx;
    74. 

  74. 

  75. 	priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
    76. 

  76. 			CRYPTO_ALG_ASYNC);
    77. 

  77. 	if (IS_ERR(priv->tx_tfm_arc4)) {
    78. 

  78. 		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
    79. 

  79. 				"crypto API arc4\n");
    80. 

  80. 		priv->tx_tfm_arc4 = NULL;
    81. 

  81. 		goto fail;
    82. 

  82. 	}
    83. 

  83. 

  84. 	priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
    85. 

  85. 			CRYPTO_ALG_ASYNC);
    86. 

  86. 	if (IS_ERR(priv->tx_tfm_michael)) {
    87. 

  87. 		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
    88. 

  88. 				"crypto API michael_mic\n");
    89. 

  89. 		priv->tx_tfm_michael = NULL;
    90. 

  90. 		goto fail;
    91. 

  91. 	}
    92. 

  92. 

  93. 	priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
    94. 

  94. 			CRYPTO_ALG_ASYNC);
    95. 

  95. 	if (IS_ERR(priv->rx_tfm_arc4)) {
    96. 

  96. 		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
    97. 

  97. 				"crypto API arc4\n");
    98. 

  98. 		priv->rx_tfm_arc4 = NULL;
    99. 

  99. 		goto fail;
    100. 

  100. 	}
    101. 

  101. 

  102. 	priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
    103. 

  103. 			CRYPTO_ALG_ASYNC);
    104. 

  104. 	if (IS_ERR(priv->rx_tfm_michael)) {
    105. 

  105. 		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
    106. 

  106. 				"crypto API michael_mic\n");
    107. 

  107. 		priv->rx_tfm_michael = NULL;
    108. 

  108. 		goto fail;
    109. 

  109. 	}
    110. 

  110. 

  111. 	return priv;
    112. 

  112. 

  113. fail:
    114. 

  114. 	if (priv) {
    115. 

  115. 		if (priv->tx_tfm_michael)
    116. 

  116. 			crypto_free_hash(priv->tx_tfm_michael);
    117. 

  117. 		if (priv->tx_tfm_arc4)
    118. 

  118. 			crypto_free_blkcipher(priv->tx_tfm_arc4);
    119. 

  119. 		if (priv->rx_tfm_michael)
    120. 

  120. 			crypto_free_hash(priv->rx_tfm_michael);
    121. 

  121. 		if (priv->rx_tfm_arc4)
    122. 

  122. 			crypto_free_blkcipher(priv->rx_tfm_arc4);
    123. 

  123. 		kfree(priv);
    124. 

  124. 	}
    125. 

  125. 

  126. 	return NULL;
    127. 

  127. }
    128. 

  128. 

  129. 

  130. static void ieee80211_tkip_deinit(void *priv)
    131. 

  131. {
    132. 

  132. 	struct ieee80211_tkip_data *_priv = priv;
    133. 

  133. 

  134. 	if (_priv) {
    135. 

  135. 		if (_priv->tx_tfm_michael)
    136. 

  136. 			crypto_free_hash(_priv->tx_tfm_michael);
    137. 

  137. 		if (_priv->tx_tfm_arc4)
    138. 

  138. 			crypto_free_blkcipher(_priv->tx_tfm_arc4);
    139. 

  139. 		if (_priv->rx_tfm_michael)
    140. 

  140. 			crypto_free_hash(_priv->rx_tfm_michael);
    141. 

  141. 		if (_priv->rx_tfm_arc4)
    142. 

  142. 			crypto_free_blkcipher(_priv->rx_tfm_arc4);
    143. 

  143. 	}
    144. 

  144. 	kfree(priv);
    145. 

  145. }
    146. 

  146. 

  147. 

  148. static inline u16 RotR1(u16 val)
    149. 

  149. {
    150. 

  150. 	return (val >> 1) | (val << 15);
    151. 

  151. }
    152. 

  152. 

  153. 

  154. static inline u8 Lo8(u16 val)
    155. 

  155. {
    156. 

  156. 	return val & 0xff;
    157. 

  157. }
    158. 

  158. 

  159. 

  160. static inline u8 Hi8(u16 val)
    161. 

  161. {
    162. 

  162. 	return val >> 8;
    163. 

  163. }
    164. 

  164. 

  165. 

  166. static inline u16 Lo16(u32 val)
    167. 

  167. {
    168. 

  168. 	return val & 0xffff;
    169. 

  169. }
    170. 

  170. 

  171. 

  172. static inline u16 Hi16(u32 val)
    173. 

  173. {
    174. 

  174. 	return val >> 16;
    175. 

  175. }
    176. 

  176. 

  177. 

  178. static inline u16 Mk16(u8 hi, u8 lo)
    179. 

  179. {
    180. 

  180. 	return lo | (((u16) hi) << 8);
    181. 

  181. }
    182. 

  182. 

  183. 

  184. static inline u16 Mk16_le(u16 *v)
    185. 

  185. {
    186. 

  186. 	return le16_to_cpu(*v);
    187. 

  187. }
    188. 

  188. 

  189. 

  190. static const u16 Sbox[256] =
    191. 

  191. {
    192. 

  192. 	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
    193. 

  193. 	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
    194. 

  194. 	0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
    195. 

  195. 	0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
    196. 

  196. 	0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
    197. 

  197. 	0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
    198. 

  198. 	0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
    199. 

  199. 	0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
    200. 

  200. 	0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
    201. 

  201. 	0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
    202. 

  202. 	0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
    203. 

  203. 	0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
    204. 

  204. 	0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
    205. 

  205. 	0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
    206. 

  206. 	0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
    207. 

  207. 	0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
    208. 

  208. 	0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
    209. 

  209. 	0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
    210. 

  210. 	0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
    211. 

  211. 	0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
    212. 

  212. 	0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
    213. 

  213. 	0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
    214. 

  214. 	0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
    215. 

  215. 	0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
    216. 

  216. 	0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
    217. 

  217. 	0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
    218. 

  218. 	0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
    219. 

  219. 	0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
    220. 

  220. 	0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
    221. 

  221. 	0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
    222. 

  222. 	0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
    223. 

  223. 	0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
    224. 

  224. };
    225. 

  225. 

  226. 

  227. static inline u16 _S_(u16 v)
    228. 

  228. {
    229. 

  229. 	u16 t = Sbox[Hi8(v)];
    230. 

  230. 	return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
    231. 

  231. }
    232. 

  232. 

  233. 

  234. #define PHASE1_LOOP_COUNT 8
    235. 

  235. 

  236. 

  237. static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
    238. 

  238. {
    239. 

  239. 	int i, j;
    240. 

  240. 

  241. 	/* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
    242. 

  242. 	TTAK[0] = Lo16(IV32);
    243. 

  243. 	TTAK[1] = Hi16(IV32);
    244. 

  244. 	TTAK[2] = Mk16(TA[1], TA[0]);
    245. 

  245. 	TTAK[3] = Mk16(TA[3], TA[2]);
    246. 

  246. 	TTAK[4] = Mk16(TA[5], TA[4]);
    247. 

  247. 

  248. 	for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
    249. 

  249. 		j = 2 * (i & 1);
    250. 

  250. 		TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
    251. 

  251. 		TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
    252. 

  252. 		TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
    253. 

  253. 		TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
    254. 

  254. 		TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
    255. 

  255. 	}
    256. 

  256. }
    257. 

  257. 

  258. 

  259. static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
    260. 

  260. 			       u16 IV16)
    261. 

  261. {
    262. 

  262. 	/* Make temporary area overlap WEP seed so that the final copy can be
    263. 

  263. 	 * avoided on little endian hosts. */
    264. 

  264. 	u16 *PPK = (u16 *) &WEPSeed[4];
    265. 

  265. 

  266. 	/* Step 1 - make copy of TTAK and bring in TSC */
    267. 

  267. 	PPK[0] = TTAK[0];
    268. 

  268. 	PPK[1] = TTAK[1];
    269. 

  269. 	PPK[2] = TTAK[2];
    270. 

  270. 	PPK[3] = TTAK[3];
    271. 

  271. 	PPK[4] = TTAK[4];
    272. 

  272. 	PPK[5] = TTAK[4] + IV16;
    273. 

  273. 

  274. 	/* Step 2 - 96-bit bijective mixing using S-box */
    275. 

  275. 	PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) &TK[0]));
    276. 

  276. 	PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) &TK[2]));
    277. 

  277. 	PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) &TK[4]));
    278. 

  278. 	PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) &TK[6]));
    279. 

  279. 	PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) &TK[8]));
    280. 

  280. 	PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) &TK[10]));
    281. 

  281. 

  282. 	PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) &TK[12]));
    283. 

  283. 	PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) &TK[14]));
    284. 

  284. 	PPK[2] += RotR1(PPK[1]);
    285. 

  285. 	PPK[3] += RotR1(PPK[2]);
    286. 

  286. 	PPK[4] += RotR1(PPK[3]);
    287. 

  287. 	PPK[5] += RotR1(PPK[4]);
    288. 

  288. 

  289. 	/* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
    290. 

  290. 	 * WEPSeed[0..2] is transmitted as WEP IV */
    291. 

  291. 	WEPSeed[0] = Hi8(IV16);
    292. 

  292. 	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
    293. 

  293. 	WEPSeed[2] = Lo8(IV16);
    294. 

  294. 	WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) &TK[0])) >> 1);
    295. 

  295. 

  296. #ifdef __BIG_ENDIAN
    297. 

  297. 	{
    298. 

  298. 		int i;
    299. 

  299. 		for (i = 0; i < 6; i++)
    300. 

  300. 			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
    301. 

  301. 	}
    302. 

  302. #endif
    303. 

  303. }
    304. 

  304. 

  305. 

  306. static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
    307. 

  307. {
    308. 

  308. 	struct ieee80211_tkip_data *tkey = priv;
    309. 

  309. 		int len;
    310. 

  310. 	u8 *pos;
    311. 

  311. 	struct ieee80211_hdr_4addr *hdr;
    312. 

  312. 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
    313. 

  313. 	struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
    314. 

  314. 	int ret = 0;
    315. 

  315. 	u8 rc4key[16],  *icv;
    316. 

  316. 	u32 crc;
    317. 

  317. 	struct scatterlist sg;
    318. 

  318. 

  319. 	if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
    320. 

  320. 	    skb->len < hdr_len)
    321. 

  321. 		return -1;
    322. 

  322. 

  323. 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
    324. 

  324. 

  325. 	if (!tcb_desc->bHwSec)
    326. 

  326. 	{
    327. 

  327. 		if (!tkey->tx_phase1_done) {
    328. 

  328. 			tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
    329. 

  329. 					tkey->tx_iv32);
    330. 

  330. 			tkey->tx_phase1_done = 1;
    331. 

  331. 		}
    332. 

  332. 		tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
    333. 

  333. 	}
    334. 

  334. 	else
    335. 

  335. 	tkey->tx_phase1_done = 1;
    336. 

  336. 

  337. 

  338. 	len = skb->len - hdr_len;
    339. 

  339. 	pos = skb_push(skb, 8);
    340. 

  340. 	memmove(pos, pos + 8, hdr_len);
    341. 

  341. 	pos += hdr_len;
    342. 

  342. 

  343. 	if (tcb_desc->bHwSec)
    344. 

  344. 	{
    345. 

  345. 		*pos++ = Hi8(tkey->tx_iv16);
    346. 

  346. 		*pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
    347. 

  347. 		*pos++ = Lo8(tkey->tx_iv16);
    348. 

  348. 	}
    349. 

  349. 	else
    350. 

  350. 	{
    351. 

  351. 		*pos++ = rc4key[0];
    352. 

  352. 		*pos++ = rc4key[1];
    353. 

  353. 		*pos++ = rc4key[2];
    354. 

  354. 	}
    355. 

  355. 

  356. 	*pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
    357. 

  357. 	*pos++ = tkey->tx_iv32 & 0xff;
    358. 

  358. 	*pos++ = (tkey->tx_iv32 >> 8) & 0xff;
    359. 

  359. 	*pos++ = (tkey->tx_iv32 >> 16) & 0xff;
    360. 

  360. 	*pos++ = (tkey->tx_iv32 >> 24) & 0xff;
    361. 

  361. 

  362. 	if (!tcb_desc->bHwSec)
    363. 

  363. 	{
    364. 

  364. 		icv = skb_put(skb, 4);
    365. 

  365. 		crc = ~crc32_le(~0, pos, len);
    366. 

  366. 		icv[0] = crc;
    367. 

  367. 		icv[1] = crc >> 8;
    368. 

  368. 		icv[2] = crc >> 16;
    369. 

  369. 		icv[3] = crc >> 24;
    370. 

  370. 		crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
    371. 

  371. 		sg_init_one(&sg, pos, len+4);
    372. 

  372. 		ret= crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
    373. 

  373. 	}
    374. 

  374. 

  375. 	tkey->tx_iv16++;
    376. 

  376. 	if (tkey->tx_iv16 == 0) {
    377. 

  377. 		tkey->tx_phase1_done = 0;
    378. 

  378. 		tkey->tx_iv32++;
    379. 

  379. 	}
    380. 

  380. 

  381. 	if (!tcb_desc->bHwSec)
    382. 

  382. 		return ret;
    383. 

  383. 	else
    384. 

  384. 		return 0;
    385. 

  385. 

  386. 

  387. }
    388. 

  388. 

  389. static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
    390. 

  390. {
    391. 

  391. 	struct ieee80211_tkip_data *tkey = priv;
    392. 

  392. 	u8 keyidx, *pos;
    393. 

  393. 	u32 iv32;
    394. 

  394. 	u16 iv16;
    395. 

  395. 	struct ieee80211_hdr_4addr *hdr;
    396. 

  396. 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
    397. 

  397. 	struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
    398. 

  398. 	u8 rc4key[16];
    399. 

  399. 	u8 icv[4];
    400. 

  400. 	u32 crc;
    401. 

  401. 	struct scatterlist sg;
    402. 

  402. 	int plen;
    403. 

  403. 	if (skb->len < hdr_len + 8 + 4)
    404. 

  404. 		return -1;
    405. 

  405. 

  406. 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
    407. 

  407. 	pos = skb->data + hdr_len;
    408. 

  408. 	keyidx = pos[3];
    409. 

  409. 	if (!(keyidx & (1 << 5))) {
    410. 

  410. 		if (net_ratelimit()) {
    411. 

  411. 			printk(KERN_DEBUG "TKIP: received packet without ExtIV"
    412. 

  412. 			       " flag from %pM\n", hdr->addr2);
    413. 

  413. 		}
    414. 

  414. 		return -2;
    415. 

  415. 	}
    416. 

  416. 	keyidx >>= 6;
    417. 

  417. 	if (tkey->key_idx != keyidx) {
    418. 

  418. 		printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
    419. 

  419. 		       "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
    420. 

  420. 		return -6;
    421. 

  421. 	}
    422. 

  422. 	if (!tkey->key_set) {
    423. 

  423. 		if (net_ratelimit()) {
    424. 

  424. 			printk(KERN_DEBUG "TKIP: received packet from %pM"
    425. 

  425. 			       " with keyid=%d that does not have a configured"
    426. 

  426. 			       " key\n", hdr->addr2, keyidx);
    427. 

  427. 		}
    428. 

  428. 		return -3;
    429. 

  429. 	}
    430. 

  430. 	iv16 = (pos[0] << 8) | pos[2];
    431. 

  431. 	iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
    432. 

  432. 	pos += 8;
    433. 

  433. 

  434. 	if (!tcb_desc->bHwSec)
    435. 

  435. 	{
    436. 

  436. 		if (iv32 < tkey->rx_iv32 ||
    437. 

  437. 		(iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) {
    438. 

  438. 			if (net_ratelimit()) {
    439. 

  439. 				printk(KERN_DEBUG "TKIP: replay detected: STA=%pM"
    440. 

  440. 				" previous TSC %08x%04x received TSC "
    441. 

  441. 				"%08x%04x\n", hdr->addr2,
    442. 

  442. 				tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
    443. 

  443. 			}
    444. 

  444. 			tkey->dot11RSNAStatsTKIPReplays++;
    445. 

  445. 			return -4;
    446. 

  446. 		}
    447. 

  447. 

  448. 		if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
    449. 

  449. 			tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
    450. 

  450. 			tkey->rx_phase1_done = 1;
    451. 

  451. 		}
    452. 

  452. 		tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
    453. 

  453. 

  454. 		plen = skb->len - hdr_len - 12;
    455. 

  455. 

  456. 		crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
    457. 

  457. 		sg_init_one(&sg, pos, plen+4);
    458. 

  458. 

  459. 		if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
    460. 

  460. 			if (net_ratelimit()) {
    461. 

  461. 				printk(KERN_DEBUG ": TKIP: failed to decrypt "
    462. 

  462. 						"received packet from %pM\n",
    463. 

  463. 						hdr->addr2);
    464. 

  464. 			}
    465. 

  465. 			return -7;
    466. 

  466. 		}
    467. 

  467. 

  468. 		crc = ~crc32_le(~0, pos, plen);
    469. 

  469. 		icv[0] = crc;
    470. 

  470. 		icv[1] = crc >> 8;
    471. 

  471. 		icv[2] = crc >> 16;
    472. 

  472. 		icv[3] = crc >> 24;
    473. 

  473. 

  474. 		if (memcmp(icv, pos + plen, 4) != 0) {
    475. 

  475. 			if (iv32 != tkey->rx_iv32) {
    476. 

  476. 				/* Previously cached Phase1 result was already lost, so
    477. 

  477. 				* it needs to be recalculated for the next packet. */
    478. 

  478. 				tkey->rx_phase1_done = 0;
    479. 

  479. 			}
    480. 

  480. 			if (net_ratelimit()) {
    481. 

  481. 				printk(KERN_DEBUG "TKIP: ICV error detected: STA="
    482. 

  482. 				"%pM\n", hdr->addr2);
    483. 

  483. 			}
    484. 

  484. 			tkey->dot11RSNAStatsTKIPICVErrors++;
    485. 

  485. 			return -5;
    486. 

  486. 		}
    487. 

  487. 

  488. 	}
    489. 

  489. 

  490. 	/* Update real counters only after Michael MIC verification has
    491. 

  491. 	 * completed */
    492. 

  492. 	tkey->rx_iv32_new = iv32;
    493. 

  493. 	tkey->rx_iv16_new = iv16;
    494. 

  494. 

  495. 	/* Remove IV and ICV */
    496. 

  496. 	memmove(skb->data + 8, skb->data, hdr_len);
    497. 

  497. 	skb_pull(skb, 8);
    498. 

  498. 	skb_trim(skb, skb->len - 4);
    499. 

  499. 

  500. 	return keyidx;
    501. 

  501. }
    502. 

  502. 

  503. static int michael_mic(struct crypto_hash *tfm_michael, u8 * key, u8 * hdr,
    504. 

  504. 		       u8 * data, size_t data_len, u8 * mic)
    505. 

  505. {
    506. 

  506. 	struct hash_desc desc;
    507. 

  507. 	struct scatterlist sg[2];
    508. 

  508. 

  509. 	if (tfm_michael == NULL) {
    510. 

  510. 		printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
    511. 

  511. 		return -1;
    512. 

  512. 	}
    513. 

  513. 

  514. 	sg_init_table(sg, 2);
    515. 

  515. 	sg_set_buf(&sg[0], hdr, 16);
    516. 

  516. 	sg_set_buf(&sg[1], data, data_len);
    517. 

  517. 

  518. 	if (crypto_hash_setkey(tfm_michael, key, 8))
    519. 

  519. 		return -1;
    520. 

  520. 

  521. 	desc.tfm = tfm_michael;
    522. 

  522. 	desc.flags = 0;
    523. 

  523. 	return crypto_hash_digest(&desc, sg, data_len + 16, mic);
    524. 

  524. }
    525. 

  525. 

  526. static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
    527. 

  527. {
    528. 

  528. 	struct ieee80211_hdr_4addr *hdr11;
    529. 

  529. 

  530. 	hdr11 = (struct ieee80211_hdr_4addr *) skb->data;
    531. 

  531. 	switch (le16_to_cpu(hdr11->frame_ctl) &
    532. 

  532. 		(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
    533. 

  533. 	case IEEE80211_FCTL_TODS:
    534. 

  534. 		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
    535. 

  535. 		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
    536. 

  536. 		break;
    537. 

  537. 	case IEEE80211_FCTL_FROMDS:
    538. 

  538. 		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
    539. 

  539. 		memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
    540. 

  540. 		break;
    541. 

  541. 	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
    542. 

  542. 		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
    543. 

  543. 		memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
    544. 

  544. 		break;
    545. 

  545. 	case 0:
    546. 

  546. 		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
    547. 

  547. 		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
    548. 

  548. 		break;
    549. 

  549. 	}
    550. 

  550. 

  551. 	hdr[12] = 0; /* priority */
    552. 

  552. 

  553. 	hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
    554. 

  554. }
    555. 

  555. 

  556. 

  557. static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
    558. 

  558. {
    559. 

  559. 	struct ieee80211_tkip_data *tkey = priv;
    560. 

  560. 	u8 *pos;
    561. 

  561. 	struct ieee80211_hdr_4addr *hdr;
    562. 

  562. 

  563. 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
    564. 

  564. 

  565. 	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
    566. 

  566. 		printk(KERN_DEBUG "Invalid packet for Michael MIC add "
    567. 

  567. 		       "(tailroom=%d hdr_len=%d skb->len=%d)\n",
    568. 

  568. 		       skb_tailroom(skb), hdr_len, skb->len);
    569. 

  569. 		return -1;
    570. 

  570. 	}
    571. 

  571. 

  572. 	michael_mic_hdr(skb, tkey->tx_hdr);
    573. 

  573. 

  574. 	// { david, 2006.9.1
    575. 

  575. 	// fix the wpa process with wmm enabled.
    576. 

  576. 	if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
    577. 

  577. 		tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
    578. 

  578. 	}
    579. 

  579. 	// }
    580. 

  580. 	pos = skb_put(skb, 8);
    581. 

  581. 

  582. 	if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
    583. 

  583. 				skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
    584. 

  584. 		return -1;
    585. 

  585. 

  586. 	return 0;
    587. 

  587. }
    588. 

  588. 

  589. static void ieee80211_michael_mic_failure(struct net_device *dev,
    590. 

  590. 				       struct ieee80211_hdr_4addr *hdr,
    591. 

  591. 				       int keyidx)
    592. 

  592. {
    593. 

  593. 	union iwreq_data wrqu;
    594. 

  594. 	struct iw_michaelmicfailure ev;
    595. 

  595. 

  596. 	/* TODO: needed parameters: count, keyid, key type, TSC */
    597. 

  597. 	memset(&ev, 0, sizeof(ev));
    598. 

  598. 	ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
    599. 

  599. 	if (hdr->addr1[0] & 0x01)
    600. 

  600. 		ev.flags |= IW_MICFAILURE_GROUP;
    601. 

  601. 	else
    602. 

  602. 		ev.flags |= IW_MICFAILURE_PAIRWISE;
    603. 

  603. 	ev.src_addr.sa_family = ARPHRD_ETHER;
    604. 

  604. 	memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
    605. 

  605. 	memset(&wrqu, 0, sizeof(wrqu));
    606. 

  606. 	wrqu.data.length = sizeof(ev);
    607. 

  607. 	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
    608. 

  608. }
    609. 

  609. 

  610. static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
    611. 

  611. 				     int hdr_len, void *priv)
    612. 

  612. {
    613. 

  613. 	struct ieee80211_tkip_data *tkey = priv;
    614. 

  614. 	u8 mic[8];
    615. 

  615. 	struct ieee80211_hdr_4addr *hdr;
    616. 

  616. 

  617. 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
    618. 

  618. 

  619. 	if (!tkey->key_set)
    620. 

  620. 		return -1;
    621. 

  621. 

  622. 	michael_mic_hdr(skb, tkey->rx_hdr);
    623. 

  623. 	// { david, 2006.9.1
    624. 

  624. 	// fix the wpa process with wmm enabled.
    625. 

  625. 	if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
    626. 

  626. 		tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
    627. 

  627. 	}
    628. 

  628. 	// }
    629. 

  629. 

  630. 	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
    631. 

  631. 				skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
    632. 

  632. 		return -1;
    633. 

  633. 	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
    634. 

  634. 		struct ieee80211_hdr_4addr *hdr;
    635. 

  635. 		hdr = (struct ieee80211_hdr_4addr *) skb->data;
    636. 

  636. 		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
    637. 

  637. 		       "MSDU from %pM keyidx=%d\n",
    638. 

  638. 		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
    639. 

  639. 		       keyidx);
    640. 

  640. 		if (skb->dev)
    641. 

  641. 			ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
    642. 

  642. 		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
    643. 

  643. 		return -1;
    644. 

  644. 	}
    645. 

  645. 

  646. 	/* Update TSC counters for RX now that the packet verification has
    647. 

  647. 	 * completed. */
    648. 

  648. 	tkey->rx_iv32 = tkey->rx_iv32_new;
    649. 

  649. 	tkey->rx_iv16 = tkey->rx_iv16_new;
    650. 

  650. 

  651. 	skb_trim(skb, skb->len - 8);
    652. 

  652. 

  653. 	return 0;
    654. 

  654. }
    655. 

  655. 

  656. 

  657. static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
    658. 

  658. {
    659. 

  659. 	struct ieee80211_tkip_data *tkey = priv;
    660. 

  660. 	int keyidx;
    661. 

  661. 	struct crypto_hash *tfm = tkey->tx_tfm_michael;
    662. 

  662. 	struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
    663. 

  663. 	struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
    664. 

  664. 	struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
    665. 

  665. 

  666. 	keyidx = tkey->key_idx;
    667. 

  667. 	memset(tkey, 0, sizeof(*tkey));
    668. 

  668. 	tkey->key_idx = keyidx;
    669. 

  669. 	tkey->tx_tfm_michael = tfm;
    670. 

  670. 	tkey->tx_tfm_arc4 = tfm2;
    671. 

  671. 	tkey->rx_tfm_michael = tfm3;
    672. 

  672. 	tkey->rx_tfm_arc4 = tfm4;
    673. 

  673. 

  674. 	if (len == TKIP_KEY_LEN) {
    675. 

  675. 		memcpy(tkey->key, key, TKIP_KEY_LEN);
    676. 

  676. 		tkey->key_set = 1;
    677. 

  677. 		tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
    678. 

  678. 		if (seq) {
    679. 

  679. 			tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
    680. 

  680. 				(seq[3] << 8) | seq[2];
    681. 

  681. 			tkey->rx_iv16 = (seq[1] << 8) | seq[0];
    682. 

  682. 		}
    683. 

  683. 	} else if (len == 0)
    684. 

  684. 		tkey->key_set = 0;
    685. 

  685. 	else
    686. 

  686. 		return -1;
    687. 

  687. 

  688. 	return 0;
    689. 

  689. }
    690. 

  690. 

  691. 

  692. static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
    693. 

  693. {
    694. 

  694. 	struct ieee80211_tkip_data *tkey = priv;
    695. 

  695. 

  696. 	if (len < TKIP_KEY_LEN)
    697. 

  697. 		return -1;
    698. 

  698. 

  699. 	if (!tkey->key_set)
    700. 

  700. 		return 0;
    701. 

  701. 	memcpy(key, tkey->key, TKIP_KEY_LEN);
    702. 

  702. 

  703. 	if (seq) {
    704. 

  704. 		/* Return the sequence number of the last transmitted frame. */
    705. 

  705. 		u16 iv16 = tkey->tx_iv16;
    706. 

  706. 		u32 iv32 = tkey->tx_iv32;
    707. 

  707. 		if (iv16 == 0)
    708. 

  708. 			iv32--;
    709. 

  709. 		iv16--;
    710. 

  710. 		seq[0] = tkey->tx_iv16;
    711. 

  711. 		seq[1] = tkey->tx_iv16 >> 8;
    712. 

  712. 		seq[2] = tkey->tx_iv32;
    713. 

  713. 		seq[3] = tkey->tx_iv32 >> 8;
    714. 

  714. 		seq[4] = tkey->tx_iv32 >> 16;
    715. 

  715. 		seq[5] = tkey->tx_iv32 >> 24;
    716. 

  716. 	}
    717. 

  717. 

  718. 	return TKIP_KEY_LEN;
    719. 

  719. }
    720. 

  720. 

  721. 

  722. static char * ieee80211_tkip_print_stats(char *p, void *priv)
    723. 

  723. {
    724. 

  724. 	struct ieee80211_tkip_data *tkip = priv;
    725. 

  725. 	p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
    726. 

  726. 		     "tx_pn=%02x%02x%02x%02x%02x%02x "
    727. 

  727. 		     "rx_pn=%02x%02x%02x%02x%02x%02x "
    728. 

  728. 		     "replays=%d icv_errors=%d local_mic_failures=%d\n",
    729. 

  729. 		     tkip->key_idx, tkip->key_set,
    730. 

  730. 		     (tkip->tx_iv32 >> 24) & 0xff,
    731. 

  731. 		     (tkip->tx_iv32 >> 16) & 0xff,
    732. 

  732. 		     (tkip->tx_iv32 >> 8) & 0xff,
    733. 

  733. 		     tkip->tx_iv32 & 0xff,
    734. 

  734. 		     (tkip->tx_iv16 >> 8) & 0xff,
    735. 

  735. 		     tkip->tx_iv16 & 0xff,
    736. 

  736. 		     (tkip->rx_iv32 >> 24) & 0xff,
    737. 

  737. 		     (tkip->rx_iv32 >> 16) & 0xff,
    738. 

  738. 		     (tkip->rx_iv32 >> 8) & 0xff,
    739. 

  739. 		     tkip->rx_iv32 & 0xff,
    740. 

  740. 		     (tkip->rx_iv16 >> 8) & 0xff,
    741. 

  741. 		     tkip->rx_iv16 & 0xff,
    742. 

  742. 		     tkip->dot11RSNAStatsTKIPReplays,
    743. 

  743. 		     tkip->dot11RSNAStatsTKIPICVErrors,
    744. 

  744. 		     tkip->dot11RSNAStatsTKIPLocalMICFailures);
    745. 

  745. 	return p;
    746. 

  746. }
    747. 

  747. 

  748. 

  749. static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
    750. 

  750. 	.name			= "TKIP",
    751. 

  751. 	.init			= ieee80211_tkip_init,
    752. 

  752. 	.deinit			= ieee80211_tkip_deinit,
    753. 

  753. 	.encrypt_mpdu		= ieee80211_tkip_encrypt,
    754. 

  754. 	.decrypt_mpdu		= ieee80211_tkip_decrypt,
    755. 

  755. 	.encrypt_msdu		= ieee80211_michael_mic_add,
    756. 

  756. 	.decrypt_msdu		= ieee80211_michael_mic_verify,
    757. 

  757. 	.set_key		= ieee80211_tkip_set_key,
    758. 

  758. 	.get_key		= ieee80211_tkip_get_key,
    759. 

  759. 	.print_stats		= ieee80211_tkip_print_stats,
    760. 

  760. 	.extra_prefix_len	= 4 + 4, /* IV + ExtIV */
    761. 

  761. 	.extra_postfix_len	= 8 + 4, /* MIC + ICV */
    762. 

  762. 	.owner			= THIS_MODULE,
    763. 

  763. };
    764. 

  764. 

  765. int __init ieee80211_crypto_tkip_init(void)
    766. 

  766. {
    767. 

  767. 	return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
    768. 

  768. }
    769. 

  769. 

  770. void __exit ieee80211_crypto_tkip_exit(void)
    771. 

  771. {
    772. 

  772. 	ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
    773. 

  773. }
    774. 

  774. 

  775. void ieee80211_tkip_null(void)
    776. 

  776. {
    777. 

  777. //    printk("============>%s()\n", __FUNCTION__);
    778. 

  778. 	return;
    779. 

  779. }
    780.