mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-01-27 06:35:20 +00:00
Code Issues Packages Projects Releases Wiki Activity

mesh: extract mesh_network

Browse Source
This commit is contained in:
Matthias Ringwald 2018-11-02 15:50:26 +01:00
parent 63cdf6b794
commit 9cc32c6c71
4 changed files with 532 additions and 403 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

492
src/ble/mesh/mesh_network.c Normal file
View File

@ -0,0 +1,492 @@
/*
* Copyright (C) 2018 BlueKitchen GmbH
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holders nor the names of
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
* 4. Any redistribution, use, or modification is done solely for
* personal benefit and not for any commercial purpose or for
* monetary gain.
*
* THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
* RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* Please inquire about commercial licensing options at
* contact@bluekitchen-gmbh.com
*
*/
#define __BTSTACK_FILE__ "mesh_network.c"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ble/mesh/adv_bearer.h"
#include "ble/mesh/pb_adv.h"
#include "ble/mesh/beacon.h"
#include "provisioning.h"
#include "provisioning_device.h"
#include "btstack.h"
// structs
typedef struct {
uint8_t nid;
uint8_t first;
} mesh_network_key_iterator_t;
// globals
static uint32_t global_iv_index;
static btstack_crypto_ccm_t mesh_ccm_request;
static btstack_crypto_aes128_t mesh_aes128_request;
// PECB calculation
static uint8_t encryption_block[16];
static uint8_t obfuscation_block[16];
// Network Nonce
static uint8_t network_nonce[13];
// INCOMING //
// unprocessed network pdu
static btstack_linked_list_t network_received;
// in validation
static mesh_network_pdu_t * network_pdu_in_validation;
static mesh_network_key_iterator_t process_network_pdu_key_it;
static const mesh_network_key_t * process_network_pdu_network_key;
//
static btstack_linked_list_t network_incoming;
// OUTGOING //
static btstack_linked_list_t network_outgoing;
static const mesh_network_key_t * outgoing_network_key;
// mesh network key list
static mesh_network_key_t mesh_network_primary_key;
// prototypes
static void mesh_network_run(void);
// network key list
static const mesh_network_key_t * mesh_network_key_list_get(uint16_t netkey_index){
if (netkey_index) return NULL;
return &mesh_network_primary_key;
}
void mesh_network_key_list_add_from_provisioning_data(const mesh_provisioning_data_t * provisioning_data){
// get single instance
mesh_network_key_t * network_key = &mesh_network_primary_key;
memset(network_key, 0, sizeof(mesh_network_key_t));
// k1
// uint8_t identity_key[16] // not used yet
uint8_t beacon_key[16];
// k2
uint8_t nid;
uint8_t encryption_key[16];
uint8_t privacy_key[16];
// k3
uint8_t network_id[8];
// NetKey
// memcpy(network_key->net_key, provisioning_data, net_key);
// IdentityKey
// memcpy(network_key->identity_key, provisioning_data->identity_key, 16);
// BeaconKey
memcpy(network_key->beacon_key, provisioning_data->beacon_key, 16);
// NID
network_key->nid = provisioning_data->nid;
// EncryptionKey
memcpy(network_key->encryption_key, provisioning_data->encryption_key, 16);
// PrivacyKey
memcpy(network_key->privacy_key, provisioning_data->privacy_key, 16);
// NetworkID
memcpy(network_key->network_id, provisioning_data->network_id, 8);
}
// mesh network key iterator
static void mesh_network_key_iterator_init(mesh_network_key_iterator_t * it, uint8_t nid){
it->nid = nid;
it->first = 1;
}
static int mesh_network_key_iterator_has_more(mesh_network_key_iterator_t * it){
return it->first && it->nid == mesh_network_primary_key.nid;
}
static const mesh_network_key_t * mesh_network_key_iterator_get_next(mesh_network_key_iterator_t * it){
it->first = 0;
return &mesh_network_primary_key;
}
// common helper
int mesh_network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst){
printf("CTL: %u\n", ctl);
printf("SRC: %04x\n", src);
printf("DST: %04x\n", dst);
if (src == 0){
printf("SRC Unassigned Addr -> ignore\n");
return 0;
}
if ((src & 0xC000) == 0x8000){
printf("SRC Virtual Addr -> ignore\n");
return 0;
}
if ((src & 0xC000) == 0xC000){
printf("SRC Group Addr -> ignore\n");
return 0;
}
if (dst == 0){
printf("DST Unassigned Addr -> ignore\n");
return 0;
}
if ( ((dst & 0xC000) == 0x8000) && (ctl == 1)){
printf("DST Virtual Addr in CONTROL -> ignore\n");
return 0;
}
if ( (0xFF00 <= dst) && (dst <= 0xfffb) && (ctl == 0) ){
printf("DST RFU Group Addr in MESSAGE -> ignore\n");
return 0;
}
printf("SRC + DST Addr valid\n");
return 1;
}
static void mesh_network_create_nonce(uint8_t * nonce, uint8_t ctl_ttl, uint32_t seq, uint16_t src, uint32_t iv_index){
unsigned int pos = 0;
nonce[pos++] = 0x0; // Network Nonce
nonce[pos++] = ctl_ttl;
big_endian_store_24(nonce, pos, seq);
pos += 3;
big_endian_store_16(nonce, pos, src);
pos += 2;
big_endian_store_16(nonce, pos, 0);
pos += 2;
big_endian_store_32(nonce, pos, iv_index);
}
// NID/IVI | obfuscated (CTL/TTL, SEQ (24), SRC (16) ), encrypted ( DST(16), TransportPDU), MIC(32 or 64)
// new
static void mesh_network_send_c(void *arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
// obfuscate
unsigned int i;
for (i=0;i<6;i++){
network_pdu->data[1+i] ^= obfuscation_block[i];
}
printf("NetworkPDU: ");
printf_hexdump(network_pdu->data, network_pdu->len);
// add to queue
btstack_linked_list_add_tail(&network_outgoing, (btstack_linked_item_t *) network_pdu);
// request to send
adv_bearer_request_can_send_now_for_mesh_message();
}
static void mesh_network_send_b(void *arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
uint32_t iv_index = global_iv_index;
// store NetMIC
uint8_t net_mic[8];
btstack_crypo_ccm_get_authentication_value(&mesh_ccm_request, net_mic);
// store MIC
uint8_t net_mic_len = network_pdu->data[1] & 0x80 ? 8 : 4;
memcpy(&network_pdu->data[network_pdu->len], net_mic, net_mic_len);
network_pdu->len += net_mic_len;
// calc PECB
memset(encryption_block, 0, 5);
big_endian_store_32(encryption_block, 5, iv_index);
memcpy(&encryption_block[9], &network_pdu->data[7], 7);
btstack_crypto_aes128_encrypt(&mesh_aes128_request, outgoing_network_key->privacy_key, encryption_block, obfuscation_block, &mesh_network_send_c, network_pdu);
}
uint8_t mesh_network_send(uint16_t netkey_index, uint8_t ctl, uint8_t ttl, uint32_t seq, uint16_t src, uint16_t dest, const uint8_t * transport_pdu_data, uint8_t transport_pdu_len){
// TODO: check transport_pdu_len depending on ctl
// TODO: lookup network by netkey_index
outgoing_network_key = mesh_network_key_list_get(netkey_index);
if (!outgoing_network_key) return 0;
uint32_t iv_index = global_iv_index;
uint8_t nid = outgoing_network_key->nid;
// allocate network_pdu
mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get();
if (!network_pdu) return 0;
memset(network_pdu, 0, sizeof(mesh_network_pdu_t));
// setup header
network_pdu->data[network_pdu->len++] = (iv_index << 7) | nid;
uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f);
network_pdu->data[network_pdu->len++] = ctl_ttl;
big_endian_store_24(network_pdu->data, 2, seq);
network_pdu->len += 3;
big_endian_store_16(network_pdu->data, network_pdu->len, src);
network_pdu->len += 2;
big_endian_store_16(network_pdu->data, network_pdu->len, dest);
network_pdu->len += 2;
memcpy(&network_pdu->data[network_pdu->len], transport_pdu_data, transport_pdu_len);
network_pdu->len += transport_pdu_len;
// get network nonce
mesh_network_create_nonce(network_nonce, ctl_ttl, seq, src, iv_index);
// start ccm
uint8_t cypher_len = 2 + transport_pdu_len;
uint8_t net_mic_len = ctl ? 8 : 4;
btstack_crypo_ccm_init(&mesh_ccm_request, outgoing_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len);
btstack_crypto_ccm_encrypt_block(&mesh_ccm_request, cypher_len, &network_pdu->data[7], &network_pdu->data[7], &mesh_network_send_b, network_pdu);
return 0;
}
static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu);
static void process_network_pdu_done(void){
btstack_memory_mesh_network_pdu_free(network_pdu_in_validation);
network_pdu_in_validation = NULL;
mesh_network_run();
}
static void process_network_pdu_validate_d(void * arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
uint8_t ctl_ttl = network_pdu->data[1];
uint8_t ctl = ctl_ttl >> 7;
uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4;
uint8_t cypher_len = network_pdu->len - 9 - net_mic_len;
// store NetMIC
uint8_t net_mic[8];
btstack_crypo_ccm_get_authentication_value(&mesh_ccm_request, net_mic);
printf("NetMIC: ");
printf_hexdump(net_mic, net_mic_len);
// store in pdu
memcpy(&network_pdu->data[network_pdu->len-net_mic_len], net_mic, net_mic_len);
printf("Decrypted DST/TransportPDU: ");
printf_hexdump(&network_pdu->data[7], 2 + cypher_len);
printf("Decrypted: ");
printf_hexdump(network_pdu->data, network_pdu->len);
// compare nic to nic in data
if (memcmp(net_mic, &network_pdu_in_validation->data[network_pdu->len-net_mic_len], net_mic_len) == 0){
// match
printf("NetMIC matches\n");
printf("TTL: 0x%02x\n", network_pdu->data[1] & 0x7f);
// validate packet
uint16_t src = big_endian_read_16(network_pdu->data, 5);
uint16_t dst = big_endian_read_16(network_pdu->data, 7);
int valid = mesh_network_addresses_valid(ctl, src, dst);
if (!valid){
btstack_memory_mesh_network_pdu_free(network_pdu);
} else {
#if 0
btstack_linked_list_add_tail(&network_incoming, (btstack_linked_item_t *) network_pdu);
#else
btstack_memory_mesh_network_pdu_free(network_pdu);
#endif
}
process_network_pdu_done();
} else {
// fail
printf("NetMIC maismatch, try next key\n");
process_network_pdu_validate(network_pdu);
}
}
static void process_network_pdu_validate_b(void * arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
//
printf("PECB: ");
printf_hexdump(obfuscation_block, 6);
// de-obfuscate
unsigned int i;
for (i=0;i<6;i++){
network_pdu->data[1+i] = network_pdu_in_validation->data[1+i] ^ obfuscation_block[i];
}
// parse header
uint32_t iv_index = global_iv_index;
uint8_t ctl_ttl = network_pdu->data[1];
uint32_t seq = big_endian_read_24(network_pdu->data, 2);
uint16_t src = big_endian_read_16(network_pdu->data, 5);
// get network nonce
mesh_network_create_nonce(network_nonce, ctl_ttl, seq, src, iv_index);
printf("Network Nonce: ");
printf_hexdump(network_nonce, 13);
//
uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4;
uint8_t cypher_len = network_pdu->len - 7 - net_mic_len;
printf("Cyper len %u, mic len %u\n", cypher_len, net_mic_len);
printf("Encryption Key: ");
printf_hexdump(process_network_pdu_network_key->encryption_key, 16);
// 034b50057e400000010000
btstack_crypo_ccm_init(&mesh_ccm_request, process_network_pdu_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len);
btstack_crypto_ccm_decrypt_block(&mesh_ccm_request, cypher_len, &network_pdu_in_validation->data[7], &network_pdu->data[7], &process_network_pdu_validate_d, network_pdu);
}
static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu){
if (!mesh_network_key_iterator_has_more(&process_network_pdu_key_it)){
printf("No valid network key found\n");
btstack_memory_mesh_network_pdu_free(network_pdu);
process_network_pdu_done();
return;
}
process_network_pdu_network_key = mesh_network_key_iterator_get_next(&process_network_pdu_key_it);
// calc PECB
memset(encryption_block, 0, 5);
big_endian_store_32(encryption_block, 5, global_iv_index);
memcpy(&encryption_block[9], &network_pdu_in_validation->data[7], 7);
btstack_crypto_aes128_encrypt(&mesh_aes128_request, process_network_pdu_network_key->privacy_key, encryption_block, obfuscation_block, &process_network_pdu_validate_b, network_pdu);
}
static void process_network_pdu(mesh_network_pdu_t * network_pdu){
//
uint8_t nid_ivi = network_pdu_in_validation->data[0];
// setup pdu object
network_pdu->data[0] = nid_ivi;
network_pdu->len = network_pdu_in_validation->len;
// init provisioning data iterator
uint8_t nid = nid_ivi & 0x7f;
// uint8_t iv_index = network_pdu_data[0] >> 7;
mesh_network_key_iterator_init(&process_network_pdu_key_it, nid);
process_network_pdu_validate(network_pdu);
}
static void mesh_network_run(void){
if (network_pdu_in_validation) return;
if (btstack_linked_list_empty(&network_received)) return;
mesh_network_pdu_t * decoded_pdu = btstack_memory_mesh_network_pdu_get();
if (!decoded_pdu) return;
// get encoded network pdu and start processing
network_pdu_in_validation = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_received);
process_network_pdu(decoded_pdu);
}
void mesh_network_received_message(const uint8_t * pdu_data, uint8_t pdu_len){
// verify len
if (pdu_len > 29) return;
// allocate network_pdu
mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get();
if (!network_pdu) return;
memset(network_pdu, 0, sizeof(mesh_network_pdu_t));
// store data
memcpy(network_pdu->data, pdu_data, pdu_len);
network_pdu->len = pdu_len;
btstack_linked_list_add_tail(&network_received, (btstack_linked_item_t *) network_pdu);
// go
mesh_network_run();
}
static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
if (packet_type != HCI_EVENT_PACKET) return;
const uint8_t * adv_data;
const uint8_t * pdu_data;
uint8_t pdu_len;
uint8_t adv_len;
mesh_network_pdu_t * network_pdu;
mesh_provisioning_data_t provisioning_data;
switch(packet[0]){
case HCI_EVENT_MESH_META:
switch(packet[2]){
case MESH_SUBEVENT_CAN_SEND_NOW:
if (btstack_linked_list_empty(&network_outgoing)) break;
network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_outgoing);
adv_bearer_send_mesh_message(network_pdu->data, network_pdu->len);
btstack_memory_mesh_network_pdu_free(network_pdu);
break;
default:
break;
}
break;
case GAP_EVENT_ADVERTISING_REPORT:
adv_len = gap_event_advertising_report_get_data_length(packet);
adv_data = gap_event_advertising_report_get_data(packet);
// validate data item len
pdu_len = adv_data[0] - 1;
printf("adv len %u pdu len %u\n", adv_len, pdu_len);
if ((pdu_len + 2) > adv_len) break;
if (pdu_len < 13) break; // transport PDU len = 0, 32 bit NetMIC
// get transport pdu
pdu_data = &adv_data[2];
printf("received mesh message: ");
printf_hexdump(pdu_data, pdu_len);
mesh_network_received_message(pdu_data, pdu_len);
break;
default:
break;
}
}
void mesh_network_init(void){
adv_bearer_register_for_mesh_message(&mesh_message_handler);
}

