From 723bf6fbce06d6f93fdae6cd168b5bfd3a2fb522 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Mon, 10 Jun 2019 21:30:53 +0200 Subject: [PATCH] mesh: move access message parser and builder to mesh_access --- test/mesh/mesh.c | 354 ---------------------------------------- test/mesh/mesh_access.c | 347 +++++++++++++++++++++++++++++++++++++++ test/mesh/mesh_access.h | 53 ++++++ 3 files changed, 400 insertions(+), 354 deletions(-) diff --git a/test/mesh/mesh.c b/test/mesh/mesh.c index a6df23ee0..4b8a1dc44 100644 --- a/test/mesh/mesh.c +++ b/test/mesh/mesh.c @@ -997,11 +997,6 @@ static void stdin_process(char cmd){ // Foundatiopn Message -typedef struct { - uint32_t opcode; - const char * format; -} mesh_access_message_t; - const mesh_access_message_t mesh_foundation_config_beacon_status = { MESH_FOUNDATION_OPERATION_BEACON_STATUS, "1" }; @@ -1054,355 +1049,6 @@ const mesh_access_message_t mesh_foundation_config_heartbeat_subscription_status MESH_FOUNDATION_OPERATION_HEARTBEAT_SUBSCRIPTION_STATUS, "1221111" }; -// message parser - -static int mesh_access_get_opcode(uint8_t * buffer, uint16_t buffer_size, uint32_t * opcode, uint16_t * opcode_size){ - switch (buffer[0] >> 6){ - case 0: - case 1: - if (buffer[0] == 0x7f) return 0; - *opcode = buffer[0]; - *opcode_size = 1; - return 1; - case 2: - if (buffer_size < 2) return 0; - *opcode = big_endian_read_16(buffer, 0); - *opcode_size = 2; - return 1; - case 3: - if (buffer_size < 3) return 0; - *opcode = (buffer[0] << 16) | little_endian_read_16(buffer, 1); - *opcode_size = 3; - return 1; - default: - return 0; - } -} - -static int mesh_access_transport_get_opcode(mesh_transport_pdu_t * transport_pdu, uint32_t * opcode, uint16_t * opcode_size){ - return mesh_access_get_opcode(transport_pdu->data, transport_pdu->len, opcode, opcode_size); -} - -static int mesh_access_network_get_opcode(mesh_network_pdu_t * network_pdu, uint32_t * opcode, uint16_t * opcode_size){ - // TransMIC already removed by mesh_upper_transport_validate_unsegmented_message_ccm - return mesh_access_get_opcode(&network_pdu->data[10], network_pdu->len - 10, opcode, opcode_size); -} - -static int mesh_access_pdu_get_opcode(mesh_pdu_t * pdu, uint32_t * opcode, uint16_t * opcode_size){ - switch (pdu->pdu_type){ - case MESH_PDU_TYPE_TRANSPORT: - return mesh_access_transport_get_opcode((mesh_transport_pdu_t*) pdu, opcode, opcode_size); - case MESH_PDU_TYPE_NETWORK: - return mesh_access_network_get_opcode((mesh_network_pdu_t *) pdu, opcode, opcode_size); - default: - return 0; - } -} - -static uint16_t mesh_pdu_src(mesh_pdu_t * pdu){ - switch (pdu->pdu_type){ - case MESH_PDU_TYPE_TRANSPORT: - return mesh_transport_src((mesh_transport_pdu_t*) pdu); - case MESH_PDU_TYPE_NETWORK: - return mesh_network_src((mesh_network_pdu_t *) pdu); - default: - return MESH_ADDRESS_UNSASSIGNED; - } -} - -static uint16_t mesh_pdu_dst(mesh_pdu_t * pdu){ - switch (pdu->pdu_type){ - case MESH_PDU_TYPE_TRANSPORT: - return mesh_transport_dst((mesh_transport_pdu_t*) pdu); - case MESH_PDU_TYPE_NETWORK: - return mesh_network_dst((mesh_network_pdu_t *) pdu); - default: - return MESH_ADDRESS_UNSASSIGNED; - } -} - -static uint16_t mesh_pdu_netkey_index(mesh_pdu_t * pdu){ - switch (pdu->pdu_type){ - case MESH_PDU_TYPE_TRANSPORT: - return ((mesh_transport_pdu_t*) pdu)->netkey_index; - case MESH_PDU_TYPE_NETWORK: - return ((mesh_network_pdu_t *) pdu)->netkey_index; - default: - return 0; - } -} - -static uint16_t mesh_pdu_len(mesh_pdu_t * pdu){ - switch (pdu->pdu_type){ - case MESH_PDU_TYPE_TRANSPORT: - return ((mesh_transport_pdu_t*) pdu)->len; - case MESH_PDU_TYPE_NETWORK: - return ((mesh_network_pdu_t *) pdu)->len - 10; - default: - return 0; - } -} - -static uint8_t * mesh_pdu_data(mesh_pdu_t * pdu){ - switch (pdu->pdu_type){ - case MESH_PDU_TYPE_TRANSPORT: - return ((mesh_transport_pdu_t*) pdu)->data; - case MESH_PDU_TYPE_NETWORK: - return &((mesh_network_pdu_t *) pdu)->data[10]; - default: - return NULL; - } -} - -typedef struct { - uint32_t opcode; - uint8_t * data; - uint16_t len; -} mesh_access_parser_state_t; - -static void mesh_access_parser_skip(mesh_access_parser_state_t * state, uint16_t bytes_to_skip){ - state->data += bytes_to_skip; - state->len -= bytes_to_skip; -} - -static int mesh_access_parser_init(mesh_access_parser_state_t * state, mesh_pdu_t * pdu){ - state->data = mesh_pdu_data(pdu); - state->len = mesh_pdu_len(pdu); - - uint16_t opcode_size = 0; - int ok = mesh_access_get_opcode(state->data, state->len, &state->opcode, &opcode_size); - if (ok){ - mesh_access_parser_skip(state, opcode_size); - } - return ok; -} - -static uint16_t mesh_access_parser_available(mesh_access_parser_state_t * state){ - return state->len; -} - -static uint8_t mesh_access_parser_get_u8(mesh_access_parser_state_t * state){ - uint8_t value = *state->data; - mesh_access_parser_skip(state, 1); - return value; -} - -static uint16_t mesh_access_parser_get_u16(mesh_access_parser_state_t * state){ - uint16_t value = little_endian_read_16(state->data, 0); - mesh_access_parser_skip(state, 2); - return value; -} - -static uint32_t mesh_access_parser_get_u24(mesh_access_parser_state_t * state){ - uint32_t value = little_endian_read_24(state->data, 0); - mesh_access_parser_skip(state, 3); - return value; -} - -static uint32_t mesh_access_parser_get_u32(mesh_access_parser_state_t * state){ - uint32_t value = little_endian_read_24(state->data, 0); - mesh_access_parser_skip(state, 4); - return value; -} - -static void mesh_access_parser_get_u128(mesh_access_parser_state_t * state, uint8_t * dest){ - reverse_128( state->data, dest); - mesh_access_parser_skip(state, 16); -} - -static void mesh_access_parser_get_label_uuid(mesh_access_parser_state_t * state, uint8_t * dest){ - memcpy( dest, state->data, 16); - mesh_access_parser_skip(state, 16); -} - -static void mesh_access_parser_get_key(mesh_access_parser_state_t * state, uint8_t * dest){ - memcpy( dest, state->data, 16); - mesh_access_parser_skip(state, 16); -} - -static uint32_t mesh_access_parser_get_model_identifier(mesh_access_parser_state_t * parser){ - if (mesh_access_parser_available(parser) == 4){ - return mesh_access_parser_get_u32(parser); - } else { - return (BLUETOOTH_COMPANY_ID_BLUETOOTH_SIG_INC << 16) | mesh_access_parser_get_u16(parser); - } -} - -// message builder -static int mesh_access_setup_opcode(uint8_t * buffer, uint32_t opcode){ - if (opcode < 0x100){ - buffer[0] = opcode; - return 1; - } - if (opcode < 0x10000){ - big_endian_store_16(buffer, 0, opcode); - return 2; - } - buffer[0] = opcode >> 16; - little_endian_store_16(buffer, 1, opcode & 0xffff); - return 3; -} - -static mesh_transport_pdu_t * mesh_access_transport_init(uint32_t opcode){ - mesh_transport_pdu_t * pdu = mesh_transport_pdu_get(); - if (!pdu) return NULL; - - pdu->len = mesh_access_setup_opcode(pdu->data, opcode); - return pdu; -} - -static void mesh_access_transport_add_uint8(mesh_transport_pdu_t * pdu, uint8_t value){ - pdu->data[pdu->len++] = value; -} - -static void mesh_access_transport_add_uint16(mesh_transport_pdu_t * pdu, uint16_t value){ - little_endian_store_16(pdu->data, pdu->len, value); - pdu->len += 2; -} - -static void mesh_access_transport_add_uint24(mesh_transport_pdu_t * pdu, uint32_t value){ - little_endian_store_24(pdu->data, pdu->len, value); - pdu->len += 3; -} - -static void mesh_access_transport_add_uint32(mesh_transport_pdu_t * pdu, uint32_t value){ - little_endian_store_32(pdu->data, pdu->len, value); - pdu->len += 4; -} -static void mesh_access_transport_add_model_identifier(mesh_transport_pdu_t * pdu, uint32_t model_identifier){ - if (mesh_model_is_bluetooth_sig(model_identifier)){ - mesh_access_transport_add_uint16( pdu, mesh_model_get_model_id(model_identifier) ); - } else { - mesh_access_transport_add_uint32( pdu, model_identifier ); - } -} - -static mesh_network_pdu_t * mesh_access_network_init(uint32_t opcode){ - mesh_network_pdu_t * pdu = mesh_network_pdu_get(); - if (!pdu) return NULL; - - pdu->len = mesh_access_setup_opcode(&pdu->data[10], opcode) + 10; - return pdu; -} - -static void mesh_access_network_add_uint8(mesh_network_pdu_t * pdu, uint8_t value){ - pdu->data[pdu->len++] = value; -} - -static void mesh_access_network_add_uint16(mesh_network_pdu_t * pdu, uint16_t value){ - little_endian_store_16(pdu->data, pdu->len, value); - pdu->len += 2; -} - -static void mesh_access_network_add_uint24(mesh_network_pdu_t * pdu, uint16_t value){ - little_endian_store_24(pdu->data, pdu->len, value); - pdu->len += 3; -} - -static void mesh_access_network_add_uint32(mesh_network_pdu_t * pdu, uint16_t value){ - little_endian_store_32(pdu->data, pdu->len, value); - pdu->len += 4; -} - -static void mesh_access_network_add_model_identifier(mesh_network_pdu_t * pdu, uint32_t model_identifier){ - if (mesh_model_is_bluetooth_sig(model_identifier)){ - mesh_access_network_add_uint16( pdu, mesh_model_get_model_id(model_identifier) ); - } else { - mesh_access_network_add_uint32( pdu, model_identifier ); - } -} - -// access message template -#include - -static mesh_network_pdu_t * mesh_access_setup_unsegmented_message(const mesh_access_message_t *template, ...){ - mesh_network_pdu_t * network_pdu = mesh_access_network_init(template->opcode); - if (!network_pdu) return NULL; - - va_list argptr; - va_start(argptr, template); - - // add params - const char * format = template->format; - uint16_t word; - uint32_t longword; - while (*format){ - switch (*format){ - case '1': - word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs - mesh_access_network_add_uint8( network_pdu, word); - break; - case '2': - word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs - mesh_access_network_add_uint16( network_pdu, word); - break; - case '3': - longword = va_arg(argptr, uint32_t); - mesh_access_network_add_uint24( network_pdu, longword); - break; - case '4': - longword = va_arg(argptr, uint32_t); - mesh_access_network_add_uint32( network_pdu, longword); - break; - case 'm': - longword = va_arg(argptr, uint32_t); - mesh_access_network_add_model_identifier( network_pdu, longword); - break; - default: - log_error("Unsupported mesh message format specifier '%c", *format); - break; - } - format++; - } - - va_end(argptr); - - return network_pdu; -} - -static mesh_transport_pdu_t * mesh_access_setup_segmented_message(const mesh_access_message_t *template, ...){ - mesh_transport_pdu_t * transport_pdu = mesh_access_transport_init(template->opcode); - if (!transport_pdu) return NULL; - - va_list argptr; - va_start(argptr, template); - - // add params - const char * format = template->format; - uint16_t word; - uint32_t longword; - while (*format){ - switch (*format++){ - case '1': - word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs - mesh_access_transport_add_uint8( transport_pdu, word); - break; - case '2': - word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs - mesh_access_transport_add_uint16( transport_pdu, word); - break; - case '3': - longword = va_arg(argptr, uint32_t); - mesh_access_transport_add_uint24( transport_pdu, longword); - break; - case '4': - longword = va_arg(argptr, uint32_t); - mesh_access_transport_add_uint32( transport_pdu, longword); - break; - case 'm': - longword = va_arg(argptr, uint32_t); - mesh_access_transport_add_model_identifier( transport_pdu, longword); - break; - default: - break; - } - } - - va_end(argptr); - - return transport_pdu; -} - // to sort // move to btstack_config.h diff --git a/test/mesh/mesh_access.c b/test/mesh/mesh_access.c index 22d55ba39..24c2a436e 100644 --- a/test/mesh/mesh_access.c +++ b/test/mesh/mesh_access.c @@ -39,8 +39,10 @@ #include #include +#include #include "mesh_access.h" #include "btstack_memory.h" +#include "btstack_debug.h" #include "bluetooth_company_id.h" static mesh_element_t primary_element; @@ -142,3 +144,348 @@ mesh_model_t * mesh_model_get_by_identifier(mesh_element_t * element, uint32_t m } return NULL; } + +uint16_t mesh_pdu_src(mesh_pdu_t * pdu){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return mesh_transport_src((mesh_transport_pdu_t*) pdu); + case MESH_PDU_TYPE_NETWORK: + return mesh_network_src((mesh_network_pdu_t *) pdu); + default: + return MESH_ADDRESS_UNSASSIGNED; + } +} + +uint16_t mesh_pdu_dst(mesh_pdu_t * pdu){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return mesh_transport_dst((mesh_transport_pdu_t*) pdu); + case MESH_PDU_TYPE_NETWORK: + return mesh_network_dst((mesh_network_pdu_t *) pdu); + default: + return MESH_ADDRESS_UNSASSIGNED; + } +} + +uint16_t mesh_pdu_netkey_index(mesh_pdu_t * pdu){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return ((mesh_transport_pdu_t*) pdu)->netkey_index; + case MESH_PDU_TYPE_NETWORK: + return ((mesh_network_pdu_t *) pdu)->netkey_index; + default: + return 0; + } +} + +uint16_t mesh_pdu_len(mesh_pdu_t * pdu){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return ((mesh_transport_pdu_t*) pdu)->len; + case MESH_PDU_TYPE_NETWORK: + return ((mesh_network_pdu_t *) pdu)->len - 10; + default: + return 0; + } +} + +uint8_t * mesh_pdu_data(mesh_pdu_t * pdu){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return ((mesh_transport_pdu_t*) pdu)->data; + case MESH_PDU_TYPE_NETWORK: + return &((mesh_network_pdu_t *) pdu)->data[10]; + default: + return NULL; + } +} + +// message parser + +static int mesh_access_get_opcode(uint8_t * buffer, uint16_t buffer_size, uint32_t * opcode, uint16_t * opcode_size){ + switch (buffer[0] >> 6){ + case 0: + case 1: + if (buffer[0] == 0x7f) return 0; + *opcode = buffer[0]; + *opcode_size = 1; + return 1; + case 2: + if (buffer_size < 2) return 0; + *opcode = big_endian_read_16(buffer, 0); + *opcode_size = 2; + return 1; + case 3: + if (buffer_size < 3) return 0; + *opcode = (buffer[0] << 16) | little_endian_read_16(buffer, 1); + *opcode_size = 3; + return 1; + default: + return 0; + } +} + +static int mesh_access_transport_get_opcode(mesh_transport_pdu_t * transport_pdu, uint32_t * opcode, uint16_t * opcode_size){ + return mesh_access_get_opcode(transport_pdu->data, transport_pdu->len, opcode, opcode_size); +} + +static int mesh_access_network_get_opcode(mesh_network_pdu_t * network_pdu, uint32_t * opcode, uint16_t * opcode_size){ + // TransMIC already removed by mesh_upper_transport_validate_unsegmented_message_ccm + return mesh_access_get_opcode(&network_pdu->data[10], network_pdu->len - 10, opcode, opcode_size); +} + +int mesh_access_pdu_get_opcode(mesh_pdu_t * pdu, uint32_t * opcode, uint16_t * opcode_size){ + switch (pdu->pdu_type){ + case MESH_PDU_TYPE_TRANSPORT: + return mesh_access_transport_get_opcode((mesh_transport_pdu_t*) pdu, opcode, opcode_size); + case MESH_PDU_TYPE_NETWORK: + return mesh_access_network_get_opcode((mesh_network_pdu_t *) pdu, opcode, opcode_size); + default: + return 0; + } +} + +void mesh_access_parser_skip(mesh_access_parser_state_t * state, uint16_t bytes_to_skip){ + state->data += bytes_to_skip; + state->len -= bytes_to_skip; +} + +int mesh_access_parser_init(mesh_access_parser_state_t * state, mesh_pdu_t * pdu){ + state->data = mesh_pdu_data(pdu); + state->len = mesh_pdu_len(pdu); + + uint16_t opcode_size = 0; + int ok = mesh_access_get_opcode(state->data, state->len, &state->opcode, &opcode_size); + if (ok){ + mesh_access_parser_skip(state, opcode_size); + } + return ok; +} + +uint16_t mesh_access_parser_available(mesh_access_parser_state_t * state){ + return state->len; +} + +uint8_t mesh_access_parser_get_u8(mesh_access_parser_state_t * state){ + uint8_t value = *state->data; + mesh_access_parser_skip(state, 1); + return value; +} + +uint16_t mesh_access_parser_get_u16(mesh_access_parser_state_t * state){ + uint16_t value = little_endian_read_16(state->data, 0); + mesh_access_parser_skip(state, 2); + return value; +} + +uint32_t mesh_access_parser_get_u24(mesh_access_parser_state_t * state){ + uint32_t value = little_endian_read_24(state->data, 0); + mesh_access_parser_skip(state, 3); + return value; +} + +uint32_t mesh_access_parser_get_u32(mesh_access_parser_state_t * state){ + uint32_t value = little_endian_read_24(state->data, 0); + mesh_access_parser_skip(state, 4); + return value; +} + +void mesh_access_parser_get_u128(mesh_access_parser_state_t * state, uint8_t * dest){ + reverse_128( state->data, dest); + mesh_access_parser_skip(state, 16); +} + +void mesh_access_parser_get_label_uuid(mesh_access_parser_state_t * state, uint8_t * dest){ + memcpy( dest, state->data, 16); + mesh_access_parser_skip(state, 16); +} + +void mesh_access_parser_get_key(mesh_access_parser_state_t * state, uint8_t * dest){ + memcpy( dest, state->data, 16); + mesh_access_parser_skip(state, 16); +} + +uint32_t mesh_access_parser_get_model_identifier(mesh_access_parser_state_t * parser){ + if (mesh_access_parser_available(parser) == 4){ + return mesh_access_parser_get_u32(parser); + } else { + return (BLUETOOTH_COMPANY_ID_BLUETOOTH_SIG_INC << 16) | mesh_access_parser_get_u16(parser); + } +} + +// Mesh Access Message Builder + +// message builder + +static int mesh_access_setup_opcode(uint8_t * buffer, uint32_t opcode){ + if (opcode < 0x100){ + buffer[0] = opcode; + return 1; + } + if (opcode < 0x10000){ + big_endian_store_16(buffer, 0, opcode); + return 2; + } + buffer[0] = opcode >> 16; + little_endian_store_16(buffer, 1, opcode & 0xffff); + return 3; +} + +mesh_transport_pdu_t * mesh_access_transport_init(uint32_t opcode){ + mesh_transport_pdu_t * pdu = mesh_transport_pdu_get(); + if (!pdu) return NULL; + + pdu->len = mesh_access_setup_opcode(pdu->data, opcode); + return pdu; +} + +void mesh_access_transport_add_uint8(mesh_transport_pdu_t * pdu, uint8_t value){ + pdu->data[pdu->len++] = value; +} + +void mesh_access_transport_add_uint16(mesh_transport_pdu_t * pdu, uint16_t value){ + little_endian_store_16(pdu->data, pdu->len, value); + pdu->len += 2; +} + +void mesh_access_transport_add_uint24(mesh_transport_pdu_t * pdu, uint32_t value){ + little_endian_store_24(pdu->data, pdu->len, value); + pdu->len += 3; +} + +void mesh_access_transport_add_uint32(mesh_transport_pdu_t * pdu, uint32_t value){ + little_endian_store_32(pdu->data, pdu->len, value); + pdu->len += 4; +} +void mesh_access_transport_add_model_identifier(mesh_transport_pdu_t * pdu, uint32_t model_identifier){ + if (mesh_model_is_bluetooth_sig(model_identifier)){ + mesh_access_transport_add_uint16( pdu, mesh_model_get_model_id(model_identifier) ); + } else { + mesh_access_transport_add_uint32( pdu, model_identifier ); + } +} + +mesh_network_pdu_t * mesh_access_network_init(uint32_t opcode){ + mesh_network_pdu_t * pdu = mesh_network_pdu_get(); + if (!pdu) return NULL; + + pdu->len = mesh_access_setup_opcode(&pdu->data[10], opcode) + 10; + return pdu; +} + +void mesh_access_network_add_uint8(mesh_network_pdu_t * pdu, uint8_t value){ + pdu->data[pdu->len++] = value; +} + +void mesh_access_network_add_uint16(mesh_network_pdu_t * pdu, uint16_t value){ + little_endian_store_16(pdu->data, pdu->len, value); + pdu->len += 2; +} + +void mesh_access_network_add_uint24(mesh_network_pdu_t * pdu, uint16_t value){ + little_endian_store_24(pdu->data, pdu->len, value); + pdu->len += 3; +} + +void mesh_access_network_add_uint32(mesh_network_pdu_t * pdu, uint16_t value){ + little_endian_store_32(pdu->data, pdu->len, value); + pdu->len += 4; +} + +void mesh_access_network_add_model_identifier(mesh_network_pdu_t * pdu, uint32_t model_identifier){ + if (mesh_model_is_bluetooth_sig(model_identifier)){ + mesh_access_network_add_uint16( pdu, mesh_model_get_model_id(model_identifier) ); + } else { + mesh_access_network_add_uint32( pdu, model_identifier ); + } +} + +// access message template + +mesh_network_pdu_t * mesh_access_setup_unsegmented_message(const mesh_access_message_t *template, ...){ + mesh_network_pdu_t * network_pdu = mesh_access_network_init(template->opcode); + if (!network_pdu) return NULL; + + va_list argptr; + va_start(argptr, template); + + // add params + const char * format = template->format; + uint16_t word; + uint32_t longword; + while (*format){ + switch (*format){ + case '1': + word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs + mesh_access_network_add_uint8( network_pdu, word); + break; + case '2': + word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs + mesh_access_network_add_uint16( network_pdu, word); + break; + case '3': + longword = va_arg(argptr, uint32_t); + mesh_access_network_add_uint24( network_pdu, longword); + break; + case '4': + longword = va_arg(argptr, uint32_t); + mesh_access_network_add_uint32( network_pdu, longword); + break; + case 'm': + longword = va_arg(argptr, uint32_t); + mesh_access_network_add_model_identifier( network_pdu, longword); + break; + default: + log_error("Unsupported mesh message format specifier '%c", *format); + break; + } + format++; + } + + va_end(argptr); + + return network_pdu; +} + +mesh_transport_pdu_t * mesh_access_setup_segmented_message(const mesh_access_message_t *template, ...){ + mesh_transport_pdu_t * transport_pdu = mesh_access_transport_init(template->opcode); + if (!transport_pdu) return NULL; + + va_list argptr; + va_start(argptr, template); + + // add params + const char * format = template->format; + uint16_t word; + uint32_t longword; + while (*format){ + switch (*format++){ + case '1': + word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs + mesh_access_transport_add_uint8( transport_pdu, word); + break; + case '2': + word = va_arg(argptr, int); // minimal va_arg is int: 2 bytes on 8+16 bit CPUs + mesh_access_transport_add_uint16( transport_pdu, word); + break; + case '3': + longword = va_arg(argptr, uint32_t); + mesh_access_transport_add_uint24( transport_pdu, longword); + break; + case '4': + longword = va_arg(argptr, uint32_t); + mesh_access_transport_add_uint32( transport_pdu, longword); + break; + case 'm': + longword = va_arg(argptr, uint32_t); + mesh_access_transport_add_model_identifier( transport_pdu, longword); + break; + default: + break; + } + } + + va_end(argptr); + + return transport_pdu; +} diff --git a/test/mesh/mesh_access.h b/test/mesh/mesh_access.h index 194157319..cd52e0cfd 100644 --- a/test/mesh/mesh_access.h +++ b/test/mesh/mesh_access.h @@ -121,6 +121,16 @@ typedef struct { } mesh_element_t; +typedef struct { + uint32_t opcode; + uint8_t * data; + uint16_t len; +} mesh_access_parser_state_t; + +typedef struct { + uint32_t opcode; + const char * format; +} mesh_access_message_t; /** * @brief Init access layer @@ -163,6 +173,49 @@ uint16_t mesh_model_get_model_id(uint32_t model_identifier); uint32_t mesh_model_get_model_identifier(uint16_t vendor_id, uint16_t model_id); +// Mesh PDU Getter +uint16_t mesh_pdu_src(mesh_pdu_t * pdu); +uint16_t mesh_pdu_dst(mesh_pdu_t * pdu); +uint16_t mesh_pdu_netkey_index(mesh_pdu_t * pdu); +uint16_t mesh_pdu_len(mesh_pdu_t * pdu); +uint8_t * mesh_pdu_data(mesh_pdu_t * pdu); + +// Mesh Access Parser +int mesh_access_pdu_get_opcode(mesh_pdu_t * pdu, uint32_t * opcode, uint16_t * opcode_size); +int mesh_access_parser_init(mesh_access_parser_state_t * state, mesh_pdu_t * pdu); +void mesh_access_parser_skip(mesh_access_parser_state_t * state, uint16_t bytes_to_skip); +uint16_t mesh_access_parser_available(mesh_access_parser_state_t * state); +uint8_t mesh_access_parser_get_u8(mesh_access_parser_state_t * state); +uint16_t mesh_access_parser_get_u16(mesh_access_parser_state_t * state); +uint32_t mesh_access_parser_get_u24(mesh_access_parser_state_t * state); +uint32_t mesh_access_parser_get_u32(mesh_access_parser_state_t * state); +void mesh_access_parser_get_u128(mesh_access_parser_state_t * state, uint8_t * dest); +void mesh_access_parser_get_label_uuid(mesh_access_parser_state_t * state, uint8_t * dest); +void mesh_access_parser_get_key(mesh_access_parser_state_t * state, uint8_t * dest); +uint32_t mesh_access_parser_get_model_identifier(mesh_access_parser_state_t * parser); + + +// message builder transport +mesh_transport_pdu_t * mesh_access_transport_init(uint32_t opcode); +void mesh_access_transport_add_uint8(mesh_transport_pdu_t * pdu, uint8_t value); +void mesh_access_transport_add_uint16(mesh_transport_pdu_t * pdu, uint16_t value); +void mesh_access_transport_add_uint24(mesh_transport_pdu_t * pdu, uint32_t value); +void mesh_access_transport_add_uint32(mesh_transport_pdu_t * pdu, uint32_t value); +void mesh_access_transport_add_model_identifier(mesh_transport_pdu_t * pdu, uint32_t model_identifier); + +// message builder network +mesh_network_pdu_t * mesh_access_network_init(uint32_t opcode); +void mesh_access_network_add_uint8(mesh_network_pdu_t * pdu, uint8_t value); +void mesh_access_network_add_uint16(mesh_network_pdu_t * pdu, uint16_t value); +void mesh_access_network_add_uint24(mesh_network_pdu_t * pdu, uint16_t value); +void mesh_access_network_add_uint32(mesh_network_pdu_t * pdu, uint16_t value); +void mesh_access_network_add_model_identifier(mesh_network_pdu_t * pdu, uint32_t model_identifier); + +// message builder using template +mesh_network_pdu_t * mesh_access_setup_unsegmented_message(const mesh_access_message_t *template, ...); +mesh_transport_pdu_t * mesh_access_setup_segmented_message(const mesh_access_message_t *template, ...); + + #ifdef __cplusplus } /* end of extern "C" */ #endif