xref: /btstack/test/crypto/aes_ccm.c (revision cd5f23a3250874824c01a2b3326a9522fea3f99f) 
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 	return 0;
240 }
241 
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