36
src/ble/mesh/mesh_network.h
View File

@ -39,6 +39,7 @@
#define __MESH_NETWORK #define __MESH_NETWORK
#include "btstack_linked_list.h" #include "btstack_linked_list.h"
#include "provisioning.h"
#if defined __cplusplus #if defined __cplusplus
extern "C" { extern "C" {
@ -75,6 +76,41 @@ typedef struct {
} mesh_network_key_t; } mesh_network_key_t;
/**
* @brief Init Mesh Network Layer
*/
void mesh_network_init(void);
/**
* @brief Initialize network key list from provisioning data
* @param provisioning_data
*/
void mesh_network_key_list_add_from_provisioning_data(const mesh_provisioning_data_t * provisioning_data);
/**
* @brief Send TransportPDU after encryption
* @param netkey_index
* @param ctl
* @param ttl
* @param seq
* @param dest
* @param transport_pdu_data
* @param transport_pdu_len
*/
uint8_t mesh_network_send(uint16_t netkey_index, uint8_t ctl, uint8_t ttl, uint32_t seq, uint16_t src, uint16_t dest,
const uint8_t * transport_pdu_data, uint8_t transport_pdu_len);
/**
* @brief Validate network addresses
* @param ctl
* @param src
* @param dst
* @returns 1 if valid,
*/
int mesh_network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst);
// Testing only
void mesh_network_received_message(const uint8_t * pdu_data, uint8_t pdu_len);
#if defined __cplusplus #if defined __cplusplus
} }

