1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36#include "device.h"
37#include "80211hdr.h"
38
39
40
41
42
43
44
45
46
47
48
49static unsigned char sbox_table[256] =
50{
51 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
52 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
53 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
54 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
55 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
56 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
57 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
58 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
59 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
60 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
61 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
62 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
63 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
64 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
65 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
66 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
67};
68
69static unsigned char dot2_table[256] = {
70 0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1a, 0x1c, 0x1e,
71 0x20, 0x22, 0x24, 0x26, 0x28, 0x2a, 0x2c, 0x2e, 0x30, 0x32, 0x34, 0x36, 0x38, 0x3a, 0x3c, 0x3e,
72 0x40, 0x42, 0x44, 0x46, 0x48, 0x4a, 0x4c, 0x4e, 0x50, 0x52, 0x54, 0x56, 0x58, 0x5a, 0x5c, 0x5e,
73 0x60, 0x62, 0x64, 0x66, 0x68, 0x6a, 0x6c, 0x6e, 0x70, 0x72, 0x74, 0x76, 0x78, 0x7a, 0x7c, 0x7e,
74 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9c, 0x9e,
75 0xa0, 0xa2, 0xa4, 0xa6, 0xa8, 0xaa, 0xac, 0xae, 0xb0, 0xb2, 0xb4, 0xb6, 0xb8, 0xba, 0xbc, 0xbe,
76 0xc0, 0xc2, 0xc4, 0xc6, 0xc8, 0xca, 0xcc, 0xce, 0xd0, 0xd2, 0xd4, 0xd6, 0xd8, 0xda, 0xdc, 0xde,
77 0xe0, 0xe2, 0xe4, 0xe6, 0xe8, 0xea, 0xec, 0xee, 0xf0, 0xf2, 0xf4, 0xf6, 0xf8, 0xfa, 0xfc, 0xfe,
78 0x1b, 0x19, 0x1f, 0x1d, 0x13, 0x11, 0x17, 0x15, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05,
79 0x3b, 0x39, 0x3f, 0x3d, 0x33, 0x31, 0x37, 0x35, 0x2b, 0x29, 0x2f, 0x2d, 0x23, 0x21, 0x27, 0x25,
80 0x5b, 0x59, 0x5f, 0x5d, 0x53, 0x51, 0x57, 0x55, 0x4b, 0x49, 0x4f, 0x4d, 0x43, 0x41, 0x47, 0x45,
81 0x7b, 0x79, 0x7f, 0x7d, 0x73, 0x71, 0x77, 0x75, 0x6b, 0x69, 0x6f, 0x6d, 0x63, 0x61, 0x67, 0x65,
82 0x9b, 0x99, 0x9f, 0x9d, 0x93, 0x91, 0x97, 0x95, 0x8b, 0x89, 0x8f, 0x8d, 0x83, 0x81, 0x87, 0x85,
83 0xbb, 0xb9, 0xbf, 0xbd, 0xb3, 0xb1, 0xb7, 0xb5, 0xab, 0xa9, 0xaf, 0xad, 0xa3, 0xa1, 0xa7, 0xa5,
84 0xdb, 0xd9, 0xdf, 0xdd, 0xd3, 0xd1, 0xd7, 0xd5, 0xcb, 0xc9, 0xcf, 0xcd, 0xc3, 0xc1, 0xc7, 0xc5,
85 0xfb, 0xf9, 0xff, 0xfd, 0xf3, 0xf1, 0xf7, 0xf5, 0xeb, 0xe9, 0xef, 0xed, 0xe3, 0xe1, 0xe7, 0xe5
86};
87
88static unsigned char dot3_table[256] = {
89 0x00, 0x03, 0x06, 0x05, 0x0c, 0x0f, 0x0a, 0x09, 0x18, 0x1b, 0x1e, 0x1d, 0x14, 0x17, 0x12, 0x11,
90 0x30, 0x33, 0x36, 0x35, 0x3c, 0x3f, 0x3a, 0x39, 0x28, 0x2b, 0x2e, 0x2d, 0x24, 0x27, 0x22, 0x21,
91 0x60, 0x63, 0x66, 0x65, 0x6c, 0x6f, 0x6a, 0x69, 0x78, 0x7b, 0x7e, 0x7d, 0x74, 0x77, 0x72, 0x71,
92 0x50, 0x53, 0x56, 0x55, 0x5c, 0x5f, 0x5a, 0x59, 0x48, 0x4b, 0x4e, 0x4d, 0x44, 0x47, 0x42, 0x41,
93 0xc0, 0xc3, 0xc6, 0xc5, 0xcc, 0xcf, 0xca, 0xc9, 0xd8, 0xdb, 0xde, 0xdd, 0xd4, 0xd7, 0xd2, 0xd1,
94 0xf0, 0xf3, 0xf6, 0xf5, 0xfc, 0xff, 0xfa, 0xf9, 0xe8, 0xeb, 0xee, 0xed, 0xe4, 0xe7, 0xe2, 0xe1,
95 0xa0, 0xa3, 0xa6, 0xa5, 0xac, 0xaf, 0xaa, 0xa9, 0xb8, 0xbb, 0xbe, 0xbd, 0xb4, 0xb7, 0xb2, 0xb1,
96 0x90, 0x93, 0x96, 0x95, 0x9c, 0x9f, 0x9a, 0x99, 0x88, 0x8b, 0x8e, 0x8d, 0x84, 0x87, 0x82, 0x81,
97 0x9b, 0x98, 0x9d, 0x9e, 0x97, 0x94, 0x91, 0x92, 0x83, 0x80, 0x85, 0x86, 0x8f, 0x8c, 0x89, 0x8a,
98 0xab, 0xa8, 0xad, 0xae, 0xa7, 0xa4, 0xa1, 0xa2, 0xb3, 0xb0, 0xb5, 0xb6, 0xbf, 0xbc, 0xb9, 0xba,
99 0xfb, 0xf8, 0xfd, 0xfe, 0xf7, 0xf4, 0xf1, 0xf2, 0xe3, 0xe0, 0xe5, 0xe6, 0xef, 0xec, 0xe9, 0xea,
100 0xcb, 0xc8, 0xcd, 0xce, 0xc7, 0xc4, 0xc1, 0xc2, 0xd3, 0xd0, 0xd5, 0xd6, 0xdf, 0xdc, 0xd9, 0xda,
101 0x5b, 0x58, 0x5d, 0x5e, 0x57, 0x54, 0x51, 0x52, 0x43, 0x40, 0x45, 0x46, 0x4f, 0x4c, 0x49, 0x4a,
102 0x6b, 0x68, 0x6d, 0x6e, 0x67, 0x64, 0x61, 0x62, 0x73, 0x70, 0x75, 0x76, 0x7f, 0x7c, 0x79, 0x7a,
103 0x3b, 0x38, 0x3d, 0x3e, 0x37, 0x34, 0x31, 0x32, 0x23, 0x20, 0x25, 0x26, 0x2f, 0x2c, 0x29, 0x2a,
104 0x0b, 0x08, 0x0d, 0x0e, 0x07, 0x04, 0x01, 0x02, 0x13, 0x10, 0x15, 0x16, 0x1f, 0x1c, 0x19, 0x1a
105};
106
107
108
109
110
111
112
113static void xor_128(unsigned char *a, unsigned char *b, unsigned char *out)
114{
115 unsigned long *dwPtrA = (unsigned long *)a;
116 unsigned long *dwPtrB = (unsigned long *)b;
117 unsigned long *dwPtrOut = (unsigned long *)out;
118
119 (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
120 (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
121 (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
122 (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
123}
124
125static void xor_32(unsigned char *a, unsigned char *b, unsigned char *out)
126{
127 unsigned long *dwPtrA = (unsigned long *)a;
128 unsigned long *dwPtrB = (unsigned long *)b;
129 unsigned long *dwPtrOut = (unsigned long *)out;
130
131 (*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
132}
133
134static void AddRoundKey(unsigned char *key, int round)
135{
136 unsigned char sbox_key[4];
137 unsigned char rcon_table[10] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36};
138
139 sbox_key[0] = sbox_table[key[13]];
140 sbox_key[1] = sbox_table[key[14]];
141 sbox_key[2] = sbox_table[key[15]];
142 sbox_key[3] = sbox_table[key[12]];
143
144 key[0] = key[0] ^ rcon_table[round];
145 xor_32(&key[0], sbox_key, &key[0]);
146
147 xor_32(&key[4], &key[0], &key[4]);
148 xor_32(&key[8], &key[4], &key[8]);
149 xor_32(&key[12], &key[8], &key[12]);
150}
151
152static void SubBytes(unsigned char *in, unsigned char *out)
153{
154 int i;
155
156 for (i = 0; i < 16; i++) {
157 out[i] = sbox_table[in[i]];
158 }
159}
160
161static void ShiftRows(unsigned char *in, unsigned char *out)
162{
163 out[0] = in[0];
164 out[1] = in[5];
165 out[2] = in[10];
166 out[3] = in[15];
167 out[4] = in[4];
168 out[5] = in[9];
169 out[6] = in[14];
170 out[7] = in[3];
171 out[8] = in[8];
172 out[9] = in[13];
173 out[10] = in[2];
174 out[11] = in[7];
175 out[12] = in[12];
176 out[13] = in[1];
177 out[14] = in[6];
178 out[15] = in[11];
179}
180
181static void MixColumns(unsigned char *in, unsigned char *out)
182{
183 out[0] = dot2_table[in[0]] ^ dot3_table[in[1]] ^ in[2] ^ in[3];
184 out[1] = in[0] ^ dot2_table[in[1]] ^ dot3_table[in[2]] ^ in[3];
185 out[2] = in[0] ^ in[1] ^ dot2_table[in[2]] ^ dot3_table[in[3]];
186 out[3] = dot3_table[in[0]] ^ in[1] ^ in[2] ^ dot2_table[in[3]];
187}
188
189static void AESv128(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
190{
191 int i;
192 int round;
193 unsigned char TmpdataA[16];
194 unsigned char TmpdataB[16];
195 unsigned char abyRoundKey[16];
196
197 for (i = 0; i < 16; i++)
198 abyRoundKey[i] = key[i];
199
200 for (round = 0; round < 11; round++) {
201 if (round == 0) {
202 xor_128(abyRoundKey, data, ciphertext);
203 AddRoundKey(abyRoundKey, round);
204 } else if (round == 10) {
205 SubBytes(ciphertext, TmpdataA);
206 ShiftRows(TmpdataA, TmpdataB);
207 xor_128(TmpdataB, abyRoundKey, ciphertext);
208 } else
209 {
210 SubBytes(ciphertext, TmpdataA);
211 ShiftRows(TmpdataA, TmpdataB);
212 MixColumns(&TmpdataB[0], &TmpdataA[0]);
213 MixColumns(&TmpdataB[4], &TmpdataA[4]);
214 MixColumns(&TmpdataB[8], &TmpdataA[8]);
215 MixColumns(&TmpdataB[12], &TmpdataA[12]);
216 xor_128(TmpdataA, abyRoundKey, ciphertext);
217 AddRoundKey(abyRoundKey, round);
218 }
219 }
220}
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236bool AESbGenCCMP(unsigned char *pbyRxKey, unsigned char *pbyFrame, unsigned short wFrameSize)
237{
238 unsigned char abyNonce[13];
239 unsigned char MIC_IV[16];
240 unsigned char MIC_HDR1[16];
241 unsigned char MIC_HDR2[16];
242 unsigned char abyMIC[16];
243 unsigned char abyCTRPLD[16];
244 unsigned char abyTmp[16];
245 unsigned char abyPlainText[16];
246 unsigned char abyLastCipher[16];
247
248 PS802_11Header pMACHeader = (PS802_11Header) pbyFrame;
249 unsigned char *pbyIV;
250 unsigned char *pbyPayload;
251 unsigned short wHLen = 22;
252 unsigned short wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;
253 bool bA4 = false;
254 unsigned char byTmp;
255 unsigned short wCnt;
256 int ii, jj, kk;
257
258 pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
259 if (WLAN_GET_FC_TODS(*(unsigned short *)pbyFrame) &&
260 WLAN_GET_FC_FROMDS(*(unsigned short *)pbyFrame)) {
261 bA4 = true;
262 pbyIV += 6;
263 wHLen += 6;
264 wPayloadSize -= 6;
265 }
266 pbyPayload = pbyIV + 8;
267
268 abyNonce[0] = 0x00;
269 memcpy(&(abyNonce[1]), pMACHeader->abyAddr2, ETH_ALEN);
270 abyNonce[7] = pbyIV[7];
271 abyNonce[8] = pbyIV[6];
272 abyNonce[9] = pbyIV[5];
273 abyNonce[10] = pbyIV[4];
274 abyNonce[11] = pbyIV[1];
275 abyNonce[12] = pbyIV[0];
276
277
278 MIC_IV[0] = 0x59;
279 memcpy(&(MIC_IV[1]), &(abyNonce[0]), 13);
280 MIC_IV[14] = (unsigned char)(wPayloadSize >> 8);
281 MIC_IV[15] = (unsigned char)(wPayloadSize & 0xff);
282
283
284 MIC_HDR1[0] = (unsigned char)(wHLen >> 8);
285 MIC_HDR1[1] = (unsigned char)(wHLen & 0xff);
286 byTmp = (unsigned char)(pMACHeader->wFrameCtl & 0xff);
287 MIC_HDR1[2] = byTmp & 0x8f;
288 byTmp = (unsigned char)(pMACHeader->wFrameCtl >> 8);
289 byTmp &= 0x87;
290 MIC_HDR1[3] = byTmp | 0x40;
291 memcpy(&(MIC_HDR1[4]), pMACHeader->abyAddr1, ETH_ALEN);
292 memcpy(&(MIC_HDR1[10]), pMACHeader->abyAddr2, ETH_ALEN);
293
294
295 memcpy(&(MIC_HDR2[0]), pMACHeader->abyAddr3, ETH_ALEN);
296 byTmp = (unsigned char)(pMACHeader->wSeqCtl & 0xff);
297 MIC_HDR2[6] = byTmp & 0x0f;
298 MIC_HDR2[7] = 0;
299 if (bA4) {
300 memcpy(&(MIC_HDR2[8]), pMACHeader->abyAddr4, ETH_ALEN);
301 } else {
302 MIC_HDR2[8] = 0x00;
303 MIC_HDR2[9] = 0x00;
304 MIC_HDR2[10] = 0x00;
305 MIC_HDR2[11] = 0x00;
306 MIC_HDR2[12] = 0x00;
307 MIC_HDR2[13] = 0x00;
308 }
309 MIC_HDR2[14] = 0x00;
310 MIC_HDR2[15] = 0x00;
311
312
313 AESv128(pbyRxKey, MIC_IV, abyMIC);
314 for (kk = 0; kk < 16; kk++) {
315 abyTmp[kk] = MIC_HDR1[kk] ^ abyMIC[kk];
316 }
317 AESv128(pbyRxKey, abyTmp, abyMIC);
318 for (kk = 0; kk < 16; kk++) {
319 abyTmp[kk] = MIC_HDR2[kk] ^ abyMIC[kk];
320 }
321 AESv128(pbyRxKey, abyTmp, abyMIC);
322
323 wCnt = 1;
324 abyCTRPLD[0] = 0x01;
325 memcpy(&(abyCTRPLD[1]), &(abyNonce[0]), 13);
326
327 for (jj = wPayloadSize; jj > 16; jj = jj - 16) {
328 abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
329 abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
330
331 AESv128(pbyRxKey, abyCTRPLD, abyTmp);
332
333 for (kk = 0; kk < 16; kk++) {
334 abyPlainText[kk] = abyTmp[kk] ^ pbyPayload[kk];
335 }
336 for (kk = 0; kk < 16; kk++) {
337 abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
338 }
339 AESv128(pbyRxKey, abyTmp, abyMIC);
340
341 memcpy(pbyPayload, abyPlainText, 16);
342 wCnt++;
343 pbyPayload += 16;
344 }
345
346
347 memcpy(&(abyLastCipher[0]), pbyPayload, jj);
348 for (ii = jj; ii < 16; ii++) {
349 abyLastCipher[ii] = 0x00;
350 }
351
352 abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
353 abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
354
355 AESv128(pbyRxKey, abyCTRPLD, abyTmp);
356 for (kk = 0; kk < 16; kk++) {
357 abyPlainText[kk] = abyTmp[kk] ^ abyLastCipher[kk];
358 }
359 memcpy(pbyPayload, abyPlainText, jj);
360 pbyPayload += jj;
361
362
363 for (ii = jj; ii < 16; ii++) {
364 abyPlainText[ii] = 0x00;
365 }
366 for (kk = 0; kk < 16; kk++) {
367 abyTmp[kk] = abyMIC[kk] ^ abyPlainText[kk];
368 }
369 AESv128(pbyRxKey, abyTmp, abyMIC);
370
371
372
373
374 wCnt = 0;
375 abyCTRPLD[14] = (unsigned char)(wCnt >> 8);
376 abyCTRPLD[15] = (unsigned char)(wCnt & 0xff);
377 AESv128(pbyRxKey, abyCTRPLD, abyTmp);
378 for (kk = 0; kk < 8; kk++) {
379 abyTmp[kk] = abyTmp[kk] ^ pbyPayload[kk];
380 }
381
382
383
384 if (!memcmp(abyMIC, abyTmp, 8))
385 return true;
386 else
387 return false;
388}
389