xref: /btstack/test/crypto/aes_ccm.c (revision a8d51f092f1b660d0f6921369ad2bc3f9368296c) 
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 
17 static void sys_put_be16(uint16_t value, uint8_t * buffer) {
18 	big_endian_store_16(buffer, 0, value);
19 }
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 
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 	if (memcmp(mic, enc_msg + msg_len, mic_size)) {
240 		return -EBADMSG;
241 	}
242 
243 	return 0;
244 }
245 
246 int bt_mesh_ccm_encrypt(const u8_t key[16], u8_t nonce[13],
247 			       const u8_t *msg, size_t msg_len,
248 			       const u8_t *aad, size_t aad_len,
249 			       u8_t *out_msg, size_t mic_size)
250 {
251 	u8_t pmsg[16], cmic[16], cmsg[16], mic[16], Xn[16];
252 	u16_t blk_cnt, last_blk;
253 	size_t i, j;
254 	int err;
255 
256 	// BT_DBG("key %s", bt_hex(key, 16));
257 	// BT_DBG("nonce %s", bt_hex(nonce, 13));
258 	// BT_DBG("msg (len %zu) %s", msg_len, bt_hex(msg, msg_len));
259 	// BT_DBG("aad_len %zu mic_size %zu", aad_len, mic_size);
260 
261 	/* Unsupported AAD size */
262 	if (aad_len >= 0xff00) {
263 		return -EINVAL;
264 	}
265 
266 	/* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
267 	pmsg[0] = 0x01;
268 	memcpy(pmsg + 1, nonce, 13);
269 	sys_put_be16(0x0000, pmsg + 14);
270 
271 #ifdef LOG_XN
272 	printf("%16s: ", "A0");
273 	printf_hexdump(pmsg, 16);
274 #endif
275 
276 	err = bt_encrypt_be(key, pmsg, cmic);
277 	if (err) {
278 		return err;
279 	}
280 
281 #ifdef LOG_XN
282 	printf("%16s: ", "S0");
283 	printf_hexdump(cmic, 16);
284 #endif
285 
286 	/* X_0 = e(AppKey, 0x09 || nonce || length) */
287 	if (mic_size == sizeof(u64_t)) {
288 		pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
289 	} else {
290 		pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
291 	}
292 
293 	memcpy(pmsg + 1, nonce, 13);
294 	sys_put_be16(msg_len, pmsg + 14);
295 
296 #ifdef LOG_XN
297 	printf("%16s: ", "B0");
298 	printf_hexdump(pmsg, 16);
299 #endif
300 
301 	err = bt_encrypt_be(key, pmsg, Xn);
302 	if (err) {
303 		return err;
304 	}
305 
306 #ifdef LOG_XN
307 	printf("%16s: ", "X1");
308 	printf_hexdump(Xn, 16);
309 #endif
310 
311 	/* If AAD is being used to authenticate, include it here */
312 	if (aad_len) {
313 		sys_put_be16(aad_len, pmsg);
314 
315 		for (i = 0; i < sizeof(u16_t); i++) {
316 			pmsg[i] = Xn[i] ^ pmsg[i];
317 		}
318 
319 		j = 0;
320 		aad_len += sizeof(u16_t);
321 		while (aad_len > 16) {
322 			do {
323 				pmsg[i] = Xn[i] ^ aad[j];
324 				i++, j++;
325 			} while (i < 16);
326 
327 			aad_len -= 16;
328 			i = 0;
329 
330 #ifdef LOG_XN
331             printf("%16s: ", "Xn XOR bn (aad)");
332             printf_hexdump(pmsg, 16);
333 #endif
334 
335 			err = bt_encrypt_be(key, pmsg, Xn);
336 			if (err) {
337 				return err;
338 			}
339 
340 #ifdef LOG_XN
341             printf("%16s: ", "Xn+1 AAD");
342             printf_hexdump(Xn, 16);
343 #endif
344 
345 		}
346 
347 		for (i = 0; i < aad_len; i++, j++) {
348 			pmsg[i] = Xn[i] ^ aad[j];
349 		}
350 
351 		for (i = aad_len; i < 16; i++) {
352 			pmsg[i] = Xn[i];
353 		}
354 
355 #ifdef LOG_XN
356         printf("%16s: ", "Xn XOR bn (aad)");
357         printf_hexdump(pmsg, 16);
358 #endif
359 
360 		err = bt_encrypt_be(key, pmsg, Xn);
361 		if (err) {
362 			return err;
363 		}
364 #ifdef LOG_XN
365         printf("%16s: ", "Xn+1 AAD");
366         printf_hexdump(Xn, 16);
367 #endif
368 
369 	}
370 
371 	last_blk = msg_len % 16;
372 	blk_cnt = (msg_len + 15) / 16;
373 	if (!last_blk) {
374 		last_blk = 16;
375 	}
376 
377 	for (j = 0; j < blk_cnt; j++) {
378 		if (j + 1 == blk_cnt) {
379 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
380 			for (i = 0; i < last_blk; i++) {
381 				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
382 			}
383 			for (i = last_blk; i < 16; i++) {
384 				pmsg[i] = Xn[i] ^ 0x00;
385 			}
386 
387 #ifdef LOG_XN
388             printf("%16s: ", "Xn XOR Bn");
389             printf_hexdump(pmsg, 16);
390 #endif
391 
392 			err = bt_encrypt_be(key, pmsg, Xn);
393 			if (err) {
394 				return err;
395 			}
396 
397 #ifdef LOG_XN
398             printf("%16s: ", "Xn+1");
399             printf_hexdump(Xn, 16);
400 #endif
401 
402 			/* MIC = C_mic ^ X_1 */
403 			for (i = 0; i < sizeof(mic); i++) {
404 				mic[i] = cmic[i] ^ Xn[i];
405 			}
406 
407 #ifdef LOG_XN
408             printf("%16s: ", "mic");
409             printf_hexdump(mic, 16);
410 #endif
411 
412 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
413 			pmsg[0] = 0x01;
414 			memcpy(pmsg + 1, nonce, 13);
415 			sys_put_be16(j + 1, pmsg + 14);
416 
417 			err = bt_encrypt_be(key, pmsg, cmsg);
418 			if (err) {
419 				return err;
420 			}
421 
422 			/* Encrypted = Payload[0-15] ^ C_1 */
423 			for (i = 0; i < last_blk; i++) {
424 				out_msg[(j * 16) + i] =
425 					msg[(j * 16) + i] ^ cmsg[i];
426 			}
427 		} else {
428 
429 #ifdef LOG_XN
430             printf("%16s: ", "bn");
431             printf_hexdump(msg, 16);
432 #endif
433 
434 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
435 			for (i = 0; i < 16; i++) {
436 				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
437 			}
438 
439 #ifdef LOG_XN
440             printf("%16s: ", "Xn XOR Bn");
441             printf_hexdump(pmsg, 16);
442 #endif
443 
444 			err = bt_encrypt_be(key, pmsg, Xn);
445 			if (err) {
446 				return err;
447 			}
448 
449 #ifdef LOG_XN
450             printf("%16s: ", "Xn+1");
451             printf_hexdump(Xn, 16);
452 #endif
453 
454 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
455 			pmsg[0] = 0x01;
456 			memcpy(pmsg + 1, nonce, 13);
457 			sys_put_be16(j + 1, pmsg + 14);
458 
459 			err = bt_encrypt_be(key, pmsg, cmsg);
460 			if (err) {
461 				return err;
462 			}
463 
464 			/* Encrypted = Payload[0-15] ^ C_N */
465 			for (i = 0; i < 16; i++) {
466 				out_msg[(j * 16) + i] =
467 					msg[(j * 16) + i] ^ cmsg[i];
468 			}
469 
470 		}
471 	}
472 
473 	memcpy(out_msg + msg_len, mic, mic_size);
474 
475 	return 0;
476 }
477