2
test/mesh/Makefile
View File

@ -41,7 +41,7 @@ CC_UNIT = g++
CFLAGS += $(shell pkg-config libusb-1.0 --cflags) CFLAGS += $(shell pkg-config libusb-1.0 --cflags)
LDFLAGS += $(shell pkg-config libusb-1.0 --libs) LDFLAGS += $(shell pkg-config libusb-1.0 --libs)
mesh: ${CORE_OBJ} ${COMMON_OBJ} ${SM_OBJ} mesh_crypto.o provisioning_device.o mesh.o mesh: ${CORE_OBJ} ${COMMON_OBJ} ${SM_OBJ} mesh_crypto.o provisioning_device.o mesh_network.o mesh.o
${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@ ${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@
provisioner: ${CORE_OBJ} ${COMMON_OBJ} ${SM_OBJ} mesh_crypto.o provisioning_provisioner.o provisioner.o provisioner: ${CORE_OBJ} ${COMMON_OBJ} ${SM_OBJ} mesh_crypto.o provisioning_provisioner.o provisioner.o

405
test/mesh/mesh.c
View File

@ -59,9 +59,6 @@ static btstack_packet_callback_registration_t hci_event_callback_registration;
static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
void mesh_network_key_list_add_from_provisioning_data(const mesh_provisioning_data_t * provisioning_data);
const mesh_network_key_t * mesh_network_key_list_get(uint16_t netkey_index);
static int counter = 'a'; static int counter = 'a';
static uint32_t global_iv_index; static uint32_t global_iv_index;
@ -75,9 +72,6 @@ static int ui_pin_offset;
static const btstack_tlv_t * btstack_tlv_singleton_impl; static const btstack_tlv_t * btstack_tlv_singleton_impl;
static void * btstack_tlv_singleton_context; static void * btstack_tlv_singleton_context;
static btstack_crypto_ccm_t mesh_ccm_request;
static btstack_crypto_aes128_t mesh_aes128_request;
static void mesh_provisioning_dump( mesh_provisioning_data_t * data){ static void mesh_provisioning_dump( mesh_provisioning_data_t * data){
printf("NID: 0x%02x\n", data->nid); printf("NID: 0x%02x\n", data->nid);
printf("IV Index: 0x%08x\n", data->iv_index); printf("IV Index: 0x%08x\n", data->iv_index);
@ -132,384 +126,11 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
} }
} }
// also used for PECB calculation
static uint8_t encryption_block[16];
static uint8_t obfuscation_block[16];
// network pdu decoding buffer
static uint8_t network_nonce[13];
// incoming processing
static mesh_network_pdu_t * network_pdu_in_validation;
// unprocessed network pdu
static btstack_linked_list_t network_received;
static btstack_linked_list_t network_outgoing;
static btstack_linked_list_t network_incoming;
// ougoing processing
static const mesh_network_key_t * outgoing_network_key;
static void mesh_network_create_nonce(uint8_t * nonce, uint8_t ctl_ttl, uint32_t seq, uint16_t src, uint32_t iv_index){
unsigned int pos = 0;
nonce[pos++] = 0x0; // Network Nonce
nonce[pos++] = ctl_ttl;
big_endian_store_24(nonce, pos, seq);
pos += 3;
big_endian_store_16(nonce, pos, src);
pos += 2;
big_endian_store_16(nonce, pos, 0);
pos += 2;
big_endian_store_32(nonce, pos, iv_index);
}
// NID/IVI | obfuscated (CTL/TTL, SEQ (24), SRC (16) ), encrypted ( DST(16), TransportPDU), MIC(32 or 64)
// new
static void mesh_network_send_c(void *arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
// obfuscate
unsigned int i;
for (i=0;i<6;i++){
network_pdu->data[1+i] ^= obfuscation_block[i];
}
printf("NetworkPDU: ");
printf_hexdump(network_pdu->data, network_pdu->len);
// add to queue
btstack_linked_list_add_tail(&network_outgoing, (btstack_linked_item_t *) network_pdu);
// request to send
adv_bearer_request_can_send_now_for_mesh_message();
}
static void mesh_network_send_b(void *arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
uint32_t iv_index = global_iv_index;
// store NetMIC
uint8_t net_mic[8];
btstack_crypo_ccm_get_authentication_value(&mesh_ccm_request, net_mic);
// store MIC
uint8_t net_mic_len = network_pdu->data[1] & 0x80 ? 8 : 4;
memcpy(&network_pdu->data[network_pdu->len], net_mic, net_mic_len);
network_pdu->len += net_mic_len;
// calc PECB
memset(encryption_block, 0, 5);
big_endian_store_32(encryption_block, 5, iv_index);
memcpy(&encryption_block[9], &network_pdu->data[7], 7);
btstack_crypto_aes128_encrypt(&mesh_aes128_request, outgoing_network_key->privacy_key, encryption_block, obfuscation_block, &mesh_network_send_c, network_pdu);
}
static void mesh_network_send(uint16_t netkey_index, uint8_t ctl, uint8_t ttl, uint32_t seq, uint16_t src, uint16_t dest, const uint8_t * transport_pdu_data, uint8_t transport_pdu_len){
// TODO: check transport_pdu_len depending on ctl
// TODO: lookup network by netkey_index
outgoing_network_key = mesh_network_key_list_get(netkey_index);
if (!outgoing_network_key) return;
uint32_t iv_index = global_iv_index;
uint8_t nid = outgoing_network_key->nid;
// allocate network_pdu
mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get();
if (!network_pdu) return;
memset(network_pdu, 0, sizeof(mesh_network_pdu_t));
// setup header
network_pdu->data[network_pdu->len++] = (iv_index << 7) | nid;
uint8_t ctl_ttl = (ctl << 7) | (ttl & 0x7f);
network_pdu->data[network_pdu->len++] = ctl_ttl;
big_endian_store_24(network_pdu->data, 2, seq);
network_pdu->len += 3;
big_endian_store_16(network_pdu->data, network_pdu->len, src);
network_pdu->len += 2;
big_endian_store_16(network_pdu->data, network_pdu->len, dest);
network_pdu->len += 2;
memcpy(&network_pdu->data[network_pdu->len], transport_pdu_data, transport_pdu_len);
network_pdu->len += transport_pdu_len;
// get network nonce
mesh_network_create_nonce(network_nonce, ctl_ttl, seq, src, iv_index);
// start ccm
uint8_t cypher_len = 2 + transport_pdu_len;
uint8_t net_mic_len = ctl ? 8 : 4;
btstack_crypo_ccm_init(&mesh_ccm_request, outgoing_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len);
btstack_crypto_ccm_encrypt_block(&mesh_ccm_request, cypher_len, &network_pdu->data[7], &network_pdu->data[7], &mesh_network_send_b, network_pdu);
}
// mesh network key list // mesh network key list
static mesh_network_key_t mesh_network_primary_key; static mesh_network_key_t mesh_network_primary_key;
const mesh_network_key_t * mesh_network_key_list_get(uint16_t netkey_index){
if (netkey_index) return NULL;
return &mesh_network_primary_key;
}
void mesh_network_key_list_add_from_provisioning_data(const mesh_provisioning_data_t * provisioning_data){
// get single instance
mesh_network_key_t * network_key = &mesh_network_primary_key;
memset(network_key, 0, sizeof(mesh_network_key_t));
// k1
// uint8_t identity_key[16] // not used yet
uint8_t beacon_key[16];
// k2
uint8_t nid;
uint8_t encryption_key[16];
uint8_t privacy_key[16];
// k3
uint8_t network_id[8];
// NetKey
// memcpy(network_key->net_key, provisioning_data, net_key);
// IdentityKey
// memcpy(network_key->identity_key, provisioning_data->identity_key, 16);
// BeaconKey
memcpy(network_key->beacon_key, provisioning_data->beacon_key, 16);
// NID
network_key->nid = provisioning_data->nid;
// EncryptionKey
memcpy(network_key->encryption_key, provisioning_data->encryption_key, 16);
// PrivacyKey
memcpy(network_key->privacy_key, provisioning_data->privacy_key, 16);
// NetworkID
memcpy(network_key->network_id, provisioning_data->network_id, 8);
}
// mesh network key iterator
typedef struct {
uint8_t nid;
uint8_t first;
} mesh_network_key_iterator_t;
static void mesh_network_key_iterator_init(mesh_network_key_iterator_t * it, uint8_t nid){
it->nid = nid;
it->first = 1;
}
static int mesh_network_key_iterator_has_more(mesh_network_key_iterator_t * it){
return it->first && it->nid == mesh_network_primary_key.nid;
}
static const mesh_network_key_t * mesh_network_key_iterator_get_next(mesh_network_key_iterator_t * it){
it->first = 0;
return &mesh_network_primary_key;
}
//
static mesh_network_key_iterator_t process_network_pdu_provisioning_data_it;
static const mesh_network_key_t * process_network_pdu_network_key;
static uint8_t process_network_pdu_decode_block; static uint8_t process_network_pdu_decode_block;
static int network_addresses_valid(uint8_t ctl, uint16_t src, uint16_t dst){
printf("CTL: %u\n", ctl);
printf("SRC: %04x\n", src);
printf("DST: %04x\n", dst);
if (src == 0){
printf("SRC Unassigned Addr -> ignore\n");
return 0;
}
if ((src & 0xC000) == 0x8000){
printf("SRC Virtual Addr -> ignore\n");
return 0;
}
if ((src & 0xC000) == 0xC000){
printf("SRC Group Addr -> ignore\n");
return 0;
}
if (dst == 0){
printf("DST Unassigned Addr -> ignore\n");
return 0;
}
if ( ((dst & 0xC000) == 0x8000) && (ctl == 1)){
printf("DST Virtual Addr in CONTROL -> ignore\n");
return 0;
}
if ( (0xFF00 <= dst) && (dst <= 0xfffb) && (ctl == 0) ){
printf("DST RFU Group Addr in MESSAGE -> ignore\n");
return 0;
}
printf("SRC + DST Addr valid\n");
return 1;
}
static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu);
static void process_network_pdu_done(void){
btstack_memory_mesh_network_pdu_free(network_pdu_in_validation);
network_pdu_in_validation = NULL;
mesh_network_run();
}
static void process_network_pdu_validate_d(void * arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
uint8_t ctl_ttl = network_pdu->data[1];
uint8_t ctl = ctl_ttl >> 7;
uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4;
uint8_t cypher_len = network_pdu->len - 9 - net_mic_len;
// store NetMIC
uint8_t net_mic[8];
btstack_crypo_ccm_get_authentication_value(&mesh_ccm_request, net_mic);
printf("NetMIC: ");
printf_hexdump(net_mic, net_mic_len);
// store in pdu
memcpy(&network_pdu->data[network_pdu->len-net_mic_len], net_mic, net_mic_len);
printf("Decrypted DST/TransportPDU: ");
printf_hexdump(&network_pdu->data[7], 2 + cypher_len);
printf("Decrypted: ");
printf_hexdump(network_pdu->data, network_pdu->len);
// compare nic to nic in data
if (memcmp(net_mic, &network_pdu_in_validation->data[network_pdu->len-net_mic_len], net_mic_len) == 0){
// match
printf("NetMIC matches\n");
printf("TTL: 0x%02x\n", network_pdu->data[1] & 0x7f);
// validate packet
uint16_t src = big_endian_read_16(network_pdu->data, 5);
uint16_t dst = big_endian_read_16(network_pdu->data, 7);
int valid = network_addresses_valid(ctl, src, dst);
if (!valid){
btstack_memory_mesh_network_pdu_free(network_pdu);
} else {
#if 0
btstack_linked_list_add_tail(&network_incoming, (btstack_linked_item_t *) network_pdu);
#else
btstack_memory_mesh_network_pdu_free(network_pdu);
#endif
}
process_network_pdu_done();
} else {
// fail
printf("NetMIC maismatch, try next key\n");
process_network_pdu_validate(network_pdu);
}
}
static void process_network_pdu_validate_b(void * arg){
mesh_network_pdu_t * network_pdu = (mesh_network_pdu_t *) arg;
//
printf("PECB: ");
printf_hexdump(obfuscation_block, 6);
// de-obfuscate
unsigned int i;
for (i=0;i<6;i++){
network_pdu->data[1+i] = network_pdu_in_validation->data[1+i] ^ obfuscation_block[i];
}
// parse header
uint32_t iv_index = global_iv_index;
uint8_t ctl_ttl = network_pdu->data[1];
uint32_t seq = big_endian_read_24(network_pdu->data, 2);
uint16_t src = big_endian_read_16(network_pdu->data, 5);
// get network nonce
mesh_network_create_nonce(network_nonce, ctl_ttl, seq, src, iv_index);
printf("Network Nonce: ");
printf_hexdump(network_nonce, 13);
//
uint8_t net_mic_len = (ctl_ttl & 0x80) ? 8 : 4;
uint8_t cypher_len = network_pdu->len - 7 - net_mic_len;
printf("Cyper len %u, mic len %u\n", cypher_len, net_mic_len);
printf("Encryption Key: ");
printf_hexdump(process_network_pdu_network_key->encryption_key, 16);
// 034b50057e400000010000
btstack_crypo_ccm_init(&mesh_ccm_request, process_network_pdu_network_key->encryption_key, network_nonce, cypher_len, 0, net_mic_len);
btstack_crypto_ccm_decrypt_block(&mesh_ccm_request, cypher_len, &network_pdu_in_validation->data[7], &network_pdu->data[7], &process_network_pdu_validate_d, network_pdu);
}
static void process_network_pdu_validate(mesh_network_pdu_t * network_pdu){
if (!mesh_network_key_iterator_has_more(&process_network_pdu_provisioning_data_it)){
printf("No valid network key found\n");
btstack_memory_mesh_network_pdu_free(network_pdu);
process_network_pdu_done();
return;
}
process_network_pdu_network_key = mesh_network_key_iterator_get_next(&process_network_pdu_provisioning_data_it);
// calc PECB
memset(encryption_block, 0, 5);
big_endian_store_32(encryption_block, 5, global_iv_index);
memcpy(&encryption_block[9], &network_pdu_in_validation->data[7], 7);
btstack_crypto_aes128_encrypt(&mesh_aes128_request, process_network_pdu_network_key->privacy_key, encryption_block, obfuscation_block, &process_network_pdu_validate_b, network_pdu);
}
static void process_network_pdu(mesh_network_pdu_t * network_pdu){
//
uint8_t nid_ivi = network_pdu_in_validation->data[0];
// setup pdu object
network_pdu->data[0] = nid_ivi;
network_pdu->len = network_pdu_in_validation->len;
// init provisioning data iterator
uint8_t nid = nid_ivi & 0x7f;
// uint8_t iv_index = network_pdu_data[0] >> 7;
mesh_network_key_iterator_init(&process_network_pdu_provisioning_data_it, nid);
process_network_pdu_validate(network_pdu);
}
static void mesh_network_run(void){
if (network_pdu_in_validation) return;
if (btstack_linked_list_empty(&network_received)) return;
mesh_network_pdu_t * decoded_pdu = btstack_memory_mesh_network_pdu_get();
if (!decoded_pdu) return;
// get encoded network pdu and start processing
network_pdu_in_validation = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_received);
process_network_pdu(decoded_pdu);
}
static void mesh_network_received_message(const uint8_t * pdu_data, uint8_t pdu_len){
// verify len
if (pdu_len > 29) return;
// allocate network_pdu
mesh_network_pdu_t * network_pdu = btstack_memory_mesh_network_pdu_get();
if (!network_pdu) return;
memset(network_pdu, 0, sizeof(mesh_network_pdu_t));
// store data
memcpy(network_pdu->data, pdu_data, pdu_len);
network_pdu->len = pdu_len;
btstack_linked_list_add_tail(&network_received, (btstack_linked_item_t *) network_pdu);
// go
mesh_network_run();
}
static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
if (packet_type != HCI_EVENT_PACKET) return; if (packet_type != HCI_EVENT_PACKET) return;
const uint8_t * adv_data; const uint8_t * adv_data;
@ -521,12 +142,6 @@ static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t
switch(packet[0]){ switch(packet[0]){
case HCI_EVENT_MESH_META: case HCI_EVENT_MESH_META:
switch(packet[2]){ switch(packet[2]){
case MESH_SUBEVENT_CAN_SEND_NOW:
if (btstack_linked_list_empty(&network_outgoing)) break;
network_pdu = (mesh_network_pdu_t *) btstack_linked_list_pop(&network_outgoing);
adv_bearer_send_mesh_message(network_pdu->data, network_pdu->len);
btstack_memory_mesh_network_pdu_free(network_pdu);
break;
case MESH_PB_ADV_LINK_OPEN: case MESH_PB_ADV_LINK_OPEN:
printf("Provisioner link opened"); printf("Provisioner link opened");
break; break;
@ -559,22 +174,6 @@ static void mesh_message_handler (uint8_t packet_type, uint16_t channel, uint8_t
break; break;
} }
break; break;
case GAP_EVENT_ADVERTISING_REPORT:
adv_len = gap_event_advertising_report_get_data_length(packet);
adv_data = gap_event_advertising_report_get_data(packet);
// validate data item len
pdu_len = adv_data[0] - 1;
printf("adv len %u pdu len %u\n", adv_len, pdu_len);
if ((pdu_len + 2) > adv_len) break;
if (pdu_len < 13) break; // transport PDU len = 0, 32 bit NetMIC
// get transport pdu
pdu_data = &adv_data[2];
printf("received mesh message: ");
printf_hexdump(pdu_data, pdu_len);
mesh_network_received_message(pdu_data, pdu_len);
break;
default: default:
break; break;
} }
@ -890,7 +489,6 @@ int btstack_main(void)
// mesh // mesh
adv_bearer_init(); adv_bearer_init();
adv_bearer_register_for_mesh_message(&mesh_message_handler);
beacon_init(device_uuid, 0); beacon_init(device_uuid, 0);
beacon_register_for_unprovisioned_device_beacons(&mesh_unprovisioned_beacon_handler); beacon_register_for_unprovisioned_device_beacons(&mesh_unprovisioned_beacon_handler);
@ -899,6 +497,9 @@ int btstack_main(void)
provisioning_device_init(device_uuid); provisioning_device_init(device_uuid);
provisioning_device_register_packet_handler(&mesh_message_handler); provisioning_device_register_packet_handler(&mesh_message_handler);
// Network layer
mesh_network_init();
// //
btstack_parse_hex(pts_device_uuid_string, 16, pts_device_uuid); btstack_parse_hex(pts_device_uuid_string, 16, pts_device_uuid);
btstack_print_hex(pts_device_uuid, 16, 0); btstack_print_hex(pts_device_uuid, 16, 0);