Return-Path: From: Brian Gix To: Marcel Holtmann , Luiz Augusto von Dentz , linux-bluetooth@vger.kernel.org Cc: Brian Gix Subject: [PATCH BlueZ v4 02/14] meshd: Mesh crypto support Date: Mon, 30 Apr 2018 14:03:07 -0700 Message-Id: <20180430210319.25137-3-brian.gix@intel.com> In-Reply-To: <20180430210319.25137-1-brian.gix@intel.com> References: <20180430210319.25137-1-brian.gix@intel.com> List-ID: --- meshd/src/crypto.c | 1607 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1607 insertions(+) create mode 100644 meshd/src/crypto.c diff --git a/meshd/src/crypto.c b/meshd/src/crypto.c new file mode 100644 index 000000000..cf35cfbe8 --- /dev/null +++ b/meshd/src/crypto.c @@ -0,0 +1,1607 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2018 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include + +#include + +#ifndef SOL_ALG +#define SOL_ALG 279 +#endif + +#ifndef ALG_SET_AEAD_AUTHSIZE +#define ALG_SET_AEAD_AUTHSIZE 5 +#endif + +#include "meshd/src/mesh.h" +#include "meshd/src/node.h" +#include "meshd/src/net.h" +#include "meshd/src/crypto.h" +#include "meshd/src/display.h" + +static int alg_new(int fd, const void *keyval, socklen_t keylen, + size_t mic_size) +{ + if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0) { + l_error("key"); + return -1; + } + + if (mic_size && + setsockopt(fd, SOL_ALG, ALG_SET_AEAD_AUTHSIZE, + NULL, mic_size) < 0) { + l_error("taglen"); + return -1; + } + + /* FIXME: This should use accept4() with SOCK_CLOEXEC */ + return accept(fd, NULL, 0); +} + +static bool alg_encrypt(int fd, const void *inbuf, size_t inlen, + void *outbuf, size_t outlen) +{ + __u32 alg_op = ALG_OP_ENCRYPT; + char cbuf[CMSG_SPACE(sizeof(alg_op))]; + struct cmsghdr *cmsg; + struct msghdr msg; + struct iovec iov; + ssize_t len; + + memset(cbuf, 0, sizeof(cbuf)); + memset(&msg, 0, sizeof(msg)); + + msg.msg_control = cbuf; + msg.msg_controllen = sizeof(cbuf); + + cmsg = CMSG_FIRSTHDR(&msg); + cmsg->cmsg_level = SOL_ALG; + cmsg->cmsg_type = ALG_SET_OP; + cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op)); + memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op)); + + iov.iov_base = (void *) inbuf; + iov.iov_len = inlen; + + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + + len = sendmsg(fd, &msg, 0); + if (len < 0) + return false; + + len = read(fd, outbuf, outlen); + if (len < 0) + return false; + + return true; +} + +static int aes_ecb_setup(const uint8_t key[16]) +{ + struct sockaddr_alg salg; + int fd, nfd; + + fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0); + if (fd < 0) + return -1; + + memset(&salg, 0, sizeof(salg)); + salg.salg_family = AF_ALG; + strcpy((char *) salg.salg_type, "skcipher"); + strcpy((char *) salg.salg_name, "ecb(aes)"); + + if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) { + close(fd); + return -1; + } + + nfd = alg_new(fd, key, 16, 0); + + close(fd); + + return nfd; +} + +static bool aes_ecb(int fd, const uint8_t plaintext[16], uint8_t encrypted[16]) +{ + return alg_encrypt(fd, plaintext, 16, encrypted, 16); +} + +static void aes_ecb_destroy(int fd) +{ + close(fd); +} + +static bool aes_ecb_one(const uint8_t key[16], + const uint8_t plaintext[16], uint8_t encrypted[16]) +{ + bool result; + int fd; + + fd = aes_ecb_setup(key); + if (fd < 0) + return false; + + result = aes_ecb(fd, plaintext, encrypted); + + aes_ecb_destroy(fd); + + return result; +} + +bool mesh_aes_ecb_one(const uint8_t key[16], + const uint8_t plaintext[16], uint8_t encrypted[16]) +{ + return aes_ecb_one(key, plaintext, encrypted); +} + +/* Maximum message length that can be passed to aes_cmac */ +#define CMAC_MSG_MAX (64 + 64 + 17) + +static int aes_cmac_setup(const uint8_t key[16]) +{ + struct sockaddr_alg salg; + int fd, nfd; + + fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0); + if (fd < 0) + return -1; + + memset(&salg, 0, sizeof(salg)); + salg.salg_family = AF_ALG; + strcpy((char *) salg.salg_type, "hash"); + strcpy((char *) salg.salg_name, "cmac(aes)"); + + if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) { + close(fd); + return -1; + } + + nfd = alg_new(fd, key, 16, 0); + + close(fd); + + return nfd; +} + +static bool aes_cmac(int fd, const uint8_t *msg, + size_t msg_len, uint8_t res[16]) +{ + ssize_t len; + + if (msg_len > CMAC_MSG_MAX) + return false; + + len = send(fd, msg, msg_len, 0); + if (len < 0) + return false; + + len = read(fd, res, 16); + if (len < 0) + return false; + + return true; +} + +static void aes_cmac_destroy(int fd) +{ + close(fd); +} + +static int aes_cmac_N_start(const uint8_t N[16]) +{ + int fd; + + fd = aes_cmac_setup(N); + return fd; +} + +static bool aes_cmac_one(const uint8_t key[16], const void *msg, + size_t msg_len, uint8_t res[16]) +{ + bool result; + int fd; + + fd = aes_cmac_setup(key); + if (fd < 0) + return false; + + result = aes_cmac(fd, msg, msg_len, res); + + aes_cmac_destroy(fd); + + return result; +} + +bool mesh_crypto_aes_cmac(const uint8_t key[16], const uint8_t *msg, + size_t msg_len, uint8_t res[16]) +{ + return aes_cmac_one(key, msg, msg_len, res); +} + +bool mesh_crypto_aes_ccm_encrypt(const uint8_t nonce[13], const uint8_t key[16], + const uint8_t *aad, uint16_t aad_len, + const uint8_t *msg, uint16_t msg_len, + uint8_t *out_msg, + void *out_mic, size_t mic_size) +{ + uint8_t pmsg[16], cmic[16], cmsg[16]; + uint8_t mic[16], Xn[16]; + uint16_t blk_cnt, last_blk; + bool result; + size_t i, j; + int fd; + + if (aad_len >= 0xff00) { + l_error("Unsupported AAD size"); + return false; + } + + fd = aes_ecb_setup(key); + if (fd < 0) + return false; + + /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + l_put_be16(0x0000, pmsg + 14); + + result = aes_ecb(fd, pmsg, cmic); + if (!result) + goto done; + + /* X_0 = e(AppKey, 0x09 || nonce || length) */ + if (mic_size == sizeof(uint64_t)) + pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00); + else + pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00); + + memcpy(pmsg + 1, nonce, 13); + l_put_be16(msg_len, pmsg + 14); + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + + /* If AAD is being used to authenticate, include it here */ + if (aad_len) { + l_put_be16(aad_len, pmsg); + + for (i = 0; i < sizeof(uint16_t); i++) + pmsg[i] = Xn[i] ^ pmsg[i]; + + j = 0; + aad_len += sizeof(uint16_t); + while (aad_len > 16) { + do { + pmsg[i] = Xn[i] ^ aad[j]; + i++, j++; + } while (i < 16); + + aad_len -= 16; + i = 0; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + } + + for (i = 0; i < aad_len; i++, j++) + pmsg[i] = Xn[i] ^ aad[j]; + + for (i = aad_len; i < 16; i++) + pmsg[i] = Xn[i]; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + } + + last_blk = msg_len % 16; + blk_cnt = (msg_len + 15) / 16; + if (!last_blk) + last_blk = 16; + + for (j = 0; j < blk_cnt; j++) { + if (j + 1 == blk_cnt) { + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < last_blk; i++) + pmsg[i] = Xn[i] ^ msg[(j * 16) + i]; + for (i = last_blk; i < 16; i++) + pmsg[i] = Xn[i] ^ 0x00; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + + /* MIC = C_mic ^ X_1 */ + for (i = 0; i < sizeof(mic); i++) + mic[i] = cmic[i] ^ Xn[i]; + + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + l_put_be16(j + 1, pmsg + 14); + + result = aes_ecb(fd, pmsg, cmsg); + if (!result) + goto done; + + if (out_msg) { + /* Encrypted = Payload[0-15] ^ C_1 */ + for (i = 0; i < last_blk; i++) + out_msg[(j * 16) + i] = + msg[(j * 16) + i] ^ cmsg[i]; + + } + } else { + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < 16; i++) + pmsg[i] = Xn[i] ^ msg[(j * 16) + i]; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + l_put_be16(j + 1, pmsg + 14); + + result = aes_ecb(fd, pmsg, cmsg); + if (!result) + goto done; + + if (out_msg) { + /* Encrypted = Payload[0-15] ^ C_N */ + for (i = 0; i < 16; i++) + out_msg[(j * 16) + i] = + msg[(j * 16) + i] ^ cmsg[i]; + } + + } + } + + if (out_msg) + memcpy(out_msg + msg_len, mic, mic_size); + + if (out_mic) { + switch (mic_size) { + case sizeof(uint32_t): + *(uint32_t *)out_mic = l_get_be32(mic); + break; + case sizeof(uint64_t): + *(uint64_t *)out_mic = l_get_be64(mic); + break; + default: + l_error("Unsupported MIC size"); + } + } + +done: + aes_ecb_destroy(fd); + + return result; +} + +bool mesh_crypto_aes_ccm_decrypt(const uint8_t nonce[13], const uint8_t key[16], + const uint8_t *aad, uint16_t aad_len, + const uint8_t *enc_msg, uint16_t enc_msg_len, + uint8_t *out_msg, + void *out_mic, size_t mic_size) +{ + uint8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16]; + uint8_t mic[16]; + uint16_t msg_len = enc_msg_len - mic_size; + uint16_t last_blk, blk_cnt; + bool result; + size_t i, j; + int fd; + + if (enc_msg_len < 5 || aad_len >= 0xff00) + return false; + + fd = aes_ecb_setup(key); + if (fd < 0) + return false; + + /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + l_put_be16(0x0000, pmsg + 14); + + result = aes_ecb(fd, pmsg, cmic); + if (!result) + goto done; + + /* X_0 = e(AppKey, 0x09 || nonce || length) */ + if (mic_size == sizeof(uint64_t)) + pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00); + else + pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00); + + memcpy(pmsg + 1, nonce, 13); + l_put_be16(msg_len, pmsg + 14); + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + + /* If AAD is being used to authenticate, include it here */ + if (aad_len) { + l_put_be16(aad_len, pmsg); + + for (i = 0; i < sizeof(uint16_t); i++) + pmsg[i] = Xn[i] ^ pmsg[i]; + + j = 0; + aad_len += sizeof(uint16_t); + while (aad_len > 16) { + do { + pmsg[i] = Xn[i] ^ aad[j]; + i++, j++; + } while (i < 16); + + aad_len -= 16; + i = 0; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + } + + for (i = 0; i < aad_len; i++, j++) + pmsg[i] = Xn[i] ^ aad[j]; + + for (i = aad_len; i < 16; i++) + pmsg[i] = Xn[i]; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + } + + last_blk = msg_len % 16; + blk_cnt = (msg_len + 15) / 16; + if (!last_blk) + last_blk = 16; + + for (j = 0; j < blk_cnt; j++) { + if (j + 1 == blk_cnt) { + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + l_put_be16(j + 1, pmsg + 14); + + result = aes_ecb(fd, pmsg, cmsg); + if (!result) + goto done; + + /* Encrypted = Payload[0-15] ^ C_1 */ + for (i = 0; i < last_blk; i++) + msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i]; + + if (out_msg) + memcpy(out_msg + (j * 16), msg, last_blk); + + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < last_blk; i++) + pmsg[i] = Xn[i] ^ msg[i]; + for (i = last_blk; i < 16; i++) + pmsg[i] = Xn[i] ^ 0x00; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + + /* MIC = C_mic ^ X_1 */ + for (i = 0; i < sizeof(mic); i++) + mic[i] = cmic[i] ^ Xn[i]; + } else { + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + l_put_be16(j + 1, pmsg + 14); + + result = aes_ecb(fd, pmsg, cmsg); + if (!result) + goto done; + + /* Encrypted = Payload[0-15] ^ C_1 */ + for (i = 0; i < 16; i++) + msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i]; + + if (out_msg) + memcpy(out_msg + (j * 16), msg, 16); + + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < 16; i++) + pmsg[i] = Xn[i] ^ msg[i]; + + result = aes_ecb(fd, pmsg, Xn); + if (!result) + goto done; + } + } + + if (out_mic) { + switch (mic_size) { + case sizeof(uint32_t): + *(uint32_t *)out_mic = l_get_be32(mic); + break; + case sizeof(uint64_t): + *(uint64_t *)out_mic = l_get_be64(mic); + break; + default: + l_error("Unsupported MIC size"); + } + } + +done: + aes_ecb_destroy(fd); + + return result; +} + +bool mesh_crypto_k1(const uint8_t ikm[16], const uint8_t salt[16], + const void *info, size_t info_len, uint8_t okm[16]) +{ + uint8_t res[16]; + + if (!aes_cmac_one(salt, ikm, 16, res)) + return false; + + return aes_cmac_one(res, info, info_len, okm); +} + +bool mesh_crypto_k2(const uint8_t n[16], const uint8_t *p, size_t p_len, + uint8_t net_id[1], + uint8_t enc_key[16], + uint8_t priv_key[16]) +{ + int fd; + uint8_t output[16]; + uint8_t t[16]; + uint8_t *stage; + bool success = false; + + print_packet("K2-N", n, 16); + stage = l_malloc(sizeof(output) + p_len + 1); + if (stage == NULL) + return false; + + if (!mesh_crypto_s1("smk2", 4, stage)) + goto fail; + print_packet("K2-S1(smk2)", stage, 16); + print_packet("K2-P", p, p_len); + + if (!aes_cmac_one(stage, n, 16, t)) + goto fail; + + print_packet("K2-T", t, 16); + + fd = aes_cmac_N_start(t); + if (fd < 0) + goto fail; + + memcpy(stage, p, p_len); + stage[p_len] = 1; + + if (!aes_cmac(fd, stage, p_len + 1, output)) + goto done; + + print_packet("K2-T1", output, 16); + + net_id[0] = output[15] & 0x7f; + + memcpy(stage, output, 16); + memcpy(stage + 16, p, p_len); + stage[p_len + 16] = 2; + + if (!aes_cmac(fd, stage, p_len + 16 + 1, output)) + goto done; + print_packet("K2-T2", output, 16); + + memcpy(enc_key, output, 16); + + memcpy(stage, output, 16); + memcpy(stage + 16, p, p_len); + stage[p_len + 16] = 3; + + if (!aes_cmac(fd, stage, p_len + 16 + 1, output)) + goto done; + print_packet("K2-T3", output, 16); + + memcpy(priv_key, output, 16); + success = true; + +done: + aes_cmac_destroy(fd); +fail: + l_free(stage); + + return success; +} + +static bool crypto_128(const uint8_t n[16], const char *s, uint8_t out128[16]) +{ + uint8_t id128[] = { 'i', 'd', '1', '2', '8', 0x01 }; + uint8_t salt[16]; + + if (!mesh_crypto_s1(s, 4, salt)) + return false; + + return mesh_crypto_k1(n, salt, id128, sizeof(id128), out128); +} + +bool mesh_crypto_nkik(const uint8_t n[16], uint8_t identity_key[16]) +{ + return crypto_128(n, "nkik", identity_key); +} + +bool mesh_crypto_identity(const uint8_t net_key[16], uint16_t addr, + uint8_t id[16]) +{ + uint8_t id_key[16]; + uint8_t tmp[16]; + + print_packet("Net_Key", net_key, 16); + + if (!mesh_crypto_nkik(net_key, id_key)) + return false; + + print_packet("ID_Key", id_key, 16); + + if (!l_get_be64(id + 8)) + l_getrandom(id + 8, 8); + + memset(tmp, 0, sizeof(tmp)); + memcpy(tmp + 6, id + 8, 8); + l_put_be16(addr, tmp + 14); + + print_packet("Nonce", tmp, 16); + if (!aes_ecb_one(id_key, tmp, tmp)) + return false; + + print_packet("result", tmp, 16); + + memcpy(id, tmp + 8, 8); + return true; +} + +bool mesh_crypto_nkbk(const uint8_t n[16], uint8_t beacon_key[16]) +{ + return crypto_128(n, "nkbk", beacon_key); +} + +bool mesh_crypto_nkpk(const uint8_t n[16], uint8_t proxy_key[16]) +{ + return crypto_128(n, "nkpk", proxy_key); +} + +bool mesh_crypto_k3(const uint8_t n[16], uint8_t out64[8]) +{ + uint8_t tmp[16]; + uint8_t t[16]; + uint8_t id64[] = { 'i', 'd', '6', '4', 0x01 }; + + if (!mesh_crypto_s1("smk3", 4, tmp)) + return false; + + if (!aes_cmac_one(tmp, n, 16, t)) + return false; + + if (!aes_cmac_one(t, id64, sizeof(id64), tmp)) + return false; + + memcpy(out64, tmp + 8, 8); + + return true; +} + +bool mesh_crypto_k4(const uint8_t a[16], uint8_t out6[1]) +{ + uint8_t tmp[16]; + uint8_t t[16]; + uint8_t id6[] = { 'i', 'd', '6', 0x01 }; + + if (!mesh_crypto_s1("smk4", 4, tmp)) + return false; + + if (!aes_cmac_one(tmp, a, 16, t)) + return false; + + if (!aes_cmac_one(t, id6, sizeof(id6), tmp)) + return false; + + out6[0] = tmp[15] & 0x3f; + return true; +} + +bool mesh_crypto_beacon_cmac(const uint8_t encryption_key[16], + const uint8_t network_id[8], + uint32_t iv_index, bool kr, bool iu, + uint64_t *cmac) +{ + uint8_t msg[13], tmp[16]; + + if (!cmac) + return false; + + msg[0] = kr ? 0x01 : 0x00; + msg[0] |= iu ? 0x02 : 0x00; + memcpy(msg + 1, network_id, 8); + l_put_be32(iv_index, msg + 9); + + if (!aes_cmac_one(encryption_key, msg, 13, tmp)) + return false; + + *cmac = l_get_be64(tmp); + + return true; +} + +bool mesh_crypto_network_nonce(bool ctl, uint8_t ttl, uint32_t seq, + uint16_t src, uint32_t iv_index, + uint8_t nonce[13]) +{ + nonce[0] = 0; + nonce[1] = (ttl & TTL_MASK) | (ctl ? CTL : 0x00); + nonce[2] = (seq >> 16) & 0xff; + nonce[3] = (seq >> 8) & 0xff; + nonce[4] = seq & 0xff; + + /* SRC */ + l_put_be16(src, nonce + 5); + + l_put_be16(0, nonce + 7); + + /* IV Index */ + l_put_be32(iv_index, nonce + 9); + + return true; +} + +bool mesh_crypto_network_encrypt(bool ctl, uint8_t ttl, + uint32_t seq, uint16_t src, + uint32_t iv_index, + const uint8_t net_key[16], + const uint8_t *enc_msg, uint8_t enc_msg_len, + uint8_t *out, void *net_mic) +{ + uint8_t nonce[13]; + + if (!mesh_crypto_network_nonce(ctl, ttl, seq, src, iv_index, nonce)) + return false; + + return mesh_crypto_aes_ccm_encrypt(nonce, net_key, NULL, 0, enc_msg, + enc_msg_len, out, net_mic, + ctl ? sizeof(uint64_t) : sizeof(uint32_t)); +} + +bool mesh_crypto_network_decrypt(bool ctl, uint8_t ttl, + uint32_t seq, uint16_t src, + uint32_t iv_index, + const uint8_t net_key[16], + const uint8_t *enc_msg, uint8_t enc_msg_len, + uint8_t *out, void *net_mic, size_t mic_size) +{ + uint8_t nonce[13]; + + if (!mesh_crypto_network_nonce(ctl, ttl, seq, src, iv_index, nonce)) + return false; + + return mesh_crypto_aes_ccm_decrypt(nonce, net_key, NULL, 0, + enc_msg, enc_msg_len, out, + net_mic, mic_size); +} + +bool mesh_crypto_application_nonce(uint32_t seq, uint16_t src, + uint16_t dst, uint32_t iv_index, + bool aszmic, uint8_t nonce[13]) +{ + nonce[0] = 0x01; + nonce[1] = aszmic ? 0x80 : 0x00; + nonce[2] = (seq & 0x00ff0000) >> 16; + nonce[3] = (seq & 0x0000ff00) >> 8; + nonce[4] = (seq & 0x000000ff); + nonce[5] = (src & 0xff00) >> 8; + nonce[6] = (src & 0x00ff); + nonce[7] = (dst & 0xff00) >> 8; + nonce[8] = (dst & 0x00ff); + l_put_be32(iv_index, nonce + 9); + + return true; +} + +bool mesh_crypto_device_nonce(uint32_t seq, uint16_t src, + uint16_t dst, uint32_t iv_index, + bool aszmic, uint8_t nonce[13]) +{ + nonce[0] = 0x02; + nonce[1] = aszmic ? 0x80 : 0x00; + nonce[2] = (seq & 0x00ff0000) >> 16; + nonce[3] = (seq & 0x0000ff00) >> 8; + nonce[4] = (seq & 0x000000ff); + nonce[5] = (src & 0xff00) >> 8; + nonce[6] = (src & 0x00ff); + nonce[7] = (dst & 0xff00) >> 8; + nonce[8] = (dst & 0x00ff); + l_put_be32(iv_index, nonce + 9); + + return true; +} + +bool mesh_crypto_application_encrypt(uint8_t key_id, uint32_t seq, uint16_t src, + uint16_t dst, uint32_t iv_index, + const uint8_t app_key[16], + const uint8_t *aad, uint8_t aad_len, + const uint8_t *msg, uint8_t msg_len, + uint8_t *out, + void *app_mic, size_t mic_size) +{ + uint8_t nonce[13]; + bool aszmic = (mic_size == sizeof(uint64_t)) ? true : false; + + if (!key_id && !mesh_crypto_device_nonce(seq, src, dst, + iv_index, aszmic, nonce)) + return false; + + if (key_id && !mesh_crypto_application_nonce(seq, src, dst, + iv_index, aszmic, nonce)) + return false; + + return mesh_crypto_aes_ccm_encrypt(nonce, app_key, aad, aad_len, + msg, msg_len, + out, app_mic, mic_size); +} + +bool mesh_crypto_application_decrypt(uint8_t key_id, uint32_t seq, uint16_t src, + uint16_t dst, uint32_t iv_index, + const uint8_t app_key[16], + const uint8_t *aad, uint8_t aad_len, + const uint8_t *enc_msg, uint8_t enc_msg_len, + uint8_t *out, void *app_mic, size_t mic_size) +{ + uint8_t nonce[13]; + bool aszmic = (mic_size == sizeof(uint64_t)) ? true : false; + + if (!key_id && !mesh_crypto_device_nonce(seq, src, dst, + iv_index, aszmic, nonce)) + return false; + + if (key_id && !mesh_crypto_application_nonce(seq, src, dst, + iv_index, aszmic, nonce)) + return false; + + return mesh_crypto_aes_ccm_decrypt(nonce, app_key, + aad, aad_len, enc_msg, + enc_msg_len, out, + app_mic, mic_size); +} + +bool mesh_crypto_session_key(const uint8_t secret[32], + const uint8_t salt[16], + uint8_t session_key[16]) +{ + const uint8_t prsk[4] = "prsk"; + + if (!aes_cmac_one(salt, secret, 32, session_key)) + return false; + + return aes_cmac_one(session_key, prsk, 4, session_key); +} + +bool mesh_crypto_nonce(const uint8_t secret[32], + const uint8_t salt[16], + uint8_t nonce[13]) +{ + const uint8_t prsn[4] = "prsn"; + uint8_t tmp[16]; + bool result; + + if (!aes_cmac_one(salt, secret, 32, tmp)) + return false; + + result = aes_cmac_one(tmp, prsn, 4, tmp); + + if (result) + memcpy(nonce, tmp + 3, 13); + + return result; +} + +bool mesh_crypto_s1(const void *info, size_t len, uint8_t salt[16]) +{ + const uint8_t zero[16] = {0}; + + return aes_cmac_one(zero, info, len, salt); +} + +bool mesh_crypto_prov_prov_salt(const uint8_t conf_salt[16], + const uint8_t prov_rand[16], + const uint8_t dev_rand[16], + uint8_t prov_salt[16]) +{ + const uint8_t zero[16] = {0}; + uint8_t tmp[16 * 3]; + + memcpy(tmp, conf_salt, 16); + memcpy(tmp + 16, prov_rand, 16); + memcpy(tmp + 32, dev_rand, 16); + + return aes_cmac_one(zero, tmp, sizeof(tmp), prov_salt); +} + +bool mesh_crypto_prov_conf_key(const uint8_t secret[32], + const uint8_t salt[16], + uint8_t conf_key[16]) +{ + const uint8_t prck[4] = "prck"; + + if (!aes_cmac_one(salt, secret, 32, conf_key)) + return false; + + return aes_cmac_one(conf_key, prck, 4, conf_key); +} + +bool mesh_crypto_device_key(const uint8_t secret[32], + const uint8_t salt[16], + uint8_t device_key[16]) +{ + const uint8_t prdk[4] = "prdk"; + + if (!aes_cmac_one(salt, secret, 32, device_key)) + return false; + + return aes_cmac_one(device_key, prdk, 4, device_key); +} + +bool mesh_crypto_virtual_addr(const uint8_t virtual_label[16], + uint16_t *addr) +{ + uint8_t tmp[16]; + + if (!mesh_crypto_s1("vtad", 4, tmp)) + return false; + + if (!addr || !aes_cmac_one(tmp, virtual_label, 16, tmp)) + return false; + + *addr = (l_get_be16(tmp + 14) & 0x3fff) | 0x8000; + + return true; +} + +bool mesh_crypto_privacy_counter(uint32_t iv_index, + const uint8_t *payload, + uint8_t privacy_counter[16]) +{ + memset(privacy_counter, 0, 5); + l_put_be32(iv_index, privacy_counter + 5); + memcpy(privacy_counter + 9, payload, 7); + + return true; +} + +bool mesh_crypto_network_obfuscate(const uint8_t privacy_key[16], + const uint8_t privacy_counter[16], + bool ctl, uint8_t ttl, uint32_t seq, + uint16_t src, uint8_t *out) +{ + uint8_t ecb[16], tmp[16]; + int i; + + if (!aes_ecb_one(privacy_key, privacy_counter, ecb)) + return false; + + tmp[0] = ((!!ctl) << 7) | (ttl & TTL_MASK); + tmp[1] = (seq & 0xff0000) >> 16; + tmp[2] = (seq & 0x00ff00) >> 8; + tmp[3] = (seq & 0x0000ff); + tmp[4] = (src & 0xff00) >> 8; + tmp[5] = (src & 0x00ff); + + if (out) { + for (i = 0; i < 6; i++) + out[i] = ecb[i] ^ tmp[i]; + } + + return true; +} + +bool mesh_crypto_network_clarify(const uint8_t privacy_key[16], + const uint8_t privacy_counter[16], + const uint8_t net_hdr[6], + bool *ctl, uint8_t *ttl, + uint32_t *seq, uint16_t *src) +{ + uint8_t ecb[16], tmp[6]; + int i; + + if (!aes_ecb_one(privacy_key, privacy_counter, ecb)) + return false; + + for (i = 0; i < 6; i++) + tmp[i] = ecb[i] ^ net_hdr[i]; + + if (ctl) + *ctl = !!(tmp[0] & CTL); + + if (ttl) + *ttl = tmp[0] & TTL_MASK; + + if (seq) + *seq = l_get_be32(tmp) & SEQ_MASK; + + if (src) + *src = l_get_be16(tmp + 4); + + return true; +} + +bool mesh_crypto_packet_build(bool ctl, uint8_t ttl, + uint32_t seq, + uint16_t src, uint16_t dst, + uint8_t opcode, + bool segmented, uint8_t key_id, + bool szmic, bool relay, uint16_t seqZero, + uint8_t segO, uint8_t segN, + const uint8_t *payload, uint8_t payload_len, + uint8_t *packet, uint8_t *packet_len) +{ + uint32_t hdr; + size_t n; + + l_put_be32(seq, packet + 1); + packet[1] = (ctl ? CTL : 0) | (ttl & TTL_MASK); + + l_put_be16(src, packet + 5); + l_put_be16(dst, packet + 7); + n = 9; + + if (!ctl) { + hdr = segmented << SEG_HDR_SHIFT; + hdr |= (key_id & KEY_ID_MASK) << KEY_HDR_SHIFT; + if (segmented) { + hdr |= szmic << SZMIC_HDR_SHIFT; + hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT; + hdr |= (segO & SEG_MASK) << SEGO_HDR_SHIFT; + hdr |= (segN & SEG_MASK) << SEGN_HDR_SHIFT; + } + l_put_be32(hdr, packet + n); + + /* Only first octet is valid for unsegmented messages */ + if (segmented) + n += 4; + else + n += 1; + + memcpy(packet + n, payload, payload_len); + + l_put_be32(0x00000000, packet + payload_len + n); + if (packet_len) + *packet_len = payload_len + n + 4; + } else { + if ((opcode & OPCODE_MASK) != opcode) + return false; + + hdr = opcode << KEY_HDR_SHIFT; + l_put_be32(hdr, packet + n); + n += 1; + + memcpy(packet + n, payload, payload_len); + n += payload_len; + + l_put_be64(0x0000000000000000, packet + n); + if (packet_len) + *packet_len = n + 8; + } + + + return true; +} + +bool mesh_crypto_packet_parse(const uint8_t *packet, uint8_t packet_len, + bool *ctl, uint8_t *ttl, uint32_t *seq, + uint16_t *src, uint16_t *dst, + uint32_t *cookie, uint8_t *opcode, + bool *segmented, uint8_t *key_id, + bool *szmic, bool *relay, uint16_t *seqZero, + uint8_t *segO, uint8_t *segN, + const uint8_t **payload, uint8_t *payload_len) +{ + uint32_t hdr; + uint16_t this_dst; + bool is_segmented; + + if (packet_len < 14) + return false; + + this_dst = l_get_be16(packet + 7); + + /* Try to keep bits in the order they exist within the packet */ + if (ctl) + *ctl = !!(packet[1] & CTL); + + if (ttl) + *ttl = packet[1] & TTL_MASK; + + if (seq) + *seq = l_get_be32(packet + 1) & SEQ_MASK; + + if (src) + *src = l_get_be16(packet + 5); + + if (dst) + *dst = this_dst; + + hdr = l_get_be32(packet + 9); + + is_segmented = !!((hdr >> SEG_HDR_SHIFT) & true); + if (segmented) + *segmented = is_segmented; + + if (packet[1] & CTL) { + uint8_t this_opcode = packet[9] & OPCODE_MASK; + + if (cookie) + *cookie = l_get_be32(packet + 9); + + if (opcode) + *opcode = this_opcode; + + if (this_dst && this_opcode == NET_OP_SEG_ACKNOWLEDGE) { + if (relay) + *relay = !!((hdr >> RELAY_HDR_SHIFT) & true); + + if (seqZero) + *seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) & + SEQ_ZERO_MASK; + + if (payload) + *payload = packet + 9; + + if (payload_len) + *payload_len = packet_len - 9 - 8; + } else { + if (payload) + *payload = packet + 10; + + if (payload_len) + *payload_len = packet_len - 10 - 8; + } + } else { + if (cookie) + *cookie = l_get_be32(packet + packet_len - 8); + + if (key_id) + *key_id = (hdr >> KEY_HDR_SHIFT) & KEY_ID_MASK; + + if (is_segmented) { + if (szmic) + *szmic = !!((hdr >> SZMIC_HDR_SHIFT) & true); + + if (seqZero) + *seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) & + SEQ_ZERO_MASK; + + if (segO) + *segO = (hdr >> SEGO_HDR_SHIFT) & SEG_MASK; + + if (segN) + *segN = (hdr >> SEGN_HDR_SHIFT) & SEG_MASK; + + if (payload) + *payload = packet + 13; + + if (payload_len) + *payload_len = packet_len - 13 - 4; + } else { + if (payload) + *payload = packet + 10; + + if (payload_len) + *payload_len = packet_len - 10 - 4; + } + } + + return true; +} + +bool mesh_crypto_payload_encrypt(uint8_t *aad, const uint8_t *payload, + uint8_t *out, uint16_t payload_len, + uint16_t src, uint16_t dst, uint8_t key_id, + uint32_t seq_num, uint32_t iv_index, + bool aszmic, + const uint8_t application_key[16]) +{ + uint8_t application_nonce[13] = { 0x01, }; + + if (payload_len < 1) + return false; + + /* Key_ID == 0 means the Device Key is being used */ + if (!key_id) + application_nonce[0] = 0x02; + + /* Seq Num */ + l_put_be32(seq_num, application_nonce + 1); + + /* ASZMIC */ + application_nonce[1] |= aszmic ? 0x80 : 0x00; + + /* SRC */ + l_put_be16(src, application_nonce + 5); + + /* DST */ + l_put_be16(dst, application_nonce + 7); + + /* IV Index */ + l_put_be32(iv_index, application_nonce + 9); + + /* print_packet("AAD", aad, aad ? 16 : 0); */ + /* print_packet("Nonce", application_nonce, 13); */ + /* print_packet("Key", application_key, 16); */ + /* print_packet("Payload[clr]", payload, payload_len); */ + + if (!mesh_crypto_aes_ccm_encrypt(application_nonce, application_key, + aad, aad ? 16 : 0, + payload, payload_len, + out, NULL, + aszmic ? 8 : 4)) + return false; + + /* print_packet("Payload[enc]", out, payload_len + (aszmic ? 8 : 4)); */ + + return true; +} + +bool mesh_crypto_payload_decrypt(uint8_t *aad, uint16_t aad_len, + const uint8_t *payload, uint16_t payload_len, + bool szmict, + uint16_t src, uint16_t dst, + uint8_t key_id, uint32_t seq_num, + uint32_t iv_index, uint8_t *out, + const uint8_t app_key[16]) +{ + uint8_t app_nonce[13] = { 0x01, }; + uint32_t mic32; + uint64_t mic64; + + if (payload_len < 5 || !out) + return false; + + /* Key_ID == 0 means the Device Key is being used */ + if (!key_id) + app_nonce[0] = 0x02; + + /* Seq Num */ + l_put_be32(seq_num, app_nonce + 1); + + /* ASZMIC */ + app_nonce[1] |= szmict ? 0x80 : 0x00; + + /* SRC */ + l_put_be16(src, app_nonce + 5); + + /* DST */ + l_put_be16(dst, app_nonce + 7); + + /* IV Index */ + l_put_be32(iv_index, app_nonce + 9); + + memcpy(out, payload, payload_len); + + /* print_packet("AAD", aad, aad_len); */ + /* print_packet("Nonce", app_nonce, 13); */ + /* print_packet("Key", app_key, 16); */ + /* print_packet("Payload[enc]", payload, payload_len); */ + + if (szmict) { + if (!mesh_crypto_aes_ccm_decrypt(app_nonce, app_key, + aad, aad_len, + payload, payload_len, + out, &mic64, sizeof(mic64))) + return false; + + mic64 ^= l_get_be64(out + payload_len - 8); + l_put_be64(mic64, out + payload_len - 8); + + /* print_packet("Payload[clr]", out, payload_len - 8); */ + + if (mic64) + return false; + } else { + if (!mesh_crypto_aes_ccm_decrypt(app_nonce, app_key, + aad, aad_len, + payload, payload_len, + out, &mic32, sizeof(mic32))) + return false; + + mic32 ^= l_get_be32(out + payload_len - 4); + l_put_be32(mic32, out + payload_len - 4); + + /* print_packet("Payload[clr]", out, payload_len - 4); */ + + if (mic32) + return false; + } + + return true; +} + +bool mesh_crypto_packet_encode(uint8_t *packet, uint8_t packet_len, + const uint8_t network_key[16], + uint32_t iv_index, + const uint8_t privacy_key[16]) +{ + uint8_t network_nonce[13] = { 0x00, 0x00 }; + uint8_t privacy_counter[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, }; + uint8_t tmp[16]; + int i; + + if (packet_len < 14) + return false; + + /* Detect Proxy packet by CTL == true && DST == 0x0000 */ + if ((packet[1] & CTL) && l_get_be16(packet + 7) == 0) + network_nonce[0] = 0x03; /* Proxy Nonce */ + else + /* CTL + TTL */ + network_nonce[1] = packet[1]; + + /* Seq Num */ + network_nonce[2] = packet[2]; + network_nonce[3] = packet[3]; + network_nonce[4] = packet[4]; + + /* SRC */ + network_nonce[5] = packet[5]; + network_nonce[6] = packet[6]; + + /* DST not available */ + network_nonce[7] = 0; + network_nonce[8] = 0; + + /* IV Index */ + l_put_be32(iv_index, network_nonce + 9); + + /* print_packet("Net-Nonce", network_nonce, 13); */ + /* print_packet("Net-Key", network_key, 16); */ + /* print_packet("Net-Payload[clr]", packet, packet_len); */ + + /* Check for Long net-MIC */ + if (packet[1] & CTL) { + if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key, + NULL, 0, + packet + 7, packet_len - 7 - 8, + packet + 7, NULL, sizeof(uint64_t))) + return false; + } else { + if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key, + NULL, 0, + packet + 7, packet_len - 7 - 4, + packet + 7, NULL, sizeof(uint32_t))) + return false; + } + + /* print_packet("Net-Payload[enc]", packet, packet_len); */ + + l_put_be32(iv_index, privacy_counter + 5); + memcpy(privacy_counter + 9, packet + 7, 7); + + /* print_packet("Priv-Random", privacy_counter, 16); */ + + /* print_packet("Priv-Key", privacy_key, 16); */ + + + if (!aes_ecb_one(privacy_key, privacy_counter, tmp)) + return false; + + for (i = 0; i < 6; i++) + packet[1 + i] ^= tmp[i]; + + /* print_packet("Net-Private", packet, packet_len); */ + + return true; +} + +bool mesh_crypto_packet_decode(const uint8_t *packet, uint8_t packet_len, + bool proxy, uint8_t *out, uint32_t iv_index, + const uint8_t network_key[16], + const uint8_t privacy_key[16]) +{ + uint8_t privacy_counter[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, }; + uint8_t network_nonce[13] = { 0x00, 0x00, }; + uint8_t tmp[16]; + uint16_t src; + int i; + + if (packet_len < 14) + return false; + + /* print_packet("Priv-Key", privacy_key, 16); */ + + l_put_be32(iv_index, privacy_counter + 5); + memcpy(privacy_counter + 9, packet + 7, 7); + + /* print_packet("Priv-Random", privacy_counter, 16); */ + + if (!aes_ecb_one(privacy_key, privacy_counter, tmp)) + return false; + + memcpy(out, packet, packet_len); + for (i = 0; i < 6; i++) + out[1 + i] ^= tmp[i]; + + src = l_get_be16(out + 5); + + /* Pre-check SRC address for illegal values */ + if (!src || src >= 0x8000) + return false; + + /* Detect Proxy packet by CTL == true && proxy == true */ + if ((out[1] & CTL) && proxy) + network_nonce[0] = 0x03; /* Proxy Nonce */ + else + /* CTL + TTL */ + network_nonce[1] = out[1]; + + /* Seq Num */ + network_nonce[2] = out[2]; + network_nonce[3] = out[3]; + network_nonce[4] = out[4]; + + /* SRC */ + network_nonce[5] = out[5]; + network_nonce[6] = out[6]; + + /* DST not available */ + network_nonce[7] = 0; + network_nonce[8] = 0; + + /* IV Index */ + l_put_be32(iv_index, network_nonce + 9); + + /* print_packet("Net-Nonce", network_nonce, 13); */ + /* print_packet("Net-Key", network_key, 16); */ + /* print_packet("Net-Pkt[enc]", out, packet_len); */ + + /* Check for Long MIC */ + if (out[1] & CTL) { + uint64_t mic; + + if (!mesh_crypto_aes_ccm_decrypt(network_nonce, network_key, + NULL, 0, packet + 7, packet_len - 7, + out + 7, &mic, sizeof(mic))) + return false; + + mic ^= l_get_be64(out + packet_len - 8); + l_put_be64(mic, out + packet_len - 8); + + if (mic) + return false; + } else { + uint32_t mic; + + if (!mesh_crypto_aes_ccm_decrypt(network_nonce, network_key, + NULL, 0, packet + 7, packet_len - 7, + out + 7, &mic, sizeof(mic))) + return false; + + mic ^= l_get_be32(out + packet_len - 4); + l_put_be32(mic, out + packet_len - 4); + + if (mic) + return false; + } + + /* print_packet("Net-Pkt[clr]", out, packet_len); */ + + return true; +} + +bool mesh_crypto_packet_label(uint8_t *packet, uint8_t packet_len, + uint16_t iv_index, uint8_t network_id) +{ + packet[0] = (iv_index & 0x0001) << 7 | (network_id & 0x7f); + + return true; +} + +/* reversed, 8-bit, poly=0x07 */ +static const uint8_t crc_table[256] = { + 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, + 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, + 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, + 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, + + 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, + 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, + 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, + 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, + + 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, + 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, + 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, + 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, + + 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, + 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, + 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, + 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, + + 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, + 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, + 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, + 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, + + 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, + 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, + 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, + 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, + + 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, + 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, + 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, + 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, + + 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, + 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, + 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, + 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf +}; + +uint8_t mesh_crypto_compute_fcs(const uint8_t *packet, uint8_t packet_len) +{ + uint8_t fcs = 0xff; + int i; + + for (i = 0; i < packet_len; i++) + fcs = crc_table[fcs ^ packet[i]]; + + return 0xff - fcs; +} + +bool mesh_crypto_check_fcs(const uint8_t *packet, uint8_t packet_len, + uint8_t received_fcs) +{ + uint8_t fcs = 0xff; + int i; + + for (i = 0; i < packet_len; i++) + fcs = crc_table[fcs ^ packet[i]]; + + fcs = crc_table[fcs ^ received_fcs]; + + if (fcs != 0xcf) + l_error("IOT Warning! CRC %2.2x != 0xcf", fcs); + + return fcs == 0xcf; +} -- 2.14.3