1 /******************************************************************************
2 *
3 * Copyright(c) 2016 - 2017 Realtek Corporation.
4 *
5 * This program is free software; you can redistribute it and/or modify it
6 * under the terms of version 2 of the GNU General Public License as
7 * published by the Free Software Foundation.
8 *
9 * This program is distributed in the hope that it will be useful, but WITHOUT
10 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
12 * more details.
13 *
14 *****************************************************************************/
15 #ifdef CONFIG_WAPI_SUPPORT
16
17 #include <linux/unistd.h>
18 #include <linux/etherdevice.h>
19 #include <drv_types.h>
20 #include <rtw_wapi.h>
21
22
23 #ifdef CONFIG_WAPI_SW_SMS4
24
25 #define WAPI_LITTLE_ENDIAN
26 /* #define BIG_ENDIAN */
27 #define ENCRYPT 0
28 #define DECRYPT 1
29
30
31 /**********************************************************
32 **********************************************************/
33 const u8 Sbox[256] = {
34 0xd6, 0x90, 0xe9, 0xfe, 0xcc, 0xe1, 0x3d, 0xb7, 0x16, 0xb6, 0x14, 0xc2, 0x28, 0xfb, 0x2c, 0x05,
35 0x2b, 0x67, 0x9a, 0x76, 0x2a, 0xbe, 0x04, 0xc3, 0xaa, 0x44, 0x13, 0x26, 0x49, 0x86, 0x06, 0x99,
36 0x9c, 0x42, 0x50, 0xf4, 0x91, 0xef, 0x98, 0x7a, 0x33, 0x54, 0x0b, 0x43, 0xed, 0xcf, 0xac, 0x62,
37 0xe4, 0xb3, 0x1c, 0xa9, 0xc9, 0x08, 0xe8, 0x95, 0x80, 0xdf, 0x94, 0xfa, 0x75, 0x8f, 0x3f, 0xa6,
38 0x47, 0x07, 0xa7, 0xfc, 0xf3, 0x73, 0x17, 0xba, 0x83, 0x59, 0x3c, 0x19, 0xe6, 0x85, 0x4f, 0xa8,
39 0x68, 0x6b, 0x81, 0xb2, 0x71, 0x64, 0xda, 0x8b, 0xf8, 0xeb, 0x0f, 0x4b, 0x70, 0x56, 0x9d, 0x35,
40 0x1e, 0x24, 0x0e, 0x5e, 0x63, 0x58, 0xd1, 0xa2, 0x25, 0x22, 0x7c, 0x3b, 0x01, 0x21, 0x78, 0x87,
41 0xd4, 0x00, 0x46, 0x57, 0x9f, 0xd3, 0x27, 0x52, 0x4c, 0x36, 0x02, 0xe7, 0xa0, 0xc4, 0xc8, 0x9e,
42 0xea, 0xbf, 0x8a, 0xd2, 0x40, 0xc7, 0x38, 0xb5, 0xa3, 0xf7, 0xf2, 0xce, 0xf9, 0x61, 0x15, 0xa1,
43 0xe0, 0xae, 0x5d, 0xa4, 0x9b, 0x34, 0x1a, 0x55, 0xad, 0x93, 0x32, 0x30, 0xf5, 0x8c, 0xb1, 0xe3,
44 0x1d, 0xf6, 0xe2, 0x2e, 0x82, 0x66, 0xca, 0x60, 0xc0, 0x29, 0x23, 0xab, 0x0d, 0x53, 0x4e, 0x6f,
45 0xd5, 0xdb, 0x37, 0x45, 0xde, 0xfd, 0x8e, 0x2f, 0x03, 0xff, 0x6a, 0x72, 0x6d, 0x6c, 0x5b, 0x51,
46 0x8d, 0x1b, 0xaf, 0x92, 0xbb, 0xdd, 0xbc, 0x7f, 0x11, 0xd9, 0x5c, 0x41, 0x1f, 0x10, 0x5a, 0xd8,
47 0x0a, 0xc1, 0x31, 0x88, 0xa5, 0xcd, 0x7b, 0xbd, 0x2d, 0x74, 0xd0, 0x12, 0xb8, 0xe5, 0xb4, 0xb0,
48 0x89, 0x69, 0x97, 0x4a, 0x0c, 0x96, 0x77, 0x7e, 0x65, 0xb9, 0xf1, 0x09, 0xc5, 0x6e, 0xc6, 0x84,
49 0x18, 0xf0, 0x7d, 0xec, 0x3a, 0xdc, 0x4d, 0x20, 0x79, 0xee, 0x5f, 0x3e, 0xd7, 0xcb, 0x39, 0x48
50 };
51
52 const u32 CK[32] = {
53 0x00070e15, 0x1c232a31, 0x383f464d, 0x545b6269,
54 0x70777e85, 0x8c939aa1, 0xa8afb6bd, 0xc4cbd2d9,
55 0xe0e7eef5, 0xfc030a11, 0x181f262d, 0x343b4249,
56 0x50575e65, 0x6c737a81, 0x888f969d, 0xa4abb2b9,
57 0xc0c7ced5, 0xdce3eaf1, 0xf8ff060d, 0x141b2229,
58 0x30373e45, 0x4c535a61, 0x686f767d, 0x848b9299,
59 0xa0a7aeb5, 0xbcc3cad1, 0xd8dfe6ed, 0xf4fb0209,
60 0x10171e25, 0x2c333a41, 0x484f565d, 0x646b7279
61 };
62
63 #define Rotl(_x, _y) (((_x) << (_y)) | ((_x) >> (32 - (_y))))
64
65 #define ByteSub(_A) (Sbox[(_A) >> 24 & 0xFF] << 24 | \
66 Sbox[(_A) >> 16 & 0xFF] << 16 | \
67 Sbox[(_A) >> 8 & 0xFF] << 8 | \
68 Sbox[(_A) & 0xFF])
69
70 #define L1(_B) ((_B) ^ Rotl(_B, 2) ^ Rotl(_B, 10) ^ Rotl(_B, 18) ^ Rotl(_B, 24))
71 #define L2(_B) ((_B) ^ Rotl(_B, 13) ^ Rotl(_B, 23))
72
73 static void
xor_block(void * dst,void * src1,void * src2)74 xor_block(void *dst, void *src1, void *src2)
75 /* 128-bit xor: *dst = *src1 xor *src2. Pointers must be 32-bit aligned */
76 {
77 ((u32 *)dst)[0] = ((u32 *)src1)[0] ^ ((u32 *)src2)[0];
78 ((u32 *)dst)[1] = ((u32 *)src1)[1] ^ ((u32 *)src2)[1];
79 ((u32 *)dst)[2] = ((u32 *)src1)[2] ^ ((u32 *)src2)[2];
80 ((u32 *)dst)[3] = ((u32 *)src1)[3] ^ ((u32 *)src2)[3];
81 }
82
83
SMS4Crypt(u8 * Input,u8 * Output,u32 * rk)84 void SMS4Crypt(u8 *Input, u8 *Output, u32 *rk)
85 {
86 u32 r, mid, x0, x1, x2, x3, *p;
87 p = (u32 *)Input;
88 x0 = p[0];
89 x1 = p[1];
90 x2 = p[2];
91 x3 = p[3];
92 #ifdef WAPI_LITTLE_ENDIAN
93 x0 = Rotl(x0, 16);
94 x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);
95 x1 = Rotl(x1, 16);
96 x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);
97 x2 = Rotl(x2, 16);
98 x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);
99 x3 = Rotl(x3, 16);
100 x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);
101 #endif
102 for (r = 0; r < 32; r += 4) {
103 mid = x1 ^ x2 ^ x3 ^ rk[r + 0];
104 mid = ByteSub(mid);
105 x0 ^= L1(mid);
106 mid = x2 ^ x3 ^ x0 ^ rk[r + 1];
107 mid = ByteSub(mid);
108 x1 ^= L1(mid);
109 mid = x3 ^ x0 ^ x1 ^ rk[r + 2];
110 mid = ByteSub(mid);
111 x2 ^= L1(mid);
112 mid = x0 ^ x1 ^ x2 ^ rk[r + 3];
113 mid = ByteSub(mid);
114 x3 ^= L1(mid);
115 }
116 #ifdef WAPI_LITTLE_ENDIAN
117 x0 = Rotl(x0, 16);
118 x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);
119 x1 = Rotl(x1, 16);
120 x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);
121 x2 = Rotl(x2, 16);
122 x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);
123 x3 = Rotl(x3, 16);
124 x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);
125 #endif
126 p = (u32 *)Output;
127 p[0] = x3;
128 p[1] = x2;
129 p[2] = x1;
130 p[3] = x0;
131 }
132
133
134
SMS4KeyExt(u8 * Key,u32 * rk,u32 CryptFlag)135 void SMS4KeyExt(u8 *Key, u32 *rk, u32 CryptFlag)
136 {
137 u32 r, mid, x0, x1, x2, x3, *p;
138
139 p = (u32 *)Key;
140 x0 = p[0];
141 x1 = p[1];
142 x2 = p[2];
143 x3 = p[3];
144 #ifdef WAPI_LITTLE_ENDIAN
145 x0 = Rotl(x0, 16);
146 x0 = ((x0 & 0xFF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);
147 x1 = Rotl(x1, 16);
148 x1 = ((x1 & 0xFF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);
149 x2 = Rotl(x2, 16);
150 x2 = ((x2 & 0xFF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);
151 x3 = Rotl(x3, 16);
152 x3 = ((x3 & 0xFF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);
153 #endif
154
155 x0 ^= 0xa3b1bac6;
156 x1 ^= 0x56aa3350;
157 x2 ^= 0x677d9197;
158 x3 ^= 0xb27022dc;
159 for (r = 0; r < 32; r += 4) {
160 mid = x1 ^ x2 ^ x3 ^ CK[r + 0];
161 mid = ByteSub(mid);
162 rk[r + 0] = x0 ^= L2(mid);
163 mid = x2 ^ x3 ^ x0 ^ CK[r + 1];
164 mid = ByteSub(mid);
165 rk[r + 1] = x1 ^= L2(mid);
166 mid = x3 ^ x0 ^ x1 ^ CK[r + 2];
167 mid = ByteSub(mid);
168 rk[r + 2] = x2 ^= L2(mid);
169 mid = x0 ^ x1 ^ x2 ^ CK[r + 3];
170 mid = ByteSub(mid);
171 rk[r + 3] = x3 ^= L2(mid);
172 }
173 if (CryptFlag == DECRYPT) {
174 for (r = 0; r < 16; r++)
175 mid = rk[r], rk[r] = rk[31 - r], rk[31 - r] = mid;
176 }
177 }
178
179
WapiSMS4Cryption(u8 * Key,u8 * IV,u8 * Input,u16 InputLength,u8 * Output,u16 * OutputLength,u32 CryptFlag)180 void WapiSMS4Cryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,
181 u8 *Output, u16 *OutputLength, u32 CryptFlag)
182 {
183 u32 blockNum, i, j, rk[32];
184 u16 remainder;
185 u8 blockIn[16], blockOut[16], tempIV[16], k;
186
187 *OutputLength = 0;
188 remainder = InputLength & 0x0F;
189 blockNum = InputLength >> 4;
190 if (remainder != 0)
191 blockNum++;
192 else
193 remainder = 16;
194
195 for (k = 0; k < 16; k++)
196 tempIV[k] = IV[15 - k];
197
198 memcpy(blockIn, tempIV, 16);
199
200 SMS4KeyExt((u8 *)Key, rk, CryptFlag);
201
202 for (i = 0; i < blockNum - 1; i++) {
203 SMS4Crypt((u8 *)blockIn, blockOut, rk);
204 xor_block(&Output[i * 16], &Input[i * 16], blockOut);
205 memcpy(blockIn, blockOut, 16);
206 }
207
208 *OutputLength = i * 16;
209
210 SMS4Crypt((u8 *)blockIn, blockOut, rk);
211
212 for (j = 0; j < remainder; j++)
213 Output[i * 16 + j] = Input[i * 16 + j] ^ blockOut[j];
214 *OutputLength += remainder;
215
216 }
217
WapiSMS4Encryption(u8 * Key,u8 * IV,u8 * Input,u16 InputLength,u8 * Output,u16 * OutputLength)218 void WapiSMS4Encryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,
219 u8 *Output, u16 *OutputLength)
220 {
221
222 WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT);
223 }
224
WapiSMS4Decryption(u8 * Key,u8 * IV,u8 * Input,u16 InputLength,u8 * Output,u16 * OutputLength)225 void WapiSMS4Decryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,
226 u8 *Output, u16 *OutputLength)
227 {
228 /* OFB mode: is also ENCRYPT flag */
229 WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT);
230 }
231
WapiSMS4CalculateMic(u8 * Key,u8 * IV,u8 * Input1,u8 Input1Length,u8 * Input2,u16 Input2Length,u8 * Output,u8 * OutputLength)232 void WapiSMS4CalculateMic(u8 *Key, u8 *IV, u8 *Input1, u8 Input1Length,
233 u8 *Input2, u16 Input2Length, u8 *Output, u8 *OutputLength)
234 {
235 u32 blockNum, i, remainder, rk[32];
236 u8 BlockIn[16], BlockOut[16], TempBlock[16], tempIV[16], k;
237
238 *OutputLength = 0;
239 remainder = Input1Length & 0x0F;
240 blockNum = Input1Length >> 4;
241
242 for (k = 0; k < 16; k++)
243 tempIV[k] = IV[15 - k];
244
245 memcpy(BlockIn, tempIV, 16);
246
247 SMS4KeyExt((u8 *)Key, rk, ENCRYPT);
248
249 SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
250
251 for (i = 0; i < blockNum; i++) {
252 xor_block(BlockIn, (Input1 + i * 16), BlockOut);
253 SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
254 }
255
256 if (remainder != 0) {
257 memset(TempBlock, 0, 16);
258 memcpy(TempBlock, (Input1 + blockNum * 16), remainder);
259
260 xor_block(BlockIn, TempBlock, BlockOut);
261 SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
262 }
263
264 remainder = Input2Length & 0x0F;
265 blockNum = Input2Length >> 4;
266
267 for (i = 0; i < blockNum; i++) {
268 xor_block(BlockIn, (Input2 + i * 16), BlockOut);
269 SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
270 }
271
272 if (remainder != 0) {
273 memset(TempBlock, 0, 16);
274 memcpy(TempBlock, (Input2 + blockNum * 16), remainder);
275
276 xor_block(BlockIn, TempBlock, BlockOut);
277 SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
278 }
279
280 memcpy(Output, BlockOut, 16);
281 *OutputLength = 16;
282 }
283
SecCalculateMicSMS4(u8 KeyIdx,u8 * MicKey,u8 * pHeader,u8 * pData,u16 DataLen,u8 * MicBuffer)284 void SecCalculateMicSMS4(
285 u8 KeyIdx,
286 u8 *MicKey,
287 u8 *pHeader,
288 u8 *pData,
289 u16 DataLen,
290 u8 *MicBuffer
291 )
292 {
293 #if 0
294 struct ieee80211_hdr_3addr_qos *header;
295 u8 TempBuf[34], TempLen = 32, MicLen, QosOffset, *IV;
296 u16 *pTemp, fc;
297
298 WAPI_TRACE(WAPI_TX | WAPI_RX, "=========>%s\n", __FUNCTION__);
299
300 header = (struct ieee80211_hdr_3addr_qos *)pHeader;
301 memset(TempBuf, 0, 34);
302 memcpy(TempBuf, pHeader, 2); /* FrameCtrl */
303 pTemp = (u16 *)TempBuf;
304 *pTemp &= 0xc78f; /* bit4,5,6,11,12,13 */
305
306 memcpy((TempBuf + 2), (pHeader + 4), 12); /* Addr1, Addr2 */
307 memcpy((TempBuf + 14), (pHeader + 22), 2); /* SeqCtrl */
308 pTemp = (u16 *)(TempBuf + 14);
309 *pTemp &= 0x000f;
310
311 memcpy((TempBuf + 16), (pHeader + 16), 6); /* Addr3 */
312
313 fc = le16_to_cpu(header->frame_ctl);
314
315
316
317 if (GetFrDs((u16 *)&fc) && GetToDs((u16 *)&fc)) {
318 memcpy((TempBuf + 22), (pHeader + 24), 6);
319 QosOffset = 30;
320 } else {
321 memset((TempBuf + 22), 0, 6);
322 QosOffset = 24;
323 }
324
325 if ((fc & 0x0088) == 0x0088) {
326 memcpy((TempBuf + 28), (pHeader + QosOffset), 2);
327 TempLen += 2;
328 /* IV = pHeader + QosOffset + 2 + SNAP_SIZE + sizeof(u16) + 2; */
329 IV = pHeader + QosOffset + 2 + 2;
330 } else {
331 IV = pHeader + QosOffset + 2;
332 /* IV = pHeader + QosOffset + SNAP_SIZE + sizeof(u16) + 2; */
333 }
334
335 TempBuf[TempLen - 1] = (u8)(DataLen & 0xff);
336 TempBuf[TempLen - 2] = (u8)((DataLen & 0xff00) >> 8);
337 TempBuf[TempLen - 4] = KeyIdx;
338
339 WAPI_DATA(WAPI_TX, "CalculateMic - KEY", MicKey, 16);
340 WAPI_DATA(WAPI_TX, "CalculateMic - IV", IV, 16);
341 WAPI_DATA(WAPI_TX, "CalculateMic - TempBuf", TempBuf, TempLen);
342 WAPI_DATA(WAPI_TX, "CalculateMic - pData", pData, DataLen);
343
344 WapiSMS4CalculateMic(MicKey, IV, TempBuf, TempLen,
345 pData, DataLen, MicBuffer, &MicLen);
346
347 if (MicLen != 16)
348 WAPI_TRACE(WAPI_ERR, "%s: MIC Length Error!!\n", __FUNCTION__);
349
350 WAPI_TRACE(WAPI_TX | WAPI_RX, "<=========%s\n", __FUNCTION__);
351 #endif
352 }
353
354 /* AddCount: 1 or 2.
355 * If overflow, return 1,
356 * else return 0.
357 */
WapiIncreasePN(u8 * PN,u8 AddCount)358 u8 WapiIncreasePN(u8 *PN, u8 AddCount)
359 {
360 u8 i;
361
362 if (NULL == PN)
363 return 1;
364 /* YJ,test,091102 */
365 /*
366 if(AddCount == 2){
367 RTW_INFO("############################%s(): PN[0]=0x%x\n", __FUNCTION__, PN[0]);
368 if(PN[0] == 0x48){
369 PN[0] += AddCount;
370 return 1;
371 }else{
372 PN[0] += AddCount;
373 return 0;
374 }
375 }
376 */
377 /* YJ,test,091102,end */
378
379 for (i = 0; i < 16; i++) {
380 if (PN[i] + AddCount <= 0xff) {
381 PN[i] += AddCount;
382 return 0;
383 } else {
384 PN[i] += AddCount;
385 AddCount = 1;
386 }
387 }
388 return 1;
389 }
390
391
WapiGetLastRxUnicastPNForQoSData(u8 UserPriority,PRT_WAPI_STA_INFO pWapiStaInfo,u8 * PNOut)392 void WapiGetLastRxUnicastPNForQoSData(
393 u8 UserPriority,
394 PRT_WAPI_STA_INFO pWapiStaInfo,
395 u8 *PNOut
396 )
397 {
398 WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);
399 switch (UserPriority) {
400 case 0:
401 case 3:
402 memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNBEQueue, 16);
403 break;
404 case 1:
405 case 2:
406 memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNBKQueue, 16);
407 break;
408 case 4:
409 case 5:
410 memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNVIQueue, 16);
411 break;
412 case 6:
413 case 7:
414 memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNVOQueue, 16);
415 break;
416 default:
417 WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__);
418 break;
419 }
420 WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__);
421 }
422
423
WapiSetLastRxUnicastPNForQoSData(u8 UserPriority,u8 * PNIn,PRT_WAPI_STA_INFO pWapiStaInfo)424 void WapiSetLastRxUnicastPNForQoSData(
425 u8 UserPriority,
426 u8 *PNIn,
427 PRT_WAPI_STA_INFO pWapiStaInfo
428 )
429 {
430 WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);
431 switch (UserPriority) {
432 case 0:
433 case 3:
434 memcpy(pWapiStaInfo->lastRxUnicastPNBEQueue, PNIn, 16);
435 break;
436 case 1:
437 case 2:
438 memcpy(pWapiStaInfo->lastRxUnicastPNBKQueue, PNIn, 16);
439 break;
440 case 4:
441 case 5:
442 memcpy(pWapiStaInfo->lastRxUnicastPNVIQueue, PNIn, 16);
443 break;
444 case 6:
445 case 7:
446 memcpy(pWapiStaInfo->lastRxUnicastPNVOQueue, PNIn, 16);
447 break;
448 default:
449 WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__);
450 break;
451 }
452 WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__);
453 }
454
455
456 /****************************************************************************
457 FALSE not RX-Reorder
458 TRUE do RX Reorder
459 add to support WAPI to N-mode
460 *****************************************************************************/
WapiCheckPnInSwDecrypt(_adapter * padapter,struct sk_buff * pskb)461 u8 WapiCheckPnInSwDecrypt(
462 _adapter *padapter,
463 struct sk_buff *pskb
464 )
465 {
466 u8 ret = false;
467
468 #if 0
469 struct ieee80211_hdr_3addr_qos *header;
470 u16 fc;
471 u8 *pDaddr, *pTaddr, *pRaddr;
472
473 header = (struct ieee80211_hdr_3addr_qos *)pskb->data;
474 pTaddr = header->addr2;
475 pRaddr = header->addr1;
476 fc = le16_to_cpu(header->frame_ctl);
477
478 if (GetToDs(&fc))
479 pDaddr = header->addr3;
480 else
481 pDaddr = header->addr1;
482
483 if ((_rtw_memcmp(pRaddr, padapter->pnetdev->dev_addr, ETH_ALEN) == 0)
484 && !(pDaddr)
485 && (GetFrameType(&fc) == WIFI_QOS_DATA_TYPE))
486 /* && ieee->pHTInfo->bCurrentHTSupport && */
487 /* ieee->pHTInfo->bCurRxReorderEnable) */
488 ret = false;
489 else
490 ret = true;
491 #endif
492 WAPI_TRACE(WAPI_RX, "%s: return %d\n", __FUNCTION__, ret);
493 return ret;
494 }
495
SecSMS4HeaderFillIV(_adapter * padapter,u8 * pxmitframe)496 int SecSMS4HeaderFillIV(_adapter *padapter, u8 *pxmitframe)
497 {
498 struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
499 u8 *frame = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
500 u8 *pSecHeader = NULL, *pos = NULL, *pRA = NULL;
501 u8 bPNOverflow = false, bFindMatchPeer = false, hdr_len = 0;
502 PWLAN_HEADER_WAPI_EXTENSION pWapiExt = NULL;
503 PRT_WAPI_T pWapiInfo = &padapter->wapiInfo;
504 PRT_WAPI_STA_INFO pWapiSta = NULL;
505 int ret = 0;
506
507 WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);
508
509 return ret;
510 #if 0
511 hdr_len = sMacHdrLng;
512 if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE)
513 hdr_len += 2;
514 /* hdr_len += SNAP_SIZE + sizeof(u16); */
515
516 pos = skb_push(pskb, padapter->wapiInfo.extra_prefix_len);
517 memmove(pos, pos + padapter->wapiInfo.extra_prefix_len, hdr_len);
518
519 pSecHeader = pskb->data + hdr_len;
520 pWapiExt = (PWLAN_HEADER_WAPI_EXTENSION)pSecHeader;
521 pRA = pskb->data + 4;
522
523 WAPI_DATA(WAPI_TX, "FillIV - Before Fill IV", pskb->data, pskb->len);
524
525 /* Address 1 is always receiver's address */
526 if (IS_MCAST(pRA)) {
527 if (!pWapiInfo->wapiTxMsk.bTxEnable) {
528 WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__);
529 return -2;
530 }
531 if (pWapiInfo->wapiTxMsk.keyId <= 1) {
532 pWapiExt->KeyIdx = pWapiInfo->wapiTxMsk.keyId;
533 pWapiExt->Reserved = 0;
534 bPNOverflow = WapiIncreasePN(pWapiInfo->lastTxMulticastPN, 1);
535 memcpy(pWapiExt->PN, pWapiInfo->lastTxMulticastPN, 16);
536 if (bPNOverflow) {
537 /* Update MSK Notification. */
538 WAPI_TRACE(WAPI_ERR, "===============>%s():multicast PN overflow\n", __FUNCTION__);
539 rtw_wapi_app_event_handler(padapter, NULL, 0, pRA, false, false, true, 0, false);
540 }
541 } else {
542 WAPI_TRACE(WAPI_ERR, "%s: Invalid Wapi Multicast KeyIdx!!\n", __FUNCTION__);
543 ret = -3;
544 }
545 } else {
546 list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {
547 if (!memcmp(pWapiSta->PeerMacAddr, pRA, 6)) {
548 bFindMatchPeer = true;
549 break;
550 }
551 }
552 if (bFindMatchPeer) {
553 if ((!pWapiSta->wapiUskUpdate.bTxEnable) && (!pWapiSta->wapiUsk.bTxEnable)) {
554 WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__);
555 return -4;
556 }
557 if (pWapiSta->wapiUsk.keyId <= 1) {
558 if (pWapiSta->wapiUskUpdate.bTxEnable)
559 pWapiExt->KeyIdx = pWapiSta->wapiUskUpdate.keyId;
560 else
561 pWapiExt->KeyIdx = pWapiSta->wapiUsk.keyId;
562
563 pWapiExt->Reserved = 0;
564 bPNOverflow = WapiIncreasePN(pWapiSta->lastTxUnicastPN, 2);
565 memcpy(pWapiExt->PN, pWapiSta->lastTxUnicastPN, 16);
566 if (bPNOverflow) {
567 /* Update USK Notification. */
568 WAPI_TRACE(WAPI_ERR, "===============>%s():unicast PN overflow\n", __FUNCTION__);
569 rtw_wapi_app_event_handler(padapter, NULL, 0, pWapiSta->PeerMacAddr, false, true, false, 0, false);
570 }
571 } else {
572 WAPI_TRACE(WAPI_ERR, "%s: Invalid Wapi Unicast KeyIdx!!\n", __FUNCTION__);
573 ret = -5;
574 }
575 } else {
576 WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT"!!\n", __FUNCTION__, MAC_ARG(pRA));
577 ret = -6;
578 }
579 }
580
581 WAPI_DATA(WAPI_TX, "FillIV - After Fill IV", pskb->data, pskb->len);
582 WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);
583 return ret;
584 #endif
585 }
586
587 /* WAPI SW Enc: must have done Coalesce! */
SecSWSMS4Encryption(_adapter * padapter,u8 * pxmitframe)588 void SecSWSMS4Encryption(
589 _adapter *padapter,
590 u8 *pxmitframe
591 )
592 {
593 PRT_WAPI_T pWapiInfo = &padapter->wapiInfo;
594 PRT_WAPI_STA_INFO pWapiSta = NULL;
595 u8 *pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_SIZE;
596 struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
597
598 u8 *SecPtr = NULL, *pRA, *pMicKey = NULL, *pDataKey = NULL, *pIV = NULL;
599 u8 IVOffset, DataOffset, bFindMatchPeer = false, KeyIdx = 0, MicBuffer[16];
600 u16 OutputLength;
601
602 WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);
603
604 WAPI_TRACE(WAPI_TX, "hdrlen: %d\n", pattrib->hdrlen);
605
606 return;
607
608 DataOffset = pattrib->hdrlen + pattrib->iv_len;
609
610 pRA = pframe + 4;
611
612
613 if (IS_MCAST(pRA)) {
614 KeyIdx = pWapiInfo->wapiTxMsk.keyId;
615 pIV = pWapiInfo->lastTxMulticastPN;
616 pMicKey = pWapiInfo->wapiTxMsk.micKey;
617 pDataKey = pWapiInfo->wapiTxMsk.dataKey;
618 } else {
619 if (!list_empty(&(pWapiInfo->wapiSTAUsedList))) {
620 list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {
621 if (0 == memcmp(pWapiSta->PeerMacAddr, pRA, 6)) {
622 bFindMatchPeer = true;
623 break;
624 }
625 }
626
627 if (bFindMatchPeer) {
628 if (pWapiSta->wapiUskUpdate.bTxEnable) {
629 KeyIdx = pWapiSta->wapiUskUpdate.keyId;
630 WAPI_TRACE(WAPI_TX, "%s(): Use update USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx);
631 pIV = pWapiSta->lastTxUnicastPN;
632 pMicKey = pWapiSta->wapiUskUpdate.micKey;
633 pDataKey = pWapiSta->wapiUskUpdate.dataKey;
634 } else {
635 KeyIdx = pWapiSta->wapiUsk.keyId;
636 WAPI_TRACE(WAPI_TX, "%s(): Use USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx);
637 pIV = pWapiSta->lastTxUnicastPN;
638 pMicKey = pWapiSta->wapiUsk.micKey;
639 pDataKey = pWapiSta->wapiUsk.dataKey;
640 }
641 } else {
642 WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta!!\n", __FUNCTION__);
643 return;
644 }
645 } else {
646 WAPI_TRACE(WAPI_ERR, "%s: wapiSTAUsedList is empty!!\n", __FUNCTION__);
647 return;
648 }
649 }
650
651 SecPtr = pframe;
652 SecCalculateMicSMS4(KeyIdx, pMicKey, SecPtr, (SecPtr + DataOffset), pattrib->pktlen, MicBuffer);
653
654 WAPI_DATA(WAPI_TX, "Encryption - MIC", MicBuffer, padapter->wapiInfo.extra_postfix_len);
655
656 memcpy(pframe + pattrib->hdrlen + pattrib->iv_len + pattrib->pktlen - pattrib->icv_len,
657 (u8 *)MicBuffer,
658 padapter->wapiInfo.extra_postfix_len
659 );
660
661
662 WapiSMS4Encryption(pDataKey, pIV, (SecPtr + DataOffset), pattrib->pktlen + pattrib->icv_len, (SecPtr + DataOffset), &OutputLength);
663
664 WAPI_DATA(WAPI_TX, "Encryption - After SMS4 encryption", pframe, pattrib->hdrlen + pattrib->iv_len + pattrib->pktlen);
665
666 WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);
667 }
668
SecSWSMS4Decryption(_adapter * padapter,u8 * precv_frame,struct recv_priv * precv_priv)669 u8 SecSWSMS4Decryption(
670 _adapter *padapter,
671 u8 *precv_frame,
672 struct recv_priv *precv_priv
673 )
674 {
675 PRT_WAPI_T pWapiInfo = &padapter->wapiInfo;
676 struct recv_frame_hdr *precv_hdr;
677 PRT_WAPI_STA_INFO pWapiSta = NULL;
678 u8 IVOffset, DataOffset, bFindMatchPeer = false, bUseUpdatedKey = false;
679 u8 KeyIdx, MicBuffer[16], lastRxPNforQoS[16];
680 u8 *pRA, *pTA, *pMicKey, *pDataKey, *pLastRxPN, *pRecvPN, *pSecData, *pRecvMic, *pos;
681 u8 TID = 0;
682 u16 OutputLength, DataLen;
683 u8 bQosData;
684 struct sk_buff *pskb;
685
686 WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__);
687
688 return 0;
689
690 precv_hdr = &((union recv_frame *)precv_frame)->u.hdr;
691 pskb = (struct sk_buff *)(precv_hdr->rx_data);
692 precv_hdr->bWapiCheckPNInDecrypt = WapiCheckPnInSwDecrypt(padapter, pskb);
693 WAPI_TRACE(WAPI_RX, "=========>%s: check PN %d\n", __FUNCTION__, precv_hdr->bWapiCheckPNInDecrypt);
694 WAPI_DATA(WAPI_RX, "Decryption - Before decryption", pskb->data, pskb->len);
695
696 IVOffset = sMacHdrLng;
697 bQosData = GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE;
698 if (bQosData)
699 IVOffset += 2;
700
701 /* if(GetHTC()) */
702 /* IVOffset += 4; */
703
704 /* IVOffset += SNAP_SIZE + sizeof(u16); */
705
706 DataOffset = IVOffset + padapter->wapiInfo.extra_prefix_len;
707
708 pRA = pskb->data + 4;
709 pTA = pskb->data + 10;
710 KeyIdx = *(pskb->data + IVOffset);
711 pRecvPN = pskb->data + IVOffset + 2;
712 pSecData = pskb->data + DataOffset;
713 DataLen = pskb->len - DataOffset;
714 pRecvMic = pskb->data + pskb->len - padapter->wapiInfo.extra_postfix_len;
715 TID = GetTid(pskb->data);
716
717 if (!list_empty(&(pWapiInfo->wapiSTAUsedList))) {
718 list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {
719 if (0 == memcmp(pWapiSta->PeerMacAddr, pTA, 6)) {
720 bFindMatchPeer = true;
721 break;
722 }
723 }
724 }
725
726 if (!bFindMatchPeer) {
727 WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT" for Key Info!!!\n", __FUNCTION__, MAC_ARG(pTA));
728 return false;
729 }
730
731 if (IS_MCAST(pRA)) {
732 WAPI_TRACE(WAPI_RX, "%s: Multicast decryption !!!\n", __FUNCTION__);
733 if (pWapiSta->wapiMsk.keyId == KeyIdx && pWapiSta->wapiMsk.bSet) {
734 pLastRxPN = pWapiSta->lastRxMulticastPN;
735 if (!WapiComparePN(pRecvPN, pLastRxPN)) {
736 WAPI_TRACE(WAPI_ERR, "%s: MSK PN is not larger than last, Dropped!!!\n", __FUNCTION__);
737 WAPI_DATA(WAPI_ERR, "pRecvPN:", pRecvPN, 16);
738 WAPI_DATA(WAPI_ERR, "pLastRxPN:", pLastRxPN, 16);
739 return false;
740 }
741
742 memcpy(pLastRxPN, pRecvPN, 16);
743 pMicKey = pWapiSta->wapiMsk.micKey;
744 pDataKey = pWapiSta->wapiMsk.dataKey;
745 } else if (pWapiSta->wapiMskUpdate.keyId == KeyIdx && pWapiSta->wapiMskUpdate.bSet) {
746 WAPI_TRACE(WAPI_RX, "%s: Use Updated MSK for Decryption !!!\n", __FUNCTION__);
747 bUseUpdatedKey = true;
748 memcpy(pWapiSta->lastRxMulticastPN, pRecvPN, 16);
749 pMicKey = pWapiSta->wapiMskUpdate.micKey;
750 pDataKey = pWapiSta->wapiMskUpdate.dataKey;
751 } else {
752 WAPI_TRACE(WAPI_ERR, "%s: Can not find MSK with matched KeyIdx(%d), Dropped !!!\n", __FUNCTION__, KeyIdx);
753 return false;
754 }
755 } else {
756 WAPI_TRACE(WAPI_RX, "%s: Unicast decryption !!!\n", __FUNCTION__);
757 if (pWapiSta->wapiUsk.keyId == KeyIdx && pWapiSta->wapiUsk.bSet) {
758 WAPI_TRACE(WAPI_RX, "%s: Use USK for Decryption!!!\n", __FUNCTION__);
759 if (precv_hdr->bWapiCheckPNInDecrypt) {
760 if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE) {
761 WapiGetLastRxUnicastPNForQoSData(TID, pWapiSta, lastRxPNforQoS);
762 pLastRxPN = lastRxPNforQoS;
763 } else
764 pLastRxPN = pWapiSta->lastRxUnicastPN;
765 if (!WapiComparePN(pRecvPN, pLastRxPN))
766 return false;
767 if (bQosData)
768 WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta);
769 else
770 memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16);
771 } else
772 memcpy(precv_hdr->WapiTempPN, pRecvPN, 16);
773
774 if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) {
775 if ((pRecvPN[0] & 0x1) == 0) {
776 WAPI_TRACE(WAPI_ERR, "%s: Rx USK PN is not odd when Infra STA mode, Dropped !!!\n", __FUNCTION__);
777 return false;
778 }
779 }
780
781 pMicKey = pWapiSta->wapiUsk.micKey;
782 pDataKey = pWapiSta->wapiUsk.dataKey;
783 } else if (pWapiSta->wapiUskUpdate.keyId == KeyIdx && pWapiSta->wapiUskUpdate.bSet) {
784 WAPI_TRACE(WAPI_RX, "%s: Use Updated USK for Decryption!!!\n", __FUNCTION__);
785 if (pWapiSta->bAuthenticatorInUpdata)
786 bUseUpdatedKey = true;
787 else
788 bUseUpdatedKey = false;
789
790 if (bQosData)
791 WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta);
792 else
793 memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16);
794 pMicKey = pWapiSta->wapiUskUpdate.micKey;
795 pDataKey = pWapiSta->wapiUskUpdate.dataKey;
796 } else {
797 WAPI_TRACE(WAPI_ERR, "%s: No valid USK!!!KeyIdx=%d pWapiSta->wapiUsk.keyId=%d pWapiSta->wapiUskUpdate.keyId=%d\n", __FUNCTION__, KeyIdx, pWapiSta->wapiUsk.keyId,
798 pWapiSta->wapiUskUpdate.keyId);
799 /* dump_buf(pskb->data,pskb->len); */
800 return false;
801 }
802 }
803
804 WAPI_DATA(WAPI_RX, "Decryption - DataKey", pDataKey, 16);
805 WAPI_DATA(WAPI_RX, "Decryption - IV", pRecvPN, 16);
806 WapiSMS4Decryption(pDataKey, pRecvPN, pSecData, DataLen, pSecData, &OutputLength);
807
808 if (OutputLength != DataLen)
809 WAPI_TRACE(WAPI_ERR, "%s: Output Length Error!!!!\n", __FUNCTION__);
810
811 WAPI_DATA(WAPI_RX, "Decryption - After decryption", pskb->data, pskb->len);
812
813 DataLen -= padapter->wapiInfo.extra_postfix_len;
814
815 SecCalculateMicSMS4(KeyIdx, pMicKey, pskb->data, pSecData, DataLen, MicBuffer);
816
817 WAPI_DATA(WAPI_RX, "Decryption - MIC received", pRecvMic, SMS4_MIC_LEN);
818 WAPI_DATA(WAPI_RX, "Decryption - MIC calculated", MicBuffer, SMS4_MIC_LEN);
819
820 if (0 == memcmp(MicBuffer, pRecvMic, padapter->wapiInfo.extra_postfix_len)) {
821 WAPI_TRACE(WAPI_RX, "%s: Check MIC OK!!\n", __FUNCTION__);
822 if (bUseUpdatedKey) {
823 /* delete the old key */
824 if (IS_MCAST(pRA)) {
825 WAPI_TRACE(WAPI_API, "%s(): AE use new update MSK!!\n", __FUNCTION__);
826 pWapiSta->wapiMsk.keyId = pWapiSta->wapiMskUpdate.keyId;
827 memcpy(pWapiSta->wapiMsk.dataKey, pWapiSta->wapiMskUpdate.dataKey, 16);
828 memcpy(pWapiSta->wapiMsk.micKey, pWapiSta->wapiMskUpdate.micKey, 16);
829 pWapiSta->wapiMskUpdate.bTxEnable = pWapiSta->wapiMskUpdate.bSet = false;
830 } else {
831 WAPI_TRACE(WAPI_API, "%s(): AE use new update USK!!\n", __FUNCTION__);
832 pWapiSta->wapiUsk.keyId = pWapiSta->wapiUskUpdate.keyId;
833 memcpy(pWapiSta->wapiUsk.dataKey, pWapiSta->wapiUskUpdate.dataKey, 16);
834 memcpy(pWapiSta->wapiUsk.micKey, pWapiSta->wapiUskUpdate.micKey, 16);
835 pWapiSta->wapiUskUpdate.bTxEnable = pWapiSta->wapiUskUpdate.bSet = false;
836 }
837 }
838 } else {
839 WAPI_TRACE(WAPI_ERR, "%s: Check MIC Error, Dropped !!!!\n", __FUNCTION__);
840 return false;
841 }
842
843 pos = pskb->data;
844 memmove(pos + padapter->wapiInfo.extra_prefix_len, pos, IVOffset);
845 skb_pull(pskb, padapter->wapiInfo.extra_prefix_len);
846
847 WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__);
848
849 return true;
850 }
851
rtw_sms4_encrypt(_adapter * padapter,u8 * pxmitframe)852 u32 rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe)
853 {
854
855 u8 *pframe;
856 u32 res = _SUCCESS;
857
858 WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);
859
860 if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) {
861 WAPI_TRACE(WAPI_TX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__);
862 return _FAIL;
863 }
864
865 if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
866 return _FAIL;
867
868 pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
869
870 SecSWSMS4Encryption(padapter, pxmitframe);
871
872 WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);
873 return res;
874 }
875
rtw_sms4_decrypt(_adapter * padapter,u8 * precvframe)876 u32 rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe)
877 {
878 u8 *pframe;
879 u32 res = _SUCCESS;
880
881 WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__);
882
883 if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) {
884 WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__);
885 return _FAIL;
886 }
887
888
889 /* drop packet when hw decrypt fail
890 * return tempraily */
891 return _FAIL;
892
893 /* pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data; */
894
895 if (false == SecSWSMS4Decryption(padapter, precvframe, &padapter->recvpriv)) {
896 WAPI_TRACE(WAPI_ERR, "%s():SMS4 decrypt frame error\n", __FUNCTION__);
897 return _FAIL;
898 }
899
900 WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__);
901 return res;
902 }
903
904 #else
905
rtw_sms4_encrypt(_adapter * padapter,u8 * pxmitframe)906 u32 rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe)
907 {
908 WAPI_TRACE(WAPI_TX, "=========>Dummy %s\n", __FUNCTION__);
909 WAPI_TRACE(WAPI_TX, "<=========Dummy %s\n", __FUNCTION__);
910 return _SUCCESS;
911 }
912
rtw_sms4_decrypt(_adapter * padapter,u8 * precvframe)913 u32 rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe)
914 {
915 WAPI_TRACE(WAPI_RX, "=========>Dummy %s\n", __FUNCTION__);
916 WAPI_TRACE(WAPI_RX, "<=========Dummy %s\n", __FUNCTION__);
917 return _SUCCESS;
918 }
919
920 #endif
921
922 #endif
923