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/kernel.h>
18 #include <linux/string.h>
19 #include <net/cfg80211.h>
20 #include <net/mac80211.h>
21 #include <linux/etherdevice.h>
22 #include "wapi_sms4.h"
23 #include "sec_wpi.h"
24 #include "sec.h"
25 #define IWAPIELEMENT 68
26 #define WID_WAPI_KEY 0x3033
27 u8 g_wapi_oui[3] = {0x00,0x14,0x72};
28 const u16 frame_cntl_mask = 0x8FC7;
29 const u16 seq_cntl_mask = 0x0F00;
30 struct lib80211_wpi_data {
31 TRUTH_VALUE_T wapi_enable;
32 TRUTH_VALUE_T wapi_key_ok;
33 u8 wapi_version[2];
34 u8 ap_address[ETH_ALEN];
35 u8 key_index;
36 u8 pn_key[WAPI_PN_LEN];
37 u8 pmsk_key[3][WAPI_PN_LEN];
38 u8 mic_key[3][WAPI_PN_LEN];
39 };
mget_wapi_key_ok(void * priv)40 TRUTH_VALUE_T mget_wapi_key_ok(void *priv)
41 {
42 struct lib80211_wpi_data *data = priv;
43 return data->wapi_key_ok;
44 }
mset_wapi_key_ok(TRUTH_VALUE_T val,void * priv)45 void mset_wapi_key_ok(TRUTH_VALUE_T val, void *priv)
46 {
47 struct lib80211_wpi_data *data = priv;
48 data->wapi_key_ok = val;
49 }
mget_wapi_enable(void * priv)50 TRUTH_VALUE_T mget_wapi_enable(void *priv)
51 {
52 struct lib80211_wpi_data *data = priv;
53 return data->wapi_enable;
54 }
mset_wapi_enable(TRUTH_VALUE_T val,void * priv)55 void mset_wapi_enable(TRUTH_VALUE_T val, void *priv)
56 {
57 struct lib80211_wpi_data *data = priv;
58 data->wapi_enable = val;
59 }
mget_wapi_version(void * priv)60 u8* mget_wapi_version(void *priv)
61 {
62 struct lib80211_wpi_data *data = priv;
63 return data->wapi_version;
64 }
mset_wapi_version(u8 * val,void * priv)65 void mset_wapi_version(u8* val, void *priv)
66 {
67 struct lib80211_wpi_data *data = priv;
68 memcpy(data->wapi_version,val,2);
69 }
mget_wapi_address(void * priv)70 u8* mget_wapi_address(void *priv)
71 {
72 struct lib80211_wpi_data *data = priv;
73 return data->ap_address;
74 }
mset_wapi_address(u8 * val,void * priv)75 void mset_wapi_address(u8* val, void *priv)
76 {
77 struct lib80211_wpi_data *data = priv;
78 memcpy(data->ap_address,val, ETH_ALEN);
79 }
mget_key_index(void * priv)80 u8 mget_key_index(void *priv)
81 {
82 struct lib80211_wpi_data *data = priv;
83 return data->key_index;
84 }
mset_key_index(int index,void * priv)85 void mset_key_index(int index, void *priv)
86 {
87 struct lib80211_wpi_data *data = priv;
88 if(index <= 3 )
89 {
90 data->key_index = index;
91 }
92 }
mget_pn_key(void * priv)93 u8* mget_pn_key(void *priv)
94 {
95 struct lib80211_wpi_data *data = priv;
96 return data->pn_key;
97 }
mset_pn_key(u8 * val,void * priv)98 void mset_pn_key(u8* val, void *priv)
99 {
100 struct lib80211_wpi_data *data = priv;
101 memcpy(data->pn_key,val,WAPI_PN_LEN);
102 }
inc_pn_key(void * priv)103 u8 *inc_pn_key(void *priv)
104 {
105 struct lib80211_wpi_data *data = priv;
106 int i;
107 data->pn_key[15] += 2;
108 if( data->pn_key[15] == 0x00 )
109 {
110 for(i = 14 ; i >= 0 ; i--)
111 {
112 if( (data->pn_key[i] += 1) != 0x00 )
113 {
114 break;
115 }
116 }
117 }
118 return data->pn_key;
119 }
mget_pmsk_key(int index,void * priv)120 u8 *mget_pmsk_key(int index, void *priv)
121 {
122 struct lib80211_wpi_data *data = priv;
123 return ( index >= 3 ) ? NULL : data->pmsk_key[index];
124 }
mset_pmsk_key(int index,u8 * val,void * priv)125 void mset_pmsk_key(int index,u8* val, void *priv)
126 {
127 struct lib80211_wpi_data *data = priv;
128 if(index < 3 )
129 {
130 memcpy(data->pmsk_key[index],val, WAPI_MIC_LEN);
131 }
132 }
mget_mic_key(int index,void * priv)133 u8* mget_mic_key(int index, void *priv)
134 {
135 struct lib80211_wpi_data *data = priv;
136 return ( index >= 3 ) ? NULL : data->mic_key[index];
137 }
mset_mic_key(int index,u8 * val,void * priv)138 void mset_mic_key(int index,u8* val, void *priv)
139 {
140 struct lib80211_wpi_data *data = priv;
141 if(index < 3 )
142 {
143 memcpy(data->mic_key[index],val,WAPI_MIC_LEN);
144 }
145 }
wlan_tx_wapi_encryption(u8 * header,u8 * data,u16 data_len,u8 * mic_pos,void * priv)146 u16 wlan_tx_wapi_encryption(u8 * header,
147 u8 * data,u16 data_len,
148 u8 * mic_pos, void *priv)
149 {
150 int i = 0;
151 u16 offset = 0;
152 BOOL_T qos_in = BFALSE;
153 BOOL_T valid_addr4 = BTRUE;
154 u8 ptk_header[36] = {0};
155 u16 ptk_headr_len = 32;
156 u8 *p_ptk_header = ptk_header;
157 u8 *data_mic = mic_pos;
158 u8 *iv = NULL;
159 u8 keyid = 0;
160 keyid = mget_key_index(priv);
161 #ifdef MULTI_THREAD_ENCRYPT
162 iv = kzalloc(WAPI_PN_LEN, GFP_KERNEL);
163 memcpy(iv, data + WAPI_KEYID_LEN + WAPI_RESERVD_LEN, WAPI_PN_LEN);
164 data_len -= WAPI_IV_LEN;
165 #else
166 iv = inc_pn_key(priv);
167 #endif
168 *data = keyid;
169 *(data + 1) = 0x00;
170 data += 2;
171 for( i = 15 ; i >= 0 ; i-- ) {
172 *data = iv[i];
173 data++;
174 }
175 *p_ptk_header = header[offset] & (frame_cntl_mask >> 8);
176 *(p_ptk_header + 1) = header[offset + 1] & (frame_cntl_mask & 0xFF);
177 if(*p_ptk_header & 0x80) {
178 qos_in = BTRUE;
179 ptk_headr_len += 2;
180 }
181 if((*(p_ptk_header + 1) & 0x03 ) != 0x03) {
182 valid_addr4 = BFALSE;
183 }
184 p_ptk_header += 2;
185 offset += 2;
186 offset += 2;
187 memcpy(p_ptk_header, &header[offset], ADDID_LEN);
188 p_ptk_header += ADDID_LEN;
189 offset += ADDID_LEN;
190 *p_ptk_header = header[offset + ETH_ALEN] & (seq_cntl_mask >> 8);
191 *(p_ptk_header + 1) = header[offset + ETH_ALEN + 1] & (seq_cntl_mask & 0xFF);
192 p_ptk_header += 2;
193 memcpy(p_ptk_header, &header[offset], ETH_ALEN);
194 p_ptk_header += ETH_ALEN;
195 offset += ETH_ALEN;
196 offset += 2;
197 if(valid_addr4) {
198 memcpy(p_ptk_header, &header[offset], ETH_ALEN);
199 p_ptk_header += ETH_ALEN;
200 offset += ETH_ALEN;
201 }
202 else {
203 memset(p_ptk_header,0x00, ETH_ALEN);
204 p_ptk_header += ETH_ALEN;
205 }
206 if(qos_in) {
207 memcpy(p_ptk_header, &header[offset], 2);
208 p_ptk_header += 2;
209 offset += 2;
210 }
211 *p_ptk_header = keyid;
212 p_ptk_header++;
213 *p_ptk_header = 0x00;
214 p_ptk_header++;
215 *p_ptk_header = (data_len >> 8);
216 *(p_ptk_header+1) = data_len & 0xFF;
217 WapiCryptoSms4Mic(iv,
218 mget_mic_key(keyid, priv),
219 ptk_header, ptk_headr_len, data, data_len, data_mic);
220 data_len += WAPI_MIC_LEN;
221 WapiCryptoSms4(iv,
222 mget_pmsk_key(keyid, priv),
223 data, data_len,
224 data);
225 #ifdef MULTI_THREAD_ENCRYPT
226 kfree(iv);
227 #endif
228 return data_len + WAPI_IV_LEN;
229 }
is_group(u8 * addr)230 BOOL_T is_group(u8* addr)
231 {
232 if((addr[0] & BIT(0)) != 0)
233 return BTRUE;
234 return BFALSE;
235 }
wlan_rx_wapi_decryption(u8 * input_ptk,u16 header_len,u16 data_len,u8 * output_buf,void * priv)236 u16 wlan_rx_wapi_decryption(u8 * input_ptk,u16 header_len,u16 data_len,
237 u8 * output_buf, void *priv)
238 {
239 u16 offset = 0;
240 BOOL_T qos_in = BFALSE;
241 BOOL_T valid_addr4 = BTRUE;
242 BOOL_T is_group_ptk = BFALSE;
243 u8 ptk_header[36] = {0};
244 u16 ptk_headr_len = 32;
245 u8 * p_ptk_header = ptk_header;
246 u8 data_mic[WAPI_MIC_LEN] = {0};
247 u8 calc_data_mic[WAPI_MIC_LEN] = {0};
248 u8 iv[WAPI_PN_LEN] = {0};
249 u8 keyid = {0};
250 u16 ral_data_len = 0;
251 u16 encryp_data_len = 0;
252 int i = 0;
253 *p_ptk_header = input_ptk[offset] & (frame_cntl_mask >> 8);
254 *(p_ptk_header+1) = input_ptk[offset+1] & (frame_cntl_mask & 0xFF);
255 if(*p_ptk_header & 0x80) {
256 qos_in = BTRUE;
257 ptk_headr_len += 2;
258 }
259 if((*(p_ptk_header+1) & 0x03 ) != 0x03) {
260 valid_addr4 = BFALSE;
261 }
262 p_ptk_header += 2;
263 offset += 2;
264 offset += 2;
265 memcpy(p_ptk_header, &input_ptk[offset], ADDID_LEN);
266 is_group_ptk = is_group(p_ptk_header);
267 p_ptk_header += ADDID_LEN;
268 offset += ADDID_LEN;
269 *p_ptk_header = input_ptk[offset+6] & (seq_cntl_mask >> 8);
270 *(p_ptk_header+1) = input_ptk[offset+6+1] & (seq_cntl_mask & 0xFF);
271 p_ptk_header += 2;
272 memcpy(p_ptk_header, &input_ptk[offset], ETH_ALEN);
273 p_ptk_header += ETH_ALEN;
274 offset += ETH_ALEN;
275 offset += 2;
276 if(valid_addr4) {
277 memcpy(p_ptk_header, &input_ptk[offset], ETH_ALEN);
278 p_ptk_header += ETH_ALEN;
279 offset += ETH_ALEN;
280 }
281 else {
282 memset(p_ptk_header, 0x00, ETH_ALEN);
283 p_ptk_header += ETH_ALEN;
284 }
285 if(qos_in) {
286 memcpy(p_ptk_header,&input_ptk[offset], 2);
287 p_ptk_header += 2;
288 offset += 2;
289 }
290 *p_ptk_header = input_ptk[offset];
291 keyid = input_ptk[offset];
292 p_ptk_header++;
293 offset++;
294 *p_ptk_header = input_ptk[offset];
295 p_ptk_header++;
296 offset++;
297 encryp_data_len = data_len - WAPI_IV_LEN;
298 ral_data_len = data_len - WAPI_IV_LEN - WAPI_MIC_LEN;
299 *p_ptk_header = (ral_data_len >> 8);
300 *(p_ptk_header+1) = ral_data_len & 0xFF;
301 for( i = 15 ; i >= 0 ; i-- ) {
302 iv[i] = input_ptk[offset];
303 offset++;
304 }
305 if(is_group_ptk) {
306 printk("%s, is_group_ptk\n", __func__);
307 }
308 else {
309 if( (iv[15] & 0x01) != 0x01 ) {
310 printk(KERN_ALERT "decry pairwise error,iv[15]=%x\n", iv[15]);
311 return 0;
312 }
313 }
314 WapiCryptoSms4(iv,
315 mget_pmsk_key(keyid, priv),
316 (input_ptk + header_len + WAPI_IV_LEN), encryp_data_len,
317 output_buf);
318 memcpy(data_mic, output_buf + ral_data_len, WAPI_MIC_LEN);
319 WapiCryptoSms4Mic(iv,
320 mget_mic_key(keyid, priv),
321 ptk_header, ptk_headr_len,
322 (output_buf), ral_data_len,
323 calc_data_mic);
324 if( memcmp(calc_data_mic, data_mic, WAPI_MIC_LEN) != 0 ) {
325 printk(KERN_ALERT "calc_data_mic != data_mic\n");
326 return 0;
327 }
328 else {
329 return ral_data_len;
330 }
331 }
lib80211_wpi_encrypt(struct sk_buff * mpdu,int hdr_len,void * priv)332 int lib80211_wpi_encrypt(struct sk_buff *mpdu, int hdr_len, void *priv)
333 {
334 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)mpdu->data;
335 u8 *pos, *mic_pos;
336 int hdrlen = 0, len = 0, out_len = 0;
337 u8 *pdata = NULL;
338 #ifdef MULTI_THREAD_ENCRYPT
339 u32 wapi_iv_icv_offset = WAPI_IV_ICV_OFFSET - WAPI_IV_LEN;
340 #else
341 u32 wapi_iv_icv_offset = WAPI_IV_ICV_OFFSET;
342 #endif
343 hdrlen = ieee80211_hdrlen(hdr->frame_control);
344 pdata = (mpdu->data) + hdrlen;
345 if (mpdu->protocol != cpu_to_be16(0x88b4)) {
346 if (WARN_ON(skb_headroom(mpdu) < wapi_iv_icv_offset)) {
347 printk("[I] skb_headroom(skb) < %d\n", wapi_iv_icv_offset);
348 return 0;
349 }
350 len = mpdu->len - hdrlen;
351 pos = skb_push(mpdu, wapi_iv_icv_offset);
352 #ifdef MULTI_THREAD_ENCRYPT
353 memmove(pos, pos + wapi_iv_icv_offset, mpdu->len - WAPI_MIC_LEN);
354 #else
355 memmove(pos, pos + wapi_iv_icv_offset, hdrlen);
356 memmove(pos + hdrlen + WAPI_IV_LEN, pos + wapi_iv_icv_offset + hdrlen, len);
357 #endif
358 hdr = (struct ieee80211_hdr *)pos;
359 pos += hdrlen;
360 mic_pos = mpdu->data + mpdu->len - WAPI_MIC_LEN;
361 out_len = wlan_tx_wapi_encryption((u8 *)hdr, pos, len, mic_pos, priv);
362 }
363 else {
364 if (ieee80211_has_protected(hdr->frame_control))
365 hdr->frame_control &= ~(cpu_to_le16(IEEE80211_FCTL_PROTECTED));
366 printk("[I] send WAPI WAI data pkt\n");
367 }
368 return 1;
369 }
lib80211_wpi_decrypt(struct sk_buff * rx_skb,int hdr_len,void * priv)370 int lib80211_wpi_decrypt(struct sk_buff *rx_skb, int hdr_len, void *priv)
371 {
372 struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(rx_skb->data);
373 int hdrlen, len, dcry_len;
374 char *pdata = NULL;
375 int ret = 0;
376 hdrlen = ieee80211_hdrlen(hdr->frame_control);
377 pdata = ((char*)(rx_skb->data)) + hdrlen;
378 len = rx_skb->len - hdrlen;
379 dcry_len = wlan_rx_wapi_decryption((u8 *)rx_skb->data, hdrlen, len, pdata, priv);
380 if (dcry_len) {
381 skb_trim(rx_skb, hdrlen + dcry_len);
382 hdr->frame_control &= ~(cpu_to_le16(IEEE80211_FCTL_PROTECTED));
383 ret = dcry_len;
384 }
385 return ret;
386 }
lib80211_wpi_init(int key_idx)387 void *lib80211_wpi_init(int key_idx)
388 {
389 struct lib80211_wpi_data *priv;
390 priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
391 if (priv == NULL) {
392 printk("allocate lib80211_wpi_data failed\n");
393 return NULL;
394 }
395 priv->key_index = key_idx;
396 return priv;
397 }
lib80211_wpi_deinit(void * priv)398 void lib80211_wpi_deinit(void *priv)
399 {
400 if (priv) {
401 printk("%s\n", __func__);
402 kfree(priv);
403 priv = NULL;
404 }
405 else
406 printk("%s, passing NULL lib80211_wpi_data?\n", __func__);
407 }
408 #ifdef MULTI_THREAD_ENCRYPT
lib80211_wpi_encrypt_prepare(struct sk_buff * mpdu,int hdr_len,void * priv)409 int lib80211_wpi_encrypt_prepare(struct sk_buff *mpdu, int hdr_len, void *priv)
410 {
411 struct lib80211_wpi_data *data = priv;
412 u8 *pos = NULL;
413 unsigned char *iv = NULL;
414 if (mpdu->protocol != cpu_to_be16(0x88b4) &&
415 (skb_headroom(mpdu) >= WAPI_IV_LEN)) {
416 pos = skb_push(mpdu, WAPI_IV_LEN);
417 memmove(pos, pos + WAPI_IV_LEN, hdr_len);
418 pos += hdr_len;
419 *pos = data->key_index;
420 pos++;
421 *pos = 0x00;
422 pos++;
423 iv = inc_pn_key(priv);
424 memcpy(pos, iv, WAPI_PN_LEN);
425 return 0;
426 }
427 printk("%s, pass through\n", __func__);
428 return 0;
429 }
430 #endif
lib80211_wpi_set_key(void * key,int len,u8 * seq,void * priv)431 int lib80211_wpi_set_key(void *key, int len, u8 *seq, void *priv)
432 {
433 struct lib80211_wpi_data *data = priv;
434 int keyidx = data->key_index;
435 u8 WapiASUEPNInitialValueSrc[16] = {
436 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36,
437 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36
438 };
439 printk("%s\n", __func__);
440 mset_key_index(keyidx, priv);
441 mset_pn_key(WapiASUEPNInitialValueSrc, priv);
442 mset_pmsk_key(keyidx, key, priv);
443 mset_mic_key(keyidx, key + WAPI_PN_LEN, priv);
444 if (seq) {
445 memcpy(data->ap_address, (u8 *)seq, ETH_ALEN);
446 printk("%s: set ap_address %pM\n", __func__, data->ap_address);
447 }
448 mset_wapi_key_ok(TV_TRUE, priv);
449 return 0;
450 }
451 static struct ssv_crypto_ops ssv_crypto_wpi = {
452 .name = "WPI",
453 .init = lib80211_wpi_init,
454 .deinit = lib80211_wpi_deinit,
455 .encrypt_mpdu = lib80211_wpi_encrypt,
456 .decrypt_mpdu = lib80211_wpi_decrypt,
457 .encrypt_msdu = NULL,
458 .decrypt_msdu = NULL,
459 .set_tx_pn = NULL,
460 .set_key = lib80211_wpi_set_key,
461 .get_key = NULL,
462 .print_stats = NULL,
463 .extra_mpdu_prefix_len = WAPI_IV_LEN,
464 .extra_mpdu_postfix_len = WAPI_MIC_LEN,
465 #ifdef MULTI_THREAD_ENCRYPT
466 .encrypt_prepare = lib80211_wpi_encrypt_prepare,
467 .decrypt_prepare = NULL,
468 #endif
469 };
get_crypto_wpi_ops(void)470 struct ssv_crypto_ops *get_crypto_wpi_ops(void)
471 {
472 return &ssv_crypto_wpi;
473 }
474