1 #include <stdio.h>
2 #include <stdint.h>
3 #include "btstack_util.h"
4 #include "aes_cmac.h"
5 #include <errno.h>
6 #include "aes_ccm.h"
7
8 // degbugging
9 // #define LOG_XN
10
11 // CCM Encrypt & Decrypt from Zephyr Project
12
13 typedef uint8_t u8_t;
14 typedef uint16_t u16_t;
15 typedef uint64_t u64_t;
16
sys_put_be16(uint16_t value,uint8_t * buffer)17 static void sys_put_be16(uint16_t value, uint8_t * buffer) {
18 big_endian_store_16(buffer, 0, value);
19 }
bt_encrypt_be(const uint8_t * key,const uint8_t * plain,uint8_t * cipher)20 static int bt_encrypt_be(const uint8_t * key, const uint8_t * plain, uint8_t * cipher) {
21 aes128_calc_cyphertext(key, plain, cipher);
22 return 0;
23 }
24
bt_mesh_ccm_decrypt(const u8_t key[16],u8_t nonce[13],const u8_t * enc_msg,size_t msg_len,const u8_t * aad,size_t aad_len,u8_t * out_msg,size_t mic_size)25 int bt_mesh_ccm_decrypt(const u8_t key[16], u8_t nonce[13],
26 const u8_t *enc_msg, size_t msg_len,
27 const u8_t *aad, size_t aad_len,
28 u8_t *out_msg, size_t mic_size)
29 {
30 u8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16], mic[16];
31 u16_t last_blk, blk_cnt;
32 size_t i, j;
33 int err;
34
35 if (msg_len < 1 || aad_len >= 0xff00) {
36 return -EINVAL;
37 }
38
39 /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
40 pmsg[0] = 0x01;
41 memcpy(pmsg + 1, nonce, 13);
42 sys_put_be16(0x0000, pmsg + 14);
43
44 #ifdef LOG_XN
45 printf("%16s: ", "A0");
46 printf_hexdump(pmsg, 16);
47 #endif
48
49 err = bt_encrypt_be(key, pmsg, cmic);
50 if (err) {
51 return err;
52 }
53
54 #ifdef LOG_XN
55 printf("%16s: ", "S0");
56 printf_hexdump(cmic, 16);
57 #endif
58
59
60 /* X_0 = e(AppKey, 0x09 || nonce || length) */
61 if (mic_size == sizeof(u64_t)) {
62 pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
63 } else {
64 pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
65 }
66
67 memcpy(pmsg + 1, nonce, 13);
68 sys_put_be16(msg_len, pmsg + 14);
69
70 #ifdef LOG_XN
71 printf("%16s: ", "B0");
72 printf_hexdump(pmsg, 16);
73 #endif
74
75 err = bt_encrypt_be(key, pmsg, Xn);
76 if (err) {
77 return err;
78 }
79
80 #ifdef LOG_XN
81 printf("%16s: ", "Xn");
82 printf_hexdump(Xn, 16);
83 #endif
84
85 /* If AAD is being used to authenticate, include it here */
86 if (aad_len) {
87 sys_put_be16(aad_len, pmsg);
88
89 for (i = 0; i < sizeof(u16_t); i++) {
90 pmsg[i] = Xn[i] ^ pmsg[i];
91 }
92
93 j = 0;
94 aad_len += sizeof(u16_t);
95 while (aad_len > 16) {
96 do {
97 pmsg[i] = Xn[i] ^ aad[j];
98 i++, j++;
99 } while (i < 16);
100
101 aad_len -= 16;
102 i = 0;
103
104 err = bt_encrypt_be(key, pmsg, Xn);
105 if (err) {
106 return err;
107 }
108 }
109
110 for (i = 0; i < aad_len; i++, j++) {
111 pmsg[i] = Xn[i] ^ aad[j];
112 }
113
114 for (i = aad_len; i < 16; i++) {
115 pmsg[i] = Xn[i];
116 }
117
118 #ifdef LOG_XN
119 printf("%16s: ", "Xn XOR bn");
120 printf_hexdump(pmsg, 16);
121 #endif
122
123 err = bt_encrypt_be(key, pmsg, Xn);
124 if (err) {
125 return err;
126 }
127 }
128
129 last_blk = msg_len % 16;
130 blk_cnt = (msg_len + 15) / 16;
131 if (!last_blk) {
132 last_blk = 16;
133 }
134
135 for (j = 0; j < blk_cnt; j++) {
136 if (j + 1 == blk_cnt) {
137 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
138 pmsg[0] = 0x01;
139 memcpy(pmsg + 1, nonce, 13);
140 sys_put_be16(j + 1, pmsg + 14);
141
142 err = bt_encrypt_be(key, pmsg, cmsg);
143 if (err) {
144 return err;
145 }
146
147 /* Encrypted = Payload[0-15] ^ C_1 */
148 for (i = 0; i < last_blk; i++) {
149 msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
150 }
151
152 memcpy(out_msg + (j * 16), msg, last_blk);
153
154 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
155 for (i = 0; i < last_blk; i++) {
156 pmsg[i] = Xn[i] ^ msg[i];
157 }
158
159 for (i = last_blk; i < 16; i++) {
160 pmsg[i] = Xn[i] ^ 0x00;
161 }
162
163 #ifdef LOG_XN
164 printf("%16s: ", "Xn XOR bn");
165 printf_hexdump(pmsg, 16);
166 #endif
167
168 err = bt_encrypt_be(key, pmsg, Xn);
169 if (err) {
170 return err;
171 }
172
173 #ifdef LOG_XN
174 printf("%16s: ", "Xn XOR bn");
175 printf_hexdump(pmsg, 16);
176 #endif
177
178 /* MIC = C_mic ^ X_1 */
179 for (i = 0; i < sizeof(mic); i++) {
180 mic[i] = cmic[i] ^ Xn[i];
181 }
182
183 #ifdef LOG_XN
184 printf("%16s: ", "mic");
185 printf_hexdump(mic, 16);
186 #endif
187
188 } else {
189 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
190 pmsg[0] = 0x01;
191 memcpy(pmsg + 1, nonce, 13);
192 sys_put_be16(j + 1, pmsg + 14);
193
194 #ifdef LOG_XN
195 printf("%16s: ", "Ai");
196 printf_hexdump(mic, 16);
197 #endif
198
199 err = bt_encrypt_be(key, pmsg, cmsg);
200 if (err) {
201 return err;
202 }
203
204 #ifdef LOG_XN
205 printf("%16s: ", "Si");
206 printf_hexdump(mic, 16);
207 #endif
208
209 /* Encrypted = Payload[0-15] ^ C_1 */
210 for (i = 0; i < 16; i++) {
211 msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
212 }
213
214 memcpy(out_msg + (j * 16), msg, 16);
215
216 #ifdef LOG_XN
217 printf("%16s: ", "bn");
218 printf_hexdump(msg, 16);
219 #endif
220
221 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
222 for (i = 0; i < 16; i++) {
223 pmsg[i] = Xn[i] ^ msg[i];
224 }
225
226 err = bt_encrypt_be(key, pmsg, Xn);
227 if (err) {
228 return err;
229 }
230
231 #ifdef LOG_XN
232 printf("%16s: ", "Xn");
233 printf_hexdump(mic, 16);
234 #endif
235
236 }
237 }
238
239 return 0;
240 }
241
bt_mesh_ccm_encrypt(const u8_t key[16],u8_t nonce[13],const u8_t * msg,size_t msg_len,const u8_t * aad,size_t aad_len,u8_t * out_msg,size_t mic_size)242 int bt_mesh_ccm_encrypt(const u8_t key[16], u8_t nonce[13],
243 const u8_t *msg, size_t msg_len,
244 const u8_t *aad, size_t aad_len,
245 u8_t *out_msg, size_t mic_size)
246 {
247 u8_t pmsg[16], cmic[16], cmsg[16], mic[16], Xn[16];
248 u16_t blk_cnt, last_blk;
249 size_t i, j;
250 int err;
251
252 // BT_DBG("key %s", bt_hex(key, 16));
253 // BT_DBG("nonce %s", bt_hex(nonce, 13));
254 // BT_DBG("msg (len %zu) %s", msg_len, bt_hex(msg, msg_len));
255 // BT_DBG("aad_len %zu mic_size %zu", aad_len, mic_size);
256
257 /* Unsupported AAD size */
258 if (aad_len >= 0xff00) {
259 return -EINVAL;
260 }
261
262 /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
263 pmsg[0] = 0x01;
264 memcpy(pmsg + 1, nonce, 13);
265 sys_put_be16(0x0000, pmsg + 14);
266
267 #ifdef LOG_XN
268 printf("%16s: ", "A0");
269 printf_hexdump(pmsg, 16);
270 #endif
271
272 err = bt_encrypt_be(key, pmsg, cmic);
273 if (err) {
274 return err;
275 }
276
277 #ifdef LOG_XN
278 printf("%16s: ", "S0");
279 printf_hexdump(cmic, 16);
280 #endif
281
282 /* X_0 = e(AppKey, 0x09 || nonce || length) */
283 if (mic_size == sizeof(u64_t)) {
284 pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
285 } else {
286 pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
287 }
288
289 memcpy(pmsg + 1, nonce, 13);
290 sys_put_be16(msg_len, pmsg + 14);
291
292 #ifdef LOG_XN
293 printf("%16s: ", "B0");
294 printf_hexdump(pmsg, 16);
295 #endif
296
297 err = bt_encrypt_be(key, pmsg, Xn);
298 if (err) {
299 return err;
300 }
301
302 #ifdef LOG_XN
303 printf("%16s: ", "X1");
304 printf_hexdump(Xn, 16);
305 #endif
306
307 /* If AAD is being used to authenticate, include it here */
308 if (aad_len) {
309 sys_put_be16(aad_len, pmsg);
310
311 for (i = 0; i < sizeof(u16_t); i++) {
312 pmsg[i] = Xn[i] ^ pmsg[i];
313 }
314
315 j = 0;
316 aad_len += sizeof(u16_t);
317 while (aad_len > 16) {
318 do {
319 pmsg[i] = Xn[i] ^ aad[j];
320 i++, j++;
321 } while (i < 16);
322
323 aad_len -= 16;
324 i = 0;
325
326 #ifdef LOG_XN
327 printf("%16s: ", "Xn XOR bn (aad)");
328 printf_hexdump(pmsg, 16);
329 #endif
330
331 err = bt_encrypt_be(key, pmsg, Xn);
332 if (err) {
333 return err;
334 }
335
336 #ifdef LOG_XN
337 printf("%16s: ", "Xn+1 AAD");
338 printf_hexdump(Xn, 16);
339 #endif
340
341 }
342
343 for (i = 0; i < aad_len; i++, j++) {
344 pmsg[i] = Xn[i] ^ aad[j];
345 }
346
347 for (i = aad_len; i < 16; i++) {
348 pmsg[i] = Xn[i];
349 }
350
351 #ifdef LOG_XN
352 printf("%16s: ", "Xn XOR bn (aad)");
353 printf_hexdump(pmsg, 16);
354 #endif
355
356 err = bt_encrypt_be(key, pmsg, Xn);
357 if (err) {
358 return err;
359 }
360 #ifdef LOG_XN
361 printf("%16s: ", "Xn+1 AAD");
362 printf_hexdump(Xn, 16);
363 #endif
364
365 }
366
367 last_blk = msg_len % 16;
368 blk_cnt = (msg_len + 15) / 16;
369 if (!last_blk) {
370 last_blk = 16;
371 }
372
373 for (j = 0; j < blk_cnt; j++) {
374 if (j + 1 == blk_cnt) {
375 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
376 for (i = 0; i < last_blk; i++) {
377 pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
378 }
379 for (i = last_blk; i < 16; i++) {
380 pmsg[i] = Xn[i] ^ 0x00;
381 }
382
383 #ifdef LOG_XN
384 printf("%16s: ", "Xn XOR Bn");
385 printf_hexdump(pmsg, 16);
386 #endif
387
388 err = bt_encrypt_be(key, pmsg, Xn);
389 if (err) {
390 return err;
391 }
392
393 #ifdef LOG_XN
394 printf("%16s: ", "Xn+1");
395 printf_hexdump(Xn, 16);
396 #endif
397
398 /* MIC = C_mic ^ X_1 */
399 for (i = 0; i < sizeof(mic); i++) {
400 mic[i] = cmic[i] ^ Xn[i];
401 }
402
403 #ifdef LOG_XN
404 printf("%16s: ", "mic");
405 printf_hexdump(mic, 16);
406 #endif
407
408 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
409 pmsg[0] = 0x01;
410 memcpy(pmsg + 1, nonce, 13);
411 sys_put_be16(j + 1, pmsg + 14);
412
413 err = bt_encrypt_be(key, pmsg, cmsg);
414 if (err) {
415 return err;
416 }
417
418 /* Encrypted = Payload[0-15] ^ C_1 */
419 for (i = 0; i < last_blk; i++) {
420 out_msg[(j * 16) + i] =
421 msg[(j * 16) + i] ^ cmsg[i];
422 }
423 } else {
424
425 #ifdef LOG_XN
426 printf("%16s: ", "bn");
427 printf_hexdump(msg, 16);
428 #endif
429
430 /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
431 for (i = 0; i < 16; i++) {
432 pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
433 }
434
435 #ifdef LOG_XN
436 printf("%16s: ", "Xn XOR Bn");
437 printf_hexdump(pmsg, 16);
438 #endif
439
440 err = bt_encrypt_be(key, pmsg, Xn);
441 if (err) {
442 return err;
443 }
444
445 #ifdef LOG_XN
446 printf("%16s: ", "Xn+1");
447 printf_hexdump(Xn, 16);
448 #endif
449
450 /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
451 pmsg[0] = 0x01;
452 memcpy(pmsg + 1, nonce, 13);
453 sys_put_be16(j + 1, pmsg + 14);
454
455 err = bt_encrypt_be(key, pmsg, cmsg);
456 if (err) {
457 return err;
458 }
459
460 /* Encrypted = Payload[0-15] ^ C_N */
461 for (i = 0; i < 16; i++) {
462 out_msg[(j * 16) + i] =
463 msg[(j * 16) + i] ^ cmsg[i];
464 }
465
466 }
467 }
468
469 memcpy(out_msg + msg_len, mic, mic_size);
470
471 return 0;
472 }
473