xref: /OK3568_Linux_fs/kernel/drivers/net/wireless/rockchip_wlan/ssv6xxx/smac/sec_tkip.c (revision 4882a59341e53eb6f0b4789bf948001014eff981) 
1 /*
2  * Copyright (c) 2015 South Silicon Valley Microelectronics Inc.
3  * Copyright (c) 2015 iComm Corporation
4  *
5  * This program is free software: you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation, either version 3 of the License, or
8  * (at your option) any later version.
9  * This program is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12  * See the GNU General Public License for more details.
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #include <linux/err.h>
18 #include <linux/module.h>
19 #include <linux/init.h>
20 #include <linux/slab.h>
21 #include <linux/random.h>
22 #include <linux/scatterlist.h>
23 #include <linux/skbuff.h>
24 #include <linux/netdevice.h>
25 #include <linux/mm.h>
26 #include <linux/if_ether.h>
27 #include <linux/if_arp.h>
28 #include <asm/string.h>
29 #include <linux/version.h>
30 #include <linux/wireless.h>
31 #include <linux/ieee80211.h>
32 #include <net/iw_handler.h>
33 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
34 #include <crypto/hash.h>
35 #include <crypto/skcipher.h>
36 #else
37 #include <linux/crypto.h>
38 #endif
39 #include <linux/crc32.h>
40 #include <net/lib80211.h>
41 #include "sec.h"
42 #define TKIP_HDR_LEN 8
43 struct lib80211_tkip_data {
44  u8 key[TKIP_KEY_LEN];
45  int key_set;
46  u32 tx_iv32;
47  u16 tx_iv16;
48  u16 tx_ttak[5];
49  int tx_phase1_done;
50  u32 rx_iv32;
51  u16 rx_iv16;
52  u16 rx_ttak[5];
53  int rx_phase1_done;
54  u32 rx_iv32_new;
55  u16 rx_iv16_new;
56  u32 dot11RSNAStatsTKIPReplays;
57  u32 dot11RSNAStatsTKIPICVErrors;
58  u32 dot11RSNAStatsTKIPLocalMICFailures;
59  int key_idx;
60 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
61  struct crypto_skcipher *rx_tfm_arc4;
62  struct crypto_ahash *rx_tfm_michael;
63  struct crypto_skcipher *tx_tfm_arc4;
64  struct crypto_ahash *tx_tfm_michael;
65 #else
66  struct crypto_blkcipher *rx_tfm_arc4;
67  struct crypto_hash *rx_tfm_michael;
68  struct crypto_blkcipher *tx_tfm_arc4;
69  struct crypto_hash *tx_tfm_michael;
70 #endif
71  u8 rx_hdr[16], tx_hdr[16];
72  unsigned long flags;
73 };
lib80211_tkip_set_flags(unsigned long flags,void * priv)74 static unsigned long lib80211_tkip_set_flags(unsigned long flags, void *priv)
75 {
76  struct lib80211_tkip_data *_priv = priv;
77  unsigned long old_flags = _priv->flags;
78  _priv->flags = flags;
79  return old_flags;
80 }
lib80211_tkip_get_flags(void * priv)81 static unsigned long lib80211_tkip_get_flags(void *priv)
82 {
83  struct lib80211_tkip_data *_priv = priv;
84  return _priv->flags;
85 }
86 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
lib80211_tkip_init(int key_idx)87 static void *lib80211_tkip_init(int key_idx)
88 {
89  struct lib80211_tkip_data *priv;
90  priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
91  if (priv == NULL)
92   goto fail;
93  priv->key_idx = key_idx;
94  priv->tx_tfm_arc4 = crypto_alloc_skcipher("ecb(arc4)", 0,
95         CRYPTO_ALG_ASYNC);
96  if (IS_ERR(priv->tx_tfm_arc4)) {
97   priv->tx_tfm_arc4 = NULL;
98   goto fail;
99  }
100  priv->tx_tfm_michael = crypto_alloc_ahash("michael_mic", 0,
101         CRYPTO_ALG_ASYNC);
102  if (IS_ERR(priv->tx_tfm_michael)) {
103   priv->tx_tfm_michael = NULL;
104   goto fail;
105  }
106  priv->rx_tfm_arc4 = crypto_alloc_skcipher("ecb(arc4)", 0,
107         CRYPTO_ALG_ASYNC);
108  if (IS_ERR(priv->rx_tfm_arc4)) {
109   priv->rx_tfm_arc4 = NULL;
110   goto fail;
111  }
112  priv->rx_tfm_michael = crypto_alloc_ahash("michael_mic", 0,
113         CRYPTO_ALG_ASYNC);
114  if (IS_ERR(priv->rx_tfm_michael)) {
115   priv->rx_tfm_michael = NULL;
116   goto fail;
117  }
118  return priv;
119       fail:
120  if (priv) {
121   crypto_free_ahash(priv->tx_tfm_michael);
122   crypto_free_skcipher(priv->tx_tfm_arc4);
123   crypto_free_ahash(priv->rx_tfm_michael);
124   crypto_free_skcipher(priv->rx_tfm_arc4);
125   kfree(priv);
126  }
127  return NULL;
128 }
129 #else
lib80211_tkip_init(int key_idx)130 static void *lib80211_tkip_init(int key_idx)
131 {
132  struct lib80211_tkip_data *priv;
133  priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
134  if (priv == NULL)
135   goto fail;
136  priv->key_idx = key_idx;
137  priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
138       CRYPTO_ALG_ASYNC);
139  if (IS_ERR(priv->tx_tfm_arc4)) {
140   priv->tx_tfm_arc4 = NULL;
141   goto fail;
142  }
143  priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
144        CRYPTO_ALG_ASYNC);
145  if (IS_ERR(priv->tx_tfm_michael)) {
146   priv->tx_tfm_michael = NULL;
147   goto fail;
148  }
149  priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
150       CRYPTO_ALG_ASYNC);
151  if (IS_ERR(priv->rx_tfm_arc4)) {
152   priv->rx_tfm_arc4 = NULL;
153   goto fail;
154  }
155  priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
156        CRYPTO_ALG_ASYNC);
157  if (IS_ERR(priv->rx_tfm_michael)) {
158   priv->rx_tfm_michael = NULL;
159   goto fail;
160  }
161  return priv;
162       fail:
163  if (priv) {
164   if (priv->tx_tfm_michael)
165    crypto_free_hash(priv->tx_tfm_michael);
166   if (priv->tx_tfm_arc4)
167    crypto_free_blkcipher(priv->tx_tfm_arc4);
168   if (priv->rx_tfm_michael)
169    crypto_free_hash(priv->rx_tfm_michael);
170   if (priv->rx_tfm_arc4)
171    crypto_free_blkcipher(priv->rx_tfm_arc4);
172   kfree(priv);
173  }
174  return NULL;
175 }
176 #endif
177 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
lib80211_tkip_deinit(void * priv)178 static void lib80211_tkip_deinit(void *priv)
179 {
180  struct lib80211_tkip_data *_priv = priv;
181  if (_priv) {
182   crypto_free_ahash(_priv->tx_tfm_michael);
183   crypto_free_skcipher(_priv->tx_tfm_arc4);
184   crypto_free_ahash(_priv->rx_tfm_michael);
185   crypto_free_skcipher(_priv->rx_tfm_arc4);
186  }
187  kfree(priv);
188 }
189 #else
lib80211_tkip_deinit(void * priv)190 static void lib80211_tkip_deinit(void *priv)
191 {
192  struct lib80211_tkip_data *_priv = priv;
193  if (_priv) {
194   if (_priv->tx_tfm_michael)
195    crypto_free_hash(_priv->tx_tfm_michael);
196   if (_priv->tx_tfm_arc4)
197    crypto_free_blkcipher(_priv->tx_tfm_arc4);
198   if (_priv->rx_tfm_michael)
199    crypto_free_hash(_priv->rx_tfm_michael);
200   if (_priv->rx_tfm_arc4)
201    crypto_free_blkcipher(_priv->rx_tfm_arc4);
202  }
203  kfree(priv);
204 }
205 #endif
RotR1(u16 val)206 static inline u16 RotR1(u16 val)
207 {
208  return (val >> 1) | (val << 15);
209 }
Lo8(u16 val)210 static inline u8 Lo8(u16 val)
211 {
212  return val & 0xff;
213 }
Hi8(u16 val)214 static inline u8 Hi8(u16 val)
215 {
216  return val >> 8;
217 }
Lo16(u32 val)218 static inline u16 Lo16(u32 val)
219 {
220  return val & 0xffff;
221 }
Hi16(u32 val)222 static inline u16 Hi16(u32 val)
223 {
224  return val >> 16;
225 }
Mk16(u8 hi,u8 lo)226 static inline u16 Mk16(u8 hi, u8 lo)
227 {
228  return lo | (((u16) hi) << 8);
229 }
Mk16_le(__le16 * v)230 static inline u16 Mk16_le(__le16 * v)
231 {
232  return le16_to_cpu(*v);
233 }
234 static const u16 Sbox[256] = {
235  0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
236  0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
237  0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
238  0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
239  0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
240  0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
241  0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
242  0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
243  0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
244  0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
245  0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
246  0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
247  0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
248  0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
249  0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
250  0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
251  0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
252  0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
253  0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
254  0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
255  0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
256  0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
257  0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
258  0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
259  0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
260  0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
261  0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
262  0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
263  0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
264  0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
265  0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
266  0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
267 };
_S_(u16 v)268 static inline u16 _S_(u16 v)
269 {
270  u16 t = Sbox[Hi8(v)];
271  return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
272 }
273 #define PHASE1_LOOP_COUNT 8
tkip_mixing_phase1(u16 * TTAK,const u8 * TK,const u8 * TA,u32 IV32)274 static void tkip_mixing_phase1(u16 * TTAK, const u8 * TK, const u8 * TA,
275           u32 IV32)
276 {
277  int i, j;
278  TTAK[0] = Lo16(IV32);
279  TTAK[1] = Hi16(IV32);
280  TTAK[2] = Mk16(TA[1], TA[0]);
281  TTAK[3] = Mk16(TA[3], TA[2]);
282  TTAK[4] = Mk16(TA[5], TA[4]);
283  for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
284   j = 2 * (i & 1);
285   TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
286   TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
287   TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
288   TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
289   TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
290  }
291 }
tkip_mixing_phase2(u8 * WEPSeed,const u8 * TK,const u16 * TTAK,u16 IV16)292 static void tkip_mixing_phase2(u8 * WEPSeed, const u8 * TK, const u16 * TTAK,
293           u16 IV16)
294 {
295  u16 *PPK = (u16 *) & WEPSeed[4];
296  PPK[0] = TTAK[0];
297  PPK[1] = TTAK[1];
298  PPK[2] = TTAK[2];
299  PPK[3] = TTAK[3];
300  PPK[4] = TTAK[4];
301  PPK[5] = TTAK[4] + IV16;
302  PPK[0] += _S_(PPK[5] ^ Mk16_le((__le16 *) & TK[0]));
303  PPK[1] += _S_(PPK[0] ^ Mk16_le((__le16 *) & TK[2]));
304  PPK[2] += _S_(PPK[1] ^ Mk16_le((__le16 *) & TK[4]));
305  PPK[3] += _S_(PPK[2] ^ Mk16_le((__le16 *) & TK[6]));
306  PPK[4] += _S_(PPK[3] ^ Mk16_le((__le16 *) & TK[8]));
307  PPK[5] += _S_(PPK[4] ^ Mk16_le((__le16 *) & TK[10]));
308  PPK[0] += RotR1(PPK[5] ^ Mk16_le((__le16 *) & TK[12]));
309  PPK[1] += RotR1(PPK[0] ^ Mk16_le((__le16 *) & TK[14]));
310  PPK[2] += RotR1(PPK[1]);
311  PPK[3] += RotR1(PPK[2]);
312  PPK[4] += RotR1(PPK[3]);
313  PPK[5] += RotR1(PPK[4]);
314  WEPSeed[0] = Hi8(IV16);
315  WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
316  WEPSeed[2] = Lo8(IV16);
317  WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((__le16 *) & TK[0])) >> 1);
318 #ifdef __BIG_ENDIAN
319  {
320   int i;
321   for (i = 0; i < 6; i++)
322    PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
323  }
324 #endif
325 }
lib80211_tkip_hdr(struct sk_buff * skb,int hdr_len,u8 * rc4key,int keylen,void * priv)326 static int lib80211_tkip_hdr(struct sk_buff *skb, int hdr_len,
327          u8 * rc4key, int keylen, void *priv)
328 {
329  struct lib80211_tkip_data *tkey = priv;
330  u8 *pos;
331  struct ieee80211_hdr *hdr;
332  hdr = (struct ieee80211_hdr *)skb->data;
333  if (skb_headroom(skb) < TKIP_HDR_LEN || skb->len < hdr_len)
334   return -1;
335  if (rc4key == NULL || keylen < 16)
336   return -1;
337  if (!tkey->tx_phase1_done) {
338   tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
339        tkey->tx_iv32);
340   tkey->tx_phase1_done = 1;
341  }
342  tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
343  pos = skb_push(skb, TKIP_HDR_LEN);
344  memmove(pos, pos + TKIP_HDR_LEN, hdr_len);
345  pos += hdr_len;
346  *pos++ = *rc4key;
347  *pos++ = *(rc4key + 1);
348  *pos++ = *(rc4key + 2);
349  *pos++ = (tkey->key_idx << 6) | (1 << 5) ;
350  *pos++ = tkey->tx_iv32 & 0xff;
351  *pos++ = (tkey->tx_iv32 >> 8) & 0xff;
352  *pos++ = (tkey->tx_iv32 >> 16) & 0xff;
353  *pos++ = (tkey->tx_iv32 >> 24) & 0xff;
354  tkey->tx_iv16++;
355  if (tkey->tx_iv16 == 0) {
356   tkey->tx_phase1_done = 0;
357   tkey->tx_iv32++;
358  }
359  return TKIP_HDR_LEN;
360 }
lib80211_tkip_encrypt(struct sk_buff * skb,int hdr_len,void * priv)361 static int lib80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
362 {
363  struct lib80211_tkip_data *tkey = priv;
364 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
365  SKCIPHER_REQUEST_ON_STACK(req, tkey->tx_tfm_arc4);
366  int err;
367 #else
368  struct blkcipher_desc desc = { .tfm = tkey->tx_tfm_arc4 };
369 #endif
370  int len;
371  u8 rc4key[16], *pos, *icv;
372  u32 crc;
373  struct scatterlist sg;
374  if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
375   if (net_ratelimit()) {
376    struct ieee80211_hdr *hdr =
377        (struct ieee80211_hdr *)skb->data;
378    printk(KERN_DEBUG ": TKIP countermeasures: dropped "
379           "TX packet to %pM\n", hdr->addr1);
380   }
381   return -1;
382  }
383  if (skb_tailroom(skb) < 4 || skb->len < hdr_len)
384   return -1;
385  len = skb->len - hdr_len;
386  pos = skb->data + hdr_len;
387  if ((lib80211_tkip_hdr(skb, hdr_len, rc4key, 16, priv)) < 0)
388   return -1;
389  crc = ~crc32_le(~0, pos, len);
390  icv = skb_put(skb, 4);
391  icv[0] = crc;
392  icv[1] = crc >> 8;
393  icv[2] = crc >> 16;
394  icv[3] = crc >> 24;
395 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
396  crypto_skcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
397  sg_init_one(&sg, pos, len + 4);
398  skcipher_request_set_tfm(req, tkey->tx_tfm_arc4);
399  skcipher_request_set_callback(req, 0, NULL, NULL);
400  skcipher_request_set_crypt(req, &sg, &sg, len + 4, NULL);
401  err = crypto_skcipher_encrypt(req);
402  skcipher_request_zero(req);
403  return err;
404 #else
405  crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
406  sg_init_one(&sg, pos, len + 4);
407  return crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
408 #endif
409 }
tkip_replay_check(u32 iv32_n,u16 iv16_n,u32 iv32_o,u16 iv16_o)410 static inline int tkip_replay_check(u32 iv32_n, u16 iv16_n,
411         u32 iv32_o, u16 iv16_o)
412 {
413  if ((s32)iv32_n - (s32)iv32_o < 0 ||
414      (iv32_n == iv32_o && iv16_n <= iv16_o))
415   return 1;
416  return 0;
417 }
lib80211_tkip_decrypt(struct sk_buff * skb,int hdr_len,void * priv)418 static int lib80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
419 {
420  struct lib80211_tkip_data *tkey = priv;
421 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
422  SKCIPHER_REQUEST_ON_STACK(req, tkey->rx_tfm_arc4);
423  int err;
424 #else
425  struct blkcipher_desc desc = { .tfm = tkey->rx_tfm_arc4 };
426 #endif
427  u8 rc4key[16];
428  u8 keyidx, *pos;
429  u32 iv32;
430  u16 iv16;
431  struct ieee80211_hdr *hdr;
432  u8 icv[4];
433  u32 crc;
434  struct scatterlist sg;
435  int plen;
436  hdr = (struct ieee80211_hdr *)skb->data;
437  if (tkey->flags & IEEE80211_CRYPTO_TKIP_COUNTERMEASURES) {
438   if (net_ratelimit()) {
439    printk(KERN_DEBUG ": TKIP countermeasures: dropped "
440           "received packet from %pM\n", hdr->addr2);
441   }
442   return -1;
443  }
444  if (skb->len < hdr_len + TKIP_HDR_LEN + 4)
445   return -1;
446  pos = skb->data + hdr_len;
447  keyidx = pos[3];
448  if (!(keyidx & (1 << 5))) {
449   if (net_ratelimit()) {
450    printk(KERN_DEBUG "TKIP: received packet without ExtIV"
451           " flag from %pM\n", hdr->addr2);
452   }
453   return -2;
454  }
455  keyidx >>= 6;
456  if (tkey->key_idx != keyidx) {
457   printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
458          "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
459   return -6;
460  }
461  if (!tkey->key_set) {
462   if (net_ratelimit()) {
463    printk(KERN_DEBUG "TKIP: received packet from %pM"
464           " with keyid=%d that does not have a configured"
465           " key\n", hdr->addr2, keyidx);
466   }
467   return -3;
468  }
469  iv16 = (pos[0] << 8) | pos[2];
470  iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
471  pos += TKIP_HDR_LEN;
472  if (tkip_replay_check(iv32, iv16, tkey->rx_iv32, tkey->rx_iv16)) {
473 #ifdef CONFIG_LIB80211_DEBUG
474   if (net_ratelimit()) {
475    printk(KERN_DEBUG "TKIP: replay detected: STA=%pM"
476           " previous TSC %08x%04x received TSC "
477           "%08x%04x\n", hdr->addr2,
478           tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
479   }
480 #endif
481   tkey->dot11RSNAStatsTKIPReplays++;
482   return -4;
483  }
484  if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
485   tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
486   tkey->rx_phase1_done = 1;
487  }
488  tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
489  plen = skb->len - hdr_len - 12;
490 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
491  crypto_skcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
492  sg_init_one(&sg, pos, plen + 4);
493  skcipher_request_set_tfm(req, tkey->rx_tfm_arc4);
494  skcipher_request_set_callback(req, 0, NULL, NULL);
495  skcipher_request_set_crypt(req, &sg, &sg, plen + 4, NULL);
496  err = crypto_skcipher_decrypt(req);
497  skcipher_request_zero(req);
498  if (err) {
499 #else
500  crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
501  sg_init_one(&sg, pos, plen + 4);
502  if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
503 #endif
504   if (net_ratelimit()) {
505    printk(KERN_DEBUG ": TKIP: failed to decrypt "
506           "received packet from %pM\n",
507           hdr->addr2);
508   }
509   return -7;
510  }
511  crc = ~crc32_le(~0, pos, plen);
512  icv[0] = crc;
513  icv[1] = crc >> 8;
514  icv[2] = crc >> 16;
515  icv[3] = crc >> 24;
516  if (memcmp(icv, pos + plen, 4) != 0) {
517   if (iv32 != tkey->rx_iv32) {
518    tkey->rx_phase1_done = 0;
519   }
520 #ifdef CONFIG_LIB80211_DEBUG
521   if (net_ratelimit()) {
522    printk(KERN_DEBUG "TKIP: ICV error detected: STA="
523           "%pM\n", hdr->addr2);
524   }
525 #endif
526   tkey->dot11RSNAStatsTKIPICVErrors++;
527   return -5;
528  }
529  tkey->rx_iv32_new = iv32;
530  tkey->rx_iv16_new = iv16;
531  memmove(skb->data + TKIP_HDR_LEN, skb->data, hdr_len);
532  skb_pull(skb, TKIP_HDR_LEN);
533  skb_trim(skb, skb->len - 4);
534  return keyidx;
535 }
536 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
537 static int michael_mic(struct crypto_ahash *tfm_michael, u8 * key, u8 * hdr,
538          u8 * data, size_t data_len, u8 * mic)
539 {
540  AHASH_REQUEST_ON_STACK(req, tfm_michael);
541  struct scatterlist sg[2];
542  int err;
543  if (tfm_michael == NULL) {
544   pr_warn("%s(): tfm_michael == NULL\n", __func__);
545   return -1;
546  }
547  sg_init_table(sg, 2);
548  sg_set_buf(&sg[0], hdr, 16);
549  sg_set_buf(&sg[1], data, data_len);
550  if (crypto_ahash_setkey(tfm_michael, key, 8))
551   return -1;
552  ahash_request_set_tfm(req, tfm_michael);
553  ahash_request_set_callback(req, 0, NULL, NULL);
554  ahash_request_set_crypt(req, sg, mic, data_len + 16);
555  err = crypto_ahash_digest(req);
556  ahash_request_zero(req);
557  return err;
558 }
559 #else
560 static int michael_mic(struct crypto_hash *tfm_michael, u8 * key, u8 * hdr,
561          u8 * data, size_t data_len, u8 * mic)
562 {
563  struct hash_desc desc;
564  struct scatterlist sg[2];
565  if (tfm_michael == NULL) {
566   pr_warn("%s(): tfm_michael == NULL\n", __func__);
567   return -1;
568  }
569  sg_init_table(sg, 2);
570  sg_set_buf(&sg[0], hdr, 16);
571  sg_set_buf(&sg[1], data, data_len);
572  if (crypto_hash_setkey(tfm_michael, key, 8))
573   return -1;
574  desc.tfm = tfm_michael;
575  desc.flags = 0;
576  return crypto_hash_digest(&desc, sg, data_len + 16, mic);
577 }
578 #endif
579 static void michael_mic_hdr(struct sk_buff *skb, u8 * hdr)
580 {
581  struct ieee80211_hdr *hdr11;
582  hdr11 = (struct ieee80211_hdr *)skb->data;
583  switch (le16_to_cpu(hdr11->frame_control) &
584   (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
585  case IEEE80211_FCTL_TODS:
586   memcpy(hdr, hdr11->addr3, ETH_ALEN);
587   memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN);
588   break;
589  case IEEE80211_FCTL_FROMDS:
590   memcpy(hdr, hdr11->addr1, ETH_ALEN);
591   memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN);
592   break;
593  case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
594   memcpy(hdr, hdr11->addr3, ETH_ALEN);
595   memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN);
596   break;
597  case 0:
598   memcpy(hdr, hdr11->addr1, ETH_ALEN);
599   memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN);
600   break;
601  }
602  if (ieee80211_is_data_qos(hdr11->frame_control)) {
603   hdr[12] = le16_to_cpu(*((__le16 *)ieee80211_get_qos_ctl(hdr11)))
604    & IEEE80211_QOS_CTL_TID_MASK;
605  } else
606   hdr[12] = 0;
607  hdr[13] = hdr[14] = hdr[15] = 0;
608 }
609 static int lib80211_michael_mic_add(struct sk_buff *skb, int hdr_len,
610          void *priv)
611 {
612  struct lib80211_tkip_data *tkey = priv;
613  u8 *pos;
614  if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
615   printk(KERN_DEBUG "Invalid packet for Michael MIC add "
616          "(tailroom=%d hdr_len=%d skb->len=%d)\n",
617          skb_tailroom(skb), hdr_len, skb->len);
618   return -1;
619  }
620  michael_mic_hdr(skb, tkey->tx_hdr);
621  pos = skb_put(skb, 8);
622  if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
623    skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
624   return -1;
625  return 0;
626 }
627 #ifdef CONFIG_WEXT_CORE
628 static void lib80211_michael_mic_failure(struct net_device *dev,
629        struct ieee80211_hdr *hdr,
630        int keyidx)
631 {
632  union iwreq_data wrqu;
633  struct iw_michaelmicfailure ev;
634  memset(&ev, 0, sizeof(ev));
635  ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
636  if (hdr->addr1[0] & 0x01)
637   ev.flags |= IW_MICFAILURE_GROUP;
638  else
639   ev.flags |= IW_MICFAILURE_PAIRWISE;
640  ev.src_addr.sa_family = ARPHRD_ETHER;
641  memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
642  memset(&wrqu, 0, sizeof(wrqu));
643  wrqu.data.length = sizeof(ev);
644  wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
645 }
646 #endif
647 static int lib80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
648      int hdr_len, void *priv)
649 {
650  struct lib80211_tkip_data *tkey = priv;
651  u8 mic[8];
652  if (!tkey->key_set)
653   return -1;
654  michael_mic_hdr(skb, tkey->rx_hdr);
655  if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
656    skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
657   return -1;
658  if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
659   struct ieee80211_hdr *hdr;
660   hdr = (struct ieee80211_hdr *)skb->data;
661   printk(KERN_DEBUG "%s: Michael MIC verification failed for "
662          "MSDU from %pM keyidx=%d\n",
663          skb->dev ? skb->dev->name : "N/A", hdr->addr2,
664          keyidx);
665 #ifdef CONFIG_WEXT_CORE
666   if (skb->dev)
667    lib80211_michael_mic_failure(skb->dev, hdr, keyidx);
668 #endif
669   tkey->dot11RSNAStatsTKIPLocalMICFailures++;
670   return -1;
671  }
672  tkey->rx_iv32 = tkey->rx_iv32_new;
673  tkey->rx_iv16 = tkey->rx_iv16_new;
674  skb_trim(skb, skb->len - 8);
675  return 0;
676 }
677 static int lib80211_tkip_set_key(void *key, int len, u8 * seq, void *priv)
678 {
679  struct lib80211_tkip_data *tkey = priv;
680  int keyidx;
681 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,6,0)
682  struct crypto_ahash *tfm = tkey->tx_tfm_michael;
683  struct crypto_skcipher *tfm2 = tkey->tx_tfm_arc4;
684  struct crypto_ahash *tfm3 = tkey->rx_tfm_michael;
685  struct crypto_skcipher *tfm4 = tkey->rx_tfm_arc4;
686 #else
687  struct crypto_hash *tfm = tkey->tx_tfm_michael;
688  struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
689  struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
690  struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
691 #endif
692  keyidx = tkey->key_idx;
693  memset(tkey, 0, sizeof(*tkey));
694  tkey->key_idx = keyidx;
695  tkey->tx_tfm_michael = tfm;
696  tkey->tx_tfm_arc4 = tfm2;
697  tkey->rx_tfm_michael = tfm3;
698  tkey->rx_tfm_arc4 = tfm4;
699  if (len == TKIP_KEY_LEN) {
700   memcpy(tkey->key, key, TKIP_KEY_LEN);
701   tkey->key_set = 1;
702   tkey->tx_iv16 = 1;
703   if (seq) {
704    tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
705        (seq[3] << 8) | seq[2];
706    tkey->rx_iv16 = (seq[1] << 8) | seq[0];
707   }
708  } else if (len == 0)
709   tkey->key_set = 0;
710  else
711   return -1;
712  return 0;
713 }
714 static int lib80211_tkip_get_key(void *key, int len, u8 * seq, void *priv)
715 {
716  struct lib80211_tkip_data *tkey = priv;
717  if (len < TKIP_KEY_LEN)
718   return -1;
719  if (!tkey->key_set)
720   return 0;
721  memcpy(key, tkey->key, TKIP_KEY_LEN);
722  if (seq) {
723   u16 iv16 = tkey->tx_iv16;
724   u32 iv32 = tkey->tx_iv32;
725   if (iv16 == 0)
726    iv32--;
727   iv16--;
728   seq[0] = tkey->tx_iv16;
729   seq[1] = tkey->tx_iv16 >> 8;
730   seq[2] = tkey->tx_iv32;
731   seq[3] = tkey->tx_iv32 >> 8;
732   seq[4] = tkey->tx_iv32 >> 16;
733   seq[5] = tkey->tx_iv32 >> 24;
734  }
735  return TKIP_KEY_LEN;
736 }
737 static char *lib80211_tkip_print_stats(char *p, void *priv)
738 {
739  struct lib80211_tkip_data *tkip = priv;
740  p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
741        "tx_pn=%02x%02x%02x%02x%02x%02x "
742        "rx_pn=%02x%02x%02x%02x%02x%02x "
743        "replays=%d icv_errors=%d local_mic_failures=%d\n",
744        tkip->key_idx, tkip->key_set,
745        (tkip->tx_iv32 >> 24) & 0xff,
746        (tkip->tx_iv32 >> 16) & 0xff,
747        (tkip->tx_iv32 >> 8) & 0xff,
748        tkip->tx_iv32 & 0xff,
749        (tkip->tx_iv16 >> 8) & 0xff,
750        tkip->tx_iv16 & 0xff,
751        (tkip->rx_iv32 >> 24) & 0xff,
752        (tkip->rx_iv32 >> 16) & 0xff,
753        (tkip->rx_iv32 >> 8) & 0xff,
754        tkip->rx_iv32 & 0xff,
755        (tkip->rx_iv16 >> 8) & 0xff,
756        tkip->rx_iv16 & 0xff,
757        tkip->dot11RSNAStatsTKIPReplays,
758        tkip->dot11RSNAStatsTKIPICVErrors,
759        tkip->dot11RSNAStatsTKIPLocalMICFailures);
760  return p;
761 }
762 static struct ssv_crypto_ops ssv_crypt_tkip = {
763  .name = "TKIP",
764  .init = lib80211_tkip_init,
765  .deinit = lib80211_tkip_deinit,
766  .encrypt_mpdu = lib80211_tkip_encrypt,
767  .decrypt_mpdu = lib80211_tkip_decrypt,
768  .encrypt_msdu = lib80211_michael_mic_add,
769  .decrypt_msdu = lib80211_michael_mic_verify,
770  .set_key = lib80211_tkip_set_key,
771  .get_key = lib80211_tkip_get_key,
772  .print_stats = lib80211_tkip_print_stats,
773  .extra_mpdu_prefix_len = 4 + 4,
774  .extra_mpdu_postfix_len = 4,
775  .extra_msdu_postfix_len = 8,
776  .get_flags = lib80211_tkip_get_flags,
777  .set_flags = lib80211_tkip_set_flags,
778 #ifdef MULTI_THREAD_ENCRYPT
779  .encrypt_prepare = NULL,
780     .decrypt_prepare = NULL,
781 #endif
782 };
783 struct ssv_crypto_ops *get_crypto_tkip_ops(void)
784 {
785     return &ssv_crypt_tkip;
786 }
787