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