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