From e9292fe87e3930bfc41dd2eb6d5180253be2d0b9 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Tue, 13 Aug 2019 18:26:40 +0200 Subject: [PATCH] mesh: provide mesh_pdu_control_opcode --- src/mesh/mesh_access.c | 11 +++++++++++ src/mesh/mesh_access.h | 1 + src/mesh/mesh_lower_transport.c | 9 ++++++--- src/mesh/mesh_lower_transport.h | 1 + src/mesh/mesh_network.c | 3 +++ src/mesh/mesh_network.h | 5 +++-- src/mesh/mesh_upper_transport.c | 10 +++++----- 7 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/mesh/mesh_access.c b/src/mesh/mesh_access.c index 192c58fd9..52483ddda 100644 --- a/src/mesh/mesh_access.c +++ b/src/mesh/mesh_access.c @@ -445,6 +445,17 @@ uint8_t * mesh_pdu_data(mesh_pdu_t * pdu){ } } +uint8_t mesh_pdu_control_opcode(mesh_pdu_t * pdu){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return mesh_transport_control_opcode((mesh_transport_pdu_t*) pdu); + case MESH_PDU_TYPE_NETWORK: + return mesh_network_control_opcode((mesh_network_pdu_t *) pdu); + default: + return 0xff; + } +} + // message parser static int mesh_access_get_opcode(uint8_t * buffer, uint16_t buffer_size, uint32_t * opcode, uint16_t * opcode_size){ diff --git a/src/mesh/mesh_access.h b/src/mesh/mesh_access.h index 85deaf987..4d3443559 100644 --- a/src/mesh/mesh_access.h +++ b/src/mesh/mesh_access.h @@ -220,6 +220,7 @@ uint16_t mesh_pdu_netkey_index(mesh_pdu_t * pdu); uint16_t mesh_pdu_appkey_index(mesh_pdu_t * pdu); uint16_t mesh_pdu_len(mesh_pdu_t * pdu); uint8_t * mesh_pdu_data(mesh_pdu_t * pdu); +uint8_t mesh_pdu_control_opcode(mesh_pdu_t * pdu); // Mesh Access Parser int mesh_access_pdu_get_opcode(mesh_pdu_t * pdu, uint32_t * opcode, uint16_t * opcode_size); diff --git a/src/mesh/mesh_lower_transport.c b/src/mesh/mesh_lower_transport.c index 13f3d3cee..585023621 100644 --- a/src/mesh/mesh_lower_transport.c +++ b/src/mesh/mesh_lower_transport.c @@ -84,6 +84,9 @@ uint16_t mesh_transport_src(mesh_transport_pdu_t * transport_pdu){ uint16_t mesh_transport_dst(mesh_transport_pdu_t * transport_pdu){ return big_endian_read_16(transport_pdu->network_header, 7); } +uint8_t mesh_transport_control_opcode(mesh_transport_pdu_t * transport_pdu){ + return transport_pdu->akf_aid_control & 0x7f; +} void mesh_transport_set_nid_ivi(mesh_transport_pdu_t * transport_pdu, uint8_t nid_ivi){ transport_pdu->network_header[0] = nid_ivi; } @@ -370,8 +373,8 @@ static void mesh_lower_transport_process_segment( mesh_transport_pdu_t * transpo uint8_t lower_transport_pdu_len = mesh_network_pdu_len(network_pdu); // get akf_aid & transmic - transport_pdu->akf_aid = lower_transport_pdu[0]; - transport_pdu->transmic_len = lower_transport_pdu[1] & 0x80 ? 8 : 4; + transport_pdu->akf_aid_control = lower_transport_pdu[0] & 0x7f; + transport_pdu->transmic_len = lower_transport_pdu[1] & 0x80 ? 8 : 4; // get seq_zero uint16_t seq_zero = ( big_endian_read_16(lower_transport_pdu, 1) >> 2) & 0x1fff; @@ -486,7 +489,7 @@ static void mesh_lower_transport_setup_segment(mesh_transport_pdu_t *transport_p uint16_t seg_offset = seg_o * max_segment_len; uint8_t lower_transport_pdu_data[16]; - lower_transport_pdu_data[0] = 0x80 | transport_pdu->akf_aid; + lower_transport_pdu_data[0] = 0x80 | transport_pdu->akf_aid_control; big_endian_store_24(lower_transport_pdu_data, 1, (szmic << 23) | (seq_zero << 10) | (seg_o << 5) | seg_n); uint16_t segment_len = btstack_min(transport_pdu->len - seg_offset, max_segment_len); memcpy(&lower_transport_pdu_data[4], &transport_pdu->data[seg_offset], segment_len); diff --git a/src/mesh/mesh_lower_transport.h b/src/mesh/mesh_lower_transport.h index b72c61c5d..8484d99a5 100644 --- a/src/mesh/mesh_lower_transport.h +++ b/src/mesh/mesh_lower_transport.h @@ -85,6 +85,7 @@ uint32_t mesh_transport_seq(mesh_transport_pdu_t * transport_pdu); uint32_t mesh_transport_seq_zero(mesh_transport_pdu_t * transport_pdu); uint16_t mesh_transport_src(mesh_transport_pdu_t * transport_pdu); uint16_t mesh_transport_dst(mesh_transport_pdu_t * transport_pdu); +uint8_t mesh_transport_control_opcode(mesh_transport_pdu_t * transport_pdu); void mesh_transport_set_nid_ivi(mesh_transport_pdu_t * transport_pdu, uint8_t nid_ivi); void mesh_transport_set_ctl_ttl(mesh_transport_pdu_t * transport_pdu, uint8_t ctl_ttl); diff --git a/src/mesh/mesh_network.c b/src/mesh/mesh_network.c index 740c62700..13fbb60d1 100644 --- a/src/mesh/mesh_network.c +++ b/src/mesh/mesh_network.c @@ -1022,6 +1022,9 @@ uint16_t mesh_network_dst(mesh_network_pdu_t * network_pdu){ int mesh_network_segmented(mesh_network_pdu_t * network_pdu){ return network_pdu->data[9] & 0x80; } +uint8_t mesh_network_control_opcode(mesh_network_pdu_t * network_pdu){ + return network_pdu->data[9] & 0x7f; +} uint8_t * mesh_network_pdu_data(mesh_network_pdu_t * network_pdu){ return &network_pdu->data[9]; } diff --git a/src/mesh/mesh_network.h b/src/mesh/mesh_network.h index 9c26ef5a2..af71b937a 100644 --- a/src/mesh/mesh_network.h +++ b/src/mesh/mesh_network.h @@ -121,8 +121,8 @@ typedef struct { uint16_t appkey_index; // transmic size uint8_t transmic_len; - // akf - aid - uint8_t akf_aid; + // akf - aid for access, opcode for control + uint8_t akf_aid_control; // network pdu header uint8_t network_header[9]; // acknowledgement timer active @@ -371,6 +371,7 @@ uint32_t mesh_network_seq(mesh_network_pdu_t * network_pdu); uint16_t mesh_network_src(mesh_network_pdu_t * network_pdu); uint16_t mesh_network_dst(mesh_network_pdu_t * network_pdu); int mesh_network_segmented(mesh_network_pdu_t * network_pdu); +uint8_t mesh_network_control_opcode(mesh_network_pdu_t * network_pdu); uint8_t * mesh_network_pdu_data(mesh_network_pdu_t * network_pdu); uint8_t mesh_network_pdu_len(mesh_network_pdu_t * network_pdu); diff --git a/src/mesh/mesh_upper_transport.c b/src/mesh/mesh_upper_transport.c index fc427e295..94a72c421 100644 --- a/src/mesh/mesh_upper_transport.c +++ b/src/mesh/mesh_upper_transport.c @@ -317,7 +317,7 @@ static void mesh_upper_transport_validate_segmented_message_ccm(void * arg){ printf("\n"); } else { - uint8_t akf = transport_pdu->akf_aid & 0x40; + uint8_t akf = transport_pdu->akf_aid_control & 0x40; if (akf){ printf("TransMIC does not match, try next key\n"); mesh_upper_transport_validate_segmented_message(transport_pdu); @@ -475,8 +475,8 @@ static void mesh_upper_transport_process_message(mesh_transport_pdu_t * transpor uint8_t upper_transport_pdu_len = transport_pdu->len - transport_pdu->transmic_len; mesh_print_hex("Upper Transport pdu", upper_transport_pdu, upper_transport_pdu_len); - uint8_t aid = transport_pdu->akf_aid & 0x3f; - uint8_t akf = (transport_pdu->akf_aid & 0x40) >> 6; + uint8_t aid = transport_pdu->akf_aid_control & 0x3f; + uint8_t akf = (transport_pdu->akf_aid_control & 0x40) >> 6; printf("AKF: %u\n", akf); printf("AID: %02x\n", aid); @@ -591,7 +591,7 @@ static uint8_t mesh_upper_transport_setup_segmented_control_pdu(mesh_transport_p memcpy(transport_pdu->data, control_pdu_data, control_pdu_len); transport_pdu->len = control_pdu_len; transport_pdu->netkey_index = netkey_index; - transport_pdu->akf_aid = opcode; + transport_pdu->akf_aid_control = opcode; transport_pdu->transmic_len = 0; // no TransMIC for control mesh_transport_set_nid_ivi(transport_pdu, network_key->nid); mesh_transport_set_seq(transport_pdu, seq); @@ -679,7 +679,7 @@ static uint8_t mesh_upper_transport_setup_segmented_access_pdu_header(mesh_trans transport_pdu->transmic_len = trans_mic_len; transport_pdu->netkey_index = netkey_index; transport_pdu->appkey_index = appkey_index; - transport_pdu->akf_aid = akf_aid; + transport_pdu->akf_aid_control = akf_aid; mesh_transport_set_nid_ivi(transport_pdu, network_key->nid | ((mesh_get_iv_index_for_tx() & 1) << 7)); mesh_transport_set_seq(transport_pdu, seq); mesh_transport_set_src(transport_pdu, src);