xref: /nrf52832-nimble/packages/NimBLE-latest/nimble/host/mesh/src/crypto.c (revision 042d53a763ad75cb1465103098bb88c245d95138)
1 /*  Bluetooth Mesh */
2 
3 /*
4  * Copyright (c) 2017 Intel Corporation
5  *
6  * SPDX-License-Identifier: Apache-2.0
7  */
8 
9 #include <string.h>
10 #include <stdbool.h>
11 #include <errno.h>
12 
13 #include <tinycrypt/constants.h>
14 #include <tinycrypt/utils.h>
15 #include <tinycrypt/aes.h>
16 #include <tinycrypt/cmac_mode.h>
17 #include <tinycrypt/ccm_mode.h>
18 
19 #include "syscfg/syscfg.h"
20 #define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_CRYPTO))
21 #include "host/ble_hs_log.h"
22 
23 #include "crypto.h"
24 
25 #define NET_MIC_LEN(pdu) (((pdu)[1] & 0x80) ? 8 : 4)
26 #define APP_MIC_LEN(aszmic) ((aszmic) ? 8 : 4)
27 
bt_mesh_aes_cmac(const u8_t key[16],struct bt_mesh_sg * sg,size_t sg_len,u8_t mac[16])28 int bt_mesh_aes_cmac(const u8_t key[16], struct bt_mesh_sg *sg,
29 		     size_t sg_len, u8_t mac[16])
30 {
31 	struct tc_aes_key_sched_struct sched;
32 	struct tc_cmac_struct state;
33 
34 	if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) {
35 		return -EIO;
36 	}
37 
38 	for (; sg_len; sg_len--, sg++) {
39 		if (tc_cmac_update(&state, sg->data,
40 				   sg->len) == TC_CRYPTO_FAIL) {
41 			return -EIO;
42 		}
43 	}
44 
45 	if (tc_cmac_final(mac, &state) == TC_CRYPTO_FAIL) {
46 		return -EIO;
47 	}
48 
49 	return 0;
50 }
51 
bt_mesh_k1(const u8_t * ikm,size_t ikm_len,const u8_t salt[16],const char * info,u8_t okm[16])52 int bt_mesh_k1(const u8_t *ikm, size_t ikm_len, const u8_t salt[16],
53 	       const char *info, u8_t okm[16])
54 {
55 	int err;
56 
57 	err = bt_mesh_aes_cmac_one(salt, ikm, ikm_len, okm);
58 	if (err < 0) {
59 		return err;
60 	}
61 
62 	return bt_mesh_aes_cmac_one(okm, info, strlen(info), okm);
63 }
64 
bt_mesh_k2(const u8_t n[16],const u8_t * p,size_t p_len,u8_t net_id[1],u8_t enc_key[16],u8_t priv_key[16])65 int bt_mesh_k2(const u8_t n[16], const u8_t *p, size_t p_len,
66 	       u8_t net_id[1], u8_t enc_key[16], u8_t priv_key[16])
67 {
68 	struct bt_mesh_sg sg[3];
69 	u8_t salt[16];
70 	u8_t out[16];
71 	u8_t t[16];
72 	u8_t pad;
73 	int err;
74 
75 	BT_DBG("n %s", bt_hex(n, 16));
76 	BT_DBG("p %s", bt_hex(p, p_len));
77 
78 	err = bt_mesh_s1("smk2", salt);
79 	if (err) {
80 		return err;
81 	}
82 
83 	err = bt_mesh_aes_cmac_one(salt, n, 16, t);
84 	if (err) {
85 		return err;
86 	}
87 
88 	pad = 0x01;
89 
90 	sg[0].data = NULL;
91 	sg[0].len  = 0;
92 	sg[1].data = p;
93 	sg[1].len  = p_len;
94 	sg[2].data = &pad;
95 	sg[2].len  = sizeof(pad);
96 
97 	err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out);
98 	if (err) {
99 		return err;
100 	}
101 
102 	net_id[0] = out[15] & 0x7f;
103 
104 	sg[0].data = out;
105 	sg[0].len  = sizeof(out);
106 	pad = 0x02;
107 
108 	err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out);
109 	if (err) {
110 		return err;
111 	}
112 
113 	memcpy(enc_key, out, 16);
114 
115 	pad = 0x03;
116 
117 	err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out);
118 	if (err) {
119 		return err;
120 	}
121 
122 	memcpy(priv_key, out, 16);
123 
124 	BT_DBG("NID 0x%02x enc_key %s", net_id[0], bt_hex(enc_key, 16));
125 	BT_DBG("priv_key %s", bt_hex(priv_key, 16));
126 
127 	return 0;
128 }
129 
bt_mesh_k3(const u8_t n[16],u8_t out[8])130 int bt_mesh_k3(const u8_t n[16], u8_t out[8])
131 {
132 	u8_t id64[] = { 'i', 'd', '6', '4', 0x01 };
133 	u8_t tmp[16];
134 	u8_t t[16];
135 	int err;
136 
137 	err = bt_mesh_s1("smk3", tmp);
138 	if (err) {
139 		return err;
140 	}
141 
142 	err = bt_mesh_aes_cmac_one(tmp, n, 16, t);
143 	if (err) {
144 		return err;
145 	}
146 
147 	err = bt_mesh_aes_cmac_one(t, id64, sizeof(id64), tmp);
148 	if (err) {
149 		return err;
150 	}
151 
152 	memcpy(out, tmp + 8, 8);
153 
154 	return 0;
155 }
156 
bt_mesh_k4(const u8_t n[16],u8_t out[1])157 int bt_mesh_k4(const u8_t n[16], u8_t out[1])
158 {
159 	u8_t id6[] = { 'i', 'd', '6', 0x01 };
160 	u8_t tmp[16];
161 	u8_t t[16];
162 	int err;
163 
164 	err = bt_mesh_s1("smk4", tmp);
165 	if (err) {
166 		return err;
167 	}
168 
169 	err = bt_mesh_aes_cmac_one(tmp, n, 16, t);
170 	if (err) {
171 		return err;
172 	}
173 
174 	err = bt_mesh_aes_cmac_one(t, id6, sizeof(id6), tmp);
175 	if (err) {
176 		return err;
177 	}
178 
179 	out[0] = tmp[15] & BIT_MASK(6);
180 
181 	return 0;
182 }
183 
bt_mesh_id128(const u8_t n[16],const char * s,u8_t out[16])184 int bt_mesh_id128(const u8_t n[16], const char *s, u8_t out[16])
185 {
186 	const char *id128 = "id128\x01";
187 	u8_t salt[16];
188 	int err;
189 
190 	err = bt_mesh_s1(s, salt);
191 	if (err) {
192 		return err;
193 	}
194 
195 	return bt_mesh_k1(n, 16, salt, id128, out);
196 }
197 
bt_mesh_ccm_decrypt(const u8_t key[16],u8_t nonce[13],const u8_t * enc_msg,size_t msg_len,const u8_t * aad,size_t aad_len,u8_t * out_msg,size_t mic_size)198 static int bt_mesh_ccm_decrypt(const u8_t key[16], u8_t nonce[13],
199 			       const u8_t *enc_msg, size_t msg_len,
200 			       const u8_t *aad, size_t aad_len,
201 			       u8_t *out_msg, size_t mic_size)
202 {
203 	u8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16], mic[16];
204 	u16_t last_blk, blk_cnt;
205 	size_t i, j;
206 	int err;
207 
208 	if (msg_len < 1 || aad_len >= 0xff00) {
209 		return -EINVAL;
210 	}
211 
212 	/* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
213 	pmsg[0] = 0x01;
214 	memcpy(pmsg + 1, nonce, 13);
215 	sys_put_be16(0x0000, pmsg + 14);
216 
217 	err = bt_encrypt_be(key, pmsg, cmic);
218 	if (err) {
219 		return err;
220 	}
221 
222 	/* X_0 = e(AppKey, 0x09 || nonce || length) */
223 	if (mic_size == sizeof(u64_t)) {
224 		pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
225 	} else {
226 		pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
227 	}
228 
229 	memcpy(pmsg + 1, nonce, 13);
230 	sys_put_be16(msg_len, pmsg + 14);
231 
232 	err = bt_encrypt_be(key, pmsg, Xn);
233 	if (err) {
234 		return err;
235 	}
236 
237 	/* If AAD is being used to authenticate, include it here */
238 	if (aad_len) {
239 		sys_put_be16(aad_len, pmsg);
240 
241 		for (i = 0; i < sizeof(u16_t); i++) {
242 			pmsg[i] = Xn[i] ^ pmsg[i];
243 		}
244 
245 		j = 0;
246 		aad_len += sizeof(u16_t);
247 		while (aad_len > 16) {
248 			do {
249 				pmsg[i] = Xn[i] ^ aad[j];
250 				i++, j++;
251 			} while (i < 16);
252 
253 			aad_len -= 16;
254 			i = 0;
255 
256 			err = bt_encrypt_be(key, pmsg, Xn);
257 			if (err) {
258 				return err;
259 			}
260 		}
261 
262 		for (i = 0; i < aad_len; i++, j++) {
263 			pmsg[i] = Xn[i] ^ aad[j];
264 		}
265 
266 		for (i = aad_len; i < 16; i++) {
267 			pmsg[i] = Xn[i];
268 		}
269 
270 		err = bt_encrypt_be(key, pmsg, Xn);
271 		if (err) {
272 			return err;
273 		}
274 	}
275 
276 	last_blk = msg_len % 16;
277 	blk_cnt = (msg_len + 15) / 16;
278 	if (!last_blk) {
279 		last_blk = 16;
280 	}
281 
282 	for (j = 0; j < blk_cnt; j++) {
283 		if (j + 1 == blk_cnt) {
284 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
285 			pmsg[0] = 0x01;
286 			memcpy(pmsg + 1, nonce, 13);
287 			sys_put_be16(j + 1, pmsg + 14);
288 
289 			err = bt_encrypt_be(key, pmsg, cmsg);
290 			if (err) {
291 				return err;
292 			}
293 
294 			/* Encrypted = Payload[0-15] ^ C_1 */
295 			for (i = 0; i < last_blk; i++) {
296 				msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
297 			}
298 
299 			memcpy(out_msg + (j * 16), msg, last_blk);
300 
301 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
302 			for (i = 0; i < last_blk; i++) {
303 				pmsg[i] = Xn[i] ^ msg[i];
304 			}
305 
306 			for (i = last_blk; i < 16; i++) {
307 				pmsg[i] = Xn[i] ^ 0x00;
308 			}
309 
310 			err = bt_encrypt_be(key, pmsg, Xn);
311 			if (err) {
312 				return err;
313 			}
314 
315 			/* MIC = C_mic ^ X_1 */
316 			for (i = 0; i < sizeof(mic); i++) {
317 				mic[i] = cmic[i] ^ Xn[i];
318 			}
319 		} else {
320 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
321 			pmsg[0] = 0x01;
322 			memcpy(pmsg + 1, nonce, 13);
323 			sys_put_be16(j + 1, pmsg + 14);
324 
325 			err = bt_encrypt_be(key, pmsg, cmsg);
326 			if (err) {
327 				return err;
328 			}
329 
330 			/* Encrypted = Payload[0-15] ^ C_1 */
331 			for (i = 0; i < 16; i++) {
332 				msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
333 			}
334 
335 			memcpy(out_msg + (j * 16), msg, 16);
336 
337 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
338 			for (i = 0; i < 16; i++) {
339 				pmsg[i] = Xn[i] ^ msg[i];
340 			}
341 
342 			err = bt_encrypt_be(key, pmsg, Xn);
343 			if (err) {
344 				return err;
345 			}
346 		}
347 	}
348 
349 	if (memcmp(mic, enc_msg + msg_len, mic_size)) {
350 		return -EBADMSG;
351 	}
352 
353 	return 0;
354 }
355 
bt_mesh_ccm_encrypt(const u8_t key[16],u8_t nonce[13],const u8_t * msg,size_t msg_len,const u8_t * aad,size_t aad_len,u8_t * out_msg,size_t mic_size)356 static int bt_mesh_ccm_encrypt(const u8_t key[16], u8_t nonce[13],
357 			       const u8_t *msg, size_t msg_len,
358 			       const u8_t *aad, size_t aad_len,
359 			       u8_t *out_msg, size_t mic_size)
360 {
361 	u8_t pmsg[16], cmic[16], cmsg[16], mic[16], Xn[16];
362 	u16_t blk_cnt, last_blk;
363 	size_t i, j;
364 	int err;
365 
366 	BT_DBG("key %s", bt_hex(key, 16));
367 	BT_DBG("nonce %s", bt_hex(nonce, 13));
368 	BT_DBG("msg (len %zu) %s", msg_len, bt_hex(msg, msg_len));
369 	BT_DBG("aad_len %zu mic_size %zu", aad_len, mic_size);
370 
371 	/* Unsupported AAD size */
372 	if (aad_len >= 0xff00) {
373 		return -EINVAL;
374 	}
375 
376 	/* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
377 	pmsg[0] = 0x01;
378 	memcpy(pmsg + 1, nonce, 13);
379 	sys_put_be16(0x0000, pmsg + 14);
380 
381 	err = bt_encrypt_be(key, pmsg, cmic);
382 	if (err) {
383 		return err;
384 	}
385 
386 	/* X_0 = e(AppKey, 0x09 || nonce || length) */
387 	if (mic_size == sizeof(u64_t)) {
388 		pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
389 	} else {
390 		pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
391 	}
392 
393 	memcpy(pmsg + 1, nonce, 13);
394 	sys_put_be16(msg_len, pmsg + 14);
395 
396 	err = bt_encrypt_be(key, pmsg, Xn);
397 	if (err) {
398 		return err;
399 	}
400 
401 	/* If AAD is being used to authenticate, include it here */
402 	if (aad_len) {
403 		sys_put_be16(aad_len, pmsg);
404 
405 		for (i = 0; i < sizeof(u16_t); i++) {
406 			pmsg[i] = Xn[i] ^ pmsg[i];
407 		}
408 
409 		j = 0;
410 		aad_len += sizeof(u16_t);
411 		while (aad_len > 16) {
412 			do {
413 				pmsg[i] = Xn[i] ^ aad[j];
414 				i++, j++;
415 			} while (i < 16);
416 
417 			aad_len -= 16;
418 			i = 0;
419 
420 			err = bt_encrypt_be(key, pmsg, Xn);
421 			if (err) {
422 				return err;
423 			}
424 		}
425 
426 		for (i = 0; i < aad_len; i++, j++) {
427 			pmsg[i] = Xn[i] ^ aad[j];
428 		}
429 
430 		for (i = aad_len; i < 16; i++) {
431 			pmsg[i] = Xn[i];
432 		}
433 
434 		err = bt_encrypt_be(key, pmsg, Xn);
435 		if (err) {
436 			return err;
437 		}
438 	}
439 
440 	last_blk = msg_len % 16;
441 	blk_cnt = (msg_len + 15) / 16;
442 	if (!last_blk) {
443 		last_blk = 16;
444 	}
445 
446 	for (j = 0; j < blk_cnt; j++) {
447 		if (j + 1 == blk_cnt) {
448 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
449 			for (i = 0; i < last_blk; i++) {
450 				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
451 			}
452 			for (i = last_blk; i < 16; i++) {
453 				pmsg[i] = Xn[i] ^ 0x00;
454 			}
455 
456 			err = bt_encrypt_be(key, pmsg, Xn);
457 			if (err) {
458 				return err;
459 			}
460 
461 			/* MIC = C_mic ^ X_1 */
462 			for (i = 0; i < sizeof(mic); i++) {
463 				mic[i] = cmic[i] ^ Xn[i];
464 			}
465 
466 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
467 			pmsg[0] = 0x01;
468 			memcpy(pmsg + 1, nonce, 13);
469 			sys_put_be16(j + 1, pmsg + 14);
470 
471 			err = bt_encrypt_be(key, pmsg, cmsg);
472 			if (err) {
473 				return err;
474 			}
475 
476 			/* Encrypted = Payload[0-15] ^ C_1 */
477 			for (i = 0; i < last_blk; i++) {
478 				out_msg[(j * 16) + i] =
479 					msg[(j * 16) + i] ^ cmsg[i];
480 			}
481 		} else {
482 			/* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
483 			for (i = 0; i < 16; i++) {
484 				pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
485 			}
486 
487 			err = bt_encrypt_be(key, pmsg, Xn);
488 			if (err) {
489 				return err;
490 			}
491 
492 			/* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
493 			pmsg[0] = 0x01;
494 			memcpy(pmsg + 1, nonce, 13);
495 			sys_put_be16(j + 1, pmsg + 14);
496 
497 			err = bt_encrypt_be(key, pmsg, cmsg);
498 			if (err) {
499 				return err;
500 			}
501 
502 			/* Encrypted = Payload[0-15] ^ C_N */
503 			for (i = 0; i < 16; i++) {
504 				out_msg[(j * 16) + i] =
505 					msg[(j * 16) + i] ^ cmsg[i];
506 			}
507 
508 		}
509 	}
510 
511 	memcpy(out_msg + msg_len, mic, mic_size);
512 
513 	return 0;
514 }
515 
516 #if (MYNEWT_VAL(BLE_MESH_PROXY))
create_proxy_nonce(u8_t nonce[13],const u8_t * pdu,u32_t iv_index)517 static void create_proxy_nonce(u8_t nonce[13], const u8_t *pdu,
518 			       u32_t iv_index)
519 {
520 	/* Nonce Type */
521 	nonce[0] = 0x03;
522 
523 	/* Pad */
524 	nonce[1] = 0x00;
525 
526 	/* Sequence Number */
527 	nonce[2] = pdu[2];
528 	nonce[3] = pdu[3];
529 	nonce[4] = pdu[4];
530 
531 	/* Source Address */
532 	nonce[5] = pdu[5];
533 	nonce[6] = pdu[6];
534 
535 	/* Pad */
536 	nonce[7] = 0;
537 	nonce[8] = 0;
538 
539 	/* IV Index */
540 	sys_put_be32(iv_index, &nonce[9]);
541 }
542 #endif /* PROXY */
543 
create_net_nonce(u8_t nonce[13],const u8_t * pdu,u32_t iv_index)544 static void create_net_nonce(u8_t nonce[13], const u8_t *pdu,
545 			     u32_t iv_index)
546 {
547 	/* Nonce Type */
548 	nonce[0] = 0x00;
549 
550 	/* FRND + TTL */
551 	nonce[1] = pdu[1];
552 
553 	/* Sequence Number */
554 	nonce[2] = pdu[2];
555 	nonce[3] = pdu[3];
556 	nonce[4] = pdu[4];
557 
558 	/* Source Address */
559 	nonce[5] = pdu[5];
560 	nonce[6] = pdu[6];
561 
562 	/* Pad */
563 	nonce[7] = 0;
564 	nonce[8] = 0;
565 
566 	/* IV Index */
567 	sys_put_be32(iv_index, &nonce[9]);
568 }
569 
bt_mesh_net_obfuscate(u8_t * pdu,u32_t iv_index,const u8_t privacy_key[16])570 int bt_mesh_net_obfuscate(u8_t *pdu, u32_t iv_index,
571 			  const u8_t privacy_key[16])
572 {
573 	u8_t priv_rand[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, };
574 	u8_t tmp[16];
575 	int err, i;
576 
577 	BT_DBG("IVIndex %u, PrivacyKey %s", iv_index, bt_hex(privacy_key, 16));
578 
579 	sys_put_be32(iv_index, &priv_rand[5]);
580 	memcpy(&priv_rand[9], &pdu[7], 7);
581 
582 	BT_DBG("PrivacyRandom %s", bt_hex(priv_rand, 16));
583 
584 	err = bt_encrypt_be(privacy_key, priv_rand, tmp);
585 	if (err) {
586 		return err;
587 	}
588 
589 	for (i = 0; i < 6; i++) {
590 		pdu[1 + i] ^= tmp[i];
591 	}
592 
593 	return 0;
594 }
595 
bt_mesh_net_encrypt(const u8_t key[16],struct os_mbuf * buf,u32_t iv_index,bool proxy)596 int bt_mesh_net_encrypt(const u8_t key[16], struct os_mbuf *buf,
597 			u32_t iv_index, bool proxy)
598 {
599 	u8_t mic_len = NET_MIC_LEN(buf->om_data);
600 	u8_t nonce[13];
601 	int err;
602 
603 	BT_DBG("IVIndex %u EncKey %s mic_len %u", iv_index, bt_hex(key, 16),
604 	       mic_len);
605 	BT_DBG("PDU (len %u) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
606 
607 #if (MYNEWT_VAL(BLE_MESH_PROXY))
608 	if (proxy) {
609 		create_proxy_nonce(nonce, buf->om_data, iv_index);
610 	} else {
611 		create_net_nonce(nonce, buf->om_data, iv_index);
612 	}
613 #else
614 	create_net_nonce(nonce, buf->om_data, iv_index);
615 #endif
616 
617 	BT_DBG("Nonce %s", bt_hex(nonce, 13));
618 
619 	err = bt_mesh_ccm_encrypt(key, nonce, &buf->om_data[7], buf->om_len - 7,
620 				  NULL, 0, &buf->om_data[7], mic_len);
621 	if (!err) {
622 		net_buf_simple_add(buf, mic_len);
623 	}
624 
625 	return err;
626 }
627 
bt_mesh_net_decrypt(const u8_t key[16],struct os_mbuf * buf,u32_t iv_index,bool proxy)628 int bt_mesh_net_decrypt(const u8_t key[16], struct os_mbuf *buf,
629 			u32_t iv_index, bool proxy)
630 {
631 	u8_t mic_len = NET_MIC_LEN(buf->om_data);
632 	u8_t nonce[13];
633 
634 	BT_DBG("PDU (%u bytes) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
635 	BT_DBG("iv_index %u, key %s mic_len %u", iv_index, bt_hex(key, 16),
636 	       mic_len);
637 
638 #if (MYNEWT_VAL(BLE_MESH_PROXY))
639 	if (proxy) {
640 		create_proxy_nonce(nonce, buf->om_data, iv_index);
641 	} else {
642 		create_net_nonce(nonce, buf->om_data, iv_index);
643 	}
644 #else
645 	create_net_nonce(nonce, buf->om_data, iv_index);
646 #endif
647 
648 	BT_DBG("Nonce %s", bt_hex(nonce, 13));
649 
650 	buf->om_len -= mic_len;
651 
652 	return bt_mesh_ccm_decrypt(key, nonce, &buf->om_data[7], buf->om_len - 7,
653 				   NULL, 0, &buf->om_data[7], mic_len);
654 }
655 
create_app_nonce(u8_t nonce[13],bool dev_key,u8_t aszmic,u16_t src,u16_t dst,u32_t seq_num,u32_t iv_index)656 static void create_app_nonce(u8_t nonce[13], bool dev_key, u8_t aszmic,
657 			     u16_t src, u16_t dst, u32_t seq_num,
658 			     u32_t iv_index)
659 {
660 	if (dev_key) {
661 		nonce[0] = 0x02;
662 	} else {
663 		nonce[0] = 0x01;
664 	}
665 
666 	sys_put_be32((seq_num | ((u32_t)aszmic << 31)), &nonce[1]);
667 
668 	sys_put_be16(src, &nonce[5]);
669 	sys_put_be16(dst, &nonce[7]);
670 
671 	sys_put_be32(iv_index, &nonce[9]);
672 }
673 
bt_mesh_app_encrypt(const u8_t key[16],bool dev_key,u8_t aszmic,struct os_mbuf * buf,const u8_t * ad,u16_t src,u16_t dst,u32_t seq_num,u32_t iv_index)674 int bt_mesh_app_encrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
675 			struct os_mbuf *buf, const u8_t *ad,
676 			u16_t src, u16_t dst, u32_t seq_num, u32_t iv_index)
677 {
678 	u8_t nonce[13];
679 	int err;
680 
681 	BT_DBG("AppKey %s", bt_hex(key, 16));
682 	BT_DBG("dev_key %u src 0x%04x dst 0x%04x", dev_key, src, dst);
683 	BT_DBG("seq_num 0x%08x iv_index 0x%08x", seq_num, iv_index);
684 	BT_DBG("Clear: %s", bt_hex(buf->om_data, buf->om_len));
685 
686 	create_app_nonce(nonce, dev_key, aszmic, src, dst, seq_num, iv_index);
687 
688 	BT_DBG("Nonce  %s", bt_hex(nonce, 13));
689 
690 	err = bt_mesh_ccm_encrypt(key, nonce, buf->om_data, buf->om_len, ad,
691 				  ad ? 16 : 0, buf->om_data, APP_MIC_LEN(aszmic));
692 	if (!err) {
693 		net_buf_simple_add(buf, APP_MIC_LEN(aszmic));
694 		BT_DBG("Encr: %s", bt_hex(buf->om_data, buf->om_len));
695 	}
696 
697 	return err;
698 }
699 
bt_mesh_app_decrypt(const u8_t key[16],bool dev_key,u8_t aszmic,struct os_mbuf * buf,struct os_mbuf * out,const u8_t * ad,u16_t src,u16_t dst,u32_t seq_num,u32_t iv_index)700 int bt_mesh_app_decrypt(const u8_t key[16], bool dev_key, u8_t aszmic,
701 			struct os_mbuf *buf, struct os_mbuf *out,
702 			const u8_t *ad, u16_t src, u16_t dst, u32_t seq_num,
703 			u32_t iv_index)
704 {
705 	u8_t nonce[13];
706 	int err;
707 
708 	BT_DBG("EncData (len %u) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len));
709 
710 	create_app_nonce(nonce, dev_key, aszmic, src, dst, seq_num, iv_index);
711 
712 	BT_DBG("AppKey %s", bt_hex(key, 16));
713 	BT_DBG("Nonce  %s", bt_hex(nonce, 13));
714 
715 	err = bt_mesh_ccm_decrypt(key, nonce, buf->om_data, buf->om_len, ad,
716 				  ad ? 16 : 0, out->om_data, APP_MIC_LEN(aszmic));
717 	if (!err) {
718 		net_buf_simple_add(out, buf->om_len);
719 	}
720 
721 	return err;
722 }
723 
724 /* reversed, 8-bit, poly=0x07 */
725 static const u8_t crc_table[256] = {
726 	0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
727 	0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
728 	0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
729 	0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
730 
731 	0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
732 	0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
733 	0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
734 	0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
735 
736 	0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
737 	0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
738 	0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
739 	0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
740 
741 	0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
742 	0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
743 	0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
744 	0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
745 
746 	0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
747 	0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
748 	0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
749 	0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
750 
751 	0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
752 	0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
753 	0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
754 	0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
755 
756 	0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
757 	0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
758 	0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
759 	0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
760 
761 	0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
762 	0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
763 	0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
764 	0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
765 };
766 
bt_mesh_fcs_calc(const u8_t * data,u8_t data_len)767 u8_t bt_mesh_fcs_calc(const u8_t *data, u8_t data_len)
768 {
769 	u8_t fcs = 0xff;
770 
771 	while (data_len--) {
772 		fcs = crc_table[fcs ^ *data++];
773 	}
774 
775 	BT_DBG("fcs 0x%02x", 0xff - fcs);
776 
777 	return 0xff - fcs;
778 }
779 
bt_mesh_fcs_check(struct os_mbuf * buf,u8_t received_fcs)780 bool bt_mesh_fcs_check(struct os_mbuf *buf, u8_t received_fcs)
781 {
782 	const u8_t *data = buf->om_data;
783 	u16_t data_len = buf->om_len;
784 	u8_t fcs = 0xff;
785 
786 	while (data_len--) {
787 		fcs = crc_table[fcs ^ *data++];
788 	}
789 
790 	return crc_table[fcs ^ received_fcs] == 0xcf;
791 }
792 
bt_mesh_virtual_addr(const u8_t virtual_label[16],u16_t * addr)793 int bt_mesh_virtual_addr(const u8_t virtual_label[16], u16_t *addr)
794 {
795 	u8_t salt[16];
796 	u8_t tmp[16];
797 	int err;
798 
799 	err = bt_mesh_s1("vtad", salt);
800 	if (err) {
801 		return err;
802 	}
803 
804 	err = bt_mesh_aes_cmac_one(salt, virtual_label, 16, tmp);
805 	if (err) {
806 		return err;
807 	}
808 
809 	*addr = (sys_get_be16(&tmp[14]) & 0x3fff) | 0x8000;
810 
811 	return 0;
812 }
813 
bt_mesh_prov_conf_salt(const u8_t conf_inputs[145],u8_t salt[16])814 int bt_mesh_prov_conf_salt(const u8_t conf_inputs[145], u8_t salt[16])
815 {
816 	const u8_t conf_salt_key[16] = { 0 };
817 
818 	return bt_mesh_aes_cmac_one(conf_salt_key, conf_inputs, 145, salt);
819 }
820 
bt_mesh_prov_conf_key(const u8_t dhkey[32],const u8_t conf_salt[16],u8_t conf_key[16])821 int bt_mesh_prov_conf_key(const u8_t dhkey[32], const u8_t conf_salt[16],
822 			  u8_t conf_key[16])
823 {
824 	return bt_mesh_k1(dhkey, 32, conf_salt, "prck", conf_key);
825 }
826 
bt_mesh_prov_conf(const u8_t conf_key[16],const u8_t rand[16],const u8_t auth[16],u8_t conf[16])827 int bt_mesh_prov_conf(const u8_t conf_key[16], const u8_t rand[16],
828 		      const u8_t auth[16], u8_t conf[16])
829 {
830 	struct bt_mesh_sg sg[] = { { rand, 16 }, { auth, 16 } };
831 
832 	BT_DBG("ConfirmationKey %s", bt_hex(conf_key, 16));
833 	BT_DBG("RandomDevice %s", bt_hex(rand, 16));
834 	BT_DBG("AuthValue %s", bt_hex(auth, 16));
835 
836 	return bt_mesh_aes_cmac(conf_key, sg, ARRAY_SIZE(sg), conf);
837 }
838 
bt_mesh_prov_decrypt(const u8_t key[16],u8_t nonce[13],const u8_t data[25+8],u8_t out[25])839 int bt_mesh_prov_decrypt(const u8_t key[16], u8_t nonce[13],
840 			 const u8_t data[25 + 8], u8_t out[25])
841 {
842 	return bt_mesh_ccm_decrypt(key, nonce, data, 25, NULL, 0, out, 8);
843 }
844 
bt_mesh_beacon_auth(const u8_t beacon_key[16],u8_t flags,const u8_t net_id[8],u32_t iv_index,u8_t auth[8])845 int bt_mesh_beacon_auth(const u8_t beacon_key[16], u8_t flags,
846 			const u8_t net_id[8], u32_t iv_index,
847 			u8_t auth[8])
848 {
849 	u8_t msg[13], tmp[16];
850 	int err;
851 
852 	BT_DBG("BeaconKey %s", bt_hex(beacon_key, 16));
853 	BT_DBG("NetId %s", bt_hex(net_id, 8));
854 	BT_DBG("IV Index 0x%08x", iv_index);
855 
856 	msg[0] = flags;
857 	memcpy(&msg[1], net_id, 8);
858 	sys_put_be32(iv_index, &msg[9]);
859 
860 	BT_DBG("BeaconMsg %s", bt_hex(msg, sizeof(msg)));
861 
862 	err = bt_mesh_aes_cmac_one(beacon_key, msg, sizeof(msg), tmp);
863 	if (!err) {
864 		memcpy(auth, tmp, 8);
865 	}
866 
867 	return err;
868 }
869