xref: /btstack/test/crypto/aes_ccm.c (revision cb93c223bc3c718fbf6e414578af3369cbf3345f) 
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 		err = bt_encrypt_be(key, pmsg, Xn);
119 		if (err) {
120 			return err;
121 		}
122 	}
123 
124 	last_blk = msg_len % 16;
125 	blk_cnt = (msg_len + 15) / 16;
126 	if (!last_blk) {
127 		last_blk = 16;
128 	}
129 
130 	for (j = 0; j < blk_cnt; j++) {
131 		if (j + 1 == blk_cnt) {
132 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
133 			pmsg[0] = 0x01;
134 			memcpy(pmsg + 1, nonce, 13);
135 			sys_put_be16(j + 1, pmsg + 14);
136 
137 			err = bt_encrypt_be(key, pmsg, cmsg);
138 			if (err) {
139 				return err;
140 			}
141 
142 			/* Encrypted = Payload[0-15] ^ C_1 */
143 			for (i = 0; i < last_blk; i++) {
144 				msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
145 			}
146 
147 			memcpy(out_msg + (j * 16), msg, last_blk);
148 
149 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
150 			for (i = 0; i < last_blk; i++) {
151 				pmsg[i] = Xn[i] ^ msg[i];
152 			}
153 
154 			for (i = last_blk; i < 16; i++) {
155 				pmsg[i] = Xn[i] ^ 0x00;
156 			}
157 
158 #ifdef LOG_XN
159 	printf("%16s: ", "Xn XOR bn");
160 	printf_hexdump(pmsg, 16);
161 #endif
162 
163 			err = bt_encrypt_be(key, pmsg, Xn);
164 			if (err) {
165 				return err;
166 			}
167 
168 #ifdef LOG_XN
169 	printf("%16s: ", "Xn XOR bn");
170 	printf_hexdump(pmsg, 16);
171 #endif
172 
173 			/* MIC = C_mic ^ X_1 */
174 			for (i = 0; i < sizeof(mic); i++) {
175 				mic[i] = cmic[i] ^ Xn[i];
176 			}
177 
178 #ifdef LOG_XN
179 	printf("%16s: ", "mic");
180 	printf_hexdump(mic, 16);
181 #endif
182 
183 		} else {
184 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
185 			pmsg[0] = 0x01;
186 			memcpy(pmsg + 1, nonce, 13);
187 			sys_put_be16(j + 1, pmsg + 14);
188 
189 #ifdef LOG_XN
190 	printf("%16s: ", "Ai");
191 	printf_hexdump(mic, 16);
192 #endif
193 
194 			err = bt_encrypt_be(key, pmsg, cmsg);
195 			if (err) {
196 				return err;
197 			}
198 
199 #ifdef LOG_XN
200 	printf("%16s: ", "Si");
201 	printf_hexdump(mic, 16);
202 #endif
203 
204 			/* Encrypted = Payload[0-15] ^ C_1 */
205 			for (i = 0; i < 16; i++) {
206 				msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
207 			}
208 
209 			memcpy(out_msg + (j * 16), msg, 16);
210 
211 #ifdef LOG_XN
212 	printf("%16s: ", "bn");
213 	printf_hexdump(msg, 16);
214 #endif
215 
216 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
217 			for (i = 0; i < 16; i++) {
218 				pmsg[i] = Xn[i] ^ msg[i];
219 			}
220 
221 			err = bt_encrypt_be(key, pmsg, Xn);
222 			if (err) {
223 				return err;
224 			}
225 
226 #ifdef LOG_XN
227 	printf("%16s: ", "Xn");
228 	printf_hexdump(mic, 16);
229 #endif
230 
231 
232 		}
233 	}
234 
235 	if (memcmp(mic, enc_msg + msg_len, mic_size)) {
236 		return -EBADMSG;
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 			err = bt_encrypt_be(key, pmsg, Xn);
327 			if (err) {
328 				return err;
329 			}
330 		}
331 
332 		for (i = 0; i < aad_len; i++, j++) {
333 			pmsg[i] = Xn[i] ^ aad[j];
334 		}
335 
336 		for (i = aad_len; i < 16; i++) {
337 			pmsg[i] = Xn[i];
338 		}
339 
340 		err = bt_encrypt_be(key, pmsg, Xn);
341 		if (err) {
342 			return err;
343 		}
344 	}
345 
346 	last_blk = msg_len % 16;
347 	blk_cnt = (msg_len + 15) / 16;
348 	if (!last_blk) {
349 		last_blk = 16;
350 	}
351 
352 	for (j = 0; j < blk_cnt; j++) {
353 		if (j + 1 == blk_cnt) {
354 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
355 			for (i = 0; i < last_blk; i++) {
356 				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
357 			}
358 			for (i = last_blk; i < 16; i++) {
359 				pmsg[i] = Xn[i] ^ 0x00;
360 			}
361 
362 #ifdef LOG_XN
363 	printf("%16s: ", "Xn XOR Bn");
364 	printf_hexdump(pmsg, 16);
365 #endif
366 
367 			err = bt_encrypt_be(key, pmsg, Xn);
368 			if (err) {
369 				return err;
370 			}
371 
372 #ifdef LOG_XN
373 	printf("%16s: ", "Xn+1");
374 	printf_hexdump(Xn, 16);
375 #endif
376 
377 			/* MIC = C_mic ^ X_1 */
378 			for (i = 0; i < sizeof(mic); i++) {
379 				mic[i] = cmic[i] ^ Xn[i];
380 			}
381 
382 #ifdef LOG_XN
383 	printf("%16s: ", "mic");
384 	printf_hexdump(mic, 16);
385 #endif
386 
387 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
388 			pmsg[0] = 0x01;
389 			memcpy(pmsg + 1, nonce, 13);
390 			sys_put_be16(j + 1, pmsg + 14);
391 
392 			err = bt_encrypt_be(key, pmsg, cmsg);
393 			if (err) {
394 				return err;
395 			}
396 
397 			/* Encrypted = Payload[0-15] ^ C_1 */
398 			for (i = 0; i < last_blk; i++) {
399 				out_msg[(j * 16) + i] =
400 					msg[(j * 16) + i] ^ cmsg[i];
401 			}
402 		} else {
403 
404 #ifdef LOG_XN
405 	printf("%16s: ", "bn");
406 	printf_hexdump(msg, 16);
407 #endif
408 
409 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
410 			for (i = 0; i < 16; i++) {
411 				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
412 			}
413 
414 #ifdef LOG_XN
415 	printf("%16s: ", "Xn XOR Bn");
416 	printf_hexdump(pmsg, 16);
417 #endif
418 
419 			err = bt_encrypt_be(key, pmsg, Xn);
420 			if (err) {
421 				return err;
422 			}
423 
424 #ifdef LOG_XN
425 	printf("%16s: ", "Xn+1");
426 	printf_hexdump(Xn, 16);
427 #endif
428 
429 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
430 			pmsg[0] = 0x01;
431 			memcpy(pmsg + 1, nonce, 13);
432 			sys_put_be16(j + 1, pmsg + 14);
433 
434 			err = bt_encrypt_be(key, pmsg, cmsg);
435 			if (err) {
436 				return err;
437 			}
438 
439 			/* Encrypted = Payload[0-15] ^ C_N */
440 			for (i = 0; i < 16; i++) {
441 				out_msg[(j * 16) + i] =
442 					msg[(j * 16) + i] ^ cmsg[i];
443 			}
444 
445 		}
446 	}
447 
448 	memcpy(out_msg + msg_len, mic, mic_size);
449 
450 	return 0;
451 }
452