diff --git a/src/mesh/mesh.c b/src/mesh/mesh.c index 4d6d3db25..00937f1d7 100644 --- a/src/mesh/mesh.c +++ b/src/mesh/mesh.c @@ -1172,7 +1172,8 @@ static int mesh_node_startup_from_tlv(void){ static void mesh_control_message_handler(mesh_transport_callback_type_t callback_type, mesh_transport_status_t status, mesh_pdu_t * pdu){ // get opcode uint8_t opcode = mesh_pdu_control_opcode(pdu); - printf("Opcode: 0x%02x\n", opcode); + printf("MESH Control Message, Opcode: 0x%02x + ", opcode); + printf_hexdump(mesh_pdu_data(pdu), mesh_pdu_len(pdu)); uint8_t init_ttl; uint8_t hops = 0; diff --git a/src/mesh/mesh_access.c b/src/mesh/mesh_access.c index 4f8251c23..18d3ad334 100644 --- a/src/mesh/mesh_access.c +++ b/src/mesh/mesh_access.c @@ -459,6 +459,8 @@ uint16_t mesh_pdu_len(mesh_pdu_t * pdu){ switch (pdu->pdu_type){ case MESH_PDU_TYPE_ACCESS: return ((mesh_access_pdu_t*) pdu)->len; + case MESH_PDU_TYPE_CONTROL: + return ((mesh_control_pdu_t*) pdu)->len; case MESH_PDU_TYPE_NETWORK: return ((mesh_network_pdu_t *) pdu)->len - 10; default: @@ -471,6 +473,8 @@ uint8_t * mesh_pdu_data(mesh_pdu_t * pdu){ switch (pdu->pdu_type){ case MESH_PDU_TYPE_ACCESS: return ((mesh_access_pdu_t*) pdu)->data; + case MESH_PDU_TYPE_CONTROL: + return ((mesh_control_pdu_t*) pdu)->data; case MESH_PDU_TYPE_NETWORK: return &((mesh_network_pdu_t *) pdu)->data[10]; default: @@ -483,6 +487,8 @@ uint8_t mesh_pdu_control_opcode(mesh_pdu_t * pdu){ switch (pdu->pdu_type){ case MESH_PDU_TYPE_NETWORK: return mesh_network_control_opcode((mesh_network_pdu_t *) pdu); + case MESH_PDU_TYPE_CONTROL: + return ((mesh_control_pdu_t *) pdu)->akf_aid_control; default: btstack_assert(false); return 0xff; diff --git a/src/mesh/mesh_upper_transport.c b/src/mesh/mesh_upper_transport.c index 6db84f724..cb377568d 100644 --- a/src/mesh/mesh_upper_transport.c +++ b/src/mesh/mesh_upper_transport.c @@ -893,11 +893,11 @@ static void mesh_upper_transport_run(void){ incoming_control_pdu->len = segmented_pdu->len; incoming_control_pdu->netkey_index = segmented_pdu->netkey_index; incoming_control_pdu->akf_aid_control = segmented_pdu->akf_aid_control; - incoming_access_decrypted->ivi_nid = segmented_pdu->ivi_nid; - incoming_access_decrypted->ctl_ttl = segmented_pdu->ctl_ttl; - incoming_access_decrypted->seq = segmented_pdu->seq; - incoming_access_decrypted->src = segmented_pdu->src; - incoming_access_decrypted->dst = segmented_pdu->dst; + incoming_control_pdu->ivi_nid = segmented_pdu->ivi_nid; + incoming_control_pdu->ctl_ttl = segmented_pdu->ctl_ttl; + incoming_control_pdu->seq = segmented_pdu->seq; + incoming_control_pdu->src = segmented_pdu->src; + incoming_control_pdu->dst = segmented_pdu->dst; mesh_print_hex("Assembled payload", incoming_control_pdu->data, incoming_control_pdu->len); @@ -906,7 +906,7 @@ static void mesh_upper_transport_run(void){ btstack_assert(mesh_control_message_handler != NULL); mesh_pdu_t * pdu = (mesh_pdu_t*) incoming_control_pdu; - mesh_access_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, pdu); + mesh_control_message_handler(MESH_TRANSPORT_PDU_RECEIVED, MESH_TRANSPORT_STATUS_SUCCESS, pdu); } else {