From 7f17076cc2f908f3b6df1653b58a9b7c253aedb2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krzysztof=20Kopy=C5=9Bci=C5=84ski?= Date: Mon, 5 Jun 2023 13:30:38 +0200 Subject: [PATCH] apps: bttester: make bttester code compliant with Mynewt code style bttester was initially ported from Zephyr. This app is independent now and should follow Mynewt Coding Standards. --- apps/bttester/src/atomic.h | 276 +-- apps/bttester/src/bttester.c | 489 ++--- apps/bttester/src/bttester.h | 1347 ++++++------- apps/bttester/src/bttester_pipe.h | 12 +- apps/bttester/src/gap.c | 2532 +++++++++++++------------ apps/bttester/src/gatt.c | 2952 +++++++++++++++-------------- apps/bttester/src/gatt_cl.c | 2530 ++++++++++++------------ apps/bttester/src/glue.c | 88 +- apps/bttester/src/glue.h | 23 +- apps/bttester/src/l2cap.c | 1044 +++++----- apps/bttester/src/main.c | 45 +- apps/bttester/src/mesh.c | 1239 ++++++------ apps/bttester/src/uart_pipe.c | 60 +- 13 files changed, 6490 insertions(+), 6147 deletions(-) diff --git a/apps/bttester/src/atomic.h b/apps/bttester/src/atomic.h index 66283e9ac..4376ad5bb 100644 --- a/apps/bttester/src/atomic.h +++ b/apps/bttester/src/atomic.h @@ -55,12 +55,13 @@ typedef atomic_t atomic_val_t; * @param new_value New value to store. * @return 1 if @a new_value is written, 0 otherwise. */ -static inline int atomic_cas(atomic_t *target, atomic_val_t old_value, - atomic_val_t new_value) +static inline int +atomic_cas(atomic_t *target, atomic_val_t old_value, + atomic_val_t new_value) { return __atomic_compare_exchange_n(target, &old_value, new_value, - 0, __ATOMIC_SEQ_CST, - __ATOMIC_SEQ_CST); + 0, __ATOMIC_SEQ_CST, + __ATOMIC_SEQ_CST); } /** @@ -74,7 +75,8 @@ static inline int atomic_cas(atomic_t *target, atomic_val_t old_value, * * @return Previous value of @a target. */ -static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_add(atomic_t *target, atomic_val_t value) { return __atomic_fetch_add(target, value, __ATOMIC_SEQ_CST); } @@ -91,7 +93,8 @@ static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_sub(atomic_t *target, atomic_val_t value) { return __atomic_fetch_sub(target, value, __ATOMIC_SEQ_CST); } @@ -107,7 +110,8 @@ static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_inc(atomic_t *target) +static inline atomic_val_t +atomic_inc(atomic_t *target) { return atomic_add(target, 1); } @@ -123,7 +127,8 @@ static inline atomic_val_t atomic_inc(atomic_t *target) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_dec(atomic_t *target) +static inline atomic_val_t +atomic_dec(atomic_t *target) { return atomic_sub(target, 1); } @@ -139,7 +144,8 @@ static inline atomic_val_t atomic_dec(atomic_t *target) * @return Value of @a target. */ -static inline atomic_val_t atomic_get(const atomic_t *target) +static inline atomic_val_t +atomic_get(const atomic_t *target) { return __atomic_load_n(target, __ATOMIC_SEQ_CST); } @@ -157,7 +163,8 @@ static inline atomic_val_t atomic_get(const atomic_t *target) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_set(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_set(atomic_t *target, atomic_val_t value) { /* This builtin, as described by Intel, is not a traditional * test-and-set operation, but rather an atomic exchange operation. It @@ -178,7 +185,8 @@ static inline atomic_val_t atomic_set(atomic_t *target, atomic_val_t value) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_clear(atomic_t *target) +static inline atomic_val_t +atomic_clear(atomic_t *target) { return atomic_set(target, 0); } @@ -196,7 +204,8 @@ static inline atomic_val_t atomic_clear(atomic_t *target) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_or(atomic_t *target, atomic_val_t value) { return __atomic_fetch_or(target, value, __ATOMIC_SEQ_CST); } @@ -214,7 +223,8 @@ static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_xor(atomic_t *target, atomic_val_t value) { return __atomic_fetch_xor(target, value, __ATOMIC_SEQ_CST); } @@ -232,7 +242,8 @@ static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_and(atomic_t *target, atomic_val_t value) { return __atomic_fetch_and(target, value, __ATOMIC_SEQ_CST); } @@ -250,149 +261,150 @@ static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value) * @return Previous value of @a target. */ -static inline atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value) +static inline atomic_val_t +atomic_nand(atomic_t *target, atomic_val_t value) { return __atomic_fetch_nand(target, value, __ATOMIC_SEQ_CST); } - /** - * @brief Initialize an atomic variable. - * - * This macro can be used to initialize an atomic variable. For example, - * @code atomic_t my_var = ATOMIC_INIT(75); @endcode - * - * @param i Value to assign to atomic variable. - */ +/** + * @brief Initialize an atomic variable. + * + * This macro can be used to initialize an atomic variable. For example, + * @code atomic_t my_var = ATOMIC_INIT(75); @endcode + * + * @param i Value to assign to atomic variable. + */ #define ATOMIC_INIT(i) (i) - /** - * @cond INTERNAL_HIDDEN - */ +/** + * @cond INTERNAL_HIDDEN + */ #define ATOMIC_BITS (sizeof(atomic_val_t) * 8) #define ATOMIC_MASK(bit) (1 << ((bit) & (ATOMIC_BITS - 1))) #define ATOMIC_ELEM(addr, bit) ((addr) + ((bit) / ATOMIC_BITS)) - /** - * INTERNAL_HIDDEN @endcond - */ +/** + * INTERNAL_HIDDEN @endcond + */ - /** - * @brief Define an array of atomic variables. - * - * This macro defines an array of atomic variables containing at least - * @a num_bits bits. - * - * @note - * If used from file scope, the bits of the array are initialized to zero; - * if used from within a function, the bits are left uninitialized. - * - * @param name Name of array of atomic variables. - * @param num_bits Number of bits needed. - */ +/** + * @brief Define an array of atomic variables. + * + * This macro defines an array of atomic variables containing at least + * @a num_bits bits. + * + * @note + * If used from file scope, the bits of the array are initialized to zero; + * if used from within a function, the bits are left uninitialized. + * + * @param name Name of array of atomic variables. + * @param num_bits Number of bits needed. + */ #define ATOMIC_DEFINE(name, num_bits) \ - atomic_t name[1 + ((num_bits) - 1) / ATOMIC_BITS] + atomic_t name[1 + ((num_bits) - 1) / ATOMIC_BITS] - /** - * @brief Atomically test a bit. - * - * This routine tests whether bit number @a bit of @a target is set or not. - * The target may be a single atomic variable or an array of them. - * - * @param target Address of atomic variable or array. - * @param bit Bit number (starting from 0). - * - * @return 1 if the bit was set, 0 if it wasn't. - */ - static inline int - atomic_test_bit(const atomic_t *target, int bit) - { - atomic_val_t val = atomic_get(ATOMIC_ELEM(target, bit)); +/** + * @brief Atomically test a bit. + * + * This routine tests whether bit number @a bit of @a target is set or not. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return 1 if the bit was set, 0 if it wasn't. + */ +static inline int +atomic_test_bit(const atomic_t *target, int bit) +{ + atomic_val_t val = atomic_get(ATOMIC_ELEM(target, bit)); - return (1 & (val >> (bit & (ATOMIC_BITS - 1)))); - } + return (1 & (val >> (bit & (ATOMIC_BITS - 1)))); +} - /** - * @brief Atomically test and clear a bit. - * - * Atomically clear bit number @a bit of @a target and return its old value. - * The target may be a single atomic variable or an array of them. - * - * @param target Address of atomic variable or array. - * @param bit Bit number (starting from 0). - * - * @return 1 if the bit was set, 0 if it wasn't. - */ - static inline int - atomic_test_and_clear_bit(atomic_t *target, int bit) - { - atomic_val_t mask = ATOMIC_MASK(bit); - atomic_val_t old; +/** + * @brief Atomically test and clear a bit. + * + * Atomically clear bit number @a bit of @a target and return its old value. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return 1 if the bit was set, 0 if it wasn't. + */ +static inline int +atomic_test_and_clear_bit(atomic_t *target, int bit) +{ + atomic_val_t mask = ATOMIC_MASK(bit); + atomic_val_t old; - old = atomic_and(ATOMIC_ELEM(target, bit), ~mask); + old = atomic_and(ATOMIC_ELEM(target, bit), ~mask); - return (old & mask) != 0; - } + return (old & mask) != 0; +} - /** - * @brief Atomically set a bit. - * - * Atomically set bit number @a bit of @a target and return its old value. - * The target may be a single atomic variable or an array of them. - * - * @param target Address of atomic variable or array. - * @param bit Bit number (starting from 0). - * - * @return 1 if the bit was set, 0 if it wasn't. - */ - static inline int - atomic_test_and_set_bit(atomic_t *target, int bit) - { - atomic_val_t mask = ATOMIC_MASK(bit); - atomic_val_t old; +/** + * @brief Atomically set a bit. + * + * Atomically set bit number @a bit of @a target and return its old value. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return 1 if the bit was set, 0 if it wasn't. + */ +static inline int +atomic_test_and_set_bit(atomic_t *target, int bit) +{ + atomic_val_t mask = ATOMIC_MASK(bit); + atomic_val_t old; - old = atomic_or(ATOMIC_ELEM(target, bit), mask); + old = atomic_or(ATOMIC_ELEM(target, bit), mask); - return (old & mask) != 0; - } + return (old & mask) != 0; +} - /** - * @brief Atomically clear a bit. - * - * Atomically clear bit number @a bit of @a target. - * The target may be a single atomic variable or an array of them. - * - * @param target Address of atomic variable or array. - * @param bit Bit number (starting from 0). - * - * @return N/A - */ - static inline void - atomic_clear_bit(atomic_t *target, int bit) - { - atomic_val_t mask = ATOMIC_MASK(bit); +/** + * @brief Atomically clear a bit. + * + * Atomically clear bit number @a bit of @a target. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return N/A + */ +static inline void +atomic_clear_bit(atomic_t *target, int bit) +{ + atomic_val_t mask = ATOMIC_MASK(bit); - atomic_and(ATOMIC_ELEM(target, bit), ~mask); - } + atomic_and(ATOMIC_ELEM(target, bit), ~mask); +} - /** - * @brief Atomically set a bit. - * - * Atomically set bit number @a bit of @a target. - * The target may be a single atomic variable or an array of them. - * - * @param target Address of atomic variable or array. - * @param bit Bit number (starting from 0). - * - * @return N/A - */ - static inline void - atomic_set_bit(atomic_t *target, int bit) - { - atomic_val_t mask = ATOMIC_MASK(bit); +/** + * @brief Atomically set a bit. + * + * Atomically set bit number @a bit of @a target. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return N/A + */ +static inline void +atomic_set_bit(atomic_t *target, int bit) +{ + atomic_val_t mask = ATOMIC_MASK(bit); - atomic_or(ATOMIC_ELEM(target, bit), mask); - } + atomic_or(ATOMIC_ELEM(target, bit), mask); +} /** * @} diff --git a/apps/bttester/src/bttester.c b/apps/bttester/src/bttester.c index 76aebbf1e..62219f565 100644 --- a/apps/bttester/src/bttester.c +++ b/apps/bttester/src/bttester.c @@ -42,338 +42,351 @@ static struct os_eventq *cmds_queue; static struct os_event bttester_ev[CMD_QUEUED]; struct btp_buf { - struct os_event *ev; - union { - uint8_t data[BTP_MTU]; - struct btp_hdr hdr; - }; + struct os_event *ev; + union { + uint8_t data[BTP_MTU]; + struct btp_hdr hdr; + }; }; static struct btp_buf cmd_buf[CMD_QUEUED]; -static void supported_commands(uint8_t *data, uint16_t len) +static void +supported_commands(uint8_t *data, uint16_t len) { - uint8_t buf[1]; - struct core_read_supported_commands_rp *rp = (void *) buf; + uint8_t buf[1]; + struct core_read_supported_commands_rp *rp = (void *) buf; - memset(buf, 0, sizeof(buf)); + memset(buf, 0, sizeof(buf)); - tester_set_bit(buf, CORE_READ_SUPPORTED_COMMANDS); - tester_set_bit(buf, CORE_READ_SUPPORTED_SERVICES); - tester_set_bit(buf, CORE_REGISTER_SERVICE); - tester_set_bit(buf, CORE_UNREGISTER_SERVICE); + tester_set_bit(buf, CORE_READ_SUPPORTED_COMMANDS); + tester_set_bit(buf, CORE_READ_SUPPORTED_SERVICES); + tester_set_bit(buf, CORE_REGISTER_SERVICE); + tester_set_bit(buf, CORE_UNREGISTER_SERVICE); - tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_COMMANDS, - BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf)); + tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_COMMANDS, + BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf)); } -static void supported_services(uint8_t *data, uint16_t len) +static void +supported_services(uint8_t *data, uint16_t len) { - uint8_t buf[1]; - struct core_read_supported_services_rp *rp = (void *) buf; + uint8_t buf[1]; + struct core_read_supported_services_rp *rp = (void *) buf; - memset(buf, 0, sizeof(buf)); + memset(buf, 0, sizeof(buf)); - tester_set_bit(buf, BTP_SERVICE_ID_CORE); - tester_set_bit(buf, BTP_SERVICE_ID_GAP); - tester_set_bit(buf, BTP_SERVICE_ID_GATT); + tester_set_bit(buf, BTP_SERVICE_ID_CORE); + tester_set_bit(buf, BTP_SERVICE_ID_GAP); + tester_set_bit(buf, BTP_SERVICE_ID_GATT); #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) - tester_set_bit(buf, BTP_SERVICE_ID_L2CAP); + tester_set_bit(buf, BTP_SERVICE_ID_L2CAP); #endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */ #if MYNEWT_VAL(BLE_MESH) - tester_set_bit(buf, BTP_SERVICE_ID_MESH); + tester_set_bit(buf, BTP_SERVICE_ID_MESH); #endif /* MYNEWT_VAL(BLE_MESH) */ - tester_set_bit(buf, BTP_SERVICE_ID_GATTC); + tester_set_bit(buf, BTP_SERVICE_ID_GATTC); - tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_SERVICES, - BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf)); + tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_SERVICES, + BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf)); } -static void register_service(uint8_t *data, uint16_t len) +static void +register_service(uint8_t *data, uint16_t len) { - struct core_register_service_cmd *cmd = (void *) data; - uint8_t status; + struct core_register_service_cmd *cmd = (void *) data; + uint8_t status; - switch (cmd->id) { - case BTP_SERVICE_ID_GAP: - status = tester_init_gap(); - /* Rsp with success status will be handled by bt enable cb */ - if (status == BTP_STATUS_FAILED) { - goto rsp; - } - return; - case BTP_SERVICE_ID_GATT: - status = tester_init_gatt(); - break; + switch (cmd->id) { + case BTP_SERVICE_ID_GAP: + status = tester_init_gap(); + /* Rsp with success status will be handled by bt enable cb */ + if (status == BTP_STATUS_FAILED) { + goto rsp; + } + return; + case BTP_SERVICE_ID_GATT: + status = tester_init_gatt(); + break; #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) - case BTP_SERVICE_ID_L2CAP: - status = tester_init_l2cap(); - break; + case BTP_SERVICE_ID_L2CAP: + status = tester_init_l2cap(); + break; #endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */ #if MYNEWT_VAL(BLE_MESH) - case BTP_SERVICE_ID_MESH: - status = tester_init_mesh(); - break; + case BTP_SERVICE_ID_MESH: + status = tester_init_mesh(); + break; #endif /* MYNEWT_VAL(BLE_MESH) */ - default: - status = BTP_STATUS_FAILED; - break; - } + default: + status = BTP_STATUS_FAILED; + break; + } rsp: - tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE, - status); + tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE, + status); } -static void unregister_service(uint8_t *data, uint16_t len) +static void +unregister_service(uint8_t *data, uint16_t len) { - struct core_unregister_service_cmd *cmd = (void *) data; - uint8_t status; + struct core_unregister_service_cmd *cmd = (void *) data; + uint8_t status; - switch (cmd->id) { - case BTP_SERVICE_ID_GAP: - status = tester_unregister_gap(); - break; - case BTP_SERVICE_ID_GATT: - status = tester_unregister_gatt(); - break; + switch (cmd->id) { + case BTP_SERVICE_ID_GAP: + status = tester_unregister_gap(); + break; + case BTP_SERVICE_ID_GATT: + status = tester_unregister_gatt(); + break; #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) - case BTP_SERVICE_ID_L2CAP: - status = tester_unregister_l2cap(); - break; + case BTP_SERVICE_ID_L2CAP: + status = tester_unregister_l2cap(); + break; #endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */ #if MYNEWT_VAL(BLE_MESH) - case BTP_SERVICE_ID_MESH: - status = tester_unregister_mesh(); - break; + case BTP_SERVICE_ID_MESH: + status = tester_unregister_mesh(); + break; #endif /* MYNEWT_VAL(BLE_MESH) */ - default: - status = BTP_STATUS_FAILED; - break; - } + default: + status = BTP_STATUS_FAILED; + break; + } - tester_rsp(BTP_SERVICE_ID_CORE, CORE_UNREGISTER_SERVICE, BTP_INDEX_NONE, - status); + tester_rsp(BTP_SERVICE_ID_CORE, CORE_UNREGISTER_SERVICE, BTP_INDEX_NONE, + status); } -static void handle_core(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len) +static void +handle_core(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len) { - if (index != BTP_INDEX_NONE) { - tester_rsp(BTP_SERVICE_ID_CORE, opcode, index, - BTP_STATUS_FAILED); - return; - } + if (index != BTP_INDEX_NONE) { + tester_rsp(BTP_SERVICE_ID_CORE, opcode, index, + BTP_STATUS_FAILED); + return; + } - switch (opcode) { - case CORE_READ_SUPPORTED_COMMANDS: - supported_commands(data, len); - return; - case CORE_READ_SUPPORTED_SERVICES: - supported_services(data, len); - return; - case CORE_REGISTER_SERVICE: - register_service(data, len); - return; - case CORE_UNREGISTER_SERVICE: - unregister_service(data, len); - return; - default: - tester_rsp(BTP_SERVICE_ID_CORE, opcode, BTP_INDEX_NONE, - BTP_STATUS_UNKNOWN_CMD); - return; - } + switch (opcode) { + case CORE_READ_SUPPORTED_COMMANDS: + supported_commands(data, len); + return; + case CORE_READ_SUPPORTED_SERVICES: + supported_services(data, len); + return; + case CORE_REGISTER_SERVICE: + register_service(data, len); + return; + case CORE_UNREGISTER_SERVICE: + unregister_service(data, len); + return; + default: + tester_rsp(BTP_SERVICE_ID_CORE, opcode, BTP_INDEX_NONE, + BTP_STATUS_UNKNOWN_CMD); + return; + } } -static void cmd_handler(struct os_event *ev) +static void +cmd_handler(struct os_event *ev) { - uint16_t len; - struct btp_buf *cmd; + uint16_t len; + struct btp_buf *cmd; - if (!ev || !ev->ev_arg) { - return; - } + if (!ev || !ev->ev_arg) { + return; + } - cmd = ev->ev_arg; + cmd = ev->ev_arg; - len = sys_le16_to_cpu(cmd->hdr.len); - if (MYNEWT_VAL(BTTESTER_BTP_LOG)) { - console_printf("[DBG] received %d bytes: %s\n", - sizeof(cmd->hdr) + len, - bt_hex(cmd->data, - sizeof(cmd->hdr) + len)); - } + len = sys_le16_to_cpu(cmd->hdr.len); + if (MYNEWT_VAL(BTTESTER_BTP_LOG)) { + console_printf("[DBG] received %d bytes: %s\n", + sizeof(cmd->hdr) + len, + bt_hex(cmd->data, + sizeof(cmd->hdr) + len)); + } - /* TODO - * verify if service is registered before calling handler - */ + /* TODO + * verify if service is registered before calling handler + */ - switch (cmd->hdr.service) { - case BTP_SERVICE_ID_CORE: - handle_core(cmd->hdr.opcode, cmd->hdr.index, - cmd->hdr.data, len); - break; - case BTP_SERVICE_ID_GAP: - tester_handle_gap(cmd->hdr.opcode, cmd->hdr.index, - cmd->hdr.data, len); - break; - case BTP_SERVICE_ID_GATT: - tester_handle_gatt(cmd->hdr.opcode, cmd->hdr.index, - cmd->hdr.data, len); - break; + switch (cmd->hdr.service) { + case BTP_SERVICE_ID_CORE: + handle_core(cmd->hdr.opcode, cmd->hdr.index, + cmd->hdr.data, len); + break; + case BTP_SERVICE_ID_GAP: + tester_handle_gap(cmd->hdr.opcode, cmd->hdr.index, + cmd->hdr.data, len); + break; + case BTP_SERVICE_ID_GATT: + tester_handle_gatt(cmd->hdr.opcode, cmd->hdr.index, + cmd->hdr.data, len); + break; #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) - case BTP_SERVICE_ID_L2CAP: - tester_handle_l2cap(cmd->hdr.opcode, cmd->hdr.index, - cmd->hdr.data, len); - break; + case BTP_SERVICE_ID_L2CAP: + tester_handle_l2cap(cmd->hdr.opcode, cmd->hdr.index, + cmd->hdr.data, len); + break; #endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */ #if MYNEWT_VAL(BLE_MESH) - case BTP_SERVICE_ID_MESH: - tester_handle_mesh(cmd->hdr.opcode, cmd->hdr.index, - cmd->hdr.data, len); - break; + case BTP_SERVICE_ID_MESH: + tester_handle_mesh(cmd->hdr.opcode, cmd->hdr.index, + cmd->hdr.data, len); + break; #endif /* MYNEWT_VAL(BLE_MESH) */ - case BTP_SERVICE_ID_GATTC: - tester_handle_gattc(cmd->hdr.opcode, cmd->hdr.index, - cmd->hdr.data, len); - break; - default: - tester_rsp(cmd->hdr.service, cmd->hdr.opcode, - cmd->hdr.index, BTP_STATUS_FAILED); - break; - } + case BTP_SERVICE_ID_GATTC: + tester_handle_gattc(cmd->hdr.opcode, cmd->hdr.index, + cmd->hdr.data, len); + break; + default: + tester_rsp(cmd->hdr.service, cmd->hdr.opcode, + cmd->hdr.index, BTP_STATUS_FAILED); + break; + } - os_eventq_put(&avail_queue, ev); + os_eventq_put(&avail_queue, ev); } -static uint8_t *recv_cb(uint8_t *buf, size_t *off) +static uint8_t * +recv_cb(uint8_t *buf, size_t *off) { - struct btp_hdr *cmd = (void *) buf; - struct os_event *new_ev; - struct btp_buf *new_buf, *old_buf; - uint16_t len; + struct btp_hdr *cmd = (void *) buf; + struct os_event *new_ev; + struct btp_buf *new_buf, *old_buf; + uint16_t len; - if (*off < sizeof(*cmd)) { - return buf; - } + if (*off < sizeof(*cmd)) { + return buf; + } - len = sys_le16_to_cpu(cmd->len); - if (len > BTP_MTU - sizeof(*cmd)) { - *off = 0; - return buf; - } + len = sys_le16_to_cpu(cmd->len); + if (len > BTP_MTU - sizeof(*cmd)) { + *off = 0; + return buf; + } - if (*off < sizeof(*cmd) + len) { - return buf; - } + if (*off < sizeof(*cmd) + len) { + return buf; + } - new_ev = os_eventq_get_no_wait(&avail_queue); - if (!new_ev) { - SYS_LOG_ERR("BT tester: RX overflow"); - *off = 0; - return buf; - } + new_ev = os_eventq_get_no_wait(&avail_queue); + if (!new_ev) { + SYS_LOG_ERR("BT tester: RX overflow"); + *off = 0; + return buf; + } - old_buf = CONTAINER_OF(buf, struct btp_buf, data); - os_eventq_put(cmds_queue, old_buf->ev); + old_buf = CONTAINER_OF(buf, struct btp_buf, data); + os_eventq_put(cmds_queue, old_buf->ev); - new_buf = new_ev->ev_arg; - *off = 0; - return new_buf->data; + new_buf = new_ev->ev_arg; + *off = 0; + return new_buf->data; } -static void avail_queue_init(void) +static void +avail_queue_init(void) { - int i; + int i; - os_eventq_init(&avail_queue); + os_eventq_init(&avail_queue); - for (i = 0; i < CMD_QUEUED; i++) { - cmd_buf[i].ev = &bttester_ev[i]; - bttester_ev[i].ev_cb = cmd_handler; - bttester_ev[i].ev_arg = &cmd_buf[i]; + for (i = 0; i < CMD_QUEUED; i++) { + cmd_buf[i].ev = &bttester_ev[i]; + bttester_ev[i].ev_cb = cmd_handler; + bttester_ev[i].ev_arg = &cmd_buf[i]; - os_eventq_put(&avail_queue, &bttester_ev[i]); - } + os_eventq_put(&avail_queue, &bttester_ev[i]); + } } -void bttester_evq_set(struct os_eventq *evq) +void +bttester_evq_set(struct os_eventq *evq) { - cmds_queue = evq; + cmds_queue = evq; } -void tester_init(void) +void +tester_init(void) { - struct os_event *ev; - struct btp_buf *buf; + struct os_event *ev; + struct btp_buf *buf; - avail_queue_init(); - bttester_evq_set(os_eventq_dflt_get()); + avail_queue_init(); + bttester_evq_set(os_eventq_dflt_get()); - ev = os_eventq_get(&avail_queue); - buf = ev->ev_arg; + ev = os_eventq_get(&avail_queue); + buf = ev->ev_arg; - if (bttester_pipe_init()) { - SYS_LOG_ERR("Failed to initialize pipe"); - return; - } + if (bttester_pipe_init()) { + SYS_LOG_ERR("Failed to initialize pipe"); + return; + } - bttester_pipe_register(buf->data, BTP_MTU, recv_cb); + bttester_pipe_register(buf->data, BTP_MTU, recv_cb); - tester_send(BTP_SERVICE_ID_CORE, CORE_EV_IUT_READY, BTP_INDEX_NONE, - NULL, 0); + tester_send(BTP_SERVICE_ID_CORE, CORE_EV_IUT_READY, BTP_INDEX_NONE, + NULL, 0); } -void tester_send(uint8_t service, uint8_t opcode, uint8_t index, uint8_t *data, - size_t len) +void +tester_send(uint8_t service, uint8_t opcode, uint8_t index, uint8_t *data, + size_t len) { - struct btp_hdr msg; + struct btp_hdr msg; - msg.service = service; - msg.opcode = opcode; - msg.index = index; - msg.len = len; + msg.service = service; + msg.opcode = opcode; + msg.index = index; + msg.len = len; - bttester_pipe_send((uint8_t *)&msg, sizeof(msg)); - if (data && len) { - bttester_pipe_send(data, len); - } + bttester_pipe_send((uint8_t *) &msg, sizeof(msg)); + if (data && len) { + bttester_pipe_send(data, len); + } - if (MYNEWT_VAL(BTTESTER_BTP_LOG)) { - console_printf("[DBG] send %d bytes hdr: %s\n", sizeof(msg), - bt_hex((char *) &msg, sizeof(msg))); - if (data && len) { - console_printf("[DBG] send %d bytes data: %s\n", len, - bt_hex((char *) data, len)); - } - } + if (MYNEWT_VAL(BTTESTER_BTP_LOG)) { + console_printf("[DBG] send %d bytes hdr: %s\n", sizeof(msg), + bt_hex((char *) &msg, sizeof(msg))); + if (data && len) { + console_printf("[DBG] send %d bytes data: %s\n", len, + bt_hex((char *) data, len)); + } + } } -void tester_send_buf(uint8_t service, uint8_t opcode, uint8_t index, - struct os_mbuf *data) +void +tester_send_buf(uint8_t service, uint8_t opcode, uint8_t index, + struct os_mbuf *data) { - struct btp_hdr msg; + struct btp_hdr msg; - msg.service = service; - msg.opcode = opcode; - msg.index = index; - msg.len = os_mbuf_len(data); + msg.service = service; + msg.opcode = opcode; + msg.index = index; + msg.len = os_mbuf_len(data); - bttester_pipe_send((uint8_t *)&msg, sizeof(msg)); - if (data && msg.len) { - bttester_pipe_send_buf(data); - } + bttester_pipe_send((uint8_t *) &msg, sizeof(msg)); + if (data && msg.len) { + bttester_pipe_send_buf(data); + } } -void tester_rsp(uint8_t service, uint8_t opcode, uint8_t index, uint8_t status) +void +tester_rsp(uint8_t service, uint8_t opcode, uint8_t index, uint8_t status) { - struct btp_status s; + struct btp_status s; - if (status == BTP_STATUS_SUCCESS) { - tester_send(service, opcode, index, NULL, 0); - return; - } + if (status == BTP_STATUS_SUCCESS) { + tester_send(service, opcode, index, NULL, 0); + return; + } - s.code = status; - tester_send(service, BTP_STATUS, index, (uint8_t *) &s, sizeof(s)); + s.code = status; + tester_send(service, BTP_STATUS, index, (uint8_t *) &s, sizeof(s)); } diff --git a/apps/bttester/src/bttester.h b/apps/bttester/src/bttester.h index 4aa34b3c2..87bb275b8 100644 --- a/apps/bttester/src/bttester.h +++ b/apps/bttester/src/bttester.h @@ -41,29 +41,29 @@ #define BTP_MTU MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX) #define BTP_DATA_MAX_SIZE (BTP_MTU - sizeof(struct btp_hdr)) -#define BTP_INDEX_NONE 0xff +#define BTP_INDEX_NONE 0xff -#define BTP_SERVICE_ID_CORE 0 -#define BTP_SERVICE_ID_GAP 1 -#define BTP_SERVICE_ID_GATT 2 -#define BTP_SERVICE_ID_L2CAP 3 -#define BTP_SERVICE_ID_MESH 4 -#define BTP_SERVICE_ID_GATTC 6 +#define BTP_SERVICE_ID_CORE 0 +#define BTP_SERVICE_ID_GAP 1 +#define BTP_SERVICE_ID_GATT 2 +#define BTP_SERVICE_ID_L2CAP 3 +#define BTP_SERVICE_ID_MESH 4 +#define BTP_SERVICE_ID_GATTC 6 -#define BTP_STATUS_SUCCESS 0x00 -#define BTP_STATUS_FAILED 0x01 -#define BTP_STATUS_UNKNOWN_CMD 0x02 -#define BTP_STATUS_NOT_READY 0x03 +#define BTP_STATUS_SUCCESS 0x00 +#define BTP_STATUS_FAILED 0x01 +#define BTP_STATUS_UNKNOWN_CMD 0x02 +#define BTP_STATUS_NOT_READY 0x03 #define SYS_LOG_DBG(fmt, ...) \ - if (MYNEWT_VAL(BTTESTER_DEBUG)) { \ - console_printf("[DBG] %s: " fmt "\n", \ - __func__, ## __VA_ARGS__); \ - } + if (MYNEWT_VAL(BTTESTER_DEBUG)) { \ + console_printf("[DBG] %s: " fmt "\n", \ + __func__, ## __VA_ARGS__); \ + } #define SYS_LOG_INF(fmt, ...) console_printf("[INF] %s: " fmt "\n", \ - __func__, ## __VA_ARGS__); + __func__, ## __VA_ARGS__); #define SYS_LOG_ERR(fmt, ...) console_printf("[WRN] %s: " fmt "\n", \ - __func__, ## __VA_ARGS__); + __func__, ## __VA_ARGS__); #define SYS_LOG_LEVEL SYS_LOG_LEVEL_DEBUG #define SYS_LOG_DOMAIN "bttester" @@ -73,217 +73,217 @@ #define sys_cpu_to_le16 htole16 struct btp_hdr { - uint8_t service; - uint8_t opcode; - uint8_t index; - uint16_t len; - uint8_t data[0]; + uint8_t service; + uint8_t opcode; + uint8_t index; + uint16_t len; + uint8_t data[0]; } __packed; -#define BTP_STATUS 0x00 +#define BTP_STATUS 0x00 struct btp_status { - uint8_t code; + uint8_t code; } __packed; /* Core Service */ -#define CORE_READ_SUPPORTED_COMMANDS 0x01 +#define CORE_READ_SUPPORTED_COMMANDS 0x01 struct core_read_supported_commands_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; -#define CORE_READ_SUPPORTED_SERVICES 0x02 +#define CORE_READ_SUPPORTED_SERVICES 0x02 struct core_read_supported_services_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; -#define CORE_REGISTER_SERVICE 0x03 +#define CORE_REGISTER_SERVICE 0x03 struct core_register_service_cmd { - uint8_t id; + uint8_t id; } __packed; -#define CORE_UNREGISTER_SERVICE 0x04 +#define CORE_UNREGISTER_SERVICE 0x04 struct core_unregister_service_cmd { - uint8_t id; + uint8_t id; } __packed; /* events */ -#define CORE_EV_IUT_READY 0x80 +#define CORE_EV_IUT_READY 0x80 /* GAP Service */ /* commands */ -#define GAP_READ_SUPPORTED_COMMANDS 0x01 +#define GAP_READ_SUPPORTED_COMMANDS 0x01 struct gap_read_supported_commands_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; -#define GAP_READ_CONTROLLER_INDEX_LIST 0x02 +#define GAP_READ_CONTROLLER_INDEX_LIST 0x02 struct gap_read_controller_index_list_rp { - uint8_t num; - uint8_t index[0]; + uint8_t num; + uint8_t index[0]; } __packed; -#define GAP_SETTINGS_POWERED 0 -#define GAP_SETTINGS_CONNECTABLE 1 -#define GAP_SETTINGS_FAST_CONNECTABLE 2 -#define GAP_SETTINGS_DISCOVERABLE 3 -#define GAP_SETTINGS_BONDABLE 4 -#define GAP_SETTINGS_LINK_SEC_3 5 -#define GAP_SETTINGS_SSP 6 -#define GAP_SETTINGS_BREDR 7 -#define GAP_SETTINGS_HS 8 -#define GAP_SETTINGS_LE 9 -#define GAP_SETTINGS_ADVERTISING 10 -#define GAP_SETTINGS_SC 11 -#define GAP_SETTINGS_DEBUG_KEYS 12 -#define GAP_SETTINGS_PRIVACY 13 -#define GAP_SETTINGS_CONTROLLER_CONFIG 14 -#define GAP_SETTINGS_STATIC_ADDRESS 15 +#define GAP_SETTINGS_POWERED 0 +#define GAP_SETTINGS_CONNECTABLE 1 +#define GAP_SETTINGS_FAST_CONNECTABLE 2 +#define GAP_SETTINGS_DISCOVERABLE 3 +#define GAP_SETTINGS_BONDABLE 4 +#define GAP_SETTINGS_LINK_SEC_3 5 +#define GAP_SETTINGS_SSP 6 +#define GAP_SETTINGS_BREDR 7 +#define GAP_SETTINGS_HS 8 +#define GAP_SETTINGS_LE 9 +#define GAP_SETTINGS_ADVERTISING 10 +#define GAP_SETTINGS_SC 11 +#define GAP_SETTINGS_DEBUG_KEYS 12 +#define GAP_SETTINGS_PRIVACY 13 +#define GAP_SETTINGS_CONTROLLER_CONFIG 14 +#define GAP_SETTINGS_STATIC_ADDRESS 15 -#define GAP_READ_CONTROLLER_INFO 0x03 +#define GAP_READ_CONTROLLER_INFO 0x03 struct gap_read_controller_info_rp { - uint8_t address[6]; - uint32_t supported_settings; - uint32_t current_settings; - uint8_t cod[3]; - uint8_t name[249]; - uint8_t short_name[11]; + uint8_t address[6]; + uint32_t supported_settings; + uint32_t current_settings; + uint8_t cod[3]; + uint8_t name[249]; + uint8_t short_name[11]; } __packed; -#define GAP_RESET 0x04 +#define GAP_RESET 0x04 struct gap_reset_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_SET_POWERED 0x05 +#define GAP_SET_POWERED 0x05 struct gap_set_powered_cmd { - uint8_t powered; + uint8_t powered; } __packed; struct gap_set_powered_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_SET_CONNECTABLE 0x06 +#define GAP_SET_CONNECTABLE 0x06 struct gap_set_connectable_cmd { - uint8_t connectable; + uint8_t connectable; } __packed; struct gap_set_connectable_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_SET_FAST_CONNECTABLE 0x07 +#define GAP_SET_FAST_CONNECTABLE 0x07 struct gap_set_fast_connectable_cmd { - uint8_t fast_connectable; + uint8_t fast_connectable; } __packed; struct gap_set_fast_connectable_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_NON_DISCOVERABLE 0x00 -#define GAP_GENERAL_DISCOVERABLE 0x01 -#define GAP_LIMITED_DISCOVERABLE 0x02 +#define GAP_NON_DISCOVERABLE 0x00 +#define GAP_GENERAL_DISCOVERABLE 0x01 +#define GAP_LIMITED_DISCOVERABLE 0x02 -#define GAP_SET_DISCOVERABLE 0x08 +#define GAP_SET_DISCOVERABLE 0x08 struct gap_set_discoverable_cmd { - uint8_t discoverable; + uint8_t discoverable; } __packed; struct gap_set_discoverable_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_SET_BONDABLE 0x09 +#define GAP_SET_BONDABLE 0x09 struct gap_set_bondable_cmd { - uint8_t bondable; + uint8_t bondable; } __packed; struct gap_set_bondable_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_START_ADVERTISING 0x0a +#define GAP_START_ADVERTISING 0x0a struct gap_start_advertising_cmd { - uint8_t adv_data_len; - uint8_t scan_rsp_len; - uint8_t adv_data[0]; - uint8_t scan_rsp[0]; + uint8_t adv_data_len; + uint8_t scan_rsp_len; + uint8_t adv_data[0]; + uint8_t scan_rsp[0]; } __packed; struct gap_start_advertising_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_STOP_ADVERTISING 0x0b +#define GAP_STOP_ADVERTISING 0x0b struct gap_stop_advertising_rp { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_DISCOVERY_FLAG_LE 0x01 -#define GAP_DISCOVERY_FLAG_BREDR 0x02 -#define GAP_DISCOVERY_FLAG_LIMITED 0x04 -#define GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN 0x08 -#define GAP_DISCOVERY_FLAG_LE_OBSERVE 0x10 +#define GAP_DISCOVERY_FLAG_LE 0x01 +#define GAP_DISCOVERY_FLAG_BREDR 0x02 +#define GAP_DISCOVERY_FLAG_LIMITED 0x04 +#define GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN 0x08 +#define GAP_DISCOVERY_FLAG_LE_OBSERVE 0x10 -#define GAP_START_DISCOVERY 0x0c +#define GAP_START_DISCOVERY 0x0c struct gap_start_discovery_cmd { - uint8_t flags; + uint8_t flags; } __packed; -#define GAP_STOP_DISCOVERY 0x0d +#define GAP_STOP_DISCOVERY 0x0d -#define GAP_CONNECT 0x0e +#define GAP_CONNECT 0x0e struct gap_connect_cmd { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define GAP_DISCONNECT 0x0f +#define GAP_DISCONNECT 0x0f struct gap_disconnect_cmd { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define GAP_IO_CAP_DISPLAY_ONLY 0 -#define GAP_IO_CAP_DISPLAY_YESNO 1 -#define GAP_IO_CAP_KEYBOARD_ONLY 2 -#define GAP_IO_CAP_NO_INPUT_OUTPUT 3 -#define GAP_IO_CAP_KEYBOARD_DISPLAY 4 +#define GAP_IO_CAP_DISPLAY_ONLY 0 +#define GAP_IO_CAP_DISPLAY_YESNO 1 +#define GAP_IO_CAP_KEYBOARD_ONLY 2 +#define GAP_IO_CAP_NO_INPUT_OUTPUT 3 +#define GAP_IO_CAP_KEYBOARD_DISPLAY 4 -#define GAP_SET_IO_CAP 0x10 +#define GAP_SET_IO_CAP 0x10 struct gap_set_io_cap_cmd { - uint8_t io_cap; + uint8_t io_cap; } __packed; -#define GAP_PAIR 0x11 +#define GAP_PAIR 0x11 struct gap_pair_cmd { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define GAP_UNPAIR 0x12 +#define GAP_UNPAIR 0x12 struct gap_unpair_cmd { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define GAP_PASSKEY_ENTRY 0x13 +#define GAP_PASSKEY_ENTRY 0x13 struct gap_passkey_entry_cmd { - uint8_t address_type; - uint8_t address[6]; - uint32_t passkey; + uint8_t address_type; + uint8_t address[6]; + uint32_t passkey; } __packed; -#define GAP_PASSKEY_CONFIRM 0x14 +#define GAP_PASSKEY_CONFIRM 0x14 struct gap_passkey_confirm_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t match; + uint8_t address_type; + uint8_t address[6]; + uint8_t match; } __packed; -#define GAP_START_DIRECT_ADV 0x15 +#define GAP_START_DIRECT_ADV 0x15 struct gap_start_direct_adv_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t high_duty; + uint8_t address_type; + uint8_t address[6]; + uint8_t high_duty; } __packed; -#define GAP_CONN_PARAM_UPDATE 0x16 +#define GAP_CONN_PARAM_UPDATE 0x16 struct gap_conn_param_update_cmd { uint8_t address_type; uint8_t address[6]; @@ -293,104 +293,104 @@ struct gap_conn_param_update_cmd { uint16_t supervision_timeout; } __packed; -#define GAP_PAIRING_CONSENT_RSP 0x17 +#define GAP_PAIRING_CONSENT_RSP 0x17 struct gap_pairing_consent_rsp_cmd { uint8_t address_type; uint8_t address[6]; uint8_t consent; } __packed; -#define GAP_OOB_LEGACY_SET_DATA 0x18 +#define GAP_OOB_LEGACY_SET_DATA 0x18 struct gap_oob_legacy_set_data_cmd { uint8_t oob_data[16]; } __packed; -#define GAP_OOB_SC_GET_LOCAL_DATA 0x19 +#define GAP_OOB_SC_GET_LOCAL_DATA 0x19 struct gap_oob_sc_get_local_data_rp { uint8_t r[16]; uint8_t c[16]; } __packed; -#define GAP_OOB_SC_SET_REMOTE_DATA 0x1a +#define GAP_OOB_SC_SET_REMOTE_DATA 0x1a struct gap_oob_sc_set_remote_data_cmd { uint8_t r[16]; uint8_t c[16]; } __packed; -#define GAP_SET_MITM 0x1b +#define GAP_SET_MITM 0x1b struct gap_set_mitm_cmd { uint8_t mitm; } __packed; -#define GAP_SET_FILTER_ACCEPT_LIST 0x1c +#define GAP_SET_FILTER_ACCEPT_LIST 0x1c struct gap_set_filter_accept_list_cmd { uint8_t list_len; ble_addr_t addrs[]; } __packed; /* events */ -#define GAP_EV_NEW_SETTINGS 0x80 +#define GAP_EV_NEW_SETTINGS 0x80 struct gap_new_settings_ev { - uint32_t current_settings; + uint32_t current_settings; } __packed; -#define GAP_DEVICE_FOUND_FLAG_RSSI 0x01 -#define GAP_DEVICE_FOUND_FLAG_AD 0x02 -#define GAP_DEVICE_FOUND_FLAG_SD 0x04 +#define GAP_DEVICE_FOUND_FLAG_RSSI 0x01 +#define GAP_DEVICE_FOUND_FLAG_AD 0x02 +#define GAP_DEVICE_FOUND_FLAG_SD 0x04 -#define GAP_EV_DEVICE_FOUND 0x81 +#define GAP_EV_DEVICE_FOUND 0x81 struct gap_device_found_ev { - uint8_t address_type; - uint8_t address[6]; - int8_t rssi; - uint8_t flags; - uint16_t eir_data_len; - uint8_t eir_data[0]; + uint8_t address_type; + uint8_t address[6]; + int8_t rssi; + uint8_t flags; + uint16_t eir_data_len; + uint8_t eir_data[0]; } __packed; -#define GAP_EV_DEVICE_CONNECTED 0x82 +#define GAP_EV_DEVICE_CONNECTED 0x82 struct gap_device_connected_ev { - uint8_t address_type; - uint8_t address[6]; - uint16_t conn_itvl; - uint16_t conn_latency; - uint16_t supervision_timeout; + uint8_t address_type; + uint8_t address[6]; + uint16_t conn_itvl; + uint16_t conn_latency; + uint16_t supervision_timeout; } __packed; -#define GAP_EV_DEVICE_DISCONNECTED 0x83 +#define GAP_EV_DEVICE_DISCONNECTED 0x83 struct gap_device_disconnected_ev { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define GAP_EV_PASSKEY_DISPLAY 0x84 +#define GAP_EV_PASSKEY_DISPLAY 0x84 struct gap_passkey_display_ev { - uint8_t address_type; - uint8_t address[6]; - uint32_t passkey; + uint8_t address_type; + uint8_t address[6]; + uint32_t passkey; } __packed; -#define GAP_EV_PASSKEY_ENTRY_REQ 0x85 +#define GAP_EV_PASSKEY_ENTRY_REQ 0x85 struct gap_passkey_entry_req_ev { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define GAP_EV_PASSKEY_CONFIRM_REQ 0x86 +#define GAP_EV_PASSKEY_CONFIRM_REQ 0x86 struct gap_passkey_confirm_req_ev { - uint8_t address_type; - uint8_t address[6]; - uint32_t passkey; + uint8_t address_type; + uint8_t address[6]; + uint32_t passkey; } __packed; -#define GAP_EV_IDENTITY_RESOLVED 0x87 +#define GAP_EV_IDENTITY_RESOLVED 0x87 struct gap_identity_resolved_ev { - uint8_t address_type; - uint8_t address[6]; - uint8_t identity_address_type; - uint8_t identity_address[6]; + uint8_t address_type; + uint8_t address[6]; + uint8_t identity_address_type; + uint8_t identity_address[6]; } __packed; -#define GAP_EV_CONN_PARAM_UPDATE 0x88 +#define GAP_EV_CONN_PARAM_UPDATE 0x88 struct gap_conn_param_update_ev { uint8_t address_type; uint8_t address[6]; @@ -399,26 +399,26 @@ struct gap_conn_param_update_ev { uint16_t supervision_timeout; } __packed; -#define GAP_EV_SEC_LEVEL_CHANGED 0x89 +#define GAP_EV_SEC_LEVEL_CHANGED 0x89 struct gap_sec_level_changed_ev { uint8_t address_type; uint8_t address[6]; uint8_t level; } __packed; -#define GAP_EV_PAIRING_CONSENT_REQ 0x8a +#define GAP_EV_PAIRING_CONSENT_REQ 0x8a struct gap_pairing_consent_req_ev { uint8_t address_type; uint8_t address[6]; } __packed; -#define GAP_EV_BOND_LOST 0x8b +#define GAP_EV_BOND_LOST 0x8b struct gap_bond_lost_ev { uint8_t address_type; uint8_t address[6]; } __packed; -#define GAP_EV_SEC_PAIRING_FAILED 0x8c +#define GAP_EV_SEC_PAIRING_FAILED 0x8c struct gap_sec_pairing_failed_ev { uint8_t address_type; uint8_t address[6]; @@ -427,109 +427,109 @@ struct gap_sec_pairing_failed_ev { /* GATT Service */ /* commands */ -#define GATT_READ_SUPPORTED_COMMANDS 0x01 +#define GATT_READ_SUPPORTED_COMMANDS 0x01 struct gatt_read_supported_commands_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; -#define GATT_SERVICE_PRIMARY 0x00 -#define GATT_SERVICE_SECONDARY 0x01 +#define GATT_SERVICE_PRIMARY 0x00 +#define GATT_SERVICE_SECONDARY 0x01 -#define GATT_ADD_SERVICE 0x02 +#define GATT_ADD_SERVICE 0x02 struct gatt_add_service_cmd { - uint8_t type; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t type; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gatt_add_service_rp { - uint16_t svc_id; + uint16_t svc_id; } __packed; -#define GATT_ADD_CHARACTERISTIC 0x03 +#define GATT_ADD_CHARACTERISTIC 0x03 struct gatt_add_characteristic_cmd { - uint16_t svc_id; - uint8_t properties; - uint8_t permissions; - uint8_t uuid_length; - uint8_t uuid[0]; + uint16_t svc_id; + uint8_t properties; + uint8_t permissions; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gatt_add_characteristic_rp { - uint16_t char_id; + uint16_t char_id; } __packed; -#define GATT_ADD_DESCRIPTOR 0x04 +#define GATT_ADD_DESCRIPTOR 0x04 struct gatt_add_descriptor_cmd { - uint16_t char_id; - uint8_t permissions; - uint8_t uuid_length; - uint8_t uuid[0]; + uint16_t char_id; + uint8_t permissions; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gatt_add_descriptor_rp { - uint16_t desc_id; + uint16_t desc_id; } __packed; -#define GATT_ADD_INCLUDED_SERVICE 0x05 +#define GATT_ADD_INCLUDED_SERVICE 0x05 struct gatt_add_included_service_cmd { - uint16_t svc_id; + uint16_t svc_id; } __packed; struct gatt_add_included_service_rp { - uint16_t included_service_id; + uint16_t included_service_id; } __packed; -#define GATT_SET_VALUE 0x06 - struct gatt_set_value_cmd { - uint16_t attr_id; - uint16_t len; - uint8_t value[0]; +#define GATT_SET_VALUE 0x06 +struct gatt_set_value_cmd { + uint16_t attr_id; + uint16_t len; + uint8_t value[0]; } __packed; -#define GATT_START_SERVER 0x07 +#define GATT_START_SERVER 0x07 struct gatt_start_server_rp { - uint16_t db_attr_off; - uint8_t db_attr_cnt; + uint16_t db_attr_off; + uint8_t db_attr_cnt; } __packed; -#define GATT_SET_ENC_KEY_SIZE 0x09 +#define GATT_SET_ENC_KEY_SIZE 0x09 struct gatt_set_enc_key_size_cmd { - uint16_t attr_id; - uint8_t key_size; + uint16_t attr_id; + uint8_t key_size; } __packed; /* Gatt Client */ struct gatt_service { - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; + uint16_t start_handle; + uint16_t end_handle; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gatt_included { - uint16_t included_handle; - struct gatt_service service; + uint16_t included_handle; + struct gatt_service service; } __packed; struct gatt_read_uuid_chr { - uint16_t handle; - uint8_t data[0]; + uint16_t handle; + uint8_t data[0]; } __packed; struct gatt_characteristic { - uint16_t characteristic_handle; - uint16_t value_handle; - uint8_t properties; - uint8_t uuid_length; - uint8_t uuid[0]; + uint16_t characteristic_handle; + uint16_t value_handle; + uint8_t properties; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gatt_descriptor { - uint16_t descriptor_handle; - uint8_t uuid_length; - uint8_t uuid[0]; + uint16_t descriptor_handle; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; -#define GATT_EXCHANGE_MTU 0x0a +#define GATT_EXCHANGE_MTU 0x0a -#define GATT_DISC_ALL_PRIM_SVCS 0x0b +#define GATT_DISC_ALL_PRIM_SVCS 0x0b struct gatt_disc_all_prim_svcs_cmd { uint8_t address_type; uint8_t address[6]; @@ -539,140 +539,140 @@ struct gatt_disc_all_prim_svcs_rp { struct gatt_service services[0]; } __packed; -#define GATT_DISC_PRIM_UUID 0x0c +#define GATT_DISC_PRIM_UUID 0x0c struct gatt_disc_prim_uuid_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gatt_disc_prim_uuid_rp { - uint8_t services_count; - struct gatt_service services[0]; + uint8_t services_count; + struct gatt_service services[0]; } __packed; -#define GATT_FIND_INCLUDED 0x0d +#define GATT_FIND_INCLUDED 0x0d struct gatt_find_included_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; } __packed; struct gatt_find_included_rp { - uint8_t services_count; - struct gatt_included included[0]; + uint8_t services_count; + struct gatt_included included[0]; } __packed; -#define GATT_DISC_ALL_CHRC 0x0e +#define GATT_DISC_ALL_CHRC 0x0e struct gatt_disc_all_chrc_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; } __packed; struct gatt_disc_chrc_rp { - uint8_t characteristics_count; - struct gatt_characteristic characteristics[0]; + uint8_t characteristics_count; + struct gatt_characteristic characteristics[0]; } __packed; -#define GATT_DISC_CHRC_UUID 0x0f +#define GATT_DISC_CHRC_UUID 0x0f struct gatt_disc_chrc_uuid_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; -#define GATT_DISC_ALL_DESC 0x10 +#define GATT_DISC_ALL_DESC 0x10 struct gatt_disc_all_desc_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; } __packed; struct gatt_disc_all_desc_rp { - uint8_t descriptors_count; - struct gatt_descriptor descriptors[0]; + uint8_t descriptors_count; + struct gatt_descriptor descriptors[0]; } __packed; -#define GATT_READ 0x11 +#define GATT_READ 0x11 struct gatt_read_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; } __packed; struct gatt_read_rp { - uint8_t att_response; - uint16_t data_length; - uint8_t data[0]; + uint8_t att_response; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define GATT_READ_UUID 0x12 +#define GATT_READ_UUID 0x12 struct gatt_read_uuid_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; -#define GATT_READ_LONG 0x13 +#define GATT_READ_LONG 0x13 struct gatt_read_long_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t offset; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t offset; } __packed; -#define GATT_READ_MULTIPLE 0x14 +#define GATT_READ_MULTIPLE 0x14 struct gatt_read_multiple_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t handles_count; - uint16_t handles[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t handles_count; + uint16_t handles[0]; } __packed; -#define GATT_WRITE_WITHOUT_RSP 0x15 +#define GATT_WRITE_WITHOUT_RSP 0x15 struct gatt_write_without_rsp_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define GATT_SIGNED_WRITE_WITHOUT_RSP 0x16 +#define GATT_SIGNED_WRITE_WITHOUT_RSP 0x16 struct gatt_signed_write_without_rsp_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define GATT_WRITE 0x17 +#define GATT_WRITE 0x17 struct gatt_write_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define GATT_WRITE_LONG 0x18 +#define GATT_WRITE_LONG 0x18 struct gatt_write_long_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t offset; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t offset; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define GATT_RELIABLE_WRITE 0x19 +#define GATT_RELIABLE_WRITE 0x19 struct gatt_reliable_write_cmd { uint8_t address_type; uint8_t address[6]; @@ -682,46 +682,46 @@ struct gatt_reliable_write_cmd { uint8_t data[0]; } __packed; -#define GATT_CFG_NOTIFY 0x1a -#define GATT_CFG_INDICATE 0x1b +#define GATT_CFG_NOTIFY 0x1a +#define GATT_CFG_INDICATE 0x1b struct gatt_cfg_notify_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t enable; - uint16_t ccc_handle; + uint8_t address_type; + uint8_t address[6]; + uint8_t enable; + uint16_t ccc_handle; } __packed; -#define GATT_GET_ATTRIBUTES 0x1c +#define GATT_GET_ATTRIBUTES 0x1c struct gatt_get_attributes_cmd { - uint16_t start_handle; - uint16_t end_handle; - uint8_t type_length; - uint8_t type[0]; + uint16_t start_handle; + uint16_t end_handle; + uint8_t type_length; + uint8_t type[0]; } __packed; struct gatt_get_attributes_rp { - uint8_t attrs_count; - uint8_t attrs[0]; + uint8_t attrs_count; + uint8_t attrs[0]; } __packed; struct gatt_attr { - uint16_t handle; - uint8_t permission; - uint8_t type_length; - uint8_t type[0]; + uint16_t handle; + uint8_t permission; + uint8_t type_length; + uint8_t type[0]; } __packed; -#define GATT_GET_ATTRIBUTE_VALUE 0x1d +#define GATT_GET_ATTRIBUTE_VALUE 0x1d struct gatt_get_attribute_value_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; } __packed; struct gatt_get_attribute_value_rp { - uint8_t att_response; - uint16_t value_length; - uint8_t value[0]; + uint8_t att_response; + uint16_t value_length; + uint8_t value[0]; } __packed; -#define GATT_CHANGE_DATABASE 0x1e +#define GATT_CHANGE_DATABASE 0x1e struct gatt_change_database { uint16_t start_handle; uint16_t end_handle; @@ -729,92 +729,94 @@ struct gatt_change_database { } __packed; /* GATT events */ -#define GATT_EV_NOTIFICATION 0x80 +#define GATT_EV_NOTIFICATION 0x80 struct gatt_notification_ev { - uint8_t address_type; - uint8_t address[6]; - uint8_t type; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t type; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define GATT_EV_ATTR_VALUE_CHANGED 0x81 +#define GATT_EV_ATTR_VALUE_CHANGED 0x81 struct gatt_attr_value_changed_ev { - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; -static inline void tester_set_bit(uint8_t *addr, unsigned int bit) +static inline void +tester_set_bit(uint8_t *addr, unsigned int bit) { - uint8_t *p = addr + (bit / 8); + uint8_t *p = addr + (bit / 8); - *p |= BIT(bit % 8); + *p |= BIT(bit % 8); } -static inline uint8_t tester_test_bit(const uint8_t *addr, unsigned int bit) +static inline uint8_t +tester_test_bit(const uint8_t *addr, unsigned int bit) { - const uint8_t *p = addr + (bit / 8); + const uint8_t *p = addr + (bit / 8); - return *p & BIT(bit % 8); + return *p & BIT(bit % 8); } /* L2CAP Service */ /* commands */ -#define L2CAP_READ_SUPPORTED_COMMANDS 0x01 +#define L2CAP_READ_SUPPORTED_COMMANDS 0x01 struct l2cap_read_supported_commands_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; -#define L2CAP_CONNECT_OPT_ECFC 0x01 -#define L2CAP_CONNECT_OPT_HOLD_CREDIT 0x02 +#define L2CAP_CONNECT_OPT_ECFC 0x01 +#define L2CAP_CONNECT_OPT_HOLD_CREDIT 0x02 -#define L2CAP_CONNECT 0x02 +#define L2CAP_CONNECT 0x02 struct l2cap_connect_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t psm; - uint16_t mtu; - uint8_t num; - uint8_t options; + uint8_t address_type; + uint8_t address[6]; + uint16_t psm; + uint16_t mtu; + uint8_t num; + uint8_t options; } __packed; struct l2cap_connect_rp { - uint8_t num; - uint8_t chan_ids[0]; + uint8_t num; + uint8_t chan_ids[0]; } __packed; -#define L2CAP_DISCONNECT 0x03 +#define L2CAP_DISCONNECT 0x03 struct l2cap_disconnect_cmd { - uint8_t chan_id; + uint8_t chan_id; } __packed; -#define L2CAP_SEND_DATA 0x04 +#define L2CAP_SEND_DATA 0x04 struct l2cap_send_data_cmd { - uint8_t chan_id; - uint16_t data_len; - uint8_t data[]; + uint8_t chan_id; + uint16_t data_len; + uint8_t data[]; } __packed; -#define L2CAP_TRANSPORT_BREDR 0x00 -#define L2CAP_TRANSPORT_LE 0x01 +#define L2CAP_TRANSPORT_BREDR 0x00 +#define L2CAP_TRANSPORT_LE 0x01 -#define L2CAP_LISTEN 0x05 +#define L2CAP_LISTEN 0x05 struct l2cap_listen_cmd { - uint16_t psm; - uint8_t transport; - uint16_t mtu; - uint16_t response; + uint16_t psm; + uint8_t transport; + uint16_t mtu; + uint16_t response; } __packed; -#define L2CAP_ACCEPT_CONNECTION 0x06 +#define L2CAP_ACCEPT_CONNECTION 0x06 struct l2cap_accept_connection_cmd { - uint8_t chan_id; - uint16_t result; + uint8_t chan_id; + uint16_t result; } __packed; -#define L2CAP_RECONFIGURE 0x07 +#define L2CAP_RECONFIGURE 0x07 struct l2cap_reconfigure_cmd { uint8_t address_type; uint8_t address[6]; @@ -823,215 +825,215 @@ struct l2cap_reconfigure_cmd { uint8_t idxs[]; } __packed; -#define L2CAP_CREDITS 0x08 +#define L2CAP_CREDITS 0x08 struct l2cap_credits_cmd { uint8_t chan_id; } __packed; /* events */ -#define L2CAP_EV_CONNECTION_REQ 0x80 +#define L2CAP_EV_CONNECTION_REQ 0x80 struct l2cap_connection_req_ev { - uint8_t chan_id; - uint16_t psm; - uint8_t address_type; - uint8_t address[6]; + uint8_t chan_id; + uint16_t psm; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define L2CAP_EV_CONNECTED 0x81 +#define L2CAP_EV_CONNECTED 0x81 struct l2cap_connected_ev { - uint8_t chan_id; - uint16_t psm; - uint16_t peer_mtu; - uint16_t peer_mps; - uint16_t our_mtu; - uint16_t our_mps; - uint8_t address_type; - uint8_t address[6]; + uint8_t chan_id; + uint16_t psm; + uint16_t peer_mtu; + uint16_t peer_mps; + uint16_t our_mtu; + uint16_t our_mps; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define L2CAP_EV_DISCONNECTED 0x82 +#define L2CAP_EV_DISCONNECTED 0x82 struct l2cap_disconnected_ev { - uint16_t result; - uint8_t chan_id; - uint16_t psm; - uint8_t address_type; - uint8_t address[6]; + uint16_t result; + uint8_t chan_id; + uint16_t psm; + uint8_t address_type; + uint8_t address[6]; } __packed; -#define L2CAP_EV_DATA_RECEIVED 0x83 +#define L2CAP_EV_DATA_RECEIVED 0x83 struct l2cap_data_received_ev { - uint8_t chan_id; - uint16_t data_length; - uint8_t data[0]; + uint8_t chan_id; + uint16_t data_length; + uint8_t data[0]; } __packed; -#define L2CAP_EV_RECONFIGURED 0x84 +#define L2CAP_EV_RECONFIGURED 0x84 struct l2cap_reconfigured_ev { - uint8_t chan_id; - uint16_t peer_mtu; - uint16_t peer_mps; - uint16_t our_mtu; - uint16_t our_mps; + uint8_t chan_id; + uint16_t peer_mtu; + uint16_t peer_mps; + uint16_t our_mtu; + uint16_t our_mps; } __packed; /* MESH Service */ /* commands */ -#define MESH_READ_SUPPORTED_COMMANDS 0x01 +#define MESH_READ_SUPPORTED_COMMANDS 0x01 struct mesh_read_supported_commands_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; -#define MESH_OUT_BLINK BIT(0) -#define MESH_OUT_BEEP BIT(1) -#define MESH_OUT_VIBRATE BIT(2) -#define MESH_OUT_DISPLAY_NUMBER BIT(3) -#define MESH_OUT_DISPLAY_STRING BIT(4) +#define MESH_OUT_BLINK BIT(0) +#define MESH_OUT_BEEP BIT(1) +#define MESH_OUT_VIBRATE BIT(2) +#define MESH_OUT_DISPLAY_NUMBER BIT(3) +#define MESH_OUT_DISPLAY_STRING BIT(4) -#define MESH_IN_PUSH BIT(0) -#define MESH_IN_TWIST BIT(1) -#define MESH_IN_ENTER_NUMBER BIT(2) -#define MESH_IN_ENTER_STRING BIT(3) +#define MESH_IN_PUSH BIT(0) +#define MESH_IN_TWIST BIT(1) +#define MESH_IN_ENTER_NUMBER BIT(2) +#define MESH_IN_ENTER_STRING BIT(3) -#define MESH_CONFIG_PROVISIONING 0x02 +#define MESH_CONFIG_PROVISIONING 0x02 struct mesh_config_provisioning_cmd { - uint8_t uuid[16]; - uint8_t static_auth[16]; - uint8_t out_size; - uint16_t out_actions; - uint8_t in_size; - uint16_t in_actions; + uint8_t uuid[16]; + uint8_t static_auth[16]; + uint8_t out_size; + uint16_t out_actions; + uint8_t in_size; + uint16_t in_actions; } __packed; -#define MESH_PROVISION_NODE 0x03 +#define MESH_PROVISION_NODE 0x03 struct mesh_provision_node_cmd { - uint8_t net_key[16]; - uint16_t net_key_idx; - uint8_t flags; - uint32_t iv_index; - uint32_t seq_num; - uint16_t addr; - uint8_t dev_key[16]; + uint8_t net_key[16]; + uint16_t net_key_idx; + uint8_t flags; + uint32_t iv_index; + uint32_t seq_num; + uint16_t addr; + uint8_t dev_key[16]; } __packed; -#define MESH_INIT 0x04 -#define MESH_RESET 0x05 -#define MESH_INPUT_NUMBER 0x06 +#define MESH_INIT 0x04 +#define MESH_RESET 0x05 +#define MESH_INPUT_NUMBER 0x06 struct mesh_input_number_cmd { - uint32_t number; + uint32_t number; } __packed; -#define MESH_INPUT_STRING 0x07 +#define MESH_INPUT_STRING 0x07 struct mesh_input_string_cmd { - uint8_t string_len; - uint8_t string[0]; + uint8_t string_len; + uint8_t string[0]; } __packed; -#define MESH_IVU_TEST_MODE 0x08 +#define MESH_IVU_TEST_MODE 0x08 struct mesh_ivu_test_mode_cmd { - uint8_t enable; + uint8_t enable; } __packed; -#define MESH_IVU_TOGGLE_STATE 0x09 +#define MESH_IVU_TOGGLE_STATE 0x09 -#define MESH_NET_SEND 0x0a +#define MESH_NET_SEND 0x0a struct mesh_net_send_cmd { - uint8_t ttl; - uint16_t src; - uint16_t dst; - uint8_t payload_len; - uint8_t payload[0]; + uint8_t ttl; + uint16_t src; + uint16_t dst; + uint8_t payload_len; + uint8_t payload[0]; } __packed; -#define MESH_HEALTH_GENERATE_FAULTS 0x0b +#define MESH_HEALTH_GENERATE_FAULTS 0x0b struct mesh_health_generate_faults_rp { - uint8_t test_id; - uint8_t cur_faults_count; - uint8_t reg_faults_count; - uint8_t current_faults[0]; - uint8_t registered_faults[0]; + uint8_t test_id; + uint8_t cur_faults_count; + uint8_t reg_faults_count; + uint8_t current_faults[0]; + uint8_t registered_faults[0]; } __packed; -#define MESH_HEALTH_CLEAR_FAULTS 0x0c +#define MESH_HEALTH_CLEAR_FAULTS 0x0c -#define MESH_LPN 0x0d +#define MESH_LPN 0x0d struct mesh_lpn_set_cmd { - uint8_t enable; + uint8_t enable; } __packed; -#define MESH_LPN_POLL 0x0e +#define MESH_LPN_POLL 0x0e -#define MESH_MODEL_SEND 0x0f +#define MESH_MODEL_SEND 0x0f struct mesh_model_send_cmd { - uint16_t src; - uint16_t dst; - uint8_t payload_len; - uint8_t payload[0]; + uint16_t src; + uint16_t dst; + uint8_t payload_len; + uint8_t payload[0]; } __packed; -#define MESH_LPN_SUBSCRIBE 0x10 +#define MESH_LPN_SUBSCRIBE 0x10 struct mesh_lpn_subscribe_cmd { - uint16_t address; + uint16_t address; } __packed; -#define MESH_LPN_UNSUBSCRIBE 0x11 +#define MESH_LPN_UNSUBSCRIBE 0x11 struct mesh_lpn_unsubscribe_cmd { - uint16_t address; + uint16_t address; } __packed; -#define MESH_RPL_CLEAR 0x12 -#define MESH_PROXY_IDENTITY 0x13 +#define MESH_RPL_CLEAR 0x12 +#define MESH_PROXY_IDENTITY 0x13 /* events */ -#define MESH_EV_OUT_NUMBER_ACTION 0x80 +#define MESH_EV_OUT_NUMBER_ACTION 0x80 struct mesh_out_number_action_ev { - uint16_t action; - uint32_t number; + uint16_t action; + uint32_t number; } __packed; -#define MESH_EV_OUT_STRING_ACTION 0x81 +#define MESH_EV_OUT_STRING_ACTION 0x81 struct mesh_out_string_action_ev { - uint8_t string_len; - uint8_t string[0]; + uint8_t string_len; + uint8_t string[0]; } __packed; -#define MESH_EV_IN_ACTION 0x82 +#define MESH_EV_IN_ACTION 0x82 struct mesh_in_action_ev { - uint16_t action; - uint8_t size; + uint16_t action; + uint8_t size; } __packed; -#define MESH_EV_PROVISIONED 0x83 +#define MESH_EV_PROVISIONED 0x83 -#define MESH_PROV_BEARER_PB_ADV 0x00 -#define MESH_PROV_BEARER_PB_GATT 0x01 -#define MESH_EV_PROV_LINK_OPEN 0x84 +#define MESH_PROV_BEARER_PB_ADV 0x00 +#define MESH_PROV_BEARER_PB_GATT 0x01 +#define MESH_EV_PROV_LINK_OPEN 0x84 struct mesh_prov_link_open_ev { - uint8_t bearer; + uint8_t bearer; } __packed; -#define MESH_EV_PROV_LINK_CLOSED 0x85 +#define MESH_EV_PROV_LINK_CLOSED 0x85 struct mesh_prov_link_closed_ev { - uint8_t bearer; + uint8_t bearer; } __packed; -#define MESH_EV_NET_RECV 0x86 +#define MESH_EV_NET_RECV 0x86 struct mesh_net_recv_ev { - uint8_t ttl; - uint8_t ctl; - uint16_t src; - uint16_t dst; - uint8_t payload_len; - uint8_t payload[0]; + uint8_t ttl; + uint8_t ctl; + uint16_t src; + uint16_t dst; + uint8_t payload_len; + uint8_t payload[0]; } __packed; -#define MESH_EV_INVALID_BEARER 0x87 +#define MESH_EV_INVALID_BEARER 0x87 struct mesh_invalid_bearer_ev { - uint8_t opcode; + uint8_t opcode; } __packed; -#define MESH_EV_INCOMP_TIMER_EXP 0x88 +#define MESH_EV_INCOMP_TIMER_EXP 0x88 -#define MESH_EV_LPN_ESTABLISHED 0x8b +#define MESH_EV_LPN_ESTABLISHED 0x8b struct mesh_lpn_established_ev { uint16_t net_idx; uint16_t friend_addr; @@ -1039,257 +1041,282 @@ struct mesh_lpn_established_ev { uint8_t recv_win; } __packed; -#define MESH_EV_LPN_TERMINATED 0x8c +#define MESH_EV_LPN_TERMINATED 0x8c struct mesh_lpn_terminated_ev { uint16_t net_idx; uint16_t friend_addr; } __packed; -#define MESH_EV_LPN_POLLED 0x8d +#define MESH_EV_LPN_POLLED 0x8d struct mesh_lpn_polled_ev { uint16_t net_idx; uint16_t friend_addr; uint8_t retry; } __packed; -void tester_init(void); -void tester_rsp(uint8_t service, uint8_t opcode, uint8_t index, uint8_t status); -void tester_send(uint8_t service, uint8_t opcode, uint8_t index, uint8_t *data, - size_t len); -void tester_send_buf(uint8_t service, uint8_t opcode, uint8_t index, - struct os_mbuf *buf); +void +tester_init(void); +void +tester_rsp(uint8_t service, uint8_t opcode, uint8_t index, uint8_t status); +void +tester_send(uint8_t service, uint8_t opcode, uint8_t index, uint8_t *data, + size_t len); +void +tester_send_buf(uint8_t service, uint8_t opcode, uint8_t index, + struct os_mbuf *buf); -uint8_t tester_init_gap(void); -uint8_t tester_unregister_gap(void); -void tester_handle_gap(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len); -uint8_t tester_init_gatt(void); -uint8_t tester_unregister_gatt(void); -void tester_handle_gatt(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len); -void tester_handle_gattc(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len); -int tester_gattc_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, - uint8_t indication, struct os_mbuf *om); -int tester_gatt_subscribe_ev(uint16_t conn_handle, uint16_t attr_handle, uint8_t reason, - uint8_t prev_notify, uint8_t cur_notify, - uint8_t prev_indicate, uint8_t cur_indicate); +uint8_t +tester_init_gap(void); +uint8_t +tester_unregister_gap(void); +void +tester_handle_gap(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len); +uint8_t +tester_init_gatt(void); +uint8_t +tester_unregister_gatt(void); +void +tester_handle_gatt(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len); +void +tester_handle_gattc(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len); +int +tester_gattc_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, + uint8_t indication, struct os_mbuf *om); +int +tester_gatt_subscribe_ev(uint16_t conn_handle, + uint16_t attr_handle, + uint8_t reason, + uint8_t prev_notify, + uint8_t cur_notify, + uint8_t prev_indicate, + uint8_t cur_indicate); #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) -uint8_t tester_init_l2cap(void); -uint8_t tester_unregister_l2cap(void); -void tester_handle_l2cap(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len); +uint8_t +tester_init_l2cap(void); +uint8_t +tester_unregister_l2cap(void); +void +tester_handle_l2cap(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len); #endif #if MYNEWT_VAL(BLE_MESH) -uint8_t tester_init_mesh(void); -uint8_t tester_unregister_mesh(void); -void tester_handle_mesh(uint8_t opcode, uint8_t index, uint8_t *data, uint16_t len); +uint8_t +tester_init_mesh(void); +uint8_t +tester_unregister_mesh(void); +void +tester_handle_mesh(uint8_t opcode, uint8_t index, uint8_t *data, uint16_t len); #endif /* MYNEWT_VAL(BLE_MESH) */ -void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg); -int gatt_svr_init(void); +void +gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg); +int +gatt_svr_init(void); /* GATT Client Service */ /* commands */ #define GATTC_READ_SUPPORTED_COMMANDS 0x01 struct gattc_read_supported_commands_rp { - uint8_t data[0]; + uint8_t data[0]; } __packed; #define GATTC_EXCHANGE_MTU 0x02 #define GATTC_DISC_ALL_PRIM_SVCS 0x03 struct gattc_disc_all_prim_svcs_cmd { - uint8_t address_type; - uint8_t address[6]; + uint8_t address_type; + uint8_t address[6]; } __packed; #define GATTC_DISC_PRIM_UUID 0x04 struct gattc_disc_prim_uuid_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; #define GATTC_FIND_INCLUDED 0x05 struct gattc_find_included_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; } __packed; #define GATTC_DISC_ALL_CHRC 0x06 struct gattc_disc_all_chrc_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; } __packed; #define GATTC_DISC_CHRC_UUID 0x07 struct gattc_disc_chrc_uuid_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; #define GATTC_DISC_ALL_DESC 0x08 struct gattc_disc_all_desc_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; } __packed; #define GATTC_READ 0x09 struct gattc_read_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; } __packed; #define GATTC_READ_UUID 0x0a struct gattc_read_uuid_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t start_handle; + uint16_t end_handle; + uint8_t uuid_length; + uint8_t uuid[0]; } __packed; struct gattc_read_uuid_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; - uint16_t data_length; - uint8_t value_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; + uint16_t data_length; + uint8_t value_length; + uint8_t data[0]; } __packed; #define GATTC_READ_LONG 0x0b struct gattc_read_long_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t offset; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t offset; } __packed; #define GATTC_READ_MULTIPLE 0x0c struct gattc_read_multiple_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t handles_count; - uint16_t handles[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t handles_count; + uint16_t handles[0]; } __packed; #define GATTC_WRITE_WITHOUT_RSP 0x0d struct gattc_write_without_rsp_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; #define GATTC_SIGNED_WRITE_WITHOUT_RSP 0x0e struct gattc_signed_write_without_rsp_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; #define GATTC_WRITE 0x0f struct gattc_write_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; #define GATTC_WRITE_LONG 0x10 struct gattc_write_long_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t offset; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t offset; + uint16_t data_length; + uint8_t data[0]; } __packed; #define GATTC_RELIABLE_WRITE 0x11 struct gattc_reliable_write_cmd { - uint8_t address_type; - uint8_t address[6]; - uint16_t handle; - uint16_t offset; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint16_t handle; + uint16_t offset; + uint16_t data_length; + uint8_t data[0]; } __packed; #define GATTC_CFG_NOTIFY 0x12 #define GATTC_CFG_INDICATE 0x13 struct gattc_cfg_notify_cmd { - uint8_t address_type; - uint8_t address[6]; - uint8_t enable; - uint16_t ccc_handle; + uint8_t address_type; + uint8_t address[6]; + uint8_t enable; + uint16_t ccc_handle; } __packed; /* events */ #define GATTC_EV_MTU_EXCHANGED 0x80 struct gattc_exchange_mtu_ev { - uint8_t address_type; - uint8_t address[6]; - uint16_t mtu; + uint8_t address_type; + uint8_t address[6]; + uint16_t mtu; } __packed; #define GATTC_DISC_ALL_PRIM_RP 0x81 struct gattc_disc_prim_svcs_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; - uint8_t services_count; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; + uint8_t services_count; + uint8_t data[0]; } __packed; #define GATTC_DISC_PRIM_UUID_RP 0x82 #define GATTC_FIND_INCLUDED_RP 0x83 struct gattc_find_included_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; - uint8_t services_count; - struct gatt_included included[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; + uint8_t services_count; + struct gatt_included included[0]; } __packed; #define GATTC_DISC_ALL_CHRC_RP 0x84 #define GATTC_DISC_CHRC_UUID_RP 0x85 struct gattc_disc_chrc_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; - uint8_t characteristics_count; - struct gatt_characteristic characteristics[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; + uint8_t characteristics_count; + struct gatt_characteristic characteristics[0]; } __packed; #define GATTC_DISC_ALL_DESC_RP 0x86 struct gattc_disc_all_desc_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; - uint8_t descriptors_count; - struct gatt_descriptor descriptors[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; + uint8_t descriptors_count; + struct gatt_descriptor descriptors[0]; } __packed; #define GATTC_READ_RP 0x87 @@ -1297,18 +1324,18 @@ struct gattc_disc_all_desc_rp { #define GATTC_READ_LONG_RP 0x89 #define GATTC_READ_MULTIPLE_RP 0x8a struct gattc_read_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; + uint16_t data_length; + uint8_t data[0]; } __packed; #define GATTC_WRITE_RP 0x8b struct gattc_write_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; } __packed; #define GATTC_WRITE_LONG_RP 0x8c #define GATTC_RELIABLE_WRITE_RP 0x8d @@ -1316,19 +1343,19 @@ struct gattc_write_rp { #define GATTC_CFG_NOTIFY_RP 0x8e #define GATTC_CFG_INDICATE_RP 0x8f struct subscribe_rp { - uint8_t address_type; - uint8_t address[6]; - uint8_t status; + uint8_t address_type; + uint8_t address[6]; + uint8_t status; } __packed; #define GATTC_EV_NOTIFICATION_RXED 0x90 struct gattc_notification_ev { - uint8_t address_type; - uint8_t address[6]; - uint8_t type; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; + uint8_t address_type; + uint8_t address[6]; + uint8_t type; + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; } __packed; #endif /* __BTTESTER_H__ */ diff --git a/apps/bttester/src/bttester_pipe.h b/apps/bttester/src/bttester_pipe.h index 64b63cd6a..5f27a41c2 100644 --- a/apps/bttester/src/bttester_pipe.h +++ b/apps/bttester/src/bttester_pipe.h @@ -28,10 +28,14 @@ extern "C" { #endif typedef uint8_t *(*bttester_pipe_recv_cb)(uint8_t *buf, size_t *off); -void bttester_pipe_register(uint8_t *buf, size_t len, bttester_pipe_recv_cb cb); -int bttester_pipe_send(const uint8_t *data, int len); -int bttester_pipe_send_buf(struct os_mbuf *buf); -int bttester_pipe_init(void); +void +bttester_pipe_register(uint8_t *buf, size_t len, bttester_pipe_recv_cb cb); +int +bttester_pipe_send(const uint8_t *data, int len); +int +bttester_pipe_send_buf(struct os_mbuf *buf); +int +bttester_pipe_init(void); #ifdef __cplusplus } diff --git a/apps/bttester/src/gap.c b/apps/bttester/src/gap.c index aee9591cc..655a82ec3 100644 --- a/apps/bttester/src/gap.c +++ b/apps/bttester/src/gap.c @@ -48,8 +48,8 @@ #define REJECT_SUPERVISION_TIMEOUT 0x0C80 const uint8_t irk[16] = { - 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, - 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, + 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, + 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, }; static uint8_t oob[16]; @@ -75,174 +75,179 @@ static struct os_callout bttester_nrpa_rotate_timer; #endif static const struct ble_gap_conn_params dflt_conn_params = { - .scan_itvl = 0x0010, - .scan_window = 0x0010, - .itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN, - .itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX, - .latency = 0, - .supervision_timeout = 0x0100, - .min_ce_len = 0x0010, - .max_ce_len = 0x0300, + .scan_itvl = 0x0010, + .scan_window = 0x0010, + .itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN, + .itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX, + .latency = 0, + .supervision_timeout = 0x0100, + .min_ce_len = 0x0010, + .max_ce_len = 0x0300, }; -static void conn_param_update(struct os_event *ev); +static void +conn_param_update(struct os_event *ev); - -static int gap_conn_find_by_addr(const ble_addr_t *dev_addr, - struct ble_gap_conn_desc *out_desc) +static int +gap_conn_find_by_addr(const ble_addr_t *dev_addr, + struct ble_gap_conn_desc *out_desc) { - ble_addr_t addr = *dev_addr; + ble_addr_t addr = *dev_addr; - if (memcmp(BLE_ADDR_ANY, &peer_id_addr, 6) == 0) { - return ble_gap_conn_find_by_addr(&addr, out_desc); - } + if (memcmp(BLE_ADDR_ANY, &peer_id_addr, 6) == 0) { + return ble_gap_conn_find_by_addr(&addr, out_desc); + } - if (BLE_ADDR_IS_RPA(&addr)) { - if(ble_addr_cmp(&peer_ota_addr, &addr) != 0) { - return -1; - } + if (BLE_ADDR_IS_RPA(&addr)) { + if (ble_addr_cmp(&peer_ota_addr, &addr) != 0) { + return -1; + } - return ble_gap_conn_find_by_addr(&addr, out_desc); - } else { - if(ble_addr_cmp(&peer_id_addr, &addr) != 0) { - return -1; - } + return ble_gap_conn_find_by_addr(&addr, out_desc); + } else { + if (ble_addr_cmp(&peer_id_addr, &addr) != 0) { + return -1; + } - if (BLE_ADDR_IS_RPA(&peer_ota_addr)) { - /* Change addr type to ID addr */ - addr.type |= 2; - } + if (BLE_ADDR_IS_RPA(&peer_ota_addr)) { + /* Change addr type to ID addr */ + addr.type |= 2; + } - return ble_gap_conn_find_by_addr(&addr, out_desc); - } + return ble_gap_conn_find_by_addr(&addr, out_desc); + } } -static int gap_event_cb(struct ble_gap_event *event, void *arg); +static int +gap_event_cb(struct ble_gap_event *event, void *arg); -static void supported_commands(uint8_t *data, uint16_t len) +static void +supported_commands(uint8_t *data, uint16_t len) { - uint8_t cmds[3]; - struct gap_read_supported_commands_rp *rp = (void *) &cmds; + uint8_t cmds[3]; + struct gap_read_supported_commands_rp *rp = (void *) &cmds; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memset(cmds, 0, sizeof(cmds)); + memset(cmds, 0, sizeof(cmds)); - tester_set_bit(cmds, GAP_READ_SUPPORTED_COMMANDS); - tester_set_bit(cmds, GAP_READ_CONTROLLER_INDEX_LIST); - tester_set_bit(cmds, GAP_READ_CONTROLLER_INFO); - tester_set_bit(cmds, GAP_SET_CONNECTABLE); - tester_set_bit(cmds, GAP_SET_DISCOVERABLE); - tester_set_bit(cmds, GAP_SET_BONDABLE); - tester_set_bit(cmds, GAP_START_ADVERTISING); - tester_set_bit(cmds, GAP_STOP_ADVERTISING); - tester_set_bit(cmds, GAP_START_DISCOVERY); - tester_set_bit(cmds, GAP_STOP_DISCOVERY); - tester_set_bit(cmds, GAP_CONNECT); - tester_set_bit(cmds, GAP_DISCONNECT); - tester_set_bit(cmds, GAP_SET_IO_CAP); - tester_set_bit(cmds, GAP_PAIR); - tester_set_bit(cmds, GAP_UNPAIR); - tester_set_bit(cmds, GAP_PASSKEY_ENTRY); - tester_set_bit(cmds, GAP_PASSKEY_CONFIRM); - tester_set_bit(cmds, GAP_START_DIRECT_ADV); - tester_set_bit(cmds, GAP_CONN_PARAM_UPDATE); - tester_set_bit(cmds, GAP_OOB_LEGACY_SET_DATA); - tester_set_bit(cmds, GAP_OOB_SC_GET_LOCAL_DATA); - tester_set_bit(cmds, GAP_OOB_SC_SET_REMOTE_DATA); - tester_set_bit(cmds, GAP_SET_MITM); + tester_set_bit(cmds, GAP_READ_SUPPORTED_COMMANDS); + tester_set_bit(cmds, GAP_READ_CONTROLLER_INDEX_LIST); + tester_set_bit(cmds, GAP_READ_CONTROLLER_INFO); + tester_set_bit(cmds, GAP_SET_CONNECTABLE); + tester_set_bit(cmds, GAP_SET_DISCOVERABLE); + tester_set_bit(cmds, GAP_SET_BONDABLE); + tester_set_bit(cmds, GAP_START_ADVERTISING); + tester_set_bit(cmds, GAP_STOP_ADVERTISING); + tester_set_bit(cmds, GAP_START_DISCOVERY); + tester_set_bit(cmds, GAP_STOP_DISCOVERY); + tester_set_bit(cmds, GAP_CONNECT); + tester_set_bit(cmds, GAP_DISCONNECT); + tester_set_bit(cmds, GAP_SET_IO_CAP); + tester_set_bit(cmds, GAP_PAIR); + tester_set_bit(cmds, GAP_UNPAIR); + tester_set_bit(cmds, GAP_PASSKEY_ENTRY); + tester_set_bit(cmds, GAP_PASSKEY_CONFIRM); + tester_set_bit(cmds, GAP_START_DIRECT_ADV); + tester_set_bit(cmds, GAP_CONN_PARAM_UPDATE); + tester_set_bit(cmds, GAP_OOB_LEGACY_SET_DATA); + tester_set_bit(cmds, GAP_OOB_SC_GET_LOCAL_DATA); + tester_set_bit(cmds, GAP_OOB_SC_SET_REMOTE_DATA); + tester_set_bit(cmds, GAP_SET_MITM); - tester_send(BTP_SERVICE_ID_GAP, GAP_READ_SUPPORTED_COMMANDS, - CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); + tester_send(BTP_SERVICE_ID_GAP, GAP_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); } -static void controller_index_list(uint8_t *data, uint16_t len) +static void +controller_index_list(uint8_t *data, uint16_t len) { - struct gap_read_controller_index_list_rp *rp; - uint8_t buf[sizeof(*rp) + 1]; + struct gap_read_controller_index_list_rp *rp; + uint8_t buf[sizeof(*rp) + 1]; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rp = (void *) buf; + rp = (void *) buf; - rp->num = 1; - rp->index[0] = CONTROLLER_INDEX; + rp->num = 1; + rp->index[0] = CONTROLLER_INDEX; - tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INDEX_LIST, - BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf)); + tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INDEX_LIST, + BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf)); } -static void controller_info(uint8_t *data, uint16_t len) +static void +controller_info(uint8_t *data, uint16_t len) { - struct gap_read_controller_info_rp rp; - uint32_t supported_settings = 0; - ble_addr_t addr; - int rc; + struct gap_read_controller_info_rp rp; + uint32_t supported_settings = 0; + ble_addr_t addr; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_hs_pvcy_set_our_irk(irk); - assert(rc == 0); + rc = ble_hs_pvcy_set_our_irk(irk); + assert(rc == 0); - memset(&rp, 0, sizeof(rp)); + memset(&rp, 0, sizeof(rp)); - /* Make sure we have proper identity address set (public preferred) */ - rc = ble_hs_util_ensure_addr(1); - assert(rc == 0); - rc = ble_hs_id_copy_addr(BLE_ADDR_RANDOM, addr.val, NULL); - assert(rc == 0); + /* Make sure we have proper identity address set (public preferred) */ + rc = ble_hs_util_ensure_addr(1); + assert(rc == 0); + rc = ble_hs_id_copy_addr(BLE_ADDR_RANDOM, addr.val, NULL); + assert(rc == 0); - if (MYNEWT_VAL(BTTESTER_PRIVACY_MODE)) { - if (MYNEWT_VAL(BTTESTER_USE_NRPA)) { - own_addr_type = BLE_OWN_ADDR_RANDOM; - rc = ble_hs_id_gen_rnd(1, &addr); - assert(rc == 0); - rc = ble_hs_id_set_rnd(addr.val); - assert(rc == 0); - } else { - own_addr_type = BLE_OWN_ADDR_RPA_RANDOM_DEFAULT; - } - current_settings |= BIT(GAP_SETTINGS_PRIVACY); - supported_settings |= BIT(GAP_SETTINGS_PRIVACY); - memcpy(rp.address, addr.val, sizeof(rp.address)); - } else { - rc = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, rp.address, NULL); - if (rc) { - own_addr_type = BLE_OWN_ADDR_RANDOM; - memcpy(rp.address, addr.val, sizeof(rp.address)); - supported_settings |= BIT(GAP_SETTINGS_STATIC_ADDRESS); - current_settings |= BIT(GAP_SETTINGS_STATIC_ADDRESS); - } else { - own_addr_type = BLE_OWN_ADDR_PUBLIC; - } - } + if (MYNEWT_VAL(BTTESTER_PRIVACY_MODE)) { + if (MYNEWT_VAL(BTTESTER_USE_NRPA)) { + own_addr_type = BLE_OWN_ADDR_RANDOM; + rc = ble_hs_id_gen_rnd(1, &addr); + assert(rc == 0); + rc = ble_hs_id_set_rnd(addr.val); + assert(rc == 0); + } else { + own_addr_type = BLE_OWN_ADDR_RPA_RANDOM_DEFAULT; + } + current_settings |= BIT(GAP_SETTINGS_PRIVACY); + supported_settings |= BIT(GAP_SETTINGS_PRIVACY); + memcpy(rp.address, addr.val, sizeof(rp.address)); + } else { + rc = ble_hs_id_copy_addr(BLE_ADDR_PUBLIC, rp.address, NULL); + if (rc) { + own_addr_type = BLE_OWN_ADDR_RANDOM; + memcpy(rp.address, addr.val, sizeof(rp.address)); + supported_settings |= BIT(GAP_SETTINGS_STATIC_ADDRESS); + current_settings |= BIT(GAP_SETTINGS_STATIC_ADDRESS); + } else { + own_addr_type = BLE_OWN_ADDR_PUBLIC; + } + } - supported_settings |= BIT(GAP_SETTINGS_POWERED); - supported_settings |= BIT(GAP_SETTINGS_CONNECTABLE); - supported_settings |= BIT(GAP_SETTINGS_BONDABLE); - supported_settings |= BIT(GAP_SETTINGS_LE); - supported_settings |= BIT(GAP_SETTINGS_ADVERTISING); - supported_settings |= BIT(GAP_SETTINGS_SC); + supported_settings |= BIT(GAP_SETTINGS_POWERED); + supported_settings |= BIT(GAP_SETTINGS_CONNECTABLE); + supported_settings |= BIT(GAP_SETTINGS_BONDABLE); + supported_settings |= BIT(GAP_SETTINGS_LE); + supported_settings |= BIT(GAP_SETTINGS_ADVERTISING); + supported_settings |= BIT(GAP_SETTINGS_SC); - if (ble_hs_cfg.sm_bonding) { - current_settings |= BIT(GAP_SETTINGS_BONDABLE); - } - if (ble_hs_cfg.sm_sc) { - current_settings |= BIT(GAP_SETTINGS_SC); - } + if (ble_hs_cfg.sm_bonding) { + current_settings |= BIT(GAP_SETTINGS_BONDABLE); + } + if (ble_hs_cfg.sm_sc) { + current_settings |= BIT(GAP_SETTINGS_SC); + } - rp.supported_settings = sys_cpu_to_le32(supported_settings); - rp.current_settings = sys_cpu_to_le32(current_settings); + rp.supported_settings = sys_cpu_to_le32(supported_settings); + rp.current_settings = sys_cpu_to_le32(current_settings); - memcpy(rp.name, CONTROLLER_NAME, sizeof(CONTROLLER_NAME)); + memcpy(rp.name, CONTROLLER_NAME, sizeof(CONTROLLER_NAME)); - tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INFO, - CONTROLLER_INDEX, (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INFO, + CONTROLLER_INDEX, (uint8_t *) &rp, sizeof(rp)); } static struct ble_gap_adv_params adv_params = { - .conn_mode = BLE_GAP_CONN_MODE_NON, - .disc_mode = BLE_GAP_DISC_MODE_NON, + .conn_mode = BLE_GAP_CONN_MODE_NON, + .disc_mode = BLE_GAP_DISC_MODE_NON, }; #if MYNEWT_VAL(BTTESTER_PRIVACY_MODE) && MYNEWT_VAL(BTTESTER_USE_NRPA) @@ -277,406 +282,417 @@ static void rotate_nrpa_cb(struct os_event *ev) } #endif -static void set_connectable(uint8_t *data, uint16_t len) +static void +set_connectable(uint8_t *data, uint16_t len) { - const struct gap_set_connectable_cmd *cmd = (void *) data; - struct gap_set_connectable_rp rp; + const struct gap_set_connectable_cmd *cmd = (void *) data; + struct gap_set_connectable_rp rp; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (cmd->connectable) { - current_settings |= BIT(GAP_SETTINGS_CONNECTABLE); - adv_params.conn_mode = BLE_GAP_CONN_MODE_UND; - } else { - current_settings &= ~BIT(GAP_SETTINGS_CONNECTABLE); - adv_params.conn_mode = BLE_GAP_CONN_MODE_NON; - } + if (cmd->connectable) { + current_settings |= BIT(GAP_SETTINGS_CONNECTABLE); + adv_params.conn_mode = BLE_GAP_CONN_MODE_UND; + } else { + current_settings &= ~BIT(GAP_SETTINGS_CONNECTABLE); + adv_params.conn_mode = BLE_GAP_CONN_MODE_NON; + } - rp.current_settings = sys_cpu_to_le32(current_settings); + rp.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_SET_CONNECTABLE, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GAP, GAP_SET_CONNECTABLE, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); } static uint8_t ad_flags = BLE_HS_ADV_F_BREDR_UNSUP; -static void set_discoverable(uint8_t *data, uint16_t len) +static void +set_discoverable(uint8_t *data, uint16_t len) { - const struct gap_set_discoverable_cmd *cmd = (void *) data; - struct gap_set_discoverable_rp rp; + const struct gap_set_discoverable_cmd *cmd = (void *) data; + struct gap_set_discoverable_rp rp; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - switch (cmd->discoverable) { - case GAP_NON_DISCOVERABLE: - ad_flags &= ~(BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_DISC_LTD); - adv_params.disc_mode = BLE_GAP_DISC_MODE_NON; - current_settings &= ~BIT(GAP_SETTINGS_DISCOVERABLE); - break; - case GAP_GENERAL_DISCOVERABLE: - ad_flags &= ~BLE_HS_ADV_F_DISC_LTD; - ad_flags |= BLE_HS_ADV_F_DISC_GEN; - adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN; - current_settings |= BIT(GAP_SETTINGS_DISCOVERABLE); - break; - case GAP_LIMITED_DISCOVERABLE: - ad_flags &= ~BLE_HS_ADV_F_DISC_GEN; - ad_flags |= BLE_HS_ADV_F_DISC_LTD; - adv_params.disc_mode = BLE_GAP_DISC_MODE_LTD; - current_settings |= BIT(GAP_SETTINGS_DISCOVERABLE); - break; - default: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - return; - } + switch (cmd->discoverable) { + case GAP_NON_DISCOVERABLE: + ad_flags &= ~(BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_DISC_LTD); + adv_params.disc_mode = BLE_GAP_DISC_MODE_NON; + current_settings &= ~BIT(GAP_SETTINGS_DISCOVERABLE); + break; + case GAP_GENERAL_DISCOVERABLE: + ad_flags &= ~BLE_HS_ADV_F_DISC_LTD; + ad_flags |= BLE_HS_ADV_F_DISC_GEN; + adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN; + current_settings |= BIT(GAP_SETTINGS_DISCOVERABLE); + break; + case GAP_LIMITED_DISCOVERABLE: + ad_flags &= ~BLE_HS_ADV_F_DISC_GEN; + ad_flags |= BLE_HS_ADV_F_DISC_LTD; + adv_params.disc_mode = BLE_GAP_DISC_MODE_LTD; + current_settings |= BIT(GAP_SETTINGS_DISCOVERABLE); + break; + default: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + return; + } - rp.current_settings = sys_cpu_to_le32(current_settings); + rp.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); } -static void set_bondable(const uint8_t *data, uint16_t len) +static void +set_bondable(const uint8_t *data, uint16_t len) { - const struct gap_set_bondable_cmd *cmd = (void *) data; - struct gap_set_bondable_rp rp; + const struct gap_set_bondable_cmd *cmd = (void *) data; + struct gap_set_bondable_rp rp; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - ble_hs_cfg.sm_bonding = cmd->bondable; - if (ble_hs_cfg.sm_bonding) { - current_settings |= BIT(GAP_SETTINGS_BONDABLE); - } else { - current_settings &= ~BIT(GAP_SETTINGS_BONDABLE); - } + ble_hs_cfg.sm_bonding = cmd->bondable; + if (ble_hs_cfg.sm_bonding) { + current_settings |= BIT(GAP_SETTINGS_BONDABLE); + } else { + current_settings &= ~BIT(GAP_SETTINGS_BONDABLE); + } - rp.current_settings = sys_cpu_to_le32(current_settings); + rp.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_SET_BONDABLE, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GAP, GAP_SET_BONDABLE, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); } static struct bt_data ad[10] = { - BT_DATA(BLE_HS_ADV_TYPE_FLAGS, &ad_flags, sizeof(ad_flags)), + BT_DATA(BLE_HS_ADV_TYPE_FLAGS, &ad_flags, sizeof(ad_flags)), }; static struct bt_data sd[10]; -static int set_ad(const struct bt_data *ad, size_t ad_len, - uint8_t *buf, uint8_t *buf_len) +static int +set_ad(const struct bt_data *ad, size_t ad_len, + uint8_t *buf, uint8_t *buf_len) { - int i; + int i; - for (i = 0; i < ad_len; i++) { - buf[(*buf_len)++] = ad[i].data_len + 1; - buf[(*buf_len)++] = ad[i].type; + for (i = 0; i < ad_len; i++) { + buf[(*buf_len)++] = ad[i].data_len + 1; + buf[(*buf_len)++] = ad[i].type; - memcpy(&buf[*buf_len], ad[i].data, - ad[i].data_len); - *buf_len += ad[i].data_len; - } + memcpy(&buf[*buf_len], ad[i].data, + ad[i].data_len); + *buf_len += ad[i].data_len; + } - return 0; + return 0; } -static void start_advertising(const uint8_t *data, uint16_t len) +static void +start_advertising(const uint8_t *data, uint16_t len) { - const struct gap_start_advertising_cmd *cmd = (void *) data; - struct gap_start_advertising_rp rp; - int32_t duration_ms = BLE_HS_FOREVER; - uint8_t buf[BLE_HS_ADV_MAX_SZ]; - uint8_t buf_len = 0; - uint8_t adv_len, sd_len; - int err; + const struct gap_start_advertising_cmd *cmd = (void *) data; + struct gap_start_advertising_rp rp; + int32_t duration_ms = BLE_HS_FOREVER; + uint8_t buf[BLE_HS_ADV_MAX_SZ]; + uint8_t buf_len = 0; + uint8_t adv_len, sd_len; + int err; - int i; + int i; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - for (i = 0, adv_len = 1; i < cmd->adv_data_len; adv_len++) { - if (adv_len >= ARRAY_SIZE(ad)) { - SYS_LOG_ERR("ad[] Out of memory"); - goto fail; - } + for (i = 0, adv_len = 1; i < cmd->adv_data_len; adv_len++) { + if (adv_len >= ARRAY_SIZE(ad)) { + SYS_LOG_ERR("ad[] Out of memory"); + goto fail; + } - ad[adv_len].type = cmd->adv_data[i++]; - ad[adv_len].data_len = cmd->adv_data[i++]; - ad[adv_len].data = &cmd->adv_data[i]; - i += ad[adv_len].data_len; - } + ad[adv_len].type = cmd->adv_data[i++]; + ad[adv_len].data_len = cmd->adv_data[i++]; + ad[adv_len].data = &cmd->adv_data[i]; + i += ad[adv_len].data_len; + } - for (i = 0, sd_len = 0; i < cmd->scan_rsp_len; sd_len++) { - if (sd_len >= ARRAY_SIZE(sd)) { - SYS_LOG_ERR("sd[] Out of memory"); - goto fail; - } + for (i = 0, sd_len = 0; i < cmd->scan_rsp_len; sd_len++) { + if (sd_len >= ARRAY_SIZE(sd)) { + SYS_LOG_ERR("sd[] Out of memory"); + goto fail; + } - sd[sd_len].type = cmd->scan_rsp[i++]; - sd[sd_len].data_len = cmd->scan_rsp[i++]; - sd[sd_len].data = &cmd->scan_rsp[i]; - i += sd[sd_len].data_len; - } + sd[sd_len].type = cmd->scan_rsp[i++]; + sd[sd_len].data_len = cmd->scan_rsp[i++]; + sd[sd_len].data = &cmd->scan_rsp[i]; + i += sd[sd_len].data_len; + } - err = set_ad(ad, adv_len, buf, &buf_len); - if (err) { - goto fail; - } + err = set_ad(ad, adv_len, buf, &buf_len); + if (err) { + goto fail; + } - err = ble_gap_adv_set_data(buf, buf_len); - if (err != 0) { - goto fail; - } + err = ble_gap_adv_set_data(buf, buf_len); + if (err != 0) { + goto fail; + } - if (sd_len) { - buf_len = 0; + if (sd_len) { + buf_len = 0; - err = set_ad(sd, sd_len, buf, &buf_len); - if (err) { - SYS_LOG_ERR("Advertising failed: err %d", err); - goto fail; - } + err = set_ad(sd, sd_len, buf, &buf_len); + if (err) { + SYS_LOG_ERR("Advertising failed: err %d", err); + goto fail; + } - err = ble_gap_adv_rsp_set_data(buf, buf_len); - if (err != 0) { - SYS_LOG_ERR("Advertising failed: err %d", err); - goto fail; - } - } + err = ble_gap_adv_rsp_set_data(buf, buf_len); + if (err != 0) { + SYS_LOG_ERR("Advertising failed: err %d", err); + goto fail; + } + } - if (adv_params.disc_mode == BLE_GAP_DISC_MODE_LTD) { - duration_ms = MYNEWT_VAL(BTTESTER_LTD_ADV_TIMEOUT); - } + if (adv_params.disc_mode == BLE_GAP_DISC_MODE_LTD) { + duration_ms = MYNEWT_VAL(BTTESTER_LTD_ADV_TIMEOUT); + } #if MYNEWT_VAL(BTTESTER_PRIVACY_MODE) && MYNEWT_VAL(BTTESTER_USE_NRPA) - if (MYNEWT_VAL(BTTESTER_NRPA_TIMEOUT) < duration_ms / 1000) { - advertising_start = os_get_uptime_usec(); - os_callout_reset(&bttester_nrpa_rotate_timer, + if (MYNEWT_VAL(BTTESTER_NRPA_TIMEOUT) < duration_ms / 1000) { + advertising_start = os_get_uptime_usec(); + os_callout_reset(&bttester_nrpa_rotate_timer, OS_TICKS_PER_SEC * MYNEWT_VAL(BTTESTER_NRPA_TIMEOUT)); - } + } #endif - err = ble_gap_adv_start(own_addr_type, NULL, duration_ms, - &adv_params, gap_event_cb, NULL); - if (err) { - SYS_LOG_ERR("Advertising failed: err %d", err); - goto fail; - } + err = ble_gap_adv_start(own_addr_type, NULL, duration_ms, + &adv_params, gap_event_cb, NULL); + if (err) { + SYS_LOG_ERR("Advertising failed: err %d", err); + goto fail; + } - current_settings |= BIT(GAP_SETTINGS_ADVERTISING); - rp.current_settings = sys_cpu_to_le32(current_settings); + current_settings |= BIT(GAP_SETTINGS_ADVERTISING); + rp.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); - return; + tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); + return; fail: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void stop_advertising(const uint8_t *data, uint16_t len) +static void +stop_advertising(const uint8_t *data, uint16_t len) { - struct gap_stop_advertising_rp rp; + struct gap_stop_advertising_rp rp; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (ble_gap_adv_stop() != 0) { - tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - return; - } + if (ble_gap_adv_stop() != 0) { + tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + return; + } - current_settings &= ~BIT(GAP_SETTINGS_ADVERTISING); - rp.current_settings = sys_cpu_to_le32(current_settings); + current_settings &= ~BIT(GAP_SETTINGS_ADVERTISING); + rp.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); } -static uint8_t get_ad_flags(const uint8_t *data, uint8_t data_len) +static uint8_t +get_ad_flags(const uint8_t *data, uint8_t data_len) { - uint8_t len, i; + uint8_t len, i; - /* Parse advertisement to get flags */ - for (i = 0; i < data_len; i += len - 1) { - len = data[i++]; - if (!len) { - break; - } + /* Parse advertisement to get flags */ + for (i = 0; i < data_len; i += len - 1) { + len = data[i++]; + if (!len) { + break; + } - /* Check if field length is correct */ - if (len > (data_len - i) || (data_len - i) < 1) { - break; - } + /* Check if field length is correct */ + if (len > (data_len - i) || (data_len - i) < 1) { + break; + } - switch (data[i++]) { - case BLE_HS_ADV_TYPE_FLAGS: - return data[i]; - default: - break; - } - } + switch (data[i++]) { + case BLE_HS_ADV_TYPE_FLAGS: + return data[i]; + default: + break; + } + } - return 0; + return 0; } static uint8_t discovery_flags; static struct os_mbuf *adv_buf; -static void store_adv(const ble_addr_t *addr, int8_t rssi, - const uint8_t *data, uint8_t len) +static void +store_adv(const ble_addr_t *addr, int8_t rssi, + const uint8_t *data, uint8_t len) { - struct gap_device_found_ev *ev; + struct gap_device_found_ev *ev; - /* cleanup */ - net_buf_simple_init(adv_buf, 0); + /* cleanup */ + net_buf_simple_init(adv_buf, 0); - ev = net_buf_simple_add(adv_buf, sizeof(*ev)); + ev = net_buf_simple_add(adv_buf, sizeof(*ev)); - memcpy(ev->address, addr->val, sizeof(ev->address)); - ev->address_type = addr->type; - ev->rssi = rssi; - ev->flags = GAP_DEVICE_FOUND_FLAG_AD | GAP_DEVICE_FOUND_FLAG_RSSI; - ev->eir_data_len = len; - memcpy(net_buf_simple_add(adv_buf, len), data, len); + memcpy(ev->address, addr->val, sizeof(ev->address)); + ev->address_type = addr->type; + ev->rssi = rssi; + ev->flags = GAP_DEVICE_FOUND_FLAG_AD | GAP_DEVICE_FOUND_FLAG_RSSI; + ev->eir_data_len = len; + memcpy(net_buf_simple_add(adv_buf, len), data, len); } -static void device_found(ble_addr_t *addr, int8_t rssi, uint8_t evtype, - const uint8_t *data, uint8_t len) +static void +device_found(ble_addr_t *addr, int8_t rssi, uint8_t evtype, + const uint8_t *data, uint8_t len) { - struct gap_device_found_ev *ev; - ble_addr_t a; + struct gap_device_found_ev *ev; + ble_addr_t a; - /* if General/Limited Discovery - parse Advertising data to get flags */ - if (!(discovery_flags & GAP_DISCOVERY_FLAG_LE_OBSERVE) && - (evtype != BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP)) { - uint8_t flags = get_ad_flags(data, len); + /* if General/Limited Discovery - parse Advertising data to get flags */ + if (!(discovery_flags & GAP_DISCOVERY_FLAG_LE_OBSERVE) && + (evtype != BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP)) { + uint8_t flags = get_ad_flags(data, len); - /* ignore non-discoverable devices */ - if (!(flags & BLE_AD_DISCOV_MASK)) { - SYS_LOG_DBG("Non discoverable, skipping"); - return; - } + /* ignore non-discoverable devices */ + if (!(flags & BLE_AD_DISCOV_MASK)) { + SYS_LOG_DBG("Non discoverable, skipping"); + return; + } - /* if Limited Discovery - ignore general discoverable devices */ - if ((discovery_flags & GAP_DISCOVERY_FLAG_LIMITED) && - !(flags & BLE_HS_ADV_F_DISC_LTD)) { - SYS_LOG_DBG("General discoverable, skipping"); - return; - } - } + /* if Limited Discovery - ignore general discoverable devices */ + if ((discovery_flags & GAP_DISCOVERY_FLAG_LIMITED) && + !(flags & BLE_HS_ADV_F_DISC_LTD)) { + SYS_LOG_DBG("General discoverable, skipping"); + return; + } + } - /* attach Scan Response data */ - if (evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) { - /* skip if there is no pending advertisement */ - if (!adv_buf->om_len) { - SYS_LOG_INF("No pending advertisement, skipping"); - return; - } + /* attach Scan Response data */ + if (evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) { + /* skip if there is no pending advertisement */ + if (!adv_buf->om_len) { + SYS_LOG_INF("No pending advertisement, skipping"); + return; + } - ev = (void *) adv_buf->om_data; - a.type = ev->address_type; - memcpy(a.val, ev->address, sizeof(a.val)); + ev = (void *) adv_buf->om_data; + a.type = ev->address_type; + memcpy(a.val, ev->address, sizeof(a.val)); - /* - * in general, the Scan Response comes right after the - * Advertisement, but if not if send stored event and ignore - * this one - */ - if (ble_addr_cmp(addr, &a)) { - SYS_LOG_INF("Address does not match, skipping"); - goto done; - } + /* + * in general, the Scan Response comes right after the + * Advertisement, but if not if send stored event and ignore + * this one + */ + if (ble_addr_cmp(addr, &a)) { + SYS_LOG_INF("Address does not match, skipping"); + goto done; + } - ev->eir_data_len += len; - ev->flags |= GAP_DEVICE_FOUND_FLAG_SD; + ev->eir_data_len += len; + ev->flags |= GAP_DEVICE_FOUND_FLAG_SD; - memcpy(net_buf_simple_add(adv_buf, len), data, len); + memcpy(net_buf_simple_add(adv_buf, len), data, len); - goto done; - } + goto done; + } - /* - * if there is another pending advertisement, send it and store the - * current one - */ - if (adv_buf->om_len) { - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND, - CONTROLLER_INDEX, adv_buf->om_data, - adv_buf->om_len); - } + /* + * if there is another pending advertisement, send it and store the + * current one + */ + if (adv_buf->om_len) { + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND, + CONTROLLER_INDEX, adv_buf->om_data, + adv_buf->om_len); + } - store_adv(addr, rssi, data, len); + store_adv(addr, rssi, data, len); - /* if Active Scan and scannable event - wait for Scan Response */ - if ((discovery_flags & GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN) && - (evtype == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || - evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND)) { - SYS_LOG_DBG("Waiting for scan response"); - return; - } + /* if Active Scan and scannable event - wait for Scan Response */ + if ((discovery_flags & GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN) && + (evtype == BLE_HCI_ADV_RPT_EVTYPE_ADV_IND || + evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND)) { + SYS_LOG_DBG("Waiting for scan response"); + return; + } done: - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND, - CONTROLLER_INDEX, adv_buf->om_data, adv_buf->om_len); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND, + CONTROLLER_INDEX, adv_buf->om_data, adv_buf->om_len); } -static int discovery_cb(struct ble_gap_event *event, void *arg) +static int +discovery_cb(struct ble_gap_event *event, void *arg) { - if (event->type == BLE_GAP_EVENT_DISC) { - device_found(&event->disc.addr, event->disc.rssi, - event->disc.event_type, event->disc.data, - event->disc.length_data); - } + if (event->type == BLE_GAP_EVENT_DISC) { + device_found(&event->disc.addr, event->disc.rssi, + event->disc.event_type, event->disc.data, + event->disc.length_data); + } - return 0; + return 0; } -static void start_discovery(const uint8_t *data, uint16_t len) +static void +start_discovery(const uint8_t *data, uint16_t len) { - const struct gap_start_discovery_cmd *cmd = (void *) data; - struct ble_gap_disc_params params = {0}; - uint8_t status; + const struct gap_start_discovery_cmd *cmd = (void *) data; + struct ble_gap_disc_params params = {0}; + uint8_t status; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - /* only LE scan is supported */ - if (cmd->flags & GAP_DISCOVERY_FLAG_BREDR) { - status = BTP_STATUS_FAILED; - goto reply; - } + /* only LE scan is supported */ + if (cmd->flags & GAP_DISCOVERY_FLAG_BREDR) { + status = BTP_STATUS_FAILED; + goto reply; + } - params.passive = (cmd->flags & GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN) == 0; - params.limited = (cmd->flags & GAP_DISCOVERY_FLAG_LIMITED) > 0; - params.filter_duplicates = 1; + params.passive = (cmd->flags & GAP_DISCOVERY_FLAG_LE_ACTIVE_SCAN) == 0; + params.limited = (cmd->flags & GAP_DISCOVERY_FLAG_LIMITED) > 0; + params.filter_duplicates = 1; - if (ble_gap_disc(own_addr_type, BLE_HS_FOREVER, - ¶ms, discovery_cb, NULL) != 0) { - status = BTP_STATUS_FAILED; - goto reply; - } + if (ble_gap_disc(own_addr_type, BLE_HS_FOREVER, + ¶ms, discovery_cb, NULL) != 0) { + status = BTP_STATUS_FAILED; + goto reply; + } - net_buf_simple_init(adv_buf, 0); - discovery_flags = cmd->flags; + net_buf_simple_init(adv_buf, 0); + discovery_flags = cmd->flags; - status = BTP_STATUS_SUCCESS; + status = BTP_STATUS_SUCCESS; reply: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DISCOVERY, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DISCOVERY, CONTROLLER_INDEX, + status); } -static void stop_discovery(const uint8_t *data, uint16_t len) +static void +stop_discovery(const uint8_t *data, uint16_t len) { - uint8_t status = BTP_STATUS_SUCCESS; + uint8_t status = BTP_STATUS_SUCCESS; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (ble_gap_disc_cancel() != 0) { - status = BTP_STATUS_FAILED; - } + if (ble_gap_disc_cancel() != 0) { + status = BTP_STATUS_FAILED; + } - tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_DISCOVERY, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_DISCOVERY, CONTROLLER_INDEX, + status); } - /* Bluetooth Core Spec v5.1 | Section 10.7.1 * If a privacy-enabled Peripheral, that has a stored bond, * receives a resolvable private address, the Host may resolve @@ -685,302 +701,316 @@ static void stop_discovery(const uint8_t *data, uint16_t len) * If the resolution procedure fails, then the Host shall disconnect * with the error code "Authentication failure" [...] */ -static void periph_privacy(struct ble_gap_conn_desc desc) +static void +periph_privacy(struct ble_gap_conn_desc desc) { #if !MYNEWT_VAL(BTTESTER_PRIVACY_MODE) - return; + return; #endif - int count; + int count; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - ble_store_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC, &count); - if (count > 0 && BLE_ADDR_IS_RPA(&desc.peer_id_addr)) { - SYS_LOG_DBG("Authentication failure, disconnecting"); - ble_gap_terminate(desc.conn_handle, BLE_ERR_AUTH_FAIL); - } + ble_store_util_count(BLE_STORE_OBJ_TYPE_PEER_SEC, &count); + if (count > 0 && BLE_ADDR_IS_RPA(&desc.peer_id_addr)) { + SYS_LOG_DBG("Authentication failure, disconnecting"); + ble_gap_terminate(desc.conn_handle, BLE_ERR_AUTH_FAIL); + } } -static void device_connected_ev_send(struct os_event *ev) +static void +device_connected_ev_send(struct os_event *ev) { - struct ble_gap_conn_desc desc; - int rc; + struct ble_gap_conn_desc desc; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = gap_conn_find_by_addr((ble_addr_t *)&connected_ev, &desc); - if (rc) { - tester_rsp(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - return; - } + rc = gap_conn_find_by_addr((ble_addr_t *) &connected_ev, &desc); + if (rc) { + tester_rsp(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + return; + } - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, - CONTROLLER_INDEX, (uint8_t *) &connected_ev, - sizeof(connected_ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, + CONTROLLER_INDEX, (uint8_t *) &connected_ev, + sizeof(connected_ev)); - periph_privacy(desc); + periph_privacy(desc); } -static void le_connected(uint16_t conn_handle, int status) +static void +le_connected(uint16_t conn_handle, int status) { - struct ble_gap_conn_desc desc; - ble_addr_t *addr; - int rc; + struct ble_gap_conn_desc desc; + ble_addr_t *addr; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (status != 0) { - return; - } + if (status != 0) { + return; + } - rc = ble_gap_conn_find(conn_handle, &desc); - if (rc) { - return; - } + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } - peer_id_addr = desc.peer_id_addr; - peer_ota_addr = desc.peer_ota_addr; + peer_id_addr = desc.peer_id_addr; + peer_ota_addr = desc.peer_ota_addr; - addr = &desc.peer_id_addr; + addr = &desc.peer_id_addr; - memcpy(connected_ev.address, addr->val, sizeof(connected_ev.address)); - connected_ev.address_type = addr->type; - connected_ev.conn_itvl = desc.conn_itvl; - connected_ev.conn_latency = desc.conn_latency; - connected_ev.supervision_timeout = desc.supervision_timeout; + memcpy(connected_ev.address, addr->val, sizeof(connected_ev.address)); + connected_ev.address_type = addr->type; + connected_ev.conn_itvl = desc.conn_itvl; + connected_ev.conn_latency = desc.conn_latency; + connected_ev.supervision_timeout = desc.supervision_timeout; #if MYNEWT_VAL(BTTESTER_CONN_RETRY) - os_callout_reset(&connected_ev_co, - os_time_ms_to_ticks32( - CONNECTED_EV_DELAY_MS(desc.conn_itvl))); + os_callout_reset(&connected_ev_co, + os_time_ms_to_ticks32( + CONNECTED_EV_DELAY_MS(desc.conn_itvl))); #else - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, - CONTROLLER_INDEX, (uint8_t *) &connected_ev, - sizeof(connected_ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, + CONTROLLER_INDEX, (uint8_t *) &connected_ev, + sizeof(connected_ev)); #endif } -static void le_disconnected(struct ble_gap_conn_desc *conn, int reason) +static void +le_disconnected(struct ble_gap_conn_desc *conn, int reason) { - struct gap_device_disconnected_ev ev; - ble_addr_t *addr = &conn->peer_ota_addr; + struct gap_device_disconnected_ev ev; + ble_addr_t *addr = &conn->peer_ota_addr; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); #if MYNEWT_VAL(BTTESTER_CONN_RETRY) - int rc; + int rc; - if ((reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_ESTABLISHMENT)) && - os_callout_queued(&connected_ev_co)) { - if (connection_attempts < MYNEWT_VAL(BTTESTER_CONN_RETRY)) { - os_callout_stop(&connected_ev_co); + if ((reason == BLE_HS_HCI_ERR(BLE_ERR_CONN_ESTABLISHMENT)) && + os_callout_queued(&connected_ev_co)) { + if (connection_attempts < MYNEWT_VAL(BTTESTER_CONN_RETRY)) { + os_callout_stop(&connected_ev_co); - /* try connecting again */ - rc = ble_gap_connect(own_addr_type, addr, 0, - &dflt_conn_params, gap_event_cb, - NULL); + /* try connecting again */ + rc = ble_gap_connect(own_addr_type, addr, 0, + &dflt_conn_params, gap_event_cb, + NULL); - if (rc == 0) { - connection_attempts++; - return; - } - } - } else if (os_callout_queued(&connected_ev_co)) { - os_callout_stop(&connected_ev_co); - return; - } + if (rc == 0) { + connection_attempts++; + return; + } + } + } else if (os_callout_queued(&connected_ev_co)) { + os_callout_stop(&connected_ev_co); + return; + } #endif - connection_attempts = 0; - memset(&connected_ev, 0, sizeof(connected_ev)); + connection_attempts = 0; + memset(&connected_ev, 0, sizeof(connected_ev)); - memcpy(ev.address, addr->val, sizeof(ev.address)); - ev.address_type = addr->type; + memcpy(ev.address, addr->val, sizeof(ev.address)); + ev.address_type = addr->type; - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_DISCONNECTED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_DISCONNECTED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void auth_passkey_oob(uint16_t conn_handle) +static void +auth_passkey_oob(uint16_t conn_handle) { - struct ble_gap_conn_desc desc; - struct ble_sm_io pk; - int rc; + struct ble_gap_conn_desc desc; + struct ble_sm_io pk; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find(conn_handle, &desc); - if (rc) { - return; - } + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } - memcpy(pk.oob, oob, sizeof(oob)); - pk.action = BLE_SM_IOACT_OOB; + memcpy(pk.oob, oob, sizeof(oob)); + pk.action = BLE_SM_IOACT_OOB; - rc = ble_sm_inject_io(conn_handle, &pk); - assert(rc == 0); + rc = ble_sm_inject_io(conn_handle, &pk); + assert(rc == 0); } -static void auth_passkey_display(uint16_t conn_handle, unsigned int passkey) +static void +auth_passkey_display(uint16_t conn_handle, unsigned int passkey) { - struct ble_gap_conn_desc desc; - struct gap_passkey_display_ev ev; - ble_addr_t *addr; - struct ble_sm_io pk; - int rc; + struct ble_gap_conn_desc desc; + struct gap_passkey_display_ev ev; + ble_addr_t *addr; + struct ble_sm_io pk; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find(conn_handle, &desc); - if (rc) { - return; - } + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } - rc = ble_hs_hci_rand(&pk.passkey, sizeof(pk.passkey)); - assert(rc == 0); - /* Max value is 999999 */ - pk.passkey %= 1000000; - pk.action = BLE_SM_IOACT_DISP; + rc = ble_hs_hci_rand(&pk.passkey, sizeof(pk.passkey)); + assert(rc == 0); + /* Max value is 999999 */ + pk.passkey %= 1000000; + pk.action = BLE_SM_IOACT_DISP; - rc = ble_sm_inject_io(conn_handle, &pk); - assert(rc == 0); + rc = ble_sm_inject_io(conn_handle, &pk); + assert(rc == 0); - addr = &desc.peer_ota_addr; + addr = &desc.peer_ota_addr; - memcpy(ev.address, addr->val, sizeof(ev.address)); - ev.address_type = addr->type; - ev.passkey = sys_cpu_to_le32(pk.passkey); + memcpy(ev.address, addr->val, sizeof(ev.address)); + ev.address_type = addr->type; + ev.passkey = sys_cpu_to_le32(pk.passkey); - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_DISPLAY, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_DISPLAY, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void auth_passkey_entry(uint16_t conn_handle) +static void +auth_passkey_entry(uint16_t conn_handle) { - struct ble_gap_conn_desc desc; - struct gap_passkey_entry_req_ev ev; - ble_addr_t *addr; - int rc; + struct ble_gap_conn_desc desc; + struct gap_passkey_entry_req_ev ev; + ble_addr_t *addr; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find(conn_handle, &desc); - if (rc) { - return; - } + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } - addr = &desc.peer_ota_addr; + addr = &desc.peer_ota_addr; - memcpy(ev.address, addr->val, sizeof(ev.address)); - ev.address_type = addr->type; + memcpy(ev.address, addr->val, sizeof(ev.address)); + ev.address_type = addr->type; - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_ENTRY_REQ, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_ENTRY_REQ, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void auth_passkey_numcmp(uint16_t conn_handle, unsigned int passkey) +static void +auth_passkey_numcmp(uint16_t conn_handle, unsigned int passkey) { - struct ble_gap_conn_desc desc; - struct gap_passkey_confirm_req_ev ev; - ble_addr_t *addr; - int rc; + struct ble_gap_conn_desc desc; + struct gap_passkey_confirm_req_ev ev; + ble_addr_t *addr; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find(conn_handle, &desc); - if (rc) { - return; - } + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } - addr = &desc.peer_ota_addr; + addr = &desc.peer_ota_addr; - memcpy(ev.address, addr->val, sizeof(ev.address)); - ev.address_type = addr->type; - ev.passkey = sys_cpu_to_le32(passkey); + memcpy(ev.address, addr->val, sizeof(ev.address)); + ev.address_type = addr->type; + ev.passkey = sys_cpu_to_le32(passkey); - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_CONFIRM_REQ, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_CONFIRM_REQ, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void auth_passkey_oob_sc(uint16_t conn_handle) +static void +auth_passkey_oob_sc(uint16_t conn_handle) { - int rc; - struct ble_sm_io pk; + int rc; + struct ble_sm_io pk; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memset(&pk, 0, sizeof(pk)); + memset(&pk, 0, sizeof(pk)); - pk.oob_sc_data.local = &oob_data_local; + pk.oob_sc_data.local = &oob_data_local; - if (ble_hs_cfg.sm_oob_data_flag) { - pk.oob_sc_data.remote = &oob_data_remote; - } + if (ble_hs_cfg.sm_oob_data_flag) { + pk.oob_sc_data.remote = &oob_data_remote; + } - pk.action = BLE_SM_IOACT_OOB_SC; - rc = ble_sm_inject_io(conn_handle, &pk); - if (rc != 0) { - console_printf("error providing oob; rc=%d\n", rc); - } + pk.action = BLE_SM_IOACT_OOB_SC; + rc = ble_sm_inject_io(conn_handle, &pk); + if (rc != 0) { + console_printf("error providing oob; rc=%d\n", rc); + } } -static void le_passkey_action(uint16_t conn_handle, - struct ble_gap_passkey_params *params) +static void +le_passkey_action(uint16_t conn_handle, + struct ble_gap_passkey_params *params) { - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - switch (params->action) { - case BLE_SM_IOACT_NONE: - break; - case BLE_SM_IOACT_OOB: - auth_passkey_oob(conn_handle); - break; - case BLE_SM_IOACT_INPUT: - auth_passkey_entry(conn_handle); - break; - case BLE_SM_IOACT_DISP: - auth_passkey_display(conn_handle, params->numcmp); - break; - case BLE_SM_IOACT_NUMCMP: - auth_passkey_numcmp(conn_handle, params->numcmp); - break; - case BLE_SM_IOACT_OOB_SC: - auth_passkey_oob_sc(conn_handle); - break; - default: - assert(0); - } + switch (params->action) { + case BLE_SM_IOACT_NONE: + break; + case BLE_SM_IOACT_OOB: + auth_passkey_oob(conn_handle); + break; + case BLE_SM_IOACT_INPUT: + auth_passkey_entry(conn_handle); + break; + case BLE_SM_IOACT_DISP: + auth_passkey_display(conn_handle, + params->numcmp); + break; + case BLE_SM_IOACT_NUMCMP: + auth_passkey_numcmp(conn_handle, + params->numcmp); + break; + case BLE_SM_IOACT_OOB_SC: + auth_passkey_oob_sc(conn_handle); + break; + default: + assert(0); + } } -static void le_identity_resolved(uint16_t conn_handle) +static void +le_identity_resolved(uint16_t conn_handle) { - struct ble_gap_conn_desc desc; - struct gap_identity_resolved_ev ev; - int rc; + struct ble_gap_conn_desc desc; + struct gap_identity_resolved_ev ev; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find(conn_handle, &desc); - if (rc) { - return; - } + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } - peer_id_addr = desc.peer_id_addr; - peer_ota_addr = desc.peer_ota_addr; + peer_id_addr = desc.peer_id_addr; + peer_ota_addr = desc.peer_ota_addr; - ev.address_type = desc.peer_ota_addr.type; - memcpy(ev.address, desc.peer_ota_addr.val, sizeof(ev.address)); + ev.address_type = desc.peer_ota_addr.type; + memcpy(ev.address, desc.peer_ota_addr.val, sizeof(ev.address)); - ev.identity_address_type = desc.peer_id_addr.type; - memcpy(ev.identity_address, desc.peer_id_addr.val, - sizeof(ev.identity_address)); + ev.identity_address_type = desc.peer_id_addr.type; + memcpy(ev.identity_address, desc.peer_id_addr.val, + sizeof(ev.identity_address)); - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_IDENTITY_RESOLVED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_IDENTITY_RESOLVED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void le_pairing_failed(uint16_t conn_handle, int reason) +static void +le_pairing_failed(uint16_t conn_handle, int reason) { struct ble_gap_conn_desc desc; struct gap_sec_pairing_failed_ev ev; @@ -1005,52 +1035,55 @@ static void le_pairing_failed(uint16_t conn_handle, int reason) CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void le_conn_param_update(struct ble_gap_conn_desc *desc) +static void +le_conn_param_update(struct ble_gap_conn_desc *desc) { - struct gap_conn_param_update_ev ev; + struct gap_conn_param_update_ev ev; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - ev.address_type = desc->peer_ota_addr.type; - memcpy(ev.address, desc->peer_ota_addr.val, sizeof(ev.address)); + ev.address_type = desc->peer_ota_addr.type; + memcpy(ev.address, desc->peer_ota_addr.val, sizeof(ev.address)); - ev.conn_itvl = desc->conn_itvl; - ev.conn_latency = desc->conn_latency; - ev.supervision_timeout = desc->supervision_timeout; + ev.conn_itvl = desc->conn_itvl; + ev.conn_latency = desc->conn_latency; + ev.supervision_timeout = desc->supervision_timeout; - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_CONN_PARAM_UPDATE, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_CONN_PARAM_UPDATE, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void le_encryption_changed(struct ble_gap_conn_desc *desc) +static void +le_encryption_changed(struct ble_gap_conn_desc *desc) { - struct gap_sec_level_changed_ev ev; + struct gap_sec_level_changed_ev ev; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - encrypted = (bool) desc->sec_state.encrypted; + encrypted = (bool) desc->sec_state.encrypted; - ev.address_type = desc->peer_ota_addr.type; - memcpy(ev.address, desc->peer_ota_addr.val, sizeof(ev.address)); - ev.level = 0; + ev.address_type = desc->peer_ota_addr.type; + memcpy(ev.address, desc->peer_ota_addr.val, sizeof(ev.address)); + ev.level = 0; - if (desc->sec_state.encrypted) { - if (desc->sec_state.authenticated) { - if (desc->sec_state.key_size == 16) { - ev.level = 3; - } else { - ev.level = 2; - } - } else { - ev.level = 1; - } - } + if (desc->sec_state.encrypted) { + if (desc->sec_state.authenticated) { + if (desc->sec_state.key_size == 16) { + ev.level = 3; + } else { + ev.level = 2; + } + } else { + ev.level = 1; + } + } - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_SEC_LEVEL_CHANGED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_SEC_LEVEL_CHANGED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void bond_lost(uint16_t conn_handle) +static void +bond_lost(uint16_t conn_handle) { struct gap_bond_lost_ev ev; struct ble_gap_conn_desc desc; @@ -1061,766 +1094,807 @@ static void bond_lost(uint16_t conn_handle) memcpy(ev.address, &desc.peer_id_addr, sizeof(ev.address)); ev.address_type = desc.peer_id_addr.type; - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_BOND_LOST, CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, + GAP_EV_BOND_LOST, + CONTROLLER_INDEX, + (uint8_t *) &ev, + sizeof(ev)); } -static void print_bytes(const uint8_t *bytes, int len) +static void +print_bytes(const uint8_t *bytes, int len) { - int i; + int i; - for (i = 0; i < len; i++) { - console_printf("%s0x%02x", i != 0 ? ":" : "", bytes[i]); - } + for (i = 0; i < len; i++) { + console_printf("%s0x%02x", i != 0 ? ":" : "", bytes[i]); + } } -static void print_mbuf(const struct os_mbuf *om) +static void +print_mbuf(const struct os_mbuf *om) { - int colon; + int colon; - colon = 0; - while (om != NULL) { - if (colon) { - console_printf(":"); - } else { - colon = 1; - } - print_bytes(om->om_data, om->om_len); - om = SLIST_NEXT(om, om_next); - } + colon = 0; + while (om != NULL) { + if (colon) { + console_printf(":"); + } else { + colon = 1; + } + print_bytes(om->om_data, om->om_len); + om = SLIST_NEXT(om, om_next); + } } -static void print_addr(const void *addr) +static void +print_addr(const void *addr) { - const uint8_t *u8p; + const uint8_t *u8p; - u8p = addr; - console_printf("%02x:%02x:%02x:%02x:%02x:%02x", - u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]); + u8p = addr; + console_printf("%02x:%02x:%02x:%02x:%02x:%02x", + u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]); } -static void print_conn_desc(const struct ble_gap_conn_desc *desc) +static void +print_conn_desc(const struct ble_gap_conn_desc *desc) { - console_printf("handle=%d our_ota_addr_type=%d our_ota_addr=", - desc->conn_handle, desc->our_ota_addr.type); - print_addr(desc->our_ota_addr.val); - console_printf(" our_id_addr_type=%d our_id_addr=", - desc->our_id_addr.type); - print_addr(desc->our_id_addr.val); - console_printf(" peer_ota_addr_type=%d peer_ota_addr=", - desc->peer_ota_addr.type); - print_addr(desc->peer_ota_addr.val); - console_printf(" peer_id_addr_type=%d peer_id_addr=", - desc->peer_id_addr.type); - print_addr(desc->peer_id_addr.val); - console_printf(" conn_itvl=%d conn_latency=%d supervision_timeout=%d " - "key_sz=%d encrypted=%d authenticated=%d bonded=%d\n", - desc->conn_itvl, desc->conn_latency, - desc->supervision_timeout, - desc->sec_state.key_size, - desc->sec_state.encrypted, - desc->sec_state.authenticated, - desc->sec_state.bonded); + console_printf("handle=%d our_ota_addr_type=%d our_ota_addr=", + desc->conn_handle, desc->our_ota_addr.type); + print_addr(desc->our_ota_addr.val); + console_printf(" our_id_addr_type=%d our_id_addr=", + desc->our_id_addr.type); + print_addr(desc->our_id_addr.val); + console_printf(" peer_ota_addr_type=%d peer_ota_addr=", + desc->peer_ota_addr.type); + print_addr(desc->peer_ota_addr.val); + console_printf(" peer_id_addr_type=%d peer_id_addr=", + desc->peer_id_addr.type); + print_addr(desc->peer_id_addr.val); + console_printf(" conn_itvl=%d conn_latency=%d supervision_timeout=%d " + "key_sz=%d encrypted=%d authenticated=%d bonded=%d\n", + desc->conn_itvl, desc->conn_latency, + desc->supervision_timeout, + desc->sec_state.key_size, + desc->sec_state.encrypted, + desc->sec_state.authenticated, + desc->sec_state.bonded); } -static void adv_complete(void) +static void +adv_complete(void) { - struct gap_new_settings_ev ev; + struct gap_new_settings_ev ev; - current_settings &= ~BIT(GAP_SETTINGS_ADVERTISING); - ev.current_settings = sys_cpu_to_le32(current_settings); + current_settings &= ~BIT(GAP_SETTINGS_ADVERTISING); + ev.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_EV_NEW_SETTINGS, CONTROLLER_INDEX, - (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_NEW_SETTINGS, CONTROLLER_INDEX, + (uint8_t *) &ev, sizeof(ev)); } -static int gap_event_cb(struct ble_gap_event *event, void *arg) +static int +gap_event_cb(struct ble_gap_event *event, void *arg) { - struct ble_gap_conn_desc desc; - int rc; + struct ble_gap_conn_desc desc; + int rc; - switch (event->type) { - case BLE_GAP_EVENT_ADV_COMPLETE: - console_printf("advertising complete; reason=%d\n", - event->adv_complete.reason); - break; - case BLE_GAP_EVENT_CONNECT: - console_printf("connection %s; status=%d ", - event->connect.status == 0 ? "established" : "failed", - event->connect.status); - if (event->connect.status == 0) { - rc = ble_gap_conn_find(event->connect.conn_handle, &desc); - assert(rc == 0); - print_conn_desc(&desc); - } + switch (event->type) { + case BLE_GAP_EVENT_ADV_COMPLETE: + console_printf("advertising complete; reason=%d\n", + event->adv_complete.reason); + break; + case BLE_GAP_EVENT_CONNECT: + console_printf("connection %s; status=%d ", + event->connect.status == 0 ? "established" + : "failed", + event->connect.status); + if (event->connect.status == 0) { + rc = ble_gap_conn_find(event->connect.conn_handle, &desc); + assert(rc == 0); + print_conn_desc(&desc); + } - if (desc.role == BLE_GAP_ROLE_SLAVE) { - adv_complete(); - } + if (desc.role == BLE_GAP_ROLE_SLAVE) { + adv_complete(); + } - le_connected(event->connect.conn_handle, - event->connect.status); - break; - case BLE_GAP_EVENT_DISCONNECT: - console_printf("disconnect; reason=%d ", event->disconnect.reason); - print_conn_desc(&event->disconnect.conn); - le_disconnected(&event->disconnect.conn, - event->disconnect.reason); - break; - case BLE_GAP_EVENT_ENC_CHANGE: - console_printf("encryption change event; status=%d ", event->enc_change.status); - rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc); - assert(rc == 0); - print_conn_desc(&desc); - le_encryption_changed(&desc); - if (event->enc_change.status == BLE_HS_HCI_ERR(BLE_ERR_PINKEY_MISSING)) { - bond_lost(event->enc_change.conn_handle); - } - break; - case BLE_GAP_EVENT_PASSKEY_ACTION: - console_printf("passkey action event; action=%d", - event->passkey.params.action); - if (event->passkey.params.action == BLE_SM_IOACT_NUMCMP) { - console_printf(" numcmp=%lu", - (unsigned long)event->passkey.params.numcmp); - } - console_printf("\n"); - le_passkey_action(event->passkey.conn_handle, - &event->passkey.params); - break; - case BLE_GAP_EVENT_IDENTITY_RESOLVED: - console_printf("identity resolved "); - rc = ble_gap_conn_find(event->identity_resolved.conn_handle, &desc); - assert(rc == 0); - print_conn_desc(&desc); - le_identity_resolved(event->identity_resolved.conn_handle); - break; - case BLE_GAP_EVENT_NOTIFY_RX: - console_printf("notification rx event; attr_handle=%d indication=%d " - "len=%d data=", - event->notify_rx.attr_handle, - event->notify_rx.indication, - OS_MBUF_PKTLEN(event->notify_rx.om)); + le_connected(event->connect.conn_handle, + event->connect.status); + break; + case BLE_GAP_EVENT_DISCONNECT: + console_printf("disconnect; reason=%d ", + event->disconnect.reason); + print_conn_desc(&event->disconnect.conn); + le_disconnected(&event->disconnect.conn, + event->disconnect.reason); + break; + case BLE_GAP_EVENT_ENC_CHANGE: + console_printf("encryption change event; status=%d ", + event->enc_change.status); + rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc); + assert(rc == 0); + print_conn_desc(&desc); + le_encryption_changed(&desc); + if (event->enc_change.status + == BLE_HS_HCI_ERR(BLE_ERR_PINKEY_MISSING)) { + bond_lost(event->enc_change.conn_handle); + } + break; + case BLE_GAP_EVENT_PASSKEY_ACTION: + console_printf("passkey action event; action=%d", + event->passkey.params.action); + if (event->passkey.params.action == BLE_SM_IOACT_NUMCMP) { + console_printf(" numcmp=%lu", + (unsigned long) event->passkey.params.numcmp); + } + console_printf("\n"); + le_passkey_action(event->passkey.conn_handle, + &event->passkey.params); + break; + case BLE_GAP_EVENT_IDENTITY_RESOLVED: + console_printf("identity resolved "); + rc = ble_gap_conn_find(event->identity_resolved.conn_handle, &desc); + assert(rc == 0); + print_conn_desc(&desc); + le_identity_resolved(event->identity_resolved.conn_handle); + break; + case BLE_GAP_EVENT_NOTIFY_RX: + console_printf( + "notification rx event; attr_handle=%d indication=%d " + "len=%d data=", + event->notify_rx.attr_handle, + event->notify_rx.indication, + OS_MBUF_PKTLEN(event->notify_rx.om)); - print_mbuf(event->notify_rx.om); - console_printf("\n"); - tester_gattc_notify_rx_ev(event->notify_rx.conn_handle, - event->notify_rx.attr_handle, - event->notify_rx.indication, - event->notify_rx.om); - break; - case BLE_GAP_EVENT_SUBSCRIBE: - console_printf("subscribe event; conn_handle=%d attr_handle=%d " - "reason=%d prevn=%d curn=%d previ=%d curi=%d\n", - event->subscribe.conn_handle, - event->subscribe.attr_handle, - event->subscribe.reason, - event->subscribe.prev_notify, - event->subscribe.cur_notify, - event->subscribe.prev_indicate, - event->subscribe.cur_indicate); - tester_gatt_subscribe_ev(event->subscribe.conn_handle, - event->subscribe.attr_handle, - event->subscribe.reason, - event->subscribe.prev_notify, - event->subscribe.cur_notify, - event->subscribe.prev_indicate, - event->subscribe.cur_indicate); - break; - case BLE_GAP_EVENT_REPEAT_PAIRING: - console_printf("repeat pairing event; conn_handle=%d " - "cur_key_sz=%d cur_auth=%d cur_sc=%d " - "new_key_sz=%d new_auth=%d new_sc=%d " - "new_bonding=%d\n", - event->repeat_pairing.conn_handle, - event->repeat_pairing.cur_key_size, - event->repeat_pairing.cur_authenticated, - event->repeat_pairing.cur_sc, - event->repeat_pairing.new_key_size, - event->repeat_pairing.new_authenticated, - event->repeat_pairing.new_sc, - event->repeat_pairing.new_bonding); - rc = ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc); - assert(rc == 0); - rc = ble_store_util_delete_peer(&desc.peer_id_addr); - assert(rc == 0); - bond_lost(event->repeat_pairing.conn_handle); - return BLE_GAP_REPEAT_PAIRING_RETRY; - case BLE_GAP_EVENT_CONN_UPDATE: - console_printf("connection update event; status=%d ", - event->conn_update.status); - rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc); - assert(rc == 0); - print_conn_desc(&desc); - le_conn_param_update(&desc); - break; - case BLE_GAP_EVENT_CONN_UPDATE_REQ: - console_printf("connection update request event; " - "conn_handle=%d itvl_min=%d itvl_max=%d " - "latency=%d supervision_timoeut=%d " - "min_ce_len=%d max_ce_len=%d\n", - event->conn_update_req.conn_handle, - event->conn_update_req.peer_params->itvl_min, - event->conn_update_req.peer_params->itvl_max, - event->conn_update_req.peer_params->latency, - event->conn_update_req.peer_params->supervision_timeout, - event->conn_update_req.peer_params->min_ce_len, - event->conn_update_req.peer_params->max_ce_len); + print_mbuf(event->notify_rx.om); + console_printf("\n"); + tester_gattc_notify_rx_ev(event->notify_rx.conn_handle, + event->notify_rx.attr_handle, + event->notify_rx.indication, + event->notify_rx.om); + break; + case BLE_GAP_EVENT_SUBSCRIBE: + console_printf("subscribe event; conn_handle=%d attr_handle=%d " + "reason=%d prevn=%d curn=%d previ=%d curi=%d\n", + event->subscribe.conn_handle, + event->subscribe.attr_handle, + event->subscribe.reason, + event->subscribe.prev_notify, + event->subscribe.cur_notify, + event->subscribe.prev_indicate, + event->subscribe.cur_indicate); + tester_gatt_subscribe_ev(event->subscribe.conn_handle, + event->subscribe.attr_handle, + event->subscribe.reason, + event->subscribe.prev_notify, + event->subscribe.cur_notify, + event->subscribe.prev_indicate, + event->subscribe.cur_indicate); + break; + case BLE_GAP_EVENT_REPEAT_PAIRING: + console_printf("repeat pairing event; conn_handle=%d " + "cur_key_sz=%d cur_auth=%d cur_sc=%d " + "new_key_sz=%d new_auth=%d new_sc=%d " + "new_bonding=%d\n", + event->repeat_pairing.conn_handle, + event->repeat_pairing.cur_key_size, + event->repeat_pairing.cur_authenticated, + event->repeat_pairing.cur_sc, + event->repeat_pairing.new_key_size, + event->repeat_pairing.new_authenticated, + event->repeat_pairing.new_sc, + event->repeat_pairing.new_bonding); + rc = ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc); + assert(rc == 0); + rc = ble_store_util_delete_peer(&desc.peer_id_addr); + assert(rc == 0); + bond_lost(event->repeat_pairing.conn_handle); + return BLE_GAP_REPEAT_PAIRING_RETRY; + case BLE_GAP_EVENT_CONN_UPDATE: + console_printf("connection update event; status=%d ", + event->conn_update.status); + rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc); + assert(rc == 0); + print_conn_desc(&desc); + le_conn_param_update(&desc); + break; + case BLE_GAP_EVENT_CONN_UPDATE_REQ: + console_printf("connection update request event; " + "conn_handle=%d itvl_min=%d itvl_max=%d " + "latency=%d supervision_timoeut=%d " + "min_ce_len=%d max_ce_len=%d\n", + event->conn_update_req.conn_handle, + event->conn_update_req.peer_params->itvl_min, + event->conn_update_req.peer_params->itvl_max, + event->conn_update_req.peer_params->latency, + event->conn_update_req.peer_params->supervision_timeout, + event->conn_update_req.peer_params->min_ce_len, + event->conn_update_req.peer_params->max_ce_len); - *event->conn_update_req.self_params = - *event->conn_update_req.peer_params; - break; - case BLE_GAP_EVENT_L2CAP_UPDATE_REQ: - console_printf("connection update request event; " - "conn_handle=%d itvl_min=%d itvl_max=%d " - "latency=%d supervision_timoeut=%d " - "min_ce_len=%d max_ce_len=%d\n", - event->conn_update_req.conn_handle, - event->conn_update_req.peer_params->itvl_min, - event->conn_update_req.peer_params->itvl_max, - event->conn_update_req.peer_params->latency, - event->conn_update_req.peer_params->supervision_timeout, - event->conn_update_req.peer_params->min_ce_len, - event->conn_update_req.peer_params->max_ce_len); - if (event->conn_update_req.peer_params->itvl_min == REJECT_INTERVAL_MIN && - event->conn_update_req.peer_params->itvl_max == REJECT_INTERVAL_MAX && - event->conn_update_req.peer_params->latency == REJECT_LATENCY && - event->conn_update_req.peer_params->supervision_timeout == REJECT_SUPERVISION_TIMEOUT) { - return EINVAL; - } + *event->conn_update_req.self_params = *event->conn_update_req.peer_params; + break; + case BLE_GAP_EVENT_L2CAP_UPDATE_REQ: + console_printf("connection update request event; " + "conn_handle=%d itvl_min=%d itvl_max=%d " + "latency=%d supervision_timoeut=%d " + "min_ce_len=%d max_ce_len=%d\n", + event->conn_update_req.conn_handle, + event->conn_update_req.peer_params->itvl_min, + event->conn_update_req.peer_params->itvl_max, + event->conn_update_req.peer_params->latency, + event->conn_update_req.peer_params->supervision_timeout, + event->conn_update_req.peer_params->min_ce_len, + event->conn_update_req.peer_params->max_ce_len); + if (event->conn_update_req.peer_params->itvl_min + == REJECT_INTERVAL_MIN && + event->conn_update_req.peer_params->itvl_max + == REJECT_INTERVAL_MAX && + event->conn_update_req.peer_params->latency == REJECT_LATENCY + && + event->conn_update_req.peer_params->supervision_timeout + == REJECT_SUPERVISION_TIMEOUT) { + return EINVAL; + } case BLE_GAP_EVENT_PARING_COMPLETE: console_printf("received pairing complete: " "conn_handle=%d status=%d\n", event->pairing_complete.conn_handle, event->pairing_complete.status); if (event->pairing_complete.status != BLE_SM_ERR_SUCCESS) { - le_pairing_failed(event->pairing_complete.conn_handle, event->pairing_complete.status); + le_pairing_failed(event->pairing_complete.conn_handle, + event->pairing_complete.status); } break; - default: - break; - } + default: + break; + } - return 0; + return 0; } -static void connect(const uint8_t *data, uint16_t len) +static void +connect(const uint8_t *data, uint16_t len) { - uint8_t status = BTP_STATUS_SUCCESS; - ble_addr_t *addr = (ble_addr_t *) data; + uint8_t status = BTP_STATUS_SUCCESS; + ble_addr_t *addr = (ble_addr_t *) data; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (ble_addr_cmp(BLE_ADDR_ANY, addr) == 0) { - addr = NULL; - } + if (ble_addr_cmp(BLE_ADDR_ANY, addr) == 0) { + addr = NULL; + } - if (ble_gap_connect(own_addr_type, addr, 0, - &dflt_conn_params, gap_event_cb, NULL)) { - status = BTP_STATUS_FAILED; - } + if (ble_gap_connect(own_addr_type, addr, 0, + &dflt_conn_params, gap_event_cb, NULL)) { + status = BTP_STATUS_FAILED; + } - tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONNECT, CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONNECT, CONTROLLER_INDEX, status); } -static void disconnect(const uint8_t *data, uint16_t len) +static void +disconnect(const uint8_t *data, uint16_t len) { - struct ble_gap_conn_desc desc; - uint8_t status; - int rc; + struct ble_gap_conn_desc desc; + uint8_t status; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = gap_conn_find_by_addr((ble_addr_t *) data, &desc); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - if (ble_gap_terminate(desc.conn_handle, BLE_ERR_REM_USER_CONN_TERM)) { - status = BTP_STATUS_FAILED; - } else { - status = BTP_STATUS_SUCCESS; - } + if (ble_gap_terminate(desc.conn_handle, BLE_ERR_REM_USER_CONN_TERM)) { + status = BTP_STATUS_FAILED; + } else { + status = BTP_STATUS_SUCCESS; + } rsp: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_DISCONNECT, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_DISCONNECT, CONTROLLER_INDEX, + status); } -static void set_io_cap(const uint8_t *data, uint16_t len) +static void +set_io_cap(const uint8_t *data, uint16_t len) { - const struct gap_set_io_cap_cmd *cmd = (void *) data; - uint8_t status; + const struct gap_set_io_cap_cmd *cmd = (void *) data; + uint8_t status; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - switch (cmd->io_cap) { - case GAP_IO_CAP_DISPLAY_ONLY: - ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_ONLY; - ble_hs_cfg.sm_mitm = 1; - break; - case GAP_IO_CAP_KEYBOARD_DISPLAY: - ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_DISP; - ble_hs_cfg.sm_mitm = 1; - break; - case GAP_IO_CAP_NO_INPUT_OUTPUT: - ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_NO_IO; - ble_hs_cfg.sm_mitm = 0; - break; - case GAP_IO_CAP_KEYBOARD_ONLY: - ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_ONLY; - ble_hs_cfg.sm_mitm = 1; - break; - case GAP_IO_CAP_DISPLAY_YESNO: - ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_YES_NO; - ble_hs_cfg.sm_mitm = 1; - break; - default: - status = BTP_STATUS_FAILED; - goto rsp; - } + switch (cmd->io_cap) { + case GAP_IO_CAP_DISPLAY_ONLY: + ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_ONLY; + ble_hs_cfg.sm_mitm = 1; + break; + case GAP_IO_CAP_KEYBOARD_DISPLAY: + ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_DISP; + ble_hs_cfg.sm_mitm = 1; + break; + case GAP_IO_CAP_NO_INPUT_OUTPUT: + ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_NO_IO; + ble_hs_cfg.sm_mitm = 0; + break; + case GAP_IO_CAP_KEYBOARD_ONLY: + ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_ONLY; + ble_hs_cfg.sm_mitm = 1; + break; + case GAP_IO_CAP_DISPLAY_YESNO: + ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_YES_NO; + ble_hs_cfg.sm_mitm = 1; + break; + default: + status = BTP_STATUS_FAILED; + goto rsp; + } - status = BTP_STATUS_SUCCESS; + status = BTP_STATUS_SUCCESS; rsp: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_IO_CAP, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_IO_CAP, CONTROLLER_INDEX, + status); } -static void pair(const uint8_t *data, uint16_t len) +static void +pair(const uint8_t *data, uint16_t len) { - struct ble_gap_conn_desc desc; - uint8_t status; - int rc; + struct ble_gap_conn_desc desc; + uint8_t status; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = gap_conn_find_by_addr((ble_addr_t *) data, &desc); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - if (ble_gap_security_initiate(desc.conn_handle)) { - status = BTP_STATUS_FAILED; - goto rsp; - } + if (ble_gap_security_initiate(desc.conn_handle)) { + status = BTP_STATUS_FAILED; + goto rsp; + } - status = BTP_STATUS_SUCCESS; + status = BTP_STATUS_SUCCESS; rsp: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_PAIR, CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_PAIR, CONTROLLER_INDEX, status); } -static void unpair(const uint8_t *data, uint16_t len) +static void +unpair(const uint8_t *data, uint16_t len) { - uint8_t status; - int err; + uint8_t status; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - err = ble_gap_unpair((ble_addr_t *) data); - status = (uint8_t) (err != 0 ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); - tester_rsp(BTP_SERVICE_ID_GAP, GAP_UNPAIR, CONTROLLER_INDEX, status); + err = ble_gap_unpair((ble_addr_t *) data); + status = (uint8_t) (err != 0 ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_UNPAIR, CONTROLLER_INDEX, status); } -static void passkey_entry(const uint8_t *data, uint16_t len) +static void +passkey_entry(const uint8_t *data, uint16_t len) { - const struct gap_passkey_entry_cmd *cmd = (void *) data; - struct ble_gap_conn_desc desc; - struct ble_sm_io pk; - uint8_t status; - int rc; + const struct gap_passkey_entry_cmd *cmd = (void *) data; + struct ble_gap_conn_desc desc; + struct ble_sm_io pk; + uint8_t status; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = gap_conn_find_by_addr((ble_addr_t *) data, &desc); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - pk.action = BLE_SM_IOACT_INPUT; - pk.passkey = sys_le32_to_cpu(cmd->passkey); + pk.action = BLE_SM_IOACT_INPUT; + pk.passkey = sys_le32_to_cpu(cmd->passkey); - rc = ble_sm_inject_io(desc.conn_handle, &pk); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_sm_inject_io(desc.conn_handle, &pk); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - status = BTP_STATUS_SUCCESS; + status = BTP_STATUS_SUCCESS; rsp: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_ENTRY, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_ENTRY, CONTROLLER_INDEX, + status); } -static void passkey_confirm(const uint8_t *data, uint16_t len) +static void +passkey_confirm(const uint8_t *data, uint16_t len) { - const struct gap_passkey_confirm_cmd *cmd = (void *) data; - struct ble_gap_conn_desc desc; - struct ble_sm_io pk; - uint8_t status; - int rc; + const struct gap_passkey_confirm_cmd *cmd = (void *) data; + struct ble_gap_conn_desc desc; + struct ble_sm_io pk; + uint8_t status; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = gap_conn_find_by_addr((ble_addr_t *)data, &desc); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = gap_conn_find_by_addr((ble_addr_t *) data, &desc); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - pk.action = BLE_SM_IOACT_NUMCMP; - pk.numcmp_accept = cmd->match; + pk.action = BLE_SM_IOACT_NUMCMP; + pk.numcmp_accept = cmd->match; - rc = ble_sm_inject_io(desc.conn_handle, &pk); - if (rc) { - console_printf("sm inject io failed"); - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_sm_inject_io(desc.conn_handle, &pk); + if (rc) { + console_printf("sm inject io failed"); + status = BTP_STATUS_FAILED; + goto rsp; + } - status = BTP_STATUS_SUCCESS; + status = BTP_STATUS_SUCCESS; rsp: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_CONFIRM, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_CONFIRM, CONTROLLER_INDEX, + status); } -static void start_direct_adv(const uint8_t *data, uint16_t len) +static void +start_direct_adv(const uint8_t *data, uint16_t len) { - const struct gap_start_direct_adv_cmd *cmd = (void *) data; - struct gap_start_advertising_rp rp; - static struct ble_gap_adv_params adv_params = { - .conn_mode = BLE_GAP_CONN_MODE_DIR, - }; - int err; + const struct gap_start_direct_adv_cmd *cmd = (void *) data; + struct gap_start_advertising_rp rp; + static struct ble_gap_adv_params adv_params = { + .conn_mode = BLE_GAP_CONN_MODE_DIR, + }; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - adv_params.high_duty_cycle = cmd->high_duty; + adv_params.high_duty_cycle = cmd->high_duty; - err = ble_gap_adv_start(own_addr_type, (ble_addr_t *)data, - BLE_HS_FOREVER, &adv_params, - gap_event_cb, NULL); - if (err) { - SYS_LOG_ERR("Advertising failed: err %d", err); - goto fail; - } + err = ble_gap_adv_start(own_addr_type, (ble_addr_t *) data, + BLE_HS_FOREVER, &adv_params, + gap_event_cb, NULL); + if (err) { + SYS_LOG_ERR("Advertising failed: err %d", err); + goto fail; + } - current_settings |= BIT(GAP_SETTINGS_ADVERTISING); - rp.current_settings = sys_cpu_to_le32(current_settings); + current_settings |= BIT(GAP_SETTINGS_ADVERTISING); + rp.current_settings = sys_cpu_to_le32(current_settings); - tester_send(BTP_SERVICE_ID_GAP, GAP_START_DIRECT_ADV, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); - return; + tester_send(BTP_SERVICE_ID_GAP, GAP_START_DIRECT_ADV, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); + return; fail: - tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DIRECT_ADV, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DIRECT_ADV, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void conn_param_update_cb(uint16_t conn_handle, int status, void *arg) +static void +conn_param_update_cb(uint16_t conn_handle, int status, void *arg) { - console_printf("conn param update complete; conn_handle=%d status=%d\n", - conn_handle, status); + console_printf("conn param update complete; conn_handle=%d status=%d\n", + conn_handle, status); } -static int conn_param_update_slave(uint16_t conn_handle, - const struct gap_conn_param_update_cmd *cmd) +static int +conn_param_update_slave(uint16_t conn_handle, + const struct gap_conn_param_update_cmd *cmd) { - int rc; - struct ble_l2cap_sig_update_params params; + int rc; + struct ble_l2cap_sig_update_params params; - params.itvl_min = cmd->conn_itvl_min; - params.itvl_max = cmd->conn_itvl_max; - params.slave_latency = cmd->conn_latency; - params.timeout_multiplier = cmd->supervision_timeout; + params.itvl_min = cmd->conn_itvl_min; + params.itvl_max = cmd->conn_itvl_max; + params.slave_latency = cmd->conn_latency; + params.timeout_multiplier = cmd->supervision_timeout; - rc = ble_l2cap_sig_update(conn_handle, ¶ms, - conn_param_update_cb, NULL); - if (rc) { - SYS_LOG_ERR("Failed to send update params: rc=%d", rc); - } + rc = ble_l2cap_sig_update(conn_handle, ¶ms, + conn_param_update_cb, NULL); + if (rc) { + SYS_LOG_ERR("Failed to send update params: rc=%d", rc); + } - return 0; + return 0; } -static int conn_param_update_master(uint16_t conn_handle, - const struct gap_conn_param_update_cmd *cmd) +static int +conn_param_update_master(uint16_t conn_handle, + const struct gap_conn_param_update_cmd *cmd) { - int rc; - struct ble_gap_upd_params params; + int rc; + struct ble_gap_upd_params params; - params.itvl_min = cmd->conn_itvl_min; - params.itvl_max = cmd->conn_itvl_max; - params.latency = cmd->conn_latency; - params.supervision_timeout = cmd->supervision_timeout; - params.min_ce_len = 0; - params.max_ce_len = 0; - rc = ble_gap_update_params(conn_handle, ¶ms); - if (rc) { - SYS_LOG_ERR("Failed to send update params: rc=%d", rc); - } + params.itvl_min = cmd->conn_itvl_min; + params.itvl_max = cmd->conn_itvl_max; + params.latency = cmd->conn_latency; + params.supervision_timeout = cmd->supervision_timeout; + params.min_ce_len = 0; + params.max_ce_len = 0; + rc = ble_gap_update_params(conn_handle, ¶ms); + if (rc) { + SYS_LOG_ERR("Failed to send update params: rc=%d", rc); + } - return rc; + return rc; } -static void conn_param_update(struct os_event *ev) +static void +conn_param_update(struct os_event *ev) { - struct ble_gap_conn_desc desc; - int rc; + struct ble_gap_conn_desc desc; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = gap_conn_find_by_addr((ble_addr_t *)&update_params, &desc); - if (rc) { - goto rsp; - } + rc = gap_conn_find_by_addr((ble_addr_t *) &update_params, &desc); + if (rc) { + goto rsp; + } - if ((desc.conn_itvl >= update_params.conn_itvl_min) && - (desc.conn_itvl <= update_params.conn_itvl_max) && - (desc.conn_latency == update_params.conn_latency) && - (desc.supervision_timeout == update_params.supervision_timeout)) { - goto rsp; - } + if ((desc.conn_itvl >= update_params.conn_itvl_min) && + (desc.conn_itvl <= update_params.conn_itvl_max) && + (desc.conn_latency == update_params.conn_latency) && + (desc.supervision_timeout == update_params.supervision_timeout)) { + goto rsp; + } - if (desc.role == BLE_GAP_ROLE_MASTER) { - rc = conn_param_update_master(desc.conn_handle, &update_params); - } else { - rc = conn_param_update_slave(desc.conn_handle, &update_params); - } + if (desc.role == BLE_GAP_ROLE_MASTER) { + rc = conn_param_update_master(desc.conn_handle, &update_params); + } else { + rc = conn_param_update_slave(desc.conn_handle, &update_params); + } - if (rc == 0) { - return; - } + if (rc == 0) { + return; + } rsp: - SYS_LOG_ERR("Conn param update fail; rc=%d", rc); + SYS_LOG_ERR("Conn param update fail; rc=%d", rc); } -static void conn_param_update_async(const uint8_t *data, uint16_t len) +static void +conn_param_update_async(const uint8_t *data, uint16_t len) { - const struct gap_conn_param_update_cmd *cmd = (void *) data; - update_params = *cmd; + const struct gap_conn_param_update_cmd *cmd = (void *) data; + update_params = *cmd; - os_callout_reset(&update_params_co, 0); + os_callout_reset(&update_params_co, 0); - tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONN_PARAM_UPDATE, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONN_PARAM_UPDATE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); } -static void oob_legacy_set_data(const uint8_t *data, uint16_t len) +static void +oob_legacy_set_data(const uint8_t *data, uint16_t len) { - const struct gap_oob_legacy_set_data_cmd *cmd = (void *) data; + const struct gap_oob_legacy_set_data_cmd *cmd = (void *) data; - ble_hs_cfg.sm_oob_data_flag = 1; - memcpy(oob, cmd->oob_data, sizeof(oob)); + ble_hs_cfg.sm_oob_data_flag = 1; + memcpy(oob, cmd->oob_data, sizeof(oob)); - tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_LEGACY_SET_DATA, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_LEGACY_SET_DATA, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); } -static void oob_sc_get_local_data(const uint8_t *data, uint16_t len) +static void +oob_sc_get_local_data(const uint8_t *data, uint16_t len) { - struct gap_oob_sc_get_local_data_rp rp; + struct gap_oob_sc_get_local_data_rp rp; - memcpy(rp.r, oob_data_local.r, 16); - memcpy(rp.c, oob_data_local.c, 16); + memcpy(rp.r, oob_data_local.r, 16); + memcpy(rp.c, oob_data_local.c, 16); - tester_send(BTP_SERVICE_ID_GAP, GAP_OOB_SC_GET_LOCAL_DATA, - CONTROLLER_INDEX, (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GAP, GAP_OOB_SC_GET_LOCAL_DATA, + CONTROLLER_INDEX, (uint8_t *) &rp, sizeof(rp)); } -static void oob_sc_set_remote_data(const uint8_t *data, uint16_t len) +static void +oob_sc_set_remote_data(const uint8_t *data, uint16_t len) { - const struct gap_oob_sc_set_remote_data_cmd *cmd = (void *) data; + const struct gap_oob_sc_set_remote_data_cmd *cmd = (void *) data; - ble_hs_cfg.sm_oob_data_flag = 1; - memcpy(oob_data_remote.r, cmd->r, 16); - memcpy(oob_data_remote.c, cmd->c, 16); + ble_hs_cfg.sm_oob_data_flag = 1; + memcpy(oob_data_remote.r, cmd->r, 16); + memcpy(oob_data_remote.c, cmd->c, 16); - tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_SC_SET_REMOTE_DATA, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_SC_SET_REMOTE_DATA, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); } -static void set_mitm(const uint8_t *data, uint16_t len) +static void +set_mitm(const uint8_t *data, uint16_t len) { - const struct gap_set_mitm_cmd *cmd = (void *) data; + const struct gap_set_mitm_cmd *cmd = (void *) data; - ble_hs_cfg.sm_mitm = cmd->mitm; + ble_hs_cfg.sm_mitm = cmd->mitm; - tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_MITM, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_MITM, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); } -static void set_filter_accept_list(const uint8_t *data, uint16_t len) +static void +set_filter_accept_list(const uint8_t *data, uint16_t len) { - uint8_t status = BTP_STATUS_SUCCESS; - struct gap_set_filter_accept_list_cmd *tmp = - (struct gap_set_filter_accept_list_cmd *) data; + uint8_t status = BTP_STATUS_SUCCESS; + struct gap_set_filter_accept_list_cmd *tmp = + (struct gap_set_filter_accept_list_cmd *) data; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - /* - * Check if the nb of bytes received matches the len of addrs list. - * Then set the filter accept list. - */ - if (((len - sizeof(tmp->list_len))/sizeof(ble_addr_t) != - tmp->list_len) || ble_gap_wl_set(tmp->addrs, tmp->list_len)) { - status = BTP_STATUS_FAILED; - } + /* + * Check if the nb of bytes received matches the len of addrs list. + * Then set the filter accept list. + */ + if (((len - sizeof(tmp->list_len)) / sizeof(ble_addr_t) != + tmp->list_len) || ble_gap_wl_set(tmp->addrs, tmp->list_len)) { + status = BTP_STATUS_FAILED; + } - tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_FILTER_ACCEPT_LIST, - CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_FILTER_ACCEPT_LIST, + CONTROLLER_INDEX, status); } -void tester_handle_gap(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len) +void +tester_handle_gap(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len) { - switch (opcode) { - case GAP_READ_SUPPORTED_COMMANDS: - case GAP_READ_CONTROLLER_INDEX_LIST: - if (index != BTP_INDEX_NONE){ - tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, - BTP_STATUS_FAILED); - return; - } - break; - default: - if (index != CONTROLLER_INDEX){ - tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, - BTP_STATUS_FAILED); - return; - } - break; - } + switch (opcode) { + case GAP_READ_SUPPORTED_COMMANDS: + case GAP_READ_CONTROLLER_INDEX_LIST: + if (index != BTP_INDEX_NONE) { + tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, + BTP_STATUS_FAILED); + return; + } + break; + default: + if (index != CONTROLLER_INDEX) { + tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, + BTP_STATUS_FAILED); + return; + } + break; + } - switch (opcode) { - case GAP_READ_SUPPORTED_COMMANDS: - supported_commands(data, len); - return; - case GAP_READ_CONTROLLER_INDEX_LIST: - controller_index_list(data, len); - return; - case GAP_READ_CONTROLLER_INFO: - controller_info(data, len); - return; - case GAP_SET_CONNECTABLE: - set_connectable(data, len); - return; - case GAP_SET_DISCOVERABLE: - set_discoverable(data, len); - return; - case GAP_SET_BONDABLE: - set_bondable(data, len); - return; - case GAP_START_ADVERTISING: - start_advertising(data, len); - return; - case GAP_STOP_ADVERTISING: - stop_advertising(data, len); - return; - case GAP_START_DISCOVERY: - start_discovery(data, len); - return; - case GAP_STOP_DISCOVERY: - stop_discovery(data, len); - return; - case GAP_CONNECT: - connect(data, len); - return; - case GAP_DISCONNECT: - disconnect(data, len); - return; - case GAP_SET_IO_CAP: - set_io_cap(data, len); - return; - case GAP_PAIR: - pair(data, len); - return; - case GAP_UNPAIR: - unpair(data, len); - return; - case GAP_PASSKEY_ENTRY: - passkey_entry(data, len); - return; - case GAP_PASSKEY_CONFIRM: - passkey_confirm(data, len); - return; - case GAP_START_DIRECT_ADV: - start_direct_adv(data, len); - return; - case GAP_CONN_PARAM_UPDATE: - conn_param_update_async(data, len); - return; - case GAP_OOB_LEGACY_SET_DATA: - oob_legacy_set_data(data, len); - return; - case GAP_OOB_SC_GET_LOCAL_DATA: - oob_sc_get_local_data(data, len); - return; - case GAP_OOB_SC_SET_REMOTE_DATA: - oob_sc_set_remote_data(data, len); - return; - case GAP_SET_MITM: - set_mitm(data, len); - return; - case GAP_SET_FILTER_ACCEPT_LIST: - set_filter_accept_list(data, len); - return; - default: - tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, - BTP_STATUS_UNKNOWN_CMD); - return; - } + switch (opcode) { + case GAP_READ_SUPPORTED_COMMANDS: + supported_commands(data, len); + return; + case GAP_READ_CONTROLLER_INDEX_LIST: + controller_index_list(data, len); + return; + case GAP_READ_CONTROLLER_INFO: + controller_info(data, len); + return; + case GAP_SET_CONNECTABLE: + set_connectable(data, len); + return; + case GAP_SET_DISCOVERABLE: + set_discoverable(data, len); + return; + case GAP_SET_BONDABLE: + set_bondable(data, len); + return; + case GAP_START_ADVERTISING: + start_advertising(data, len); + return; + case GAP_STOP_ADVERTISING: + stop_advertising(data, len); + return; + case GAP_START_DISCOVERY: + start_discovery(data, len); + return; + case GAP_STOP_DISCOVERY: + stop_discovery(data, len); + return; + case GAP_CONNECT: + connect(data, len); + return; + case GAP_DISCONNECT: + disconnect(data, len); + return; + case GAP_SET_IO_CAP: + set_io_cap(data, len); + return; + case GAP_PAIR: + pair(data, len); + return; + case GAP_UNPAIR: + unpair(data, len); + return; + case GAP_PASSKEY_ENTRY: + passkey_entry(data, len); + return; + case GAP_PASSKEY_CONFIRM: + passkey_confirm(data, len); + return; + case GAP_START_DIRECT_ADV: + start_direct_adv(data, len); + return; + case GAP_CONN_PARAM_UPDATE: + conn_param_update_async(data, len); + return; + case GAP_OOB_LEGACY_SET_DATA: + oob_legacy_set_data(data, len); + return; + case GAP_OOB_SC_GET_LOCAL_DATA: + oob_sc_get_local_data(data, len); + return; + case GAP_OOB_SC_SET_REMOTE_DATA: + oob_sc_set_remote_data(data, len); + return; + case GAP_SET_MITM: + set_mitm(data, len); + return; + case GAP_SET_FILTER_ACCEPT_LIST: + set_filter_accept_list(data, len); + return; + default: + tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } } -static void tester_init_gap_cb(int err) +static void +tester_init_gap_cb(int err) { - if (err) { - tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, - BTP_INDEX_NONE, BTP_STATUS_FAILED); - return; - } + if (err) { + tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, + BTP_INDEX_NONE, BTP_STATUS_FAILED); + return; + } - current_settings = 0; - current_settings |= BIT(GAP_SETTINGS_POWERED); - current_settings |= BIT(GAP_SETTINGS_LE); + current_settings = 0; + current_settings |= BIT(GAP_SETTINGS_POWERED); + current_settings |= BIT(GAP_SETTINGS_LE); - os_callout_init(&update_params_co, os_eventq_dflt_get(), - conn_param_update, NULL); + os_callout_init(&update_params_co, os_eventq_dflt_get(), + conn_param_update, NULL); - os_callout_init(&connected_ev_co, os_eventq_dflt_get(), - device_connected_ev_send, NULL); + os_callout_init(&connected_ev_co, os_eventq_dflt_get(), + device_connected_ev_send, NULL); - tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE, - BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE, + BTP_STATUS_SUCCESS); } -uint8_t tester_init_gap(void) +uint8_t +tester_init_gap(void) { #if MYNEWT_VAL(BLE_SM_SC) - int rc; + int rc; - rc = ble_sm_sc_oob_generate_data(&oob_data_local); - if (rc) { - console_printf("Error: generating oob data; reason=%d\n", rc); - return BTP_STATUS_FAILED; - } + rc = ble_sm_sc_oob_generate_data(&oob_data_local); + if (rc) { + console_printf("Error: generating oob data; reason=%d\n", rc); + return BTP_STATUS_FAILED; + } #endif #if MYNEWT_VAL(BTTESTER_PRIVACY_MODE) && MYNEWT_VAL(BTTESTER_USE_NRPA) - os_callout_init(&bttester_nrpa_rotate_timer, os_eventq_dflt_get(), + os_callout_init(&bttester_nrpa_rotate_timer, os_eventq_dflt_get(), rotate_nrpa_cb, NULL); #endif - adv_buf = NET_BUF_SIMPLE(ADV_BUF_LEN); + adv_buf = NET_BUF_SIMPLE(ADV_BUF_LEN); - tester_init_gap_cb(BTP_STATUS_SUCCESS); - return BTP_STATUS_SUCCESS; + tester_init_gap_cb(BTP_STATUS_SUCCESS); + return BTP_STATUS_SUCCESS; } -uint8_t tester_unregister_gap(void) +uint8_t +tester_unregister_gap(void) { - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } diff --git a/apps/bttester/src/gatt.c b/apps/bttester/src/gatt.c index 130d9f1ee..02ea3ec72 100644 --- a/apps/bttester/src/gatt.c +++ b/apps/bttester/src/gatt.c @@ -44,15 +44,15 @@ /* 0000xxxx-8c26-476f-89a7-a108033a69c7 */ #define PTS_UUID_DECLARE(uuid16) \ ((const ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT( \ - 0xc7, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89, \ - 0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00 \ + 0xc7, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89, \ + 0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00 \ ))) /* 0000xxxx-8c26-476f-89a7-a108033a69c6 */ #define PTS_UUID_DECLARE_ALT(uuid16) \ ((const ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT( \ - 0xc6, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89, \ - 0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00 \ + 0xc6, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89, \ + 0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00 \ ))) #define PTS_SVC 0x0001 @@ -84,528 +84,530 @@ uint16_t notify_handle; uint8_t notify_value = 90; struct find_attr_data { - ble_uuid_any_t *uuid; - int attr_type; - void *ptr; - uint16_t handle; + ble_uuid_any_t *uuid; + int attr_type; + void *ptr; + uint16_t handle; }; static int gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static int gatt_svr_dsc_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg); + struct ble_gatt_access_ctxt *ctxt, + void *arg); static const struct ble_gatt_svc_def gatt_svr_inc_svcs[] = { - { - .type = BLE_GATT_SVC_TYPE_PRIMARY, - .uuid = PTS_UUID_DECLARE(PTS_INC_SVC), - .characteristics = (struct ble_gatt_chr_def[]) {{ - .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ALT), - .access_cb = gatt_svr_read_write_test, - .flags = BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_READ, - }, { - 0, - } }, - }, + { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = PTS_UUID_DECLARE(PTS_INC_SVC), + .characteristics = (struct ble_gatt_chr_def[]) {{ + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ALT), + .access_cb = gatt_svr_read_write_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + 0, + }}, - { - 0, /* No more services. */ - }, + }, + + { + 0, /* No more services. */ + }, }; static const struct ble_gatt_svc_def *inc_svcs[] = { - &gatt_svr_inc_svcs[0], - NULL, + &gatt_svr_inc_svcs[0], + NULL, }; static const struct ble_gatt_svc_def gatt_svr_svcs[] = { - { - /*** Service: PTS test. */ - .type = BLE_GATT_SVC_TYPE_PRIMARY, - .uuid = PTS_UUID_DECLARE(PTS_SVC), - .includes = inc_svcs, - .characteristics = (struct ble_gatt_chr_def[]) { { - .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE), - .access_cb = gatt_svr_read_write_test, - .flags = BLE_GATT_CHR_F_READ | - BLE_GATT_CHR_F_WRITE, - .descriptors = (struct ble_gatt_dsc_def[]) { { - .uuid = PTS_UUID_DECLARE(PTS_DSC_READ_WRITE), - .access_cb = gatt_svr_dsc_read_write_test, - .att_flags = BLE_ATT_F_READ | - BLE_ATT_F_WRITE, - }, { - .uuid = PTS_UUID_DECLARE(PTS_LONG_DSC_READ_WRITE), - .access_cb = gatt_svr_dsc_read_write_long_test, - .att_flags = BLE_ATT_F_READ | - BLE_ATT_F_WRITE, - }, { - .uuid = PTS_UUID_DECLARE(PTS_DSC_READ), - .access_cb = gatt_svr_dsc_read_test, - .att_flags = BLE_ATT_F_READ, - }, { - 0, /* No more descriptors in this characteristic */ - } } - }, { - .uuid = PTS_UUID_DECLARE(PTS_CHR_WRITE_NO_RSP), - .access_cb = gatt_svr_write_no_rsp_test, - .flags = BLE_GATT_CHR_F_READ | - BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_WRITE_NO_RSP, - }, { - .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_AUTHEN), - .access_cb = gatt_svr_read_write_auth_test, - .flags = BLE_GATT_CHR_F_READ_AUTHEN | - BLE_GATT_CHR_F_READ | - BLE_GATT_CHR_F_WRITE_AUTHEN | - BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_WRITE_AUTHEN, - }, { - .uuid = PTS_UUID_DECLARE(PTS_CHR_RELIABLE_WRITE), - .access_cb = gatt_svr_rel_write_test, - .flags = BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_RELIABLE_WRITE, - }, { - .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ENC), - .access_cb = gatt_svr_read_write_enc_test, - .flags = BLE_GATT_CHR_F_READ_ENC | - BLE_GATT_CHR_F_READ | - BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_WRITE_ENC, - .min_key_size = 16, - }, { - .uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE), - .access_cb = gatt_svr_read_write_long_test, - .flags = BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_READ, - }, { - .uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE_ALT), - .access_cb = gatt_svr_read_write_long_test, - .flags = BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_READ, - }, { - .uuid = PTS_UUID_DECLARE(PTS_CHR_NOTIFY), - .access_cb = gatt_svr_read_write_test, - .val_handle = ¬ify_handle, - .flags = BLE_GATT_CHR_F_NOTIFY | - BLE_GATT_CHR_F_INDICATE, - }, { - 0, /* No more characteristics in this service. */ - } }, - }, - - { - .type = BLE_GATT_SVC_TYPE_PRIMARY, - .uuid = PTS_UUID_DECLARE_ALT(PTS_SVC), - .characteristics = (struct ble_gatt_chr_def[]) { { - .uuid = PTS_UUID_DECLARE_ALT(PTS_CHR_READ_WRITE), - .access_cb = gatt_svr_read_write_test, - .flags = BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_READ, - }, { - 0, /* No more characteristics in this service */ - } }, - }, - - { - 0, /* No more services. */ - }, + { + /*** Service: PTS test. */ + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = PTS_UUID_DECLARE(PTS_SVC), + .includes = inc_svcs, + .characteristics = (struct ble_gatt_chr_def[]) { + { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE), + .access_cb = gatt_svr_read_write_test, + .flags = BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE, + .descriptors = (struct ble_gatt_dsc_def[]) {{ + .uuid = PTS_UUID_DECLARE(PTS_DSC_READ_WRITE), + .access_cb = gatt_svr_dsc_read_write_test, + .att_flags = BLE_ATT_F_READ | + BLE_ATT_F_WRITE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_LONG_DSC_READ_WRITE), + .access_cb = gatt_svr_dsc_read_write_long_test, + .att_flags = BLE_ATT_F_READ | + BLE_ATT_F_WRITE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_DSC_READ), + .access_cb = gatt_svr_dsc_read_test, + .att_flags = BLE_ATT_F_READ, + }, { + 0, /* No more descriptors in this characteristic */ + }} + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_WRITE_NO_RSP), + .access_cb = gatt_svr_write_no_rsp_test, + .flags = BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_WRITE_NO_RSP, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_AUTHEN), + .access_cb = gatt_svr_read_write_auth_test, + .flags = BLE_GATT_CHR_F_READ_AUTHEN | + BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE_AUTHEN | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_WRITE_AUTHEN, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_RELIABLE_WRITE), + .access_cb = gatt_svr_rel_write_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_RELIABLE_WRITE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ENC), + .access_cb = gatt_svr_read_write_enc_test, + .flags = BLE_GATT_CHR_F_READ_ENC | + BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_WRITE_ENC, + .min_key_size = 16, + }, { + .uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE), + .access_cb = gatt_svr_read_write_long_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + .uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE_ALT), + .access_cb = gatt_svr_read_write_long_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_NOTIFY), + .access_cb = gatt_svr_read_write_test, + .val_handle = ¬ify_handle, + .flags = BLE_GATT_CHR_F_NOTIFY | + BLE_GATT_CHR_F_INDICATE, + }, { + 0, /* No more characteristics in this service. */ + } + }, + }, { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = PTS_UUID_DECLARE_ALT(PTS_SVC), + .characteristics = (struct ble_gatt_chr_def[]) {{ + .uuid = PTS_UUID_DECLARE_ALT(PTS_CHR_READ_WRITE), + .access_cb = gatt_svr_read_write_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + 0, /* No more characteristics in this service */ + }}, + }, { + 0, /* No more services. */ + }, }; -static void attr_value_changed_ev(uint16_t handle, struct os_mbuf *data) +static void +attr_value_changed_ev(uint16_t handle, struct os_mbuf *data) { - struct gatt_attr_value_changed_ev *ev; - struct os_mbuf *buf = os_msys_get(0, 0); + struct gatt_attr_value_changed_ev *ev; + struct os_mbuf *buf = os_msys_get(0, 0); - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - net_buf_simple_init(buf, 0); - ev = net_buf_simple_add(buf, sizeof(*ev)); + net_buf_simple_init(buf, 0); + ev = net_buf_simple_add(buf, sizeof(*ev)); - ev->handle = sys_cpu_to_le16(handle); - ev->data_length = sys_cpu_to_le16(os_mbuf_len(data)); - os_mbuf_appendfrom(buf, data, 0, os_mbuf_len(data)); + ev->handle = sys_cpu_to_le16(handle); + ev->data_length = sys_cpu_to_le16(os_mbuf_len(data)); + os_mbuf_appendfrom(buf, data, 0, os_mbuf_len(data)); - tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_ATTR_VALUE_CHANGED, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_ATTR_VALUE_CHANGED, + CONTROLLER_INDEX, buf); } static int gatt_svr_chr_write(uint16_t conn_handle, uint16_t attr_handle, - struct os_mbuf *om, uint16_t min_len, uint16_t max_len, - void *dst, uint16_t *len) + struct os_mbuf *om, uint16_t min_len, uint16_t max_len, + void *dst, uint16_t *len) { - uint16_t om_len; - int rc; + uint16_t om_len; + int rc; - om_len = OS_MBUF_PKTLEN(om); - if (om_len < min_len || om_len > max_len) { - return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; - } + om_len = OS_MBUF_PKTLEN(om); + if (om_len < min_len || om_len > max_len) { + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } - rc = ble_hs_mbuf_to_flat(om, dst, max_len, len); - if (rc != 0) { - return BLE_ATT_ERR_UNLIKELY; - } + rc = ble_hs_mbuf_to_flat(om, dst, max_len, len); + if (rc != 0) { + return BLE_ATT_ERR_UNLIKELY; + } - attr_value_changed_ev(attr_handle, om); + attr_value_changed_ev(attr_handle, om); - return 0; + return 0; } static uint16_t extract_uuid16_from_pts_uuid128(const ble_uuid_t *uuid) { - const uint8_t *u8ptr; - uint16_t uuid16; + const uint8_t *u8ptr; + uint16_t uuid16; - u8ptr = BLE_UUID128(uuid)->value; - uuid16 = u8ptr[12]; - uuid16 |= (uint16_t)u8ptr[13] << 8; - return uuid16; + u8ptr = BLE_UUID128(uuid)->value; + uuid16 = u8ptr[12]; + uuid16 |= (uint16_t) u8ptr[13] << 8; + return uuid16; } static int gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_CHR_READ_WRITE: - case PTS_CHR_READ_WRITE_ALT: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_short_val, - &gatt_svr_pts_static_short_val, NULL); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, - sizeof gatt_svr_pts_static_short_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_CHR_READ_WRITE: + case PTS_CHR_READ_WRITE_ALT: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_short_val, + &gatt_svr_pts_static_short_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, + sizeof gatt_svr_pts_static_short_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_LONG_CHR_READ_WRITE: - case PTS_LONG_CHR_READ_WRITE_ALT: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_long_val, - &gatt_svr_pts_static_long_val, NULL); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, - sizeof gatt_svr_pts_static_long_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_LONG_CHR_READ_WRITE: + case PTS_LONG_CHR_READ_WRITE_ALT: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_long_val, + &gatt_svr_pts_static_long_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, + sizeof gatt_svr_pts_static_long_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_CHR_READ_WRITE_AUTHEN: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_val, - &gatt_svr_pts_static_val, NULL); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val, - sizeof gatt_svr_pts_static_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_CHR_READ_WRITE_AUTHEN: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_val, + &gatt_svr_pts_static_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val, + sizeof gatt_svr_pts_static_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_CHR_READ_WRITE_ENC: - if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val, - sizeof gatt_svr_pts_static_val); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_val, - &gatt_svr_pts_static_val, NULL); - return rc; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_CHR_READ_WRITE_ENC: + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val, + sizeof gatt_svr_pts_static_val); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_val, + &gatt_svr_pts_static_val, NULL); + return rc; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_DSC_READ_WRITE: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_short_val, - &gatt_svr_pts_static_short_val, NULL); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, - sizeof gatt_svr_pts_static_short_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_DSC_READ_WRITE: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_short_val, + &gatt_svr_pts_static_short_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, + sizeof gatt_svr_pts_static_short_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_dsc_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_LONG_DSC_READ_WRITE: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_long_val, - &gatt_svr_pts_static_long_val, NULL); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, - sizeof gatt_svr_pts_static_long_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_LONG_DSC_READ_WRITE: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_long_val, + &gatt_svr_pts_static_long_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, + sizeof gatt_svr_pts_static_long_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_DSC_READ: - if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, - sizeof gatt_svr_pts_static_long_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_DSC_READ: + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, + sizeof gatt_svr_pts_static_long_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_CHR_WRITE_NO_RSP: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_short_val, - &gatt_svr_pts_static_short_val, NULL); - return rc; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, - sizeof gatt_svr_pts_static_short_val); - return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; - } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + switch (uuid16) { + case PTS_CHR_WRITE_NO_RSP: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_short_val, + &gatt_svr_pts_static_short_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, + sizeof gatt_svr_pts_static_short_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } static int gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) + struct ble_gatt_access_ctxt *ctxt, + void *arg) { - uint16_t uuid16; - int rc; + uint16_t uuid16; + int rc; - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); - switch (uuid16) { - case PTS_CHR_RELIABLE_WRITE: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - rc = gatt_svr_chr_write(conn_handle, attr_handle, - ctxt->om, 0, - sizeof gatt_svr_pts_static_val, - &gatt_svr_pts_static_val, NULL); - return rc; - } + switch (uuid16) { + case PTS_CHR_RELIABLE_WRITE: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_val, + &gatt_svr_pts_static_val, NULL); + return rc; + } - default: - assert(0); - return BLE_ATT_ERR_UNLIKELY; - } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } } -static void start_server(uint8_t *data, uint16_t len) +static void +start_server(uint8_t *data, uint16_t len) { - struct gatt_start_server_rp rp; + struct gatt_start_server_rp rp; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - ble_gatts_show_local(); + ble_gatts_show_local(); - ble_svc_gatt_changed(0x0001, 0xffff); + ble_svc_gatt_changed(0x0001, 0xffff); - rp.db_attr_off = 0; - rp.db_attr_cnt = 0; + rp.db_attr_off = 0; + rp.db_attr_cnt = 0; - tester_send(BTP_SERVICE_ID_GATT, GATT_START_SERVER, CONTROLLER_INDEX, - (uint8_t *) &rp, sizeof(rp)); + tester_send(BTP_SERVICE_ID_GATT, GATT_START_SERVER, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); } /* Convert UUID from BTP command to bt_uuid */ -static uint8_t btp2bt_uuid(const uint8_t *uuid, uint8_t len, - ble_uuid_any_t *bt_uuid) +static uint8_t +btp2bt_uuid(const uint8_t *uuid, uint8_t len, + ble_uuid_any_t *bt_uuid) { - uint16_t le16; + uint16_t le16; - switch (len) { - case 0x02: /* UUID 16 */ - bt_uuid->u.type = BLE_UUID_TYPE_16; - memcpy(&le16, uuid, sizeof(le16)); - BLE_UUID16(bt_uuid)->value = sys_le16_to_cpu(le16); - break; - case 0x10: /* UUID 128*/ - bt_uuid->u.type = BLE_UUID_TYPE_128; - memcpy(BLE_UUID128(bt_uuid)->value, uuid, 16); - break; - default: - return BTP_STATUS_FAILED; - } - return BTP_STATUS_SUCCESS; + switch (len) { + case 0x02: /* UUID 16 */ + bt_uuid->u.type = BLE_UUID_TYPE_16; + memcpy(&le16, uuid, sizeof(le16)); + BLE_UUID16(bt_uuid)->value = sys_le16_to_cpu(le16); + break; + case 0x10: /* UUID 128*/ + bt_uuid->u.type = BLE_UUID_TYPE_128; + memcpy(BLE_UUID128(bt_uuid)->value, uuid, 16); + break; + default: + return BTP_STATUS_FAILED; + } + return BTP_STATUS_SUCCESS; } /* @@ -614,995 +616,1026 @@ static uint8_t btp2bt_uuid(const uint8_t *uuid, uint8_t len, * It is not intended to be used by client and server at the same time. */ static struct { - uint16_t len; - uint8_t buf[MAX_BUFFER_SIZE]; + uint16_t len; + uint8_t buf[MAX_BUFFER_SIZE]; } gatt_buf; -static void *gatt_buf_add(const void *data, size_t len) +static void * +gatt_buf_add(const void *data, size_t len) { - void *ptr = gatt_buf.buf + gatt_buf.len; + void *ptr = gatt_buf.buf + gatt_buf.len; - if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) { - return NULL; - } + if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) { + return NULL; + } - if (data) { - memcpy(ptr, data, len); - } else { - (void)memset(ptr, 0, len); - } + if (data) { + memcpy(ptr, data, len); + } else { + (void) memset(ptr, 0, len); + } - gatt_buf.len += len; + gatt_buf.len += len; - SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); + SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); - return ptr; + return ptr; } -static void *gatt_buf_reserve(size_t len) +static void * +gatt_buf_reserve(size_t len) { - return gatt_buf_add(NULL, len); + return gatt_buf_add(NULL, len); } -static void gatt_buf_clear(void) +static void +gatt_buf_clear(void) { - (void)memset(&gatt_buf, 0, sizeof(gatt_buf)); + (void) memset(&gatt_buf, 0, sizeof(gatt_buf)); } -static void discover_destroy(void) +static void +discover_destroy(void) { - gatt_buf_clear(); + gatt_buf_clear(); } -static void read_destroy() +static void +read_destroy() { - gatt_buf_clear(); + gatt_buf_clear(); } -static int read_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) +static int +read_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) { - struct gatt_read_rp *rp = (void *) gatt_buf.buf; - uint8_t btp_opcode = (uint8_t) (int) arg; + struct gatt_read_rp *rp = (void *) gatt_buf.buf; + uint8_t btp_opcode = (uint8_t) (int) arg; - SYS_LOG_DBG("status=%d", error->status); + SYS_LOG_DBG("status=%d", error->status); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status); - tester_send(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - read_destroy(); - return 0; - } + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status); + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + return 0; + } - if (!gatt_buf_add(attr->om->om_data, attr->om->om_len)) { - tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - read_destroy(); - return 0; - } + if (!gatt_buf_add(attr->om->om_data, attr->om->om_len)) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + read_destroy(); + return 0; + } - rp->data_length += attr->om->om_len; - tester_send(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - read_destroy(); + rp->data_length += attr->om->om_len; + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); - return 0; + return 0; } -static void read(uint8_t *data, uint16_t len) +static void +read(uint8_t *data, uint16_t len) { - const struct gatt_read_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - int rc; + const struct gatt_read_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - /* Clear buffer */ - read_destroy(); + /* Clear buffer */ + read_destroy(); - if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } - if (ble_gattc_read(conn.conn_handle, sys_le16_to_cpu(cmd->handle), - read_cb, (void *)GATT_READ)) { - read_destroy(); - goto fail; - } + if (ble_gattc_read(conn.conn_handle, sys_le16_to_cpu(cmd->handle), + read_cb, (void *) GATT_READ)) { + read_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static int read_long_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) +static int +read_long_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) { - struct gatt_read_rp *rp = (void *) gatt_buf.buf; - uint8_t btp_opcode = (uint8_t) (int) arg; + struct gatt_read_rp *rp = (void *) gatt_buf.buf; + uint8_t btp_opcode = (uint8_t) (int) arg; - SYS_LOG_DBG("status=%d", error->status); + SYS_LOG_DBG("status=%d", error->status); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status); - tester_send(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - read_destroy(); - return 0; - } + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status); + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + return 0; + } - if (error->status == BLE_HS_EDONE) { - tester_send(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - read_destroy(); - return 0; - } + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + return 0; + } - if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { - tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - read_destroy(); - return BLE_HS_ENOMEM; - } + if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + read_destroy(); + return BLE_HS_ENOMEM; + } - rp->data_length += attr->om->om_len; + rp->data_length += attr->om->om_len; - return 0; + return 0; } -static void read_long(uint8_t *data, uint16_t len) +static void +read_long(uint8_t *data, uint16_t len) { - const struct gatt_read_long_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - int rc; + const struct gatt_read_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - /* Clear buffer */ - read_destroy(); + /* Clear buffer */ + read_destroy(); - if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } - if (ble_gattc_read_long(conn.conn_handle, - sys_le16_to_cpu(cmd->handle), - sys_le16_to_cpu(cmd->offset), - read_long_cb, (void *)GATT_READ_LONG)) { - read_destroy(); - goto fail; - } + if (ble_gattc_read_long(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), + sys_le16_to_cpu(cmd->offset), + read_long_cb, (void *) GATT_READ_LONG)) { + read_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void read_multiple(uint8_t *data, uint16_t len) +static void +read_multiple(uint8_t *data, uint16_t len) { - const struct gatt_read_multiple_cmd *cmd = (void *) data; - uint16_t handles[cmd->handles_count]; - struct ble_gap_conn_desc conn; - int rc, i; + const struct gatt_read_multiple_cmd *cmd = (void *) data; + uint16_t handles[cmd->handles_count]; + struct ble_gap_conn_desc conn; + int rc, i; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - for (i = 0; i < ARRAY_SIZE(handles); i++) { - handles[i] = sys_le16_to_cpu(cmd->handles[i]); - } + for (i = 0; i < ARRAY_SIZE(handles); i++) { + handles[i] = sys_le16_to_cpu(cmd->handles[i]); + } - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - /* Clear buffer */ - read_destroy(); + /* Clear buffer */ + read_destroy(); - if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } - if (ble_gattc_read_mult(conn.conn_handle, handles, - cmd->handles_count, read_cb, - (void *)GATT_READ_MULTIPLE)) { - read_destroy(); - goto fail; - } + if (ble_gattc_read_mult(conn.conn_handle, handles, + cmd->handles_count, read_cb, + (void *) GATT_READ_MULTIPLE)) { + read_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void write_without_rsp(uint8_t *data, uint16_t len, uint8_t op, bool sign) +static void +write_without_rsp(uint8_t *data, uint16_t len, uint8_t op, bool sign) { - const struct gatt_write_without_rsp_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + const struct gatt_write_without_rsp_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - if (ble_gattc_write_no_rsp_flat(conn.conn_handle, - sys_le16_to_cpu(cmd->handle), cmd->data, - sys_le16_to_cpu(cmd->data_length))) { - status = BTP_STATUS_FAILED; - } + if (ble_gattc_write_no_rsp_flat(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), cmd->data, + sys_le16_to_cpu(cmd->data_length))) { + status = BTP_STATUS_FAILED; + } rsp: - tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); } -static int write_rsp(uint16_t conn_handle, const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) +static int +write_rsp(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) { - uint8_t err = (uint8_t) error->status; - uint8_t btp_opcode = (uint8_t) (int) arg; + uint8_t err = (uint8_t) error->status; + uint8_t btp_opcode = (uint8_t) (int) arg; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - tester_send(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, &err, sizeof(err)); - return 0; + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, &err, sizeof(err)); + return 0; } -static void write(uint8_t *data, uint16_t len) +static void +write(uint8_t *data, uint16_t len) { - const struct gatt_write_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - int rc; + const struct gatt_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (ble_gattc_write_flat(conn.conn_handle, sys_le16_to_cpu(cmd->handle), - cmd->data, sys_le16_to_cpu(cmd->data_length), - write_rsp, (void *) GATT_WRITE)) { - goto fail; - } + if (ble_gattc_write_flat(conn.conn_handle, sys_le16_to_cpu(cmd->handle), + cmd->data, sys_le16_to_cpu(cmd->data_length), + write_rsp, (void *) GATT_WRITE)) { + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void write_long(uint8_t *data, uint16_t len) +static void +write_long(uint8_t *data, uint16_t len) { - const struct gatt_write_long_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - struct os_mbuf *om = NULL; - int rc = 0; + const struct gatt_write_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + struct os_mbuf *om = NULL; + int rc = 0; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); - if (!om) { - SYS_LOG_ERR("Insufficient resources"); - goto fail; - } + om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); + if (!om) { + SYS_LOG_ERR("Insufficient resources"); + goto fail; + } - rc = ble_gattc_write_long(conn.conn_handle, - sys_le16_to_cpu(cmd->handle), - sys_le16_to_cpu(cmd->offset), - om, write_rsp, - (void *) GATT_WRITE_LONG); - if (!rc) { - return; - } + rc = ble_gattc_write_long(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), + sys_le16_to_cpu(cmd->offset), + om, write_rsp, + (void *) GATT_WRITE_LONG); + if (!rc) { + return; + } fail: - SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc); - os_mbuf_free_chain(om); - tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc); + os_mbuf_free_chain(om); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static int reliable_write_rsp(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attrs, - uint8_t num_attrs, - void *arg) +static int +reliable_write_rsp(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attrs, + uint8_t num_attrs, + void *arg) { - uint8_t err = (uint8_t) error->status; + uint8_t err = (uint8_t) error->status; - SYS_LOG_DBG("Reliable write status %d", err); + SYS_LOG_DBG("Reliable write status %d", err); - tester_send(BTP_SERVICE_ID_GATT, GATT_RELIABLE_WRITE, - CONTROLLER_INDEX, &err, sizeof(err)); - return 0; + tester_send(BTP_SERVICE_ID_GATT, GATT_RELIABLE_WRITE, + CONTROLLER_INDEX, &err, sizeof(err)); + return 0; } -static void reliable_write(uint8_t *data, uint16_t len) +static void +reliable_write(uint8_t *data, uint16_t len) { - const struct gatt_reliable_write_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - struct ble_gatt_attr attr; - struct os_mbuf *om = NULL; - int rc; + const struct gatt_reliable_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + struct ble_gatt_attr attr; + struct os_mbuf *om = NULL; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); - /* This is required, because Nimble checks if - * the data is longer than offset - */ - if (os_mbuf_extend(om, sys_le16_to_cpu(cmd->offset) + 1) == NULL) { - goto fail; - } + om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); + /* This is required, because Nimble checks if + * the data is longer than offset + */ + if (os_mbuf_extend(om, sys_le16_to_cpu(cmd->offset) + 1) == NULL) { + goto fail; + } - attr.handle = sys_le16_to_cpu(cmd->handle); - attr.offset = sys_le16_to_cpu(cmd->offset); - attr.om = om; + attr.handle = sys_le16_to_cpu(cmd->handle); + attr.offset = sys_le16_to_cpu(cmd->offset); + attr.om = om; - if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, - reliable_write_rsp, NULL)) { - goto fail; - } + if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, + reliable_write_rsp, NULL)) { + goto fail; + } - return; + return; fail: - os_mbuf_free_chain(om); + os_mbuf_free_chain(om); - tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } static struct bt_gatt_subscribe_params { - uint16_t ccc_handle; - uint16_t value; - uint16_t value_handle; + uint16_t ccc_handle; + uint16_t value; + uint16_t value_handle; } subscribe_params; -static void read_uuid(uint8_t *data, uint16_t len) +static void +read_uuid(uint8_t *data, uint16_t len) { - const struct gatt_read_uuid_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - ble_uuid_any_t uuid; - int rc; + const struct gatt_read_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + ble_uuid_any_t uuid; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { - goto fail; - } + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + goto fail; + } - /* Clear buffer */ - read_destroy(); + /* Clear buffer */ + read_destroy(); - if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } - if (ble_gattc_read_by_uuid(conn.conn_handle, - sys_le16_to_cpu(cmd->start_handle), - sys_le16_to_cpu(cmd->end_handle), &uuid.u, - read_long_cb, (void *)GATT_READ_UUID)) { - read_destroy(); - goto fail; - } + if (ble_gattc_read_by_uuid(conn.conn_handle, + sys_le16_to_cpu(cmd->start_handle), + sys_le16_to_cpu(cmd->end_handle), &uuid.u, + read_long_cb, (void *) GATT_READ_UUID)) { + read_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static int disc_prim_uuid_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - const struct ble_gatt_svc *gatt_svc, void *arg) +static int +disc_prim_uuid_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_svc *gatt_svc, void *arg) { - struct gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf; - struct gatt_service *service; - const ble_uuid_any_t *uuid; - uint8_t uuid_length; - uint8_t opcode = (uint8_t) (int) arg; + struct gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf; + struct gatt_service *service; + const ble_uuid_any_t *uuid; + uint8_t uuid_length; + uint8_t opcode = (uint8_t) (int) arg; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - tester_rsp(BTP_SERVICE_ID_GATT, opcode, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } - if (error->status == BLE_HS_EDONE) { - tester_send(BTP_SERVICE_ID_GATT, opcode, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - discover_destroy(); - return 0; - } + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } - uuid = &gatt_svc->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + uuid = &gatt_svc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - service = gatt_buf_reserve(sizeof(*service) + uuid_length); - if (!service) { - tester_rsp(BTP_SERVICE_ID_GATT, opcode, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } + service = gatt_buf_reserve(sizeof(*service) + uuid_length); + if (!service) { + tester_rsp(BTP_SERVICE_ID_GATT, opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return BLE_HS_ENOMEM; + } - service->start_handle = sys_cpu_to_le16(gatt_svc->start_handle); - service->end_handle = sys_cpu_to_le16(gatt_svc->end_handle); - service->uuid_length = uuid_length; + service->start_handle = sys_cpu_to_le16(gatt_svc->start_handle); + service->end_handle = sys_cpu_to_le16(gatt_svc->end_handle); + service->uuid_length = uuid_length; - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(service->uuid, &u16, uuid_length); - } else { - memcpy(service->uuid, BLE_UUID128(uuid)->value, - uuid_length); - } + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(service->uuid, &u16, uuid_length); + } else { + memcpy(service->uuid, BLE_UUID128(uuid)->value, + uuid_length); + } - rp->services_count++; + rp->services_count++; - return 0; + return 0; } -static int disc_all_desc_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - uint16_t chr_val_handle, - const struct ble_gatt_dsc *gatt_dsc, - void *arg) +static int +disc_all_desc_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + uint16_t chr_val_handle, + const struct ble_gatt_dsc *gatt_dsc, + void *arg) { - struct gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf; - struct gatt_descriptor *dsc; - const ble_uuid_any_t *uuid; - uint8_t uuid_length; + struct gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf; + struct gatt_descriptor *dsc; + const ble_uuid_any_t *uuid; + uint8_t uuid_length; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } - if (error->status == BLE_HS_EDONE) { - tester_send(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - discover_destroy(); - return 0; - } + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } - uuid = &gatt_dsc->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + uuid = &gatt_dsc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - dsc = gatt_buf_reserve(sizeof(*dsc) + uuid_length); - if (!dsc) { - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } + dsc = gatt_buf_reserve(sizeof(*dsc) + uuid_length); + if (!dsc) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return BLE_HS_ENOMEM; + } - dsc->descriptor_handle = sys_cpu_to_le16(gatt_dsc->handle); - dsc->uuid_length = uuid_length; + dsc->descriptor_handle = sys_cpu_to_le16(gatt_dsc->handle); + dsc->uuid_length = uuid_length; - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(dsc->uuid, &u16, uuid_length); - } else { - memcpy(dsc->uuid, BLE_UUID128(uuid)->value, uuid_length); - } + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(dsc->uuid, &u16, uuid_length); + } else { + memcpy(dsc->uuid, BLE_UUID128(uuid)->value, uuid_length); + } - rp->descriptors_count++; + rp->descriptors_count++; - return 0; + return 0; } -static void disc_all_prim_svcs(uint8_t *data, uint16_t len) +static void +disc_all_prim_svcs(uint8_t *data, uint16_t len) { - struct ble_gap_conn_desc conn; - int rc; + struct ble_gap_conn_desc conn; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_prim_svcs_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_prim_svcs_rp))) { + goto fail; + } - if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_uuid_cb, - (void *) GATT_DISC_ALL_PRIM_SVCS)) { - discover_destroy(); - goto fail; - } + if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_uuid_cb, + (void *) GATT_DISC_ALL_PRIM_SVCS)) { + discover_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_PRIM_SVCS, - CONTROLLER_INDEX, BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_PRIM_SVCS, + CONTROLLER_INDEX, BTP_STATUS_FAILED); } -static void disc_all_desc(uint8_t *data, uint16_t len) +static void +disc_all_desc(uint8_t *data, uint16_t len) { - const struct gatt_disc_all_desc_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - int rc; + const struct gatt_disc_all_desc_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_desc_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_desc_rp))) { + goto fail; + } - start_handle = sys_le16_to_cpu(cmd->start_handle) - 1; - end_handle = sys_le16_to_cpu(cmd->end_handle); + start_handle = sys_le16_to_cpu(cmd->start_handle) - 1; + end_handle = sys_le16_to_cpu(cmd->end_handle); - rc = ble_gattc_disc_all_dscs(conn.conn_handle, start_handle, end_handle, - disc_all_desc_cb, NULL); + rc = ble_gattc_disc_all_dscs(conn.conn_handle, start_handle, end_handle, + disc_all_desc_cb, NULL); - SYS_LOG_DBG("rc=%d", rc); + SYS_LOG_DBG("rc=%d", rc); - if (rc) { - discover_destroy(); - goto fail; - } + if (rc) { + discover_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static int find_included_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - const struct ble_gatt_svc *gatt_svc, void *arg) +static int +find_included_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_svc *gatt_svc, void *arg) { - struct gatt_find_included_rp *rp = (void *) gatt_buf.buf; - struct gatt_included *included; - const ble_uuid_any_t *uuid; - int service_handle = (int) arg; - uint8_t uuid_length; + struct gatt_find_included_rp *rp = (void *) gatt_buf.buf; + struct gatt_included *included; + const ble_uuid_any_t *uuid; + int service_handle = (int) arg; + uint8_t uuid_length; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } - if (error->status == BLE_HS_EDONE) { - tester_send(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - discover_destroy(); - return 0; - } + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } - uuid = &gatt_svc->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + uuid = &gatt_svc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + included = gatt_buf_reserve(sizeof(*included) + uuid_length); + if (!included) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return BLE_HS_ENOMEM; + } - included = gatt_buf_reserve(sizeof(*included) + uuid_length); - if (!included) { - tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } + /* FIXME */ + included->included_handle = sys_cpu_to_le16(service_handle + 1 + + rp->services_count); + included->service.start_handle = sys_cpu_to_le16(gatt_svc->start_handle); + included->service.end_handle = sys_cpu_to_le16(gatt_svc->end_handle); + included->service.uuid_length = uuid_length; - /* FIXME */ - included->included_handle = sys_cpu_to_le16(service_handle + 1 + - rp->services_count); - included->service.start_handle = sys_cpu_to_le16(gatt_svc->start_handle); - included->service.end_handle = sys_cpu_to_le16(gatt_svc->end_handle); - included->service.uuid_length = uuid_length; + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(included->service.uuid, &u16, uuid_length); + } else { + memcpy(included->service.uuid, BLE_UUID128(uuid)->value, + uuid_length); + } - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(included->service.uuid, &u16, uuid_length); - } else { - memcpy(included->service.uuid, BLE_UUID128(uuid)->value, - uuid_length); - } + rp->services_count++; - rp->services_count++; - - return 0; + return 0; } -static int disc_chrc_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - const struct ble_gatt_chr *gatt_chr, void *arg) +static int +disc_chrc_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_chr *gatt_chr, void *arg) { - struct gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf; - struct gatt_characteristic *chrc; - const ble_uuid_any_t *uuid; - uint8_t btp_opcode = (uint8_t) (int) arg; - uint8_t uuid_length; + struct gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf; + struct gatt_characteristic *chrc; + const ble_uuid_any_t *uuid; + uint8_t btp_opcode = (uint8_t) (int) arg; + uint8_t uuid_length; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } - if (error->status == BLE_HS_EDONE) { - tester_send(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); - discover_destroy(); - return 0; - } + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } - uuid = &gatt_chr->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + uuid = &gatt_chr->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - chrc = gatt_buf_reserve(sizeof(*chrc) + uuid_length); - if (!chrc) { - tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } + chrc = gatt_buf_reserve(sizeof(*chrc) + uuid_length); + if (!chrc) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return BLE_HS_ENOMEM; + } - chrc->characteristic_handle = sys_cpu_to_le16(gatt_chr->def_handle); - chrc->properties = gatt_chr->properties; - chrc->value_handle = sys_cpu_to_le16(gatt_chr->val_handle); - chrc->uuid_length = uuid_length; + chrc->characteristic_handle = sys_cpu_to_le16(gatt_chr->def_handle); + chrc->properties = gatt_chr->properties; + chrc->value_handle = sys_cpu_to_le16(gatt_chr->val_handle); + chrc->uuid_length = uuid_length; - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(chrc->uuid, &u16, uuid_length); - } else { - memcpy(chrc->uuid, BLE_UUID128(uuid)->value, - uuid_length); - } + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(chrc->uuid, &u16, uuid_length); + } else { + memcpy(chrc->uuid, BLE_UUID128(uuid)->value, + uuid_length); + } - rp->characteristics_count++; + rp->characteristics_count++; - return 0; + return 0; } -static void disc_chrc_uuid(uint8_t *data, uint16_t len) +static void +disc_chrc_uuid(uint8_t *data, uint16_t len) { - const struct gatt_disc_chrc_uuid_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - ble_uuid_any_t uuid; - int rc; + const struct gatt_disc_chrc_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + ble_uuid_any_t uuid; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { - goto fail; - } + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + goto fail; + } - if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { + goto fail; + } - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); - if (ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle, - end_handle, &uuid.u, disc_chrc_cb, - (void *)GATT_DISC_CHRC_UUID)) { - discover_destroy(); - goto fail; - } + if (ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle, + end_handle, &uuid.u, disc_chrc_cb, + (void *) GATT_DISC_CHRC_UUID)) { + discover_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_CHRC_UUID, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_CHRC_UUID, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void disc_prim_uuid(uint8_t *data, uint16_t len) +static void +disc_prim_uuid(uint8_t *data, uint16_t len) { - const struct gatt_disc_prim_uuid_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - ble_uuid_any_t uuid; - int rc; + const struct gatt_disc_prim_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + ble_uuid_any_t uuid; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { - goto fail; - } + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + goto fail; + } - if (!gatt_buf_reserve(sizeof(struct gatt_disc_prim_uuid_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_disc_prim_uuid_rp))) { + goto fail; + } - if (ble_gattc_disc_svc_by_uuid(conn.conn_handle, - &uuid.u, disc_prim_uuid_cb, - (void *) GATT_DISC_PRIM_UUID)) { - discover_destroy(); - goto fail; - } + if (ble_gattc_disc_svc_by_uuid(conn.conn_handle, + &uuid.u, disc_prim_uuid_cb, + (void *) GATT_DISC_PRIM_UUID)) { + discover_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void disc_all_chrc(uint8_t *data, uint16_t len) +static void +disc_all_chrc(uint8_t *data, uint16_t len) { - const struct gatt_disc_all_chrc_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - int rc; + const struct gatt_disc_all_chrc_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - SYS_LOG_DBG("Conn find failed"); - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + SYS_LOG_DBG("Conn find failed"); + goto fail; + } - if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { - SYS_LOG_DBG("Buf reserve failed"); - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { + SYS_LOG_DBG("Buf reserve failed"); + goto fail; + } - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); - rc = ble_gattc_disc_all_chrs(conn.conn_handle, start_handle, end_handle, - disc_chrc_cb, (void *)GATT_DISC_ALL_CHRC); - if (rc) { - discover_destroy(); - goto fail; - } + rc = ble_gattc_disc_all_chrs(conn.conn_handle, start_handle, end_handle, + disc_chrc_cb, (void *) GATT_DISC_ALL_CHRC); + if (rc) { + discover_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_CHRC, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_CHRC, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void find_included(uint8_t *data, uint16_t len) +static void +find_included(uint8_t *data, uint16_t len) { - const struct gatt_find_included_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - int service_handle_arg; - int rc; + const struct gatt_find_included_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int service_handle_arg; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (!gatt_buf_reserve(sizeof(struct gatt_find_included_rp))) { - goto fail; - } + if (!gatt_buf_reserve(sizeof(struct gatt_find_included_rp))) { + goto fail; + } - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); - service_handle_arg = start_handle; + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + service_handle_arg = start_handle; - if (ble_gattc_find_inc_svcs(conn.conn_handle, start_handle, end_handle, - find_included_cb, - (void *)service_handle_arg)) { - discover_destroy(); - goto fail; - } + if (ble_gattc_find_inc_svcs(conn.conn_handle, start_handle, end_handle, + find_included_cb, + (void *) service_handle_arg)) { + discover_destroy(); + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static int exchange_func(uint16_t conn_handle, - const struct ble_gatt_error *error, - uint16_t mtu, void *arg) +static int +exchange_func(uint16_t conn_handle, + const struct ble_gatt_error *error, + uint16_t mtu, void *arg) { - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (error->status) { - tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, - CONTROLLER_INDEX, BTP_STATUS_FAILED); + if (error->status) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, + CONTROLLER_INDEX, BTP_STATUS_FAILED); - return 0; + return 0; + } + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + + return 0; } - tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); - - return 0; -} - -static void exchange_mtu(uint8_t *data, uint16_t len) +static void +exchange_mtu(uint8_t *data, uint16_t len) { - struct ble_gap_conn_desc conn; - int rc; + struct ble_gap_conn_desc conn; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } - if (ble_gattc_exchange_mtu(conn.conn_handle, exchange_func, NULL)) { - goto fail; - } + if (ble_gattc_exchange_mtu(conn.conn_handle, exchange_func, NULL)) { + goto fail; + } - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, - CONTROLLER_INDEX, BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, + CONTROLLER_INDEX, BTP_STATUS_FAILED); } -static int enable_subscription(uint16_t conn_handle, uint16_t ccc_handle, - uint16_t value) +static int +enable_subscription(uint16_t conn_handle, uint16_t ccc_handle, + uint16_t value) { - uint8_t op; + uint8_t op; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - op = (uint8_t) (value == 0x0001 ? GATT_CFG_NOTIFY : GATT_CFG_INDICATE); + op = (uint8_t) (value == 0x0001 ? GATT_CFG_NOTIFY : GATT_CFG_INDICATE); - if (ble_gattc_write_flat(conn_handle, ccc_handle, - &value, sizeof(value), NULL, NULL)) { - return -EINVAL; - } + if (ble_gattc_write_flat(conn_handle, ccc_handle, + &value, sizeof(value), NULL, NULL)) { + return -EINVAL; + } - subscribe_params.ccc_handle = value; + subscribe_params.ccc_handle = value; - tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); - return 0; + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + return 0; } -static int disable_subscription(uint16_t conn_handle, uint16_t ccc_handle) +static int +disable_subscription(uint16_t conn_handle, uint16_t ccc_handle) { - uint16_t value = 0x00; + uint16_t value = 0x00; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - /* Fail if CCC handle doesn't match */ - if (ccc_handle != subscribe_params.ccc_handle) { - SYS_LOG_ERR("CCC handle doesn't match"); - return -EINVAL; - } + /* Fail if CCC handle doesn't match */ + if (ccc_handle != subscribe_params.ccc_handle) { + SYS_LOG_ERR("CCC handle doesn't match"); + return -EINVAL; + } - if (ble_gattc_write_no_rsp_flat(conn_handle, ccc_handle, - &value, sizeof(value))) { - return -EINVAL; - } + if (ble_gattc_write_no_rsp_flat(conn_handle, ccc_handle, + &value, sizeof(value))) { + return -EINVAL; + } - subscribe_params.ccc_handle = 0; - return 0; + subscribe_params.ccc_handle = 0; + return 0; } -static void config_subscription(uint8_t *data, uint16_t len, uint8_t op) +static void +config_subscription(uint8_t *data, uint16_t len, uint8_t op) { - const struct gatt_cfg_notify_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); - uint8_t status; - int rc; + const struct gatt_cfg_notify_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); + uint8_t status; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); - if (rc) { - tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, - BTP_STATUS_FAILED); - return; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, + BTP_STATUS_FAILED); + return; + } - if (cmd->enable) { - uint16_t value; + if (cmd->enable) { + uint16_t value; - if (op == GATT_CFG_NOTIFY) { - value = 0x0001; - } else { - value = 0x0002; - } + if (op == GATT_CFG_NOTIFY) { + value = 0x0001; + } else { + value = 0x0002; + } - /* on success response will be sent from callback */ - if (enable_subscription(conn.conn_handle, - ccc_handle, value) == 0) { - return; - } + /* on success response will be sent from callback */ + if (enable_subscription(conn.conn_handle, + ccc_handle, value) == 0) { + return; + } - status = BTP_STATUS_FAILED; - } else { - if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { - status = BTP_STATUS_FAILED; - } else { - status = BTP_STATUS_SUCCESS; - } - } + status = BTP_STATUS_FAILED; + } else { + if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { + status = BTP_STATUS_FAILED; + } else { + status = BTP_STATUS_SUCCESS; + } + } - SYS_LOG_DBG("Config subscription (op %u) status %u", op, status); + SYS_LOG_DBG("Config subscription (op %u) status %u", op, status); - tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); } #define BTP_PERM_F_READ 0x01 @@ -1615,484 +1648,513 @@ static void config_subscription(uint8_t *data, uint16_t len, uint8_t op) #define BTP_PERM_F_WRITE_AUTHOR 0x80 static int flags_hs2btp_map[] = { - BTP_PERM_F_READ, - BTP_PERM_F_WRITE, - BTP_PERM_F_READ_ENC, - BTP_PERM_F_READ_AUTHEN, - BTP_PERM_F_READ_AUTHOR, - BTP_PERM_F_WRITE_ENC, - BTP_PERM_F_WRITE_AUTHEN, - BTP_PERM_F_WRITE_AUTHOR, + BTP_PERM_F_READ, + BTP_PERM_F_WRITE, + BTP_PERM_F_READ_ENC, + BTP_PERM_F_READ_AUTHEN, + BTP_PERM_F_READ_AUTHOR, + BTP_PERM_F_WRITE_ENC, + BTP_PERM_F_WRITE_AUTHEN, + BTP_PERM_F_WRITE_AUTHOR, }; -static uint8_t flags_hs2btp(uint8_t flags) +static uint8_t +flags_hs2btp(uint8_t flags) { - int i; - uint8_t ret = 0; + int i; + uint8_t ret = 0; - for (i = 0; i < 8; ++i) { - if (flags & BIT(i)) { - ret |= flags_hs2btp_map[i]; - } - } + for (i = 0; i < 8; ++i) { + if (flags & BIT(i)) { + ret |= flags_hs2btp_map[i]; + } + } - return ret; + return ret; } -static void get_attrs(uint8_t *data, uint16_t len) +static void +get_attrs(uint8_t *data, uint16_t len) { - const struct gatt_get_attributes_cmd *cmd = (void *) data; - struct gatt_get_attributes_rp *rp; - struct gatt_attr *gatt_attr; - struct os_mbuf *buf = os_msys_get(0, 0); - uint16_t start_handle, end_handle; - struct ble_att_svr_entry *entry = NULL; - ble_uuid_any_t uuid; - ble_uuid_t *uuid_ptr = NULL; - uint8_t count = 0; - char str[BLE_UUID_STR_LEN]; + const struct gatt_get_attributes_cmd *cmd = (void *) data; + struct gatt_get_attributes_rp *rp; + struct gatt_attr *gatt_attr; + struct os_mbuf *buf = os_msys_get(0, 0); + uint16_t start_handle, end_handle; + struct ble_att_svr_entry *entry = NULL; + ble_uuid_any_t uuid; + ble_uuid_t *uuid_ptr = NULL; + uint8_t count = 0; + char str[BLE_UUID_STR_LEN]; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memset(str, 0, sizeof(str)); - memset(&uuid, 0, sizeof(uuid)); - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); + memset(str, 0, sizeof(str)); + memset(&uuid, 0, sizeof(uuid)); + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); - if (cmd->type_length) { - if (btp2bt_uuid(cmd->type, cmd->type_length, &uuid)) { - goto fail; - } + if (cmd->type_length) { + if (btp2bt_uuid(cmd->type, cmd->type_length, &uuid)) { + goto fail; + } - ble_uuid_to_str(&uuid.u, str); - SYS_LOG_DBG("start 0x%04x end 0x%04x, uuid %s", start_handle, - end_handle, str); + ble_uuid_to_str(&uuid.u, str); + SYS_LOG_DBG("start 0x%04x end 0x%04x, uuid %s", start_handle, + end_handle, str); - uuid_ptr = &uuid.u; - } else { - SYS_LOG_DBG("start 0x%04x end 0x%04x", start_handle, end_handle); - } + uuid_ptr = &uuid.u; + } else { + SYS_LOG_DBG("start 0x%04x end 0x%04x", start_handle, end_handle); + } - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); - while (entry) { + entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); + while (entry) { - if (entry->ha_handle_id < start_handle) { - entry = ble_att_svr_find_by_uuid(entry, - uuid_ptr, end_handle); - continue; - } + if (entry->ha_handle_id < start_handle) { + entry = ble_att_svr_find_by_uuid(entry, + uuid_ptr, end_handle); + continue; + } - gatt_attr = net_buf_simple_add(buf, sizeof(*gatt_attr)); - gatt_attr->handle = sys_cpu_to_le16(entry->ha_handle_id); - gatt_attr->permission = flags_hs2btp(entry->ha_flags); + gatt_attr = net_buf_simple_add(buf, sizeof(*gatt_attr)); + gatt_attr->handle = sys_cpu_to_le16(entry->ha_handle_id); + gatt_attr->permission = flags_hs2btp(entry->ha_flags); - if (entry->ha_uuid->type == BLE_UUID_TYPE_16) { - gatt_attr->type_length = 2; - net_buf_simple_add_le16(buf, - BLE_UUID16(entry->ha_uuid)->value); - } else { - gatt_attr->type_length = 16; - net_buf_simple_add_mem(buf, - BLE_UUID128(entry->ha_uuid)->value, - gatt_attr->type_length); - } + if (entry->ha_uuid->type == BLE_UUID_TYPE_16) { + gatt_attr->type_length = 2; + net_buf_simple_add_le16(buf, + BLE_UUID16(entry->ha_uuid)->value); + } else { + gatt_attr->type_length = 16; + net_buf_simple_add_mem(buf, + BLE_UUID128(entry->ha_uuid)->value, + gatt_attr->type_length); + } - count++; + count++; - entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); - } + entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); + } - rp->attrs_count = count; + rp->attrs_count = count; - tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, + CONTROLLER_INDEX, buf); - goto free; + goto free; fail: - tester_rsp(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, CONTROLLER_INDEX, + BTP_STATUS_FAILED); free: - os_mbuf_free_chain(buf); + os_mbuf_free_chain(buf); } -static void get_attr_val(uint8_t *data, uint16_t len) +static void +get_attr_val(uint8_t *data, uint16_t len) { - const struct gatt_get_attribute_value_cmd *cmd = (void *) data; - struct gatt_get_attribute_value_rp *rp; - struct ble_gap_conn_desc conn; - struct os_mbuf *buf = os_msys_get(0, 0); - uint16_t handle = sys_cpu_to_le16(cmd->handle); - uint8_t out_att_err; - int conn_status; + const struct gatt_get_attribute_value_cmd *cmd = (void *) data; + struct gatt_get_attribute_value_rp *rp; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + uint16_t handle = sys_cpu_to_le16(cmd->handle); + uint8_t out_att_err; + int conn_status; - conn_status = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + conn_status = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (conn_status) { - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); + if (conn_status) { + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, - handle, 0, buf, - &out_att_err); + ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, + handle, 0, buf, + &out_att_err); - rp->att_response = out_att_err; - rp->value_length = os_mbuf_len(buf) - sizeof(*rp); + rp->att_response = out_att_err; + rp->value_length = os_mbuf_len(buf) - sizeof(*rp); - tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE, + CONTROLLER_INDEX, buf); - goto free; - } else { - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); + goto free; + } else { + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - ble_att_svr_read_handle(conn.conn_handle, - handle, 0, buf, - &out_att_err); + ble_att_svr_read_handle(conn.conn_handle, + handle, 0, buf, + &out_att_err); - rp->att_response = out_att_err; - rp->value_length = os_mbuf_len(buf) - sizeof(*rp); + rp->att_response = out_att_err; + rp->value_length = os_mbuf_len(buf) - sizeof(*rp); - tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE, + CONTROLLER_INDEX, buf); - goto free; - } + goto free; + } free: - os_mbuf_free_chain(buf); + os_mbuf_free_chain(buf); } -static void change_database(uint8_t *data, uint16_t len) +static void +change_database(uint8_t *data, uint16_t len) { - const struct gatt_change_database *cmd = (void *) data; + const struct gatt_change_database *cmd = (void *) data; - SYS_LOG_DBG("") + SYS_LOG_DBG("") - ble_gatts_show_local(); + ble_gatts_show_local(); - ble_svc_gatt_changed(cmd->start_handle, cmd->end_handle); + ble_svc_gatt_changed(cmd->start_handle, cmd->end_handle); - tester_rsp(BTP_SERVICE_ID_GATT, GATT_CHANGE_DATABASE, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_CHANGE_DATABASE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); - return; + return; } -static void supported_commands(uint8_t *data, uint16_t len) +static void +supported_commands(uint8_t *data, uint16_t len) { - uint8_t cmds[4]; - struct gatt_read_supported_commands_rp *rp = (void *) cmds; + uint8_t cmds[4]; + struct gatt_read_supported_commands_rp *rp = (void *) cmds; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memset(cmds, 0, sizeof(cmds)); + memset(cmds, 0, sizeof(cmds)); - tester_set_bit(cmds, GATT_READ_SUPPORTED_COMMANDS); - tester_set_bit(cmds, GATT_START_SERVER); - tester_set_bit(cmds, GATT_EXCHANGE_MTU); - tester_set_bit(cmds, GATT_DISC_ALL_PRIM_SVCS); - tester_set_bit(cmds, GATT_DISC_PRIM_UUID); - tester_set_bit(cmds, GATT_FIND_INCLUDED); - tester_set_bit(cmds, GATT_DISC_ALL_CHRC); - tester_set_bit(cmds, GATT_DISC_CHRC_UUID); - tester_set_bit(cmds, GATT_DISC_ALL_DESC); - tester_set_bit(cmds, GATT_READ); - tester_set_bit(cmds, GATT_READ_LONG); - tester_set_bit(cmds, GATT_READ_MULTIPLE); - tester_set_bit(cmds, GATT_WRITE_WITHOUT_RSP); + tester_set_bit(cmds, GATT_READ_SUPPORTED_COMMANDS); + tester_set_bit(cmds, GATT_START_SERVER); + tester_set_bit(cmds, GATT_EXCHANGE_MTU); + tester_set_bit(cmds, GATT_DISC_ALL_PRIM_SVCS); + tester_set_bit(cmds, GATT_DISC_PRIM_UUID); + tester_set_bit(cmds, GATT_FIND_INCLUDED); + tester_set_bit(cmds, GATT_DISC_ALL_CHRC); + tester_set_bit(cmds, GATT_DISC_CHRC_UUID); + tester_set_bit(cmds, GATT_DISC_ALL_DESC); + tester_set_bit(cmds, GATT_READ); + tester_set_bit(cmds, GATT_READ_LONG); + tester_set_bit(cmds, GATT_READ_MULTIPLE); + tester_set_bit(cmds, GATT_WRITE_WITHOUT_RSP); #if 0 - tester_set_bit(cmds, GATT_SIGNED_WRITE_WITHOUT_RSP); + tester_set_bit(cmds, GATT_SIGNED_WRITE_WITHOUT_RSP); #endif - tester_set_bit(cmds, GATT_WRITE); - tester_set_bit(cmds, GATT_WRITE_LONG); - tester_set_bit(cmds, GATT_CFG_NOTIFY); - tester_set_bit(cmds, GATT_CFG_INDICATE); - tester_set_bit(cmds, GATT_GET_ATTRIBUTES); - tester_set_bit(cmds, GATT_GET_ATTRIBUTE_VALUE); - tester_set_bit(cmds, GATT_CHANGE_DATABASE); + tester_set_bit(cmds, GATT_WRITE); + tester_set_bit(cmds, GATT_WRITE_LONG); + tester_set_bit(cmds, GATT_CFG_NOTIFY); + tester_set_bit(cmds, GATT_CFG_INDICATE); + tester_set_bit(cmds, GATT_GET_ATTRIBUTES); + tester_set_bit(cmds, GATT_GET_ATTRIBUTE_VALUE); + tester_set_bit(cmds, GATT_CHANGE_DATABASE); - tester_send(BTP_SERVICE_ID_GATT, GATT_READ_SUPPORTED_COMMANDS, - CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); + tester_send(BTP_SERVICE_ID_GATT, GATT_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); } enum attr_type { - BLE_GATT_ATTR_SVC = 0, - BLE_GATT_ATTR_CHR, - BLE_GATT_ATTR_DSC, + BLE_GATT_ATTR_SVC = 0, + BLE_GATT_ATTR_CHR, + BLE_GATT_ATTR_DSC, }; -void tester_handle_gatt(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len) +void +tester_handle_gatt(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len) { - switch (opcode) { - case GATT_READ_SUPPORTED_COMMANDS: - supported_commands(data, len); - return; - case GATT_START_SERVER: - start_server(data, len); - return; - case GATT_EXCHANGE_MTU: - exchange_mtu(data, len); - return; - case GATT_DISC_ALL_PRIM_SVCS: - disc_all_prim_svcs(data, len); - return; - case GATT_DISC_PRIM_UUID: - disc_prim_uuid(data, len); - return; - case GATT_FIND_INCLUDED: - find_included(data, len); - return; - case GATT_DISC_ALL_CHRC: - disc_all_chrc(data, len); - return; - case GATT_DISC_CHRC_UUID: - disc_chrc_uuid(data, len); - return; - case GATT_DISC_ALL_DESC: - disc_all_desc(data, len); - return; - case GATT_CHANGE_DATABASE: - change_database(data, len); - return; - case GATT_READ: - read(data, len); - return; - case GATT_READ_UUID: - read_uuid(data, len); - return; - case GATT_READ_LONG: - read_long(data, len); - return; - case GATT_READ_MULTIPLE: - read_multiple(data, len); - return; - case GATT_WRITE_WITHOUT_RSP: - write_without_rsp(data, len, opcode, false); - return; + switch (opcode) { + case GATT_READ_SUPPORTED_COMMANDS: + supported_commands(data, len); + return; + case GATT_START_SERVER: + start_server(data, len); + return; + case GATT_EXCHANGE_MTU: + exchange_mtu(data, len); + return; + case GATT_DISC_ALL_PRIM_SVCS: + disc_all_prim_svcs(data, len); + return; + case GATT_DISC_PRIM_UUID: + disc_prim_uuid(data, len); + return; + case GATT_FIND_INCLUDED: + find_included(data, len); + return; + case GATT_DISC_ALL_CHRC: + disc_all_chrc(data, len); + return; + case GATT_DISC_CHRC_UUID: + disc_chrc_uuid(data, len); + return; + case GATT_DISC_ALL_DESC: + disc_all_desc(data, len); + return; + case GATT_CHANGE_DATABASE: + change_database(data, len); + return; + case GATT_READ: + read(data, len); + return; + case GATT_READ_UUID: + read_uuid(data, len); + return; + case GATT_READ_LONG: + read_long(data, len); + return; + case GATT_READ_MULTIPLE: + read_multiple(data, len); + return; + case GATT_WRITE_WITHOUT_RSP: + write_without_rsp(data, + len, + opcode, + false); + return; #if 0 - case GATT_SIGNED_WRITE_WITHOUT_RSP: - write_without_rsp(data, len, opcode, true); - return; + case GATT_SIGNED_WRITE_WITHOUT_RSP: + write_without_rsp(data, len, opcode, true); + return; #endif - case GATT_WRITE: - write(data, len); - return; - case GATT_WRITE_LONG: - write_long(data, len); - return; - case GATT_RELIABLE_WRITE: - reliable_write(data, len); - return; - case GATT_CFG_NOTIFY: - case GATT_CFG_INDICATE: - config_subscription(data, len, opcode); - return; - case GATT_GET_ATTRIBUTES: - get_attrs(data, len); - return; - case GATT_GET_ATTRIBUTE_VALUE: - get_attr_val(data, len); - return; - default: - tester_rsp(BTP_SERVICE_ID_GATT, opcode, index, - BTP_STATUS_UNKNOWN_CMD); - return; - } + case GATT_WRITE: + write(data, len); + return; + case GATT_WRITE_LONG: + write_long(data, len); + return; + case GATT_RELIABLE_WRITE: + reliable_write(data, len); + return; + case GATT_CFG_NOTIFY: + case GATT_CFG_INDICATE: + config_subscription(data, len, opcode); + return; + case GATT_GET_ATTRIBUTES: + get_attrs(data, len); + return; + case GATT_GET_ATTRIBUTE_VALUE: + get_attr_val(data, len); + return; + default: + tester_rsp(BTP_SERVICE_ID_GATT, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } } -int tester_gatt_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, - uint8_t indication, struct os_mbuf *om) +int +tester_gatt_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, + uint8_t indication, struct os_mbuf *om) { - struct gatt_notification_ev *ev; - struct ble_gap_conn_desc conn; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; + struct gatt_notification_ev *ev; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (!subscribe_params.ccc_handle) { - goto fail; - } + if (!subscribe_params.ccc_handle) { + goto fail; + } - if (ble_gap_conn_find(conn_handle, &conn)) { - goto fail; - } + if (ble_gap_conn_find(conn_handle, &conn)) { + goto fail; + } - net_buf_simple_init(buf, 0); - ev = net_buf_simple_add(buf, sizeof(*ev)); + net_buf_simple_init(buf, 0); + ev = net_buf_simple_add(buf, sizeof(*ev)); - addr = &conn.peer_ota_addr; + addr = &conn.peer_ota_addr; - ev->address_type = addr->type; - memcpy(ev->address, addr->val, sizeof(ev->address)); - ev->type = (uint8_t) (indication ? 0x02 : 0x01); - ev->handle = sys_cpu_to_le16(attr_handle); - ev->data_length = sys_cpu_to_le16(os_mbuf_len(om)); - os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om)); + ev->address_type = addr->type; + memcpy(ev->address, addr->val, sizeof(ev->address)); + ev->type = (uint8_t) (indication ? 0x02 : 0x01); + ev->handle = sys_cpu_to_le16(attr_handle); + ev->data_length = sys_cpu_to_le16(os_mbuf_len(om)); + os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om)); - tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_NOTIFICATION, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_NOTIFICATION, + CONTROLLER_INDEX, buf); fail: - os_mbuf_free_chain(buf); - return 0; + os_mbuf_free_chain(buf); + return 0; } -void notify_test_stop(void) +void +notify_test_stop(void) { - os_callout_stop(¬ify_tx_timer); + os_callout_stop(¬ify_tx_timer); } -void notify_test_reset(void) +void +notify_test_reset(void) { - int rc; + int rc; - rc = os_callout_reset(¬ify_tx_timer, OS_TICKS_PER_SEC); - assert(rc == 0); + rc = os_callout_reset(¬ify_tx_timer, OS_TICKS_PER_SEC); + assert(rc == 0); } -void notify_test(struct os_event *ev) +void +notify_test(struct os_event *ev) { - static uint8_t ntf[1]; - struct os_mbuf *om; - int rc; + static uint8_t ntf[1]; + struct os_mbuf *om; + int rc; - if (!notify_state && !indicate_state) { - notify_test_stop(); - notify_value = 90; - return; - } + if (!notify_state && !indicate_state) { + notify_test_stop(); + notify_value = 90; + return; + } - ntf[0] = notify_value; + ntf[0] = notify_value; - notify_value++; - if (notify_value == 160) { - notify_value = 90; - } + notify_value++; + if (notify_value == 160) { + notify_value = 90; + } - om = ble_hs_mbuf_from_flat(ntf, sizeof(ntf)); + om = ble_hs_mbuf_from_flat(ntf, sizeof(ntf)); - if (notify_state) { - rc = ble_gatts_notify_custom(myconn_handle, notify_handle, om); - assert(rc == 0); - } + if (notify_state) { + rc = ble_gatts_notify_custom(myconn_handle, notify_handle, om); + assert(rc == 0); + } - if (indicate_state) { - rc = ble_gatts_indicate_custom(myconn_handle, notify_handle, om); - assert(rc == 0); - } + if (indicate_state) { + rc = ble_gatts_indicate_custom(myconn_handle, notify_handle, om); + assert(rc == 0); + } } -int tester_gatt_subscribe_ev(uint16_t conn_handle, uint16_t attr_handle, uint8_t reason, - uint8_t prev_notify, uint8_t cur_notify, - uint8_t prev_indicate, uint8_t cur_indicate) +int +tester_gatt_subscribe_ev(uint16_t conn_handle, + uint16_t attr_handle, + uint8_t reason, + uint8_t prev_notify, + uint8_t cur_notify, + uint8_t prev_indicate, + uint8_t cur_indicate) { - SYS_LOG_DBG(""); - myconn_handle = conn_handle; + SYS_LOG_DBG(""); + myconn_handle = conn_handle; - if (cur_notify == 0 && cur_indicate == 0) { - SYS_LOG_INF("Unsubscribed"); - memset(&subscribe_params, 0, sizeof(subscribe_params)); - return 0; - } + if (cur_notify == 0 && cur_indicate == 0) { + SYS_LOG_INF("Unsubscribed"); + memset(&subscribe_params, 0, sizeof(subscribe_params)); + return 0; + } - if (cur_notify) { - SYS_LOG_INF("Subscribed to notifications"); - if (attr_handle == notify_handle) { - notify_state = cur_notify; - } - } + if (cur_notify) { + SYS_LOG_INF("Subscribed to notifications"); + if (attr_handle == notify_handle) { + notify_state = cur_notify; + } + } - if (cur_indicate) { - SYS_LOG_INF("Subscribed to indications"); - if (attr_handle == notify_handle) { - indicate_state = cur_indicate; - } - } + if (cur_indicate) { + SYS_LOG_INF("Subscribed to indications"); + if (attr_handle == notify_handle) { + indicate_state = cur_indicate; + } + } + if (notify_state || indicate_state) { + notify_test_reset(); + } else { + notify_test_stop(); + } - if (notify_state || indicate_state) { - notify_test_reset(); - } else { - notify_test_stop(); - } - - return 0; + return 0; } void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) { - char buf[BLE_UUID_STR_LEN]; + char buf[BLE_UUID_STR_LEN]; - switch (ctxt->op) { - case BLE_GATT_REGISTER_OP_SVC: - MODLOG_DFLT(DEBUG, "registered service %s with handle=%d\n", - ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf), - ctxt->svc.handle); - break; + switch (ctxt->op) { + case BLE_GATT_REGISTER_OP_SVC: + MODLOG_DFLT(DEBUG, + "registered service %s with handle=%d\n", + ble_uuid_to_str( + ctxt->svc.svc_def->uuid, + buf), + ctxt->svc.handle); + break; - case BLE_GATT_REGISTER_OP_CHR: - MODLOG_DFLT(DEBUG, "registering characteristic %s with " - "def_handle=%d val_handle=%d\n", - ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf), - ctxt->chr.def_handle, - ctxt->chr.val_handle); - break; + case BLE_GATT_REGISTER_OP_CHR: + MODLOG_DFLT(DEBUG, + "registering characteristic %s with " + "def_handle=%d val_handle=%d\n", + ble_uuid_to_str( + ctxt->chr.chr_def->uuid, + buf), + ctxt->chr.def_handle, + ctxt->chr.val_handle); + break; - case BLE_GATT_REGISTER_OP_DSC: - MODLOG_DFLT(DEBUG, "registering descriptor %s with handle=%d\n", - ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf), - ctxt->dsc.handle); - break; + case BLE_GATT_REGISTER_OP_DSC: + MODLOG_DFLT(DEBUG, + "registering descriptor %s with handle=%d\n", + ble_uuid_to_str( + ctxt->dsc.dsc_def->uuid, + buf), + ctxt->dsc.handle); + break; - default: - assert(0); - break; - } + default: + assert(0); + break; + } } -int gatt_svr_init(void) +int +gatt_svr_init(void) { - int rc; + int rc; - rc = ble_gatts_count_cfg(gatt_svr_inc_svcs); - if (rc != 0) { - return rc; - } + rc = ble_gatts_count_cfg(gatt_svr_inc_svcs); + if (rc != 0) { + return rc; + } - rc = ble_gatts_add_svcs(gatt_svr_inc_svcs); - if (rc != 0) { - return rc; - } + rc = ble_gatts_add_svcs(gatt_svr_inc_svcs); + if (rc != 0) { + return rc; + } - rc = ble_gatts_count_cfg(gatt_svr_svcs); - if (rc != 0) { - return rc; - } + rc = ble_gatts_count_cfg(gatt_svr_svcs); + if (rc != 0) { + return rc; + } - rc = ble_gatts_add_svcs(gatt_svr_svcs); - if (rc != 0) { - return rc; - } + rc = ble_gatts_add_svcs(gatt_svr_svcs); + if (rc != 0) { + return rc; + } - return 0; + return 0; } -uint8_t tester_init_gatt(void) +uint8_t +tester_init_gatt(void) { - os_callout_init(¬ify_tx_timer, os_eventq_dflt_get(), - notify_test, NULL); + os_callout_init(¬ify_tx_timer, os_eventq_dflt_get(), + notify_test, NULL); - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } -uint8_t tester_unregister_gatt(void) +uint8_t +tester_unregister_gatt(void) { - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } diff --git a/apps/bttester/src/gatt_cl.c b/apps/bttester/src/gatt_cl.c index c551bb173..3ca5ecfc0 100644 --- a/apps/bttester/src/gatt_cl.c +++ b/apps/bttester/src/gatt_cl.c @@ -34,25 +34,26 @@ #define MAX_BUFFER_SIZE 2048 /* Convert UUID from BTP command to bt_uuid */ -static uint8_t btp2bt_uuid(const uint8_t *uuid, uint8_t len, - ble_uuid_any_t *bt_uuid) +static uint8_t +btp2bt_uuid(const uint8_t *uuid, uint8_t len, + ble_uuid_any_t *bt_uuid) { - uint16_t le16; + uint16_t le16; - switch (len) { - case 0x02: /* UUID 16 */ - bt_uuid->u.type = BLE_UUID_TYPE_16; - memcpy(&le16, uuid, sizeof(le16)); - BLE_UUID16(bt_uuid)->value = sys_le16_to_cpu(le16); - break; - case 0x10: /* UUID 128*/ - bt_uuid->u.type = BLE_UUID_TYPE_128; - memcpy(BLE_UUID128(bt_uuid)->value, uuid, 16); - break; - default: - return BTP_STATUS_FAILED; - } - return BTP_STATUS_SUCCESS; + switch (len) { + case 0x02: /* UUID 16 */ + bt_uuid->u.type = BLE_UUID_TYPE_16; + memcpy(&le16, uuid, sizeof(le16)); + BLE_UUID16(bt_uuid)->value = sys_le16_to_cpu(le16); + break; + case 0x10: /* UUID 128*/ + bt_uuid->u.type = BLE_UUID_TYPE_128; + memcpy(BLE_UUID128(bt_uuid)->value, uuid, 16); + break; + default: + return BTP_STATUS_FAILED; + } + return BTP_STATUS_SUCCESS; } /* @@ -61,1423 +62,1484 @@ static uint8_t btp2bt_uuid(const uint8_t *uuid, uint8_t len, * It is not intended to be used by client and server at the same time. */ static struct { - uint16_t len; - uint8_t buf[MAX_BUFFER_SIZE]; - uint16_t cnt; + uint16_t len; + uint8_t buf[MAX_BUFFER_SIZE]; + uint16_t cnt; } gatt_buf; static struct bt_gatt_subscribe_params { - uint16_t ccc_handle; - uint16_t value; - uint16_t value_handle; + uint16_t ccc_handle; + uint16_t value; + uint16_t value_handle; } subscribe_params; -static void *gatt_buf_add(const void *data, size_t len) +static void * +gatt_buf_add(const void *data, size_t len) { - void *ptr = gatt_buf.buf + gatt_buf.len; + void *ptr = gatt_buf.buf + gatt_buf.len; - if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) { - return NULL; - } + if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) { + return NULL; + } - if (data) { - memcpy(ptr, data, len); - } else { - (void) memset(ptr, 0, len); - } + if (data) { + memcpy(ptr, data, len); + } else { + (void) memset(ptr, 0, len); + } - gatt_buf.len += len; + gatt_buf.len += len; - SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); + SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); - return ptr; + return ptr; } -static void *gatt_buf_reserve(size_t len) +static void * +gatt_buf_reserve(size_t len) { - return gatt_buf_add(NULL, len); + return gatt_buf_add(NULL, len); } -static void gatt_buf_clear(void) +static void +gatt_buf_clear(void) { - (void) memset(&gatt_buf, 0, sizeof(gatt_buf)); + (void) memset(&gatt_buf, 0, sizeof(gatt_buf)); } -static void discover_destroy(void) +static void +discover_destroy(void) { - gatt_buf_clear(); + gatt_buf_clear(); } -static void read_destroy() +static void +read_destroy() { - gatt_buf_clear(); + gatt_buf_clear(); } -static int tester_mtu_exchanged_ev(uint16_t conn_handle, - const struct ble_gatt_error *error, - uint16_t mtu, void *arg) +static int +tester_mtu_exchanged_ev(uint16_t conn_handle, + const struct ble_gatt_error *error, + uint16_t mtu, void *arg) { - struct gattc_exchange_mtu_ev *ev; - struct ble_gap_conn_desc conn; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; + struct gattc_exchange_mtu_ev *ev; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (ble_gap_conn_find(conn_handle, &conn)) { - goto fail; - } + if (ble_gap_conn_find(conn_handle, &conn)) { + goto fail; + } - net_buf_simple_init(buf, 0); - ev = net_buf_simple_add(buf, sizeof(*ev)); + net_buf_simple_init(buf, 0); + ev = net_buf_simple_add(buf, sizeof(*ev)); - addr = &conn.peer_ota_addr; + addr = &conn.peer_ota_addr; - ev->address_type = addr->type; - memcpy(ev->address, addr->val, sizeof(ev->address)); + ev->address_type = addr->type; + memcpy(ev->address, addr->val, sizeof(ev->address)); - ev->mtu = mtu; + ev->mtu = mtu; - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_EV_MTU_EXCHANGED, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_EV_MTU_EXCHANGED, + CONTROLLER_INDEX, buf); fail: - os_mbuf_free_chain(buf); - return 0; + os_mbuf_free_chain(buf); + return 0; } -static void exchange_mtu(uint8_t *data, uint16_t len) +static void +exchange_mtu(uint8_t *data, uint16_t len) { - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - if (ble_gattc_exchange_mtu(conn.conn_handle, tester_mtu_exchanged_ev, NULL)) { - status = BTP_STATUS_FAILED; - goto rsp; - } + if (ble_gattc_exchange_mtu(conn.conn_handle, + tester_mtu_exchanged_ev, + NULL)) { + status = BTP_STATUS_FAILED; + goto rsp; + } rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_EXCHANGE_MTU, - CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_EXCHANGE_MTU, + CONTROLLER_INDEX, status); } -static int disc_prim_svcs_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - const struct ble_gatt_svc *gatt_svc, void *arg) +static int +disc_prim_svcs_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_svc *gatt_svc, void *arg) { - struct gattc_disc_prim_svcs_rp *rp; - struct ble_gap_conn_desc conn; - struct gatt_service *service; - const ble_uuid_any_t *uuid; - const ble_addr_t *addr; - uint8_t uuid_length; - struct os_mbuf *buf = os_msys_get(0, 0); - uint8_t opcode = (uint8_t) (int) arg; - uint8_t err = (uint8_t) error->status; - int rc = 0; - - SYS_LOG_DBG(""); - - if (ble_gap_conn_find(conn_handle, &conn)) { - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - rp->status = err; - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->services_count = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - if (error->status == BLE_HS_EDONE) { - rp->status = 0; - rp->services_count = gatt_buf.cnt; - os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - uuid = &gatt_svc->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - - service = gatt_buf_reserve(sizeof(*service) + uuid_length); - if (!service) { - discover_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } - - service->start_handle = sys_cpu_to_le16(gatt_svc->start_handle); - service->end_handle = sys_cpu_to_le16(gatt_svc->end_handle); - service->uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(service->uuid, &u16, uuid_length); - } else { - memcpy(service->uuid, BLE_UUID128(uuid)->value, - uuid_length); - } - - gatt_buf.cnt++; - -free: - os_mbuf_free_chain(buf); - return rc; -} - -static void disc_all_prim_svcs(uint8_t *data, uint16_t len) -{ - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_svcs_cb, - (void *) GATTC_DISC_ALL_PRIM_RP)) { - discover_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_PRIM_SVCS, - CONTROLLER_INDEX, status); -} - -static void disc_prim_uuid(uint8_t *data, uint16_t len) -{ - const struct gattc_disc_prim_uuid_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - ble_uuid_any_t uuid; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - if (ble_gattc_disc_svc_by_uuid(conn.conn_handle, - &uuid.u, disc_prim_svcs_cb, - (void *) GATTC_DISC_PRIM_UUID_RP)) { - discover_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_PRIM_UUID, CONTROLLER_INDEX, - status); -} - -static int find_included_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - const struct ble_gatt_svc *gatt_svc, void *arg) -{ - struct gattc_find_included_rp *rp; - struct gatt_included *included; - const ble_uuid_any_t *uuid; - int service_handle = (int) arg; - uint8_t uuid_length; - uint8_t err = (uint8_t) error->status; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; - - SYS_LOG_DBG(""); - - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - rp->status = err; - - SYS_LOG_DBG(""); - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->services_count = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_FIND_INCLUDED_RP, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - if (error->status == BLE_HS_EDONE) { - rp->status = 0; - rp->services_count = gatt_buf.cnt; - os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_FIND_INCLUDED_RP, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - uuid = &gatt_svc->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - - included = gatt_buf_reserve(sizeof(*included) + uuid_length); - if (!included) { - discover_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } - - included->included_handle = sys_cpu_to_le16(service_handle + 1 + - rp->services_count); - included->service.start_handle = sys_cpu_to_le16(gatt_svc->start_handle); - included->service.end_handle = sys_cpu_to_le16(gatt_svc->end_handle); - included->service.uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(included->service.uuid, &u16, uuid_length); - } else { - memcpy(included->service.uuid, BLE_UUID128(uuid)->value, - uuid_length); - } - - gatt_buf.cnt++; - -free: - os_mbuf_free_chain(buf); - return rc; -} - -static void find_included(uint8_t *data, uint16_t len) -{ - const struct gattc_find_included_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - int service_handle_arg; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); - service_handle_arg = start_handle; - - if (ble_gattc_find_inc_svcs(conn.conn_handle, start_handle, end_handle, - find_included_cb, - (void *) service_handle_arg)) { - discover_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_FIND_INCLUDED, CONTROLLER_INDEX, - status); -} - -static int disc_chrc_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - const struct ble_gatt_chr *gatt_chr, void *arg) -{ - struct gattc_disc_chrc_rp *rp; - struct gatt_characteristic *chrc; - const ble_uuid_any_t *uuid; - uint8_t uuid_length; - uint8_t opcode = (uint8_t) (int) arg; - uint8_t err = (uint8_t) error->status; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; - - SYS_LOG_DBG(""); - if (ble_gap_conn_find(conn_handle, &conn)) { - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - rp->status = err; - - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } - - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->characteristics_count = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - if (error->status == BLE_HS_EDONE) { - rp->status = 0; - rp->characteristics_count = gatt_buf.cnt; - os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - uuid = &gatt_chr->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - - chrc = gatt_buf_reserve(sizeof(*chrc) + uuid_length); - if (!chrc) { - discover_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } - - chrc->characteristic_handle = sys_cpu_to_le16(gatt_chr->def_handle); - chrc->properties = gatt_chr->properties; - chrc->value_handle = sys_cpu_to_le16(gatt_chr->val_handle); - chrc->uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(chrc->uuid, &u16, uuid_length); - } else { - memcpy(chrc->uuid, BLE_UUID128(uuid)->value, - uuid_length); - } - - gatt_buf.cnt++; -free: - os_mbuf_free_chain(buf); - return rc; -} - -static void disc_all_chrc(uint8_t *data, uint16_t len) -{ - const struct gattc_disc_all_chrc_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - SYS_LOG_DBG("Conn find rsped"); - status = BTP_STATUS_FAILED; - goto rsp; - } - - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); - - rc = ble_gattc_disc_all_chrs(conn.conn_handle, start_handle, end_handle, - disc_chrc_cb, (void *) GATTC_DISC_ALL_CHRC_RP); - if (rc) { - discover_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_CHRC, CONTROLLER_INDEX, - status); -} - -static void disc_chrc_uuid(uint8_t *data, uint16_t len) -{ - const struct gattc_disc_chrc_uuid_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - ble_uuid_any_t uuid; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - start_handle = sys_le16_to_cpu(cmd->start_handle); - end_handle = sys_le16_to_cpu(cmd->end_handle); - - rc = ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle, - end_handle, &uuid.u, disc_chrc_cb, - (void *) GATTC_DISC_CHRC_UUID_RP); - if (rc) { - discover_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_CHRC_UUID, CONTROLLER_INDEX, - status); -} - -static int disc_all_desc_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - uint16_t chr_val_handle, - const struct ble_gatt_dsc *gatt_dsc, - void *arg) -{ - struct gattc_disc_all_desc_rp *rp; - struct gatt_descriptor *dsc; - const ble_uuid_any_t *uuid; - uint8_t uuid_length; - uint8_t err = (uint8_t) error->status; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; - - SYS_LOG_DBG(""); - - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - rp->status = err; - - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->descriptors_count = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_DESC_RP, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - if (error->status == BLE_HS_EDONE) { - rp->status = 0; - rp->descriptors_count = gatt_buf.cnt; - os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_DESC_RP, - CONTROLLER_INDEX, buf); - discover_destroy(); - goto free; - } - - uuid = &gatt_dsc->uuid; - uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - - dsc = gatt_buf_reserve(sizeof(*dsc) + uuid_length); - if (!dsc) { - discover_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } - - dsc->descriptor_handle = sys_cpu_to_le16(gatt_dsc->handle); - dsc->uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); - memcpy(dsc->uuid, &u16, uuid_length); - } else { - memcpy(dsc->uuid, BLE_UUID128(uuid)->value, uuid_length); - } - - gatt_buf.cnt++; - -free: - os_mbuf_free_chain(buf); - return rc; -} - -static void disc_all_desc(uint8_t *data, uint16_t len) -{ - const struct gattc_disc_all_desc_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - start_handle = sys_le16_to_cpu(cmd->start_handle) - 1; - end_handle = sys_le16_to_cpu(cmd->end_handle); - - rc = ble_gattc_disc_all_dscs(conn.conn_handle, start_handle, end_handle, - disc_all_desc_cb, (void *) GATTC_DISC_ALL_DESC); - - SYS_LOG_DBG("rc=%d", rc); - - if (rc) { - discover_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_DESC, CONTROLLER_INDEX, - status); -} - -static int read_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) -{ - struct gattc_read_rp *rp; - uint8_t opcode = (uint8_t) (int) arg; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; - - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - SYS_LOG_DBG("status=%d", error->status); - - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->status = (uint8_t) BLE_HS_ATT_ERR(error->status); - rp->data_length = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - read_destroy(); - goto free; - } - - if (!gatt_buf_add(attr->om->om_data, attr->om->om_len)) { - read_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } - - rp->status = 0; - rp->data_length = attr->om->om_len; - os_mbuf_appendfrom(buf, attr->om, 0, os_mbuf_len(attr->om)); - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - read_destroy(); -free: - os_mbuf_free_chain(buf); - return rc; -} - -static void read(uint8_t *data, uint16_t len) -{ - const struct gattc_read_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - /* Clear buffer */ - read_destroy(); - - if (ble_gattc_read(conn.conn_handle, sys_le16_to_cpu(cmd->handle), - read_cb, (void *) GATTC_READ_RP)) { - read_destroy(); - status = BTP_STATUS_FAILED; - goto rsp; - } - -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ, CONTROLLER_INDEX, - status); -} - -static int read_uuid_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) -{ - struct gattc_read_uuid_rp *rp; - struct gatt_read_uuid_chr *chr; - uint8_t opcode = (uint8_t) (int) arg; - uint8_t err = (uint8_t) error->status; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; - static uint16_t attr_len; - - SYS_LOG_DBG("status=%d", error->status); - - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - rp->status = err; - - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->data_length = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - read_destroy(); - goto free; - } - - if (error->status == BLE_HS_EDONE) { - rp->data_length = gatt_buf.len; - rp->value_length = attr_len; - rp->status = 0; - os_mbuf_append(buf, gatt_buf.buf+1, gatt_buf.len-1); - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + struct gattc_disc_prim_svcs_rp *rp; + struct ble_gap_conn_desc conn; + struct gatt_service *service; + const ble_uuid_any_t *uuid; + const ble_addr_t *addr; + uint8_t uuid_length; + struct os_mbuf *buf = os_msys_get(0, 0); + uint8_t opcode = (uint8_t) (int) arg; + uint8_t err = (uint8_t) error->status; + int rc = 0; + + SYS_LOG_DBG(""); + + if (ble_gap_conn_find(conn_handle, &conn)) { + goto free; + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + rp->status = err; + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->services_count = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, CONTROLLER_INDEX, buf); - read_destroy(); - goto free; - } + discover_destroy(); + goto free; + } - if (error->status == 0) { - attr_len = attr->om->om_len; - } + if (error->status == BLE_HS_EDONE) { + rp->status = 0; + rp->services_count = gatt_buf.cnt; + os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } - if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { - read_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } + uuid = &gatt_svc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - chr = gatt_buf_reserve(sizeof(*chr) + attr->om->om_len); - if (!chr) { - read_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } + service = gatt_buf_reserve(sizeof(*service) + uuid_length); + if (!service) { + discover_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } - chr->handle = sys_cpu_to_be16(attr->handle); - memcpy(chr->data, attr->om->om_data, attr->om->om_len); + service->start_handle = sys_cpu_to_le16(gatt_svc->start_handle); + service->end_handle = sys_cpu_to_le16(gatt_svc->end_handle); + service->uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(service->uuid, &u16, uuid_length); + } else { + memcpy(service->uuid, BLE_UUID128(uuid)->value, + uuid_length); + } + + gatt_buf.cnt++; free: - os_mbuf_free_chain(buf); - return rc; + os_mbuf_free_chain(buf); + return rc; } -static void read_uuid(uint8_t *data, uint16_t len) +static void +disc_all_prim_svcs(uint8_t *data, uint16_t len) { - const struct gattc_read_uuid_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - ble_uuid_any_t uuid; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { - status = BTP_STATUS_FAILED; - goto rsp; - } - - /* Clear buffer */ - read_destroy(); - - if (ble_gattc_read_by_uuid(conn.conn_handle, - sys_le16_to_cpu(cmd->start_handle), - sys_le16_to_cpu(cmd->end_handle), &uuid.u, - read_uuid_cb, (void *) GATTC_READ_UUID_RP)) { - read_destroy(); - status = BTP_STATUS_FAILED; - } + if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_svcs_cb, + (void *) GATTC_DISC_ALL_PRIM_RP)) { + discover_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ_UUID, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_PRIM_SVCS, + CONTROLLER_INDEX, status); } -static int read_long_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) +static void +disc_prim_uuid(uint8_t *data, uint16_t len) { - struct gattc_read_rp *rp;; - uint8_t opcode = (uint8_t) (int) arg; - uint8_t err = (uint8_t) error->status; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; + const struct gattc_disc_prim_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + ble_uuid_any_t uuid; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG("status=%d", error->status); + SYS_LOG_DBG(""); - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + status = BTP_STATUS_FAILED; + goto rsp; + } - addr = &conn.peer_ota_addr; + if (ble_gattc_disc_svc_by_uuid(conn.conn_handle, + &uuid.u, disc_prim_svcs_cb, + (void *) GATTC_DISC_PRIM_UUID_RP)) { + discover_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_PRIM_UUID, CONTROLLER_INDEX, + status); +} - rp->status = err; +static int +find_included_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_svc *gatt_svc, void *arg) +{ + struct gattc_find_included_rp *rp; + struct gatt_included *included; + const ble_uuid_any_t *uuid; + int service_handle = (int) arg; + uint8_t uuid_length; + uint8_t err = (uint8_t) error->status; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; - if (error->status != 0 && error->status != BLE_HS_EDONE) { - rp->data_length = 0; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - read_destroy(); - goto free; - } + SYS_LOG_DBG(""); - if (error->status == BLE_HS_EDONE) { - rp->status = 0; - rp->data_length = gatt_buf.len; - os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); - read_destroy(); - goto free; - } + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } - if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { - read_destroy(); - rc = BLE_HS_ENOMEM; - goto free; - } + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - rp->data_length += attr->om->om_len; + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + rp->status = err; + + SYS_LOG_DBG(""); + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->services_count = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_FIND_INCLUDED_RP, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } + + if (error->status == BLE_HS_EDONE) { + rp->status = 0; + rp->services_count = gatt_buf.cnt; + os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_FIND_INCLUDED_RP, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } + + uuid = &gatt_svc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + + included = gatt_buf_reserve(sizeof(*included) + uuid_length); + if (!included) { + discover_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } + + included->included_handle = sys_cpu_to_le16(service_handle + 1 + + rp->services_count); + included->service.start_handle = sys_cpu_to_le16(gatt_svc->start_handle); + included->service.end_handle = sys_cpu_to_le16(gatt_svc->end_handle); + included->service.uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(included->service.uuid, &u16, uuid_length); + } else { + memcpy(included->service.uuid, BLE_UUID128(uuid)->value, + uuid_length); + } + + gatt_buf.cnt++; free: - os_mbuf_free_chain(buf); - return rc; + os_mbuf_free_chain(buf); + return rc; } -static void read_long(uint8_t *data, uint16_t len) +static void +find_included(uint8_t *data, uint16_t len) { - const struct gattc_read_long_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + const struct gattc_find_included_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int service_handle_arg; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - /* Clear buffer */ - read_destroy(); + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + service_handle_arg = start_handle; - if (ble_gattc_read_long(conn.conn_handle, - sys_le16_to_cpu(cmd->handle), - sys_le16_to_cpu(cmd->offset), - read_long_cb, (void *) GATTC_READ_LONG_RP)) { - read_destroy(); - status = BTP_STATUS_FAILED; - } + if (ble_gattc_find_inc_svcs(conn.conn_handle, start_handle, end_handle, + find_included_cb, + (void *) service_handle_arg)) { + discover_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ_LONG, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_FIND_INCLUDED, CONTROLLER_INDEX, + status); } -static void read_multiple(uint8_t *data, uint16_t len) +static int +disc_chrc_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_chr *gatt_chr, void *arg) { - const struct gattc_read_multiple_cmd *cmd = (void *) data; - uint16_t handles[cmd->handles_count]; - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc, i; + struct gattc_disc_chrc_rp *rp; + struct gatt_characteristic *chrc; + const ble_uuid_any_t *uuid; + uint8_t uuid_length; + uint8_t opcode = (uint8_t) (int) arg; + uint8_t err = (uint8_t) error->status; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); + if (ble_gap_conn_find(conn_handle, &conn)) { + goto free; + } - for (i = 0; i < ARRAY_SIZE(handles); i++) { - handles[i] = sys_le16_to_cpu(cmd->handles[i]); - } + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + addr = &conn.peer_ota_addr; - /* Clear buffer */ - read_destroy(); + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); - if (ble_gattc_read_mult(conn.conn_handle, handles, - cmd->handles_count, read_cb, (void *) GATTC_READ_MULTIPLE_RP)) { - read_destroy(); - status = BTP_STATUS_FAILED; - } + rp->status = err; -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ_MULTIPLE, CONTROLLER_INDEX, - status); -} + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } -static void write_without_rsp(uint8_t *data, uint16_t len, uint8_t op, bool sign) -{ - const struct gattc_write_without_rsp_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->characteristics_count = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } - SYS_LOG_DBG(""); + if (error->status == BLE_HS_EDONE) { + rp->status = 0; + rp->characteristics_count = gatt_buf.cnt; + os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + uuid = &gatt_chr->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); - if (ble_gattc_write_no_rsp_flat(conn.conn_handle, - sys_le16_to_cpu(cmd->handle), cmd->data, - sys_le16_to_cpu(cmd->data_length))) { - status = BTP_STATUS_FAILED; - } + chrc = gatt_buf_reserve(sizeof(*chrc) + uuid_length); + if (!chrc) { + discover_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } -rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, op, CONTROLLER_INDEX, status); -} + chrc->characteristic_handle = sys_cpu_to_le16(gatt_chr->def_handle); + chrc->properties = gatt_chr->properties; + chrc->value_handle = sys_cpu_to_le16(gatt_chr->val_handle); + chrc->uuid_length = uuid_length; -static int write_cb(uint16_t conn_handle, const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) -{ - struct gattc_write_rp *rp; - uint8_t err = (uint8_t) error->status; - uint8_t opcode = (uint8_t) (int) arg; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(chrc->uuid, &u16, uuid_length); + } else { + memcpy(chrc->uuid, BLE_UUID128(uuid)->value, + uuid_length); + } - SYS_LOG_DBG(""); - - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } - - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); - - addr = &conn.peer_ota_addr; - - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); - - rp->status = err; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); + gatt_buf.cnt++; free: - os_mbuf_free_chain(buf); - return rc; + os_mbuf_free_chain(buf); + return rc; } -static void write(uint8_t *data, uint16_t len) +static void +disc_all_chrc(uint8_t *data, uint16_t len) { - const struct gattc_write_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + const struct gattc_disc_all_chrc_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + SYS_LOG_DBG("Conn find rsped"); + status = BTP_STATUS_FAILED; + goto rsp; + } - if (ble_gattc_write_flat(conn.conn_handle, sys_le16_to_cpu(cmd->handle), - cmd->data, sys_le16_to_cpu(cmd->data_length), - write_cb, (void *) GATTC_WRITE_RP)) { - status = BTP_STATUS_FAILED; - } + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + + rc = ble_gattc_disc_all_chrs(conn.conn_handle, + start_handle, + end_handle, + disc_chrc_cb, + (void *) GATTC_DISC_ALL_CHRC_RP); + if (rc) { + discover_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_WRITE, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_CHRC, CONTROLLER_INDEX, + status); } -static void write_long(uint8_t *data, uint16_t len) +static void +disc_chrc_uuid(uint8_t *data, uint16_t len) { - const struct gattc_write_long_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - struct os_mbuf *om = NULL; - uint8_t status = BTP_STATUS_SUCCESS; - int rc = 0; + const struct gattc_disc_chrc_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + ble_uuid_any_t uuid; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); - if (!om) { - SYS_LOG_ERR("Insufficient resources"); - status = BTP_STATUS_FAILED; - goto fail; - } + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + status = BTP_STATUS_FAILED; + goto rsp; + } - rc = ble_gattc_write_long(conn.conn_handle, - sys_le16_to_cpu(cmd->handle), - sys_le16_to_cpu(cmd->offset), - om, write_cb, - (void *) GATTC_WRITE_LONG_RP); - if (rc) { - status = BTP_STATUS_FAILED; - } else { - goto rsp; - } + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + + rc = ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle, + end_handle, &uuid.u, disc_chrc_cb, + (void *) GATTC_DISC_CHRC_UUID_RP); + if (rc) { + discover_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_CHRC_UUID, CONTROLLER_INDEX, + status); +} + +static int +disc_all_desc_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + uint16_t chr_val_handle, + const struct ble_gatt_dsc *gatt_dsc, + void *arg) +{ + struct gattc_disc_all_desc_rp *rp; + struct gatt_descriptor *dsc; + const ble_uuid_any_t *uuid; + uint8_t uuid_length; + uint8_t err = (uint8_t) error->status; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; + + SYS_LOG_DBG(""); + + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + rp->status = err; + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->descriptors_count = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_DESC_RP, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } + + if (error->status == BLE_HS_EDONE) { + rp->status = 0; + rp->descriptors_count = gatt_buf.cnt; + os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_DESC_RP, + CONTROLLER_INDEX, buf); + discover_destroy(); + goto free; + } + + uuid = &gatt_dsc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + + dsc = gatt_buf_reserve(sizeof(*dsc) + uuid_length); + if (!dsc) { + discover_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } + + dsc->descriptor_handle = sys_cpu_to_le16(gatt_dsc->handle); + dsc->uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(dsc->uuid, &u16, uuid_length); + } else { + memcpy(dsc->uuid, BLE_UUID128(uuid)->value, uuid_length); + } + + gatt_buf.cnt++; + +free: + os_mbuf_free_chain(buf); + return rc; +} + +static void +disc_all_desc(uint8_t *data, uint16_t len) +{ + const struct gattc_disc_all_desc_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + start_handle = sys_le16_to_cpu(cmd->start_handle) - 1; + end_handle = sys_le16_to_cpu(cmd->end_handle); + + rc = ble_gattc_disc_all_dscs(conn.conn_handle, + start_handle, + end_handle, + disc_all_desc_cb, + (void *) GATTC_DISC_ALL_DESC); + + SYS_LOG_DBG("rc=%d", rc); + + if (rc) { + discover_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_DISC_ALL_DESC, CONTROLLER_INDEX, + status); +} + +static int +read_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + struct gattc_read_rp *rp; + uint8_t opcode = (uint8_t) (int) arg; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; + + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + SYS_LOG_DBG("status=%d", error->status); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->status = (uint8_t) BLE_HS_ATT_ERR(error->status); + rp->data_length = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + read_destroy(); + goto free; + } + + if (!gatt_buf_add(attr->om->om_data, attr->om->om_len)) { + read_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } + + rp->status = 0; + rp->data_length = attr->om->om_len; + os_mbuf_appendfrom(buf, attr->om, 0, os_mbuf_len(attr->om)); + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + read_destroy(); +free: + os_mbuf_free_chain(buf); + return rc; +} + +static void +read(uint8_t *data, uint16_t len) +{ + const struct gattc_read_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + /* Clear buffer */ + read_destroy(); + + if (ble_gattc_read(conn.conn_handle, sys_le16_to_cpu(cmd->handle), + read_cb, (void *) GATTC_READ_RP)) { + read_destroy(); + status = BTP_STATUS_FAILED; + goto rsp; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ, CONTROLLER_INDEX, + status); +} + +static int +read_uuid_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + struct gattc_read_uuid_rp *rp; + struct gatt_read_uuid_chr *chr; + uint8_t opcode = (uint8_t) (int) arg; + uint8_t err = (uint8_t) error->status; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; + static uint16_t attr_len; + + SYS_LOG_DBG("status=%d", error->status); + + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + rp->status = err; + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->data_length = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + read_destroy(); + goto free; + } + + if (error->status == BLE_HS_EDONE) { + rp->data_length = gatt_buf.len; + rp->value_length = attr_len; + rp->status = 0; + os_mbuf_append(buf, gatt_buf.buf + 1, gatt_buf.len - 1); + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + read_destroy(); + goto free; + } + + if (error->status == 0) { + attr_len = attr->om->om_len; + } + + if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { + read_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } + + chr = gatt_buf_reserve(sizeof(*chr) + attr->om->om_len); + if (!chr) { + read_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } + + chr->handle = sys_cpu_to_be16(attr->handle); + memcpy(chr->data, attr->om->om_data, attr->om->om_len); + +free: + os_mbuf_free_chain(buf); + return rc; +} + +static void +read_uuid(uint8_t *data, uint16_t len) +{ + const struct gattc_read_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + ble_uuid_any_t uuid; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + /* Clear buffer */ + read_destroy(); + + if (ble_gattc_read_by_uuid(conn.conn_handle, + sys_le16_to_cpu(cmd->start_handle), + sys_le16_to_cpu(cmd->end_handle), &uuid.u, + read_uuid_cb, (void *) GATTC_READ_UUID_RP)) { + read_destroy(); + status = BTP_STATUS_FAILED; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ_UUID, CONTROLLER_INDEX, + status); +} + +static int +read_long_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + struct gattc_read_rp *rp;; + uint8_t opcode = (uint8_t) (int) arg; + uint8_t err = (uint8_t) error->status; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; + + SYS_LOG_DBG("status=%d", error->status); + + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + rp->status = err; + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->data_length = 0; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + read_destroy(); + goto free; + } + + if (error->status == BLE_HS_EDONE) { + rp->status = 0; + rp->data_length = gatt_buf.len; + os_mbuf_append(buf, gatt_buf.buf, gatt_buf.len); + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); + read_destroy(); + goto free; + } + + if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { + read_destroy(); + rc = BLE_HS_ENOMEM; + goto free; + } + + rp->data_length += attr->om->om_len; + +free: + os_mbuf_free_chain(buf); + return rc; +} + +static void +read_long(uint8_t *data, uint16_t len) +{ + const struct gattc_read_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + /* Clear buffer */ + read_destroy(); + + if (ble_gattc_read_long(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), + sys_le16_to_cpu(cmd->offset), + read_long_cb, (void *) GATTC_READ_LONG_RP)) { + read_destroy(); + status = BTP_STATUS_FAILED; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ_LONG, CONTROLLER_INDEX, + status); +} + +static void +read_multiple(uint8_t *data, uint16_t len) +{ + const struct gattc_read_multiple_cmd *cmd = (void *) data; + uint16_t handles[cmd->handles_count]; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc, i; + + SYS_LOG_DBG(""); + + for (i = 0; i < ARRAY_SIZE(handles); i++) { + handles[i] = sys_le16_to_cpu(cmd->handles[i]); + } + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + /* Clear buffer */ + read_destroy(); + + if (ble_gattc_read_mult(conn.conn_handle, + handles, + cmd->handles_count, + read_cb, + (void *) GATTC_READ_MULTIPLE_RP)) { + read_destroy(); + status = BTP_STATUS_FAILED; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_READ_MULTIPLE, CONTROLLER_INDEX, + status); +} + +static void +write_without_rsp(uint8_t *data, uint16_t len, uint8_t op, bool sign) +{ + const struct gattc_write_without_rsp_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + if (ble_gattc_write_no_rsp_flat(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), cmd->data, + sys_le16_to_cpu(cmd->data_length))) { + status = BTP_STATUS_FAILED; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, op, CONTROLLER_INDEX, status); +} + +static int +write_cb(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + struct gattc_write_rp *rp; + uint8_t err = (uint8_t) error->status; + uint8_t opcode = (uint8_t) (int) arg; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; + + SYS_LOG_DBG(""); + + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + addr = &conn.peer_ota_addr; + + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); + + rp->status = err; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); +free: + os_mbuf_free_chain(buf); + return rc; +} + +static void +write(uint8_t *data, uint16_t len) +{ + const struct gattc_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + if (ble_gattc_write_flat(conn.conn_handle, sys_le16_to_cpu(cmd->handle), + cmd->data, sys_le16_to_cpu(cmd->data_length), + write_cb, (void *) GATTC_WRITE_RP)) { + status = BTP_STATUS_FAILED; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_WRITE, CONTROLLER_INDEX, + status); +} + +static void +write_long(uint8_t *data, uint16_t len) +{ + const struct gattc_write_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + struct os_mbuf *om = NULL; + uint8_t status = BTP_STATUS_SUCCESS; + int rc = 0; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + goto fail; + } + + om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); + if (!om) { + SYS_LOG_ERR("Insufficient resources"); + status = BTP_STATUS_FAILED; + goto fail; + } + + rc = ble_gattc_write_long(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), + sys_le16_to_cpu(cmd->offset), + om, write_cb, + (void *) GATTC_WRITE_LONG_RP); + if (rc) { + status = BTP_STATUS_FAILED; + } else { + goto rsp; + } fail: - SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc); - os_mbuf_free_chain(om); + SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc); + os_mbuf_free_chain(om); rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_WRITE_LONG, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_WRITE_LONG, CONTROLLER_INDEX, + status); } -static int reliable_write_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attrs, - uint8_t num_attrs, - void *arg) +static int +reliable_write_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attrs, + uint8_t num_attrs, + void *arg) { - struct gattc_write_rp *rp; - uint8_t err = (uint8_t) error->status; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; + struct gattc_write_rp *rp; + uint8_t err = (uint8_t) error->status; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - addr = &conn.peer_ota_addr; + addr = &conn.peer_ota_addr; - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); - rp->status = err; - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_RELIABLE_WRITE_RP, - CONTROLLER_INDEX, buf); + rp->status = err; + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_RELIABLE_WRITE_RP, + CONTROLLER_INDEX, buf); free: - os_mbuf_free_chain(buf); - return rc; + os_mbuf_free_chain(buf); + return rc; } -static void reliable_write(uint8_t *data, uint16_t len) +static void +reliable_write(uint8_t *data, uint16_t len) { - const struct gattc_reliable_write_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - struct ble_gatt_attr attr; - struct os_mbuf *om = NULL; - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + const struct gattc_reliable_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + struct ble_gatt_attr attr; + struct os_mbuf *om = NULL; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto fail; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto fail; + } - om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); - /* This is required, because Nimble checks if - * the data is longer than offset - */ - if (os_mbuf_extend(om, sys_le16_to_cpu(cmd->offset) + 1) == NULL) { - status = BTP_STATUS_FAILED; - goto fail; - } + om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); + /* This is required, because Nimble checks if + * the data is longer than offset + */ + if (os_mbuf_extend(om, sys_le16_to_cpu(cmd->offset) + 1) == NULL) { + status = BTP_STATUS_FAILED; + goto fail; + } - attr.handle = sys_le16_to_cpu(cmd->handle); - attr.offset = sys_le16_to_cpu(cmd->offset); - attr.om = om; + attr.handle = sys_le16_to_cpu(cmd->handle); + attr.offset = sys_le16_to_cpu(cmd->offset); + attr.om = om; - if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, - reliable_write_cb, NULL)) { - status = BTP_STATUS_FAILED; - goto fail; - } else { - goto rsp; - } + if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, + reliable_write_cb, NULL)) { + status = BTP_STATUS_FAILED; + goto fail; + } else { + goto rsp; + } fail: - os_mbuf_free_chain(om); + os_mbuf_free_chain(om); rsp: - tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_WRITE_LONG, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_GATTC, GATTC_WRITE_LONG, CONTROLLER_INDEX, + status); } -static int subscribe_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attrs, - void *arg) +static int +subscribe_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attrs, + void *arg) { - struct subscribe_rp *rp; - uint8_t err = (uint8_t) error->status; - uint8_t opcode = (uint8_t) (int) arg; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; - struct ble_gap_conn_desc conn; - int rc = 0; + struct subscribe_rp *rp; + uint8_t err = (uint8_t) error->status; + uint8_t opcode = (uint8_t) (int) arg; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + struct ble_gap_conn_desc conn; + int rc = 0; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (ble_gap_conn_find(conn_handle, &conn)) { - rc = BLE_HS_EINVAL; - goto free; - } + if (ble_gap_conn_find(conn_handle, &conn)) { + rc = BLE_HS_EINVAL; + goto free; + } - net_buf_simple_init(buf, 0); - rp = net_buf_simple_add(buf, sizeof(*rp)); + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); - addr = &conn.peer_ota_addr; + addr = &conn.peer_ota_addr; - rp->address_type = addr->type; - memcpy(rp->address, addr->val, sizeof(rp->address)); + rp->address_type = addr->type; + memcpy(rp->address, addr->val, sizeof(rp->address)); - rp->status = err; - tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, - CONTROLLER_INDEX, buf); + rp->status = err; + tester_send_buf(BTP_SERVICE_ID_GATTC, opcode, + CONTROLLER_INDEX, buf); free: - os_mbuf_free_chain(buf); - return rc; + os_mbuf_free_chain(buf); + return rc; } -static int enable_subscription(uint16_t conn_handle, uint16_t ccc_handle, - uint16_t value) +static int +enable_subscription(uint16_t conn_handle, uint16_t ccc_handle, + uint16_t value) { - uint32_t opcode; + uint32_t opcode; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - opcode = (uint32_t) (value == 0x0001 ? GATTC_CFG_NOTIFY_RP : GATTC_CFG_INDICATE_RP); + opcode = (uint32_t) (value == 0x0001 ? GATTC_CFG_NOTIFY_RP + : GATTC_CFG_INDICATE_RP); - if (ble_gattc_write_flat(conn_handle, ccc_handle, - &value, sizeof(value), subscribe_cb, (void *) opcode)) { - return -EINVAL; - } + if (ble_gattc_write_flat(conn_handle, + ccc_handle, + &value, + sizeof(value), + subscribe_cb, + (void *) opcode)) { + return -EINVAL; + } - subscribe_params.ccc_handle = value; + subscribe_params.ccc_handle = value; - return 0; + return 0; } -static int disable_subscription(uint16_t conn_handle, uint16_t ccc_handle) +static int +disable_subscription(uint16_t conn_handle, uint16_t ccc_handle) { - uint16_t value = 0x00; - uint32_t opcode; + uint16_t value = 0x00; + uint32_t opcode; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - opcode = (uint32_t) (value == 0x0001 ? GATTC_CFG_NOTIFY_RP : GATTC_CFG_INDICATE_RP); + opcode = (uint32_t) (value == 0x0001 ? GATTC_CFG_NOTIFY_RP + : GATTC_CFG_INDICATE_RP); - /* Fail if CCC handle doesn't match */ - if (ccc_handle != subscribe_params.ccc_handle) { - SYS_LOG_ERR("CCC handle doesn't match"); - return -EINVAL; - } + /* Fail if CCC handle doesn't match */ + if (ccc_handle != subscribe_params.ccc_handle) { + SYS_LOG_ERR("CCC handle doesn't match"); + return -EINVAL; + } - if (ble_gattc_write_flat(conn_handle, ccc_handle, - &value, sizeof(value), subscribe_cb, (void *) opcode)) { - return -EINVAL; - } + if (ble_gattc_write_flat(conn_handle, + ccc_handle, + &value, + sizeof(value), + subscribe_cb, + (void *) opcode)) { + return -EINVAL; + } - subscribe_params.ccc_handle = 0; - return 0; + subscribe_params.ccc_handle = 0; + return 0; } -static void config_subscription(uint8_t *data, uint16_t len, uint8_t op) +static void +config_subscription(uint8_t *data, uint16_t len, uint8_t op) { - const struct gattc_cfg_notify_cmd *cmd = (void *) data; - struct ble_gap_conn_desc conn; - uint16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); - uint8_t status = BTP_STATUS_SUCCESS; - int rc; + const struct gattc_cfg_notify_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); + uint8_t status = BTP_STATUS_SUCCESS; + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); - if (rc) { - status = BTP_STATUS_FAILED; - goto rsp; - } + rc = ble_gap_conn_find_by_addr((ble_addr_t *) data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } - if (cmd->enable) { - uint16_t value; + if (cmd->enable) { + uint16_t value; - if (op == GATTC_CFG_NOTIFY) { - value = 0x0001; - } else { - value = 0x0002; - } + if (op == GATTC_CFG_NOTIFY) { + value = 0x0001; + } else { + value = 0x0002; + } - if (enable_subscription(conn.conn_handle, - ccc_handle, value) != 0) { - status = BTP_STATUS_FAILED; - goto rsp; - } - } else { - if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { - status = BTP_STATUS_FAILED; - } else { - status = BTP_STATUS_SUCCESS; - } - } + if (enable_subscription(conn.conn_handle, + ccc_handle, value) != 0) { + status = BTP_STATUS_FAILED; + goto rsp; + } + } else { + if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { + status = BTP_STATUS_FAILED; + } else { + status = BTP_STATUS_SUCCESS; + } + } rsp: - SYS_LOG_DBG("Config subscription (op %u) status %u", op, status); + SYS_LOG_DBG("Config subscription (op %u) status %u", op, status); - tester_rsp(BTP_SERVICE_ID_GATTC, op, CONTROLLER_INDEX, status); + tester_rsp(BTP_SERVICE_ID_GATTC, op, CONTROLLER_INDEX, status); } -int tester_gattc_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, - uint8_t indication, struct os_mbuf *om) +int +tester_gattc_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, + uint8_t indication, struct os_mbuf *om) { - struct gattc_notification_ev *ev; - struct ble_gap_conn_desc conn; - struct os_mbuf *buf = os_msys_get(0, 0); - const ble_addr_t *addr; + struct gattc_notification_ev *ev; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (!subscribe_params.ccc_handle) { - goto fail; - } + if (!subscribe_params.ccc_handle) { + goto fail; + } - if (ble_gap_conn_find(conn_handle, &conn)) { - goto fail; - } + if (ble_gap_conn_find(conn_handle, &conn)) { + goto fail; + } - net_buf_simple_init(buf, 0); - ev = net_buf_simple_add(buf, sizeof(*ev)); + net_buf_simple_init(buf, 0); + ev = net_buf_simple_add(buf, sizeof(*ev)); - addr = &conn.peer_ota_addr; + addr = &conn.peer_ota_addr; - ev->address_type = addr->type; - memcpy(ev->address, addr->val, sizeof(ev->address)); - ev->type = (uint8_t) (indication ? 0x02 : 0x01); - ev->handle = sys_cpu_to_le16(attr_handle); - ev->data_length = sys_cpu_to_le16(os_mbuf_len(om)); - os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om)); + ev->address_type = addr->type; + memcpy(ev->address, addr->val, sizeof(ev->address)); + ev->type = (uint8_t) (indication ? 0x02 : 0x01); + ev->handle = sys_cpu_to_le16(attr_handle); + ev->data_length = sys_cpu_to_le16(os_mbuf_len(om)); + os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om)); - tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_EV_NOTIFICATION_RXED, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_GATTC, GATTC_EV_NOTIFICATION_RXED, + CONTROLLER_INDEX, buf); fail: - os_mbuf_free_chain(buf); - return 0; + os_mbuf_free_chain(buf); + return 0; } -static void supported_commands(uint8_t *data, uint16_t len) +static void +supported_commands(uint8_t *data, uint16_t len) { - uint8_t cmds[3]; - struct gatt_read_supported_commands_rp *rp = (void *) cmds; + uint8_t cmds[3]; + struct gatt_read_supported_commands_rp *rp = (void *) cmds; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memset(cmds, 0, sizeof(cmds)); + memset(cmds, 0, sizeof(cmds)); - tester_set_bit(cmds, GATTC_READ_SUPPORTED_COMMANDS); - tester_set_bit(cmds, GATTC_EXCHANGE_MTU); - tester_set_bit(cmds, GATTC_DISC_ALL_PRIM_SVCS); - tester_set_bit(cmds, GATTC_DISC_PRIM_UUID); - tester_set_bit(cmds, GATTC_FIND_INCLUDED); - tester_set_bit(cmds, GATTC_DISC_ALL_CHRC); - tester_set_bit(cmds, GATTC_DISC_CHRC_UUID); - tester_set_bit(cmds, GATTC_DISC_ALL_DESC); - tester_set_bit(cmds, GATTC_READ); - tester_set_bit(cmds, GATTC_READ_UUID); - tester_set_bit(cmds, GATTC_READ_LONG); - tester_set_bit(cmds, GATTC_READ_MULTIPLE); - tester_set_bit(cmds, GATTC_WRITE_WITHOUT_RSP); + tester_set_bit(cmds, GATTC_READ_SUPPORTED_COMMANDS); + tester_set_bit(cmds, GATTC_EXCHANGE_MTU); + tester_set_bit(cmds, GATTC_DISC_ALL_PRIM_SVCS); + tester_set_bit(cmds, GATTC_DISC_PRIM_UUID); + tester_set_bit(cmds, GATTC_FIND_INCLUDED); + tester_set_bit(cmds, GATTC_DISC_ALL_CHRC); + tester_set_bit(cmds, GATTC_DISC_CHRC_UUID); + tester_set_bit(cmds, GATTC_DISC_ALL_DESC); + tester_set_bit(cmds, GATTC_READ); + tester_set_bit(cmds, GATTC_READ_UUID); + tester_set_bit(cmds, GATTC_READ_LONG); + tester_set_bit(cmds, GATTC_READ_MULTIPLE); + tester_set_bit(cmds, GATTC_WRITE_WITHOUT_RSP); #if 0 - tester_set_bit(cmds, GATTC_SIGNED_WRITE_WITHOUT_RSP); + tester_set_bit(cmds, GATTC_SIGNED_WRITE_WITHOUT_RSP); #endif - tester_set_bit(cmds, GATTC_WRITE); - tester_set_bit(cmds, GATTC_WRITE_LONG); - tester_set_bit(cmds, GATTC_RELIABLE_WRITE); - tester_set_bit(cmds, GATTC_CFG_NOTIFY); - tester_set_bit(cmds, GATTC_CFG_INDICATE); + tester_set_bit(cmds, GATTC_WRITE); + tester_set_bit(cmds, GATTC_WRITE_LONG); + tester_set_bit(cmds, GATTC_RELIABLE_WRITE); + tester_set_bit(cmds, GATTC_CFG_NOTIFY); + tester_set_bit(cmds, GATTC_CFG_INDICATE); - tester_send(BTP_SERVICE_ID_GATTC, GATTC_READ_SUPPORTED_COMMANDS, - CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); + tester_send(BTP_SERVICE_ID_GATTC, GATTC_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); } -void tester_handle_gattc(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len) +void +tester_handle_gattc(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len) { - switch (opcode) { - case GATTC_READ_SUPPORTED_COMMANDS: - supported_commands(data, len); - return; - case GATTC_EXCHANGE_MTU: - exchange_mtu(data, len); - return; - case GATTC_DISC_ALL_PRIM_SVCS: - disc_all_prim_svcs(data, len); - return; - case GATTC_DISC_PRIM_UUID: - disc_prim_uuid(data, len); - return; - case GATTC_FIND_INCLUDED: - find_included(data, len); - return; - case GATTC_DISC_ALL_CHRC: - disc_all_chrc(data, len); - return; - case GATTC_DISC_CHRC_UUID: - disc_chrc_uuid(data, len); - return; - case GATTC_DISC_ALL_DESC: - disc_all_desc(data, len); - return; - case GATTC_READ: - read(data, len); - return; - case GATTC_READ_UUID: - read_uuid(data, len); - return; - case GATTC_READ_LONG: - read_long(data, len); - return; - case GATTC_READ_MULTIPLE: - read_multiple(data, len); - return; - case GATTC_WRITE_WITHOUT_RSP: - write_without_rsp(data, len, opcode, false); - return; + switch (opcode) { + case GATTC_READ_SUPPORTED_COMMANDS: + supported_commands(data, len); + return; + case GATTC_EXCHANGE_MTU: + exchange_mtu(data, len); + return; + case GATTC_DISC_ALL_PRIM_SVCS: + disc_all_prim_svcs(data, len); + return; + case GATTC_DISC_PRIM_UUID: + disc_prim_uuid(data, len); + return; + case GATTC_FIND_INCLUDED: + find_included(data, len); + return; + case GATTC_DISC_ALL_CHRC: + disc_all_chrc(data, len); + return; + case GATTC_DISC_CHRC_UUID: + disc_chrc_uuid(data, len); + return; + case GATTC_DISC_ALL_DESC: + disc_all_desc(data, len); + return; + case GATTC_READ: + read(data, len); + return; + case GATTC_READ_UUID: + read_uuid(data, len); + return; + case GATTC_READ_LONG: + read_long(data, len); + return; + case GATTC_READ_MULTIPLE: + read_multiple(data, len); + return; + case GATTC_WRITE_WITHOUT_RSP: + write_without_rsp(data, + len, + opcode, + false); + return; #if 0 - case GATTC_SIGNED_WRITE_WITHOUT_RSP: - write_without_rsp(data, len, opcode, true); - return; + case GATTC_SIGNED_WRITE_WITHOUT_RSP: + write_without_rsp(data, len, opcode, true); + return; #endif - case GATTC_WRITE: - write(data, len); - return; - case GATTC_WRITE_LONG: - write_long(data, len); - return; - case GATTC_RELIABLE_WRITE: - reliable_write(data, len); - return; - case GATTC_CFG_NOTIFY: - case GATTC_CFG_INDICATE: - config_subscription(data, len, opcode); - return; - default: - tester_rsp(BTP_SERVICE_ID_GATTC, opcode, index, - BTP_STATUS_UNKNOWN_CMD); - return; - } + case GATTC_WRITE: + write(data, len); + return; + case GATTC_WRITE_LONG: + write_long(data, len); + return; + case GATTC_RELIABLE_WRITE: + reliable_write(data, len); + return; + case GATTC_CFG_NOTIFY: + case GATTC_CFG_INDICATE: + config_subscription(data, len, opcode); + return; + default: + tester_rsp(BTP_SERVICE_ID_GATTC, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } } \ No newline at end of file diff --git a/apps/bttester/src/glue.c b/apps/bttester/src/glue.c index 3e606062a..f4c838184 100644 --- a/apps/bttester/src/glue.c +++ b/apps/bttester/src/glue.c @@ -31,99 +31,99 @@ const char *bt_hex(const void *buf, size_t len) { - static const char hex[] = "0123456789abcdef"; - static char hexbufs[4][137]; - static uint8_t curbuf; - const uint8_t *b = buf; - char *str; - int i; + static const char hex[] = "0123456789abcdef"; + static char hexbufs[4][137]; + static uint8_t curbuf; + const uint8_t *b = buf; + char *str; + int i; - str = hexbufs[curbuf++]; - curbuf %= ARRAY_SIZE(hexbufs); + str = hexbufs[curbuf++]; + curbuf %= ARRAY_SIZE(hexbufs); - len = min(len, (sizeof(hexbufs[0]) - 1) / 2); + len = min(len, (sizeof(hexbufs[0]) - 1) / 2); - for (i = 0; i < len; i++) { - str[i * 2] = hex[b[i] >> 4]; - str[i * 2 + 1] = hex[b[i] & 0xf]; - } + for (i = 0; i < len; i++) { + str[i * 2] = hex[b[i] >> 4]; + str[i * 2 + 1] = hex[b[i] & 0xf]; + } - str[i * 2] = '\0'; + str[i * 2] = '\0'; - return str; + return str; } struct os_mbuf * NET_BUF_SIMPLE(uint16_t size) { - struct os_mbuf *buf; + struct os_mbuf *buf; - buf = os_msys_get(size, 0); - assert(buf); + buf = os_msys_get(size, 0); + assert(buf); - return buf; + return buf; } /* This is by purpose */ void net_buf_simple_init(struct os_mbuf *buf, - size_t reserve_head) + size_t reserve_head) { - /* This is called in Zephyr after init. - * Note in Mynewt case we don't care abour reserved head*/ - buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + reserve_head; - buf->om_len = 0; + /* This is called in Zephyr after init. + * Note in Mynewt case we don't care abour reserved head*/ + buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + reserve_head; + buf->om_len = 0; } void net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val) { - val = htole16(val); - os_mbuf_append(om, &val, sizeof(val)); - ASSERT_NOT_CHAIN(om); + val = htole16(val); + os_mbuf_append(om, &val, sizeof(val)); + ASSERT_NOT_CHAIN(om); } void net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val) { - val = htobe16(val); - os_mbuf_append(om, &val, sizeof(val)); - ASSERT_NOT_CHAIN(om); + val = htobe16(val); + os_mbuf_append(om, &val, sizeof(val)); + ASSERT_NOT_CHAIN(om); } void net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val) { - val = htobe32(val); - os_mbuf_append(om, &val, sizeof(val)); - ASSERT_NOT_CHAIN(om); + val = htobe32(val); + os_mbuf_append(om, &val, sizeof(val)); + ASSERT_NOT_CHAIN(om); } void net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val) { - os_mbuf_append(om, &val, 1); - ASSERT_NOT_CHAIN(om); + os_mbuf_append(om, &val, 1); + ASSERT_NOT_CHAIN(om); } void* net_buf_simple_add(struct os_mbuf *om, uint8_t len) { - void * tmp; + void * tmp; - tmp = os_mbuf_extend(om, len); - ASSERT_NOT_CHAIN(om); + tmp = os_mbuf_extend(om, len); + ASSERT_NOT_CHAIN(om); - return tmp; + return tmp; } uint8_t * net_buf_simple_push(struct os_mbuf *om, uint8_t len) { - uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len]; + uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len]; - assert(headroom >= len); - om->om_data -= len; - om->om_len += len; + assert(headroom >= len); + om->om_data -= len; + om->om_len += len; - return om->om_data; + return om->om_data; } #endif diff --git a/apps/bttester/src/glue.h b/apps/bttester/src/glue.h index 650856015..0372d757f 100644 --- a/apps/bttester/src/glue.h +++ b/apps/bttester/src/glue.h @@ -49,15 +49,22 @@ struct bt_data { .data = (const uint8_t *)(_data), \ } -struct os_mbuf * NET_BUF_SIMPLE(uint16_t size); -void net_buf_simple_init(struct os_mbuf *buf, size_t reserve_head); -void net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val); -void net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val); -void *net_buf_simple_add(struct os_mbuf *om, uint8_t len); -uint8_t *net_buf_simple_push(struct os_mbuf *om, uint8_t len); +struct os_mbuf * +NET_BUF_SIMPLE(uint16_t size); +void +net_buf_simple_init(struct os_mbuf *buf, size_t reserve_head); +void +net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val); +void +net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val); +void * +net_buf_simple_add(struct os_mbuf *om, uint8_t len); +uint8_t * +net_buf_simple_push(struct os_mbuf *om, uint8_t len); -#define net_buf_simple_add_mem(a,b,c) os_mbuf_append(a,b,c) +#define net_buf_simple_add_mem(a, b, c) os_mbuf_append(a,b,c) -const char *bt_hex(const void *buf, size_t len); +const char * +bt_hex(const void *buf, size_t len); #endif /* __GLUE_H__ */ diff --git a/apps/bttester/src/l2cap.c b/apps/bttester/src/l2cap.c index 7890e7cce..32baab1d9 100644 --- a/apps/bttester/src/l2cap.c +++ b/apps/bttester/src/l2cap.c @@ -43,7 +43,7 @@ #define TESTER_COC_BUF_COUNT (3 * MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)) static os_membuf_t tester_sdu_coc_mem[ - OS_MEMPOOL_SIZE(TESTER_COC_BUF_COUNT, TESTER_COC_MTU) + OS_MEMPOOL_SIZE(TESTER_COC_BUF_COUNT, TESTER_COC_MTU) ]; struct os_mbuf_pool sdu_os_mbuf_pool; @@ -51,574 +51,595 @@ static struct os_mempool sdu_coc_mbuf_mempool; static bool hold_credit = false; static struct channel { - uint8_t chan_id; /* Internal number that identifies L2CAP channel. */ - uint8_t state; - struct ble_l2cap_chan *chan; + uint8_t chan_id; /* Internal number that identifies L2CAP channel. */ + uint8_t state; + struct ble_l2cap_chan *chan; } channels[CHANNELS]; -static uint8_t recv_cb_buf[TESTER_COC_MTU + sizeof(struct l2cap_data_received_ev)]; +static uint8_t + recv_cb_buf[TESTER_COC_MTU + sizeof(struct l2cap_data_received_ev)]; -static struct channel *get_free_channel(void) +static struct channel * +get_free_channel(void) { - uint8_t i; - struct channel *chan; + uint8_t i; + struct channel *chan; - for (i = 0; i < CHANNELS; i++) { - if (channels[i].state) { - continue; - } + for (i = 0; i < CHANNELS; i++) { + if (channels[i].state) { + continue; + } - chan = &channels[i]; - chan->chan_id = i; + chan = &channels[i]; + chan->chan_id = i; - return chan; - } + return chan; + } - return NULL; + return NULL; } -struct channel *find_channel(struct ble_l2cap_chan *chan) +struct channel * +find_channel(struct ble_l2cap_chan *chan) { - int i; + int i; - for (i = 0; i < CHANNELS; ++i) { - if (channels[i].chan == chan) { - return &channels[i]; - } - } + for (i = 0; i < CHANNELS; ++i) { + if (channels[i].chan == chan) { + return &channels[i]; + } + } - return NULL; + return NULL; } -struct channel *get_channel(uint8_t chan_id) +struct channel * +get_channel(uint8_t chan_id) { - if (chan_id >= CHANNELS) { - return NULL; - } + if (chan_id >= CHANNELS) { + return NULL; + } - return &channels[chan_id]; + return &channels[chan_id]; } static void tester_l2cap_coc_recv(struct ble_l2cap_chan *chan, struct os_mbuf *sdu) { - SYS_LOG_DBG("LE CoC SDU received, chan: 0x%08lx, data len %d", - (uint32_t) chan, OS_MBUF_PKTLEN(sdu)); + SYS_LOG_DBG("LE CoC SDU received, chan: 0x%08lx, data len %d", + (uint32_t) chan, OS_MBUF_PKTLEN(sdu)); - os_mbuf_free_chain(sdu); - if (!hold_credit) { - sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); - assert(sdu != NULL); + os_mbuf_free_chain(sdu); + if (!hold_credit) { + sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); + assert(sdu != NULL); - - ble_l2cap_recv_ready(chan, sdu); - } + ble_l2cap_recv_ready(chan, sdu); + } } -static void recv_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, - struct os_mbuf *buf, void *arg) +static void +recv_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, + struct os_mbuf *buf, void *arg) { - struct l2cap_data_received_ev *ev = (void *) recv_cb_buf; - struct channel *channel = find_channel(chan); - assert(channel != NULL); + struct l2cap_data_received_ev *ev = (void *) recv_cb_buf; + struct channel *channel = find_channel(chan); + assert(channel != NULL); - ev->chan_id = channel->chan_id; - ev->data_length = OS_MBUF_PKTLEN(buf); + ev->chan_id = channel->chan_id; + ev->data_length = OS_MBUF_PKTLEN(buf); - if (ev->data_length > TESTER_COC_MTU) { - SYS_LOG_ERR("Too large sdu received, truncating data"); - ev->data_length = TESTER_COC_MTU; - } - os_mbuf_copydata(buf, 0, ev->data_length, ev->data); + if (ev->data_length > TESTER_COC_MTU) { + SYS_LOG_ERR("Too large sdu received, truncating data"); + ev->data_length = TESTER_COC_MTU; + } + os_mbuf_copydata(buf, 0, ev->data_length, ev->data); - tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DATA_RECEIVED, - CONTROLLER_INDEX, recv_cb_buf, sizeof(*ev) + ev->data_length); + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DATA_RECEIVED, + CONTROLLER_INDEX, recv_cb_buf, sizeof(*ev) + ev->data_length); - tester_l2cap_coc_recv(chan, buf); + tester_l2cap_coc_recv(chan, buf); } -static void unstalled_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, - int status, void *arg) +static void +unstalled_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, + int status, void *arg) { - if (status) { - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, - CONTROLLER_INDEX, BTP_STATUS_FAILED); - } else { - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); - } + if (status) { + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + } else { + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + } } -static void reconfigured_ev(uint16_t conn_handle, struct ble_l2cap_chan *chan, - struct ble_l2cap_chan_info *chan_info, - int status) +static void +reconfigured_ev(uint16_t conn_handle, struct ble_l2cap_chan *chan, + struct ble_l2cap_chan_info *chan_info, + int status) { - struct l2cap_reconfigured_ev ev; - struct channel *channel; + struct l2cap_reconfigured_ev ev; + struct channel *channel; - if (status != 0) { - return; - } + if (status != 0) { + return; + } - channel = find_channel(chan); - assert(channel != NULL); + channel = find_channel(chan); + assert(channel != NULL); - ev.chan_id = channel->chan_id; - ev.peer_mtu = chan_info->peer_coc_mtu; - ev.peer_mps = chan_info->peer_l2cap_mtu; - ev.our_mtu = chan_info->our_coc_mtu; - ev.our_mps = chan_info->our_l2cap_mtu; + ev.chan_id = channel->chan_id; + ev.peer_mtu = chan_info->peer_coc_mtu; + ev.peer_mps = chan_info->peer_l2cap_mtu; + ev.our_mtu = chan_info->our_coc_mtu; + ev.our_mps = chan_info->our_l2cap_mtu; - tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_RECONFIGURED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_RECONFIGURED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void connected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, - struct ble_l2cap_chan_info *chan_info, void *arg) +static void +connected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, + struct ble_l2cap_chan_info *chan_info, void *arg) { - struct l2cap_connected_ev ev; - struct ble_gap_conn_desc desc; - struct channel *channel = find_channel(chan); + struct l2cap_connected_ev ev; + struct ble_gap_conn_desc desc; + struct channel *channel = find_channel(chan); - if (channel == NULL) { - channel = get_free_channel(); - } + if (channel == NULL) { + channel = get_free_channel(); + } - ev.chan_id = channel->chan_id; - ev.psm = chan_info->psm; - ev.peer_mtu = chan_info->peer_coc_mtu; - ev.peer_mps = chan_info->peer_l2cap_mtu; - ev.our_mtu = chan_info->our_coc_mtu; - ev.our_mps = chan_info->our_l2cap_mtu; - channel->state = 1; - channel->chan = chan; + ev.chan_id = channel->chan_id; + ev.psm = chan_info->psm; + ev.peer_mtu = chan_info->peer_coc_mtu; + ev.peer_mps = chan_info->peer_l2cap_mtu; + ev.our_mtu = chan_info->our_coc_mtu; + ev.our_mps = chan_info->our_l2cap_mtu; + channel->state = 1; + channel->chan = chan; - if (!ble_gap_conn_find(conn_handle, &desc)) { - ev.address_type = desc.peer_ota_addr.type; - memcpy(ev.address, desc.peer_ota_addr.val, - sizeof(ev.address)); - } + if (!ble_gap_conn_find(conn_handle, &desc)) { + ev.address_type = desc.peer_ota_addr.type; + memcpy(ev.address, desc.peer_ota_addr.val, + sizeof(ev.address)); + } - tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_CONNECTED, CONTROLLER_INDEX, - (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_CONNECTED, CONTROLLER_INDEX, + (uint8_t *) &ev, sizeof(ev)); } -static void disconnected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, - struct ble_l2cap_chan_info *chan_info, void *arg) +static void +disconnected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, + struct ble_l2cap_chan_info *chan_info, void *arg) { - struct l2cap_disconnected_ev ev; - struct ble_gap_conn_desc desc; - struct channel *channel; + struct l2cap_disconnected_ev ev; + struct ble_gap_conn_desc desc; + struct channel *channel; - memset(&ev, 0, sizeof(struct l2cap_disconnected_ev)); + memset(&ev, 0, sizeof(struct l2cap_disconnected_ev)); - channel = find_channel(chan); - assert(channel != NULL); + channel = find_channel(chan); + assert(channel != NULL); - channel->state = 0; - channel->chan = chan; - ev.chan_id = channel->chan_id; - ev.psm = chan_info->psm; + channel->state = 0; + channel->chan = chan; + ev.chan_id = channel->chan_id; + ev.psm = chan_info->psm; - if (!ble_gap_conn_find(conn_handle, &desc)) { - ev.address_type = desc.peer_ota_addr.type; - memcpy(ev.address, desc.peer_ota_addr.val, - sizeof(ev.address)); - } + if (!ble_gap_conn_find(conn_handle, &desc)) { + ev.address_type = desc.peer_ota_addr.type; + memcpy(ev.address, desc.peer_ota_addr.val, + sizeof(ev.address)); + } - tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DISCONNECTED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DISCONNECTED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static int accept_cb(uint16_t conn_handle, uint16_t peer_mtu, - struct ble_l2cap_chan *chan) +static int +accept_cb(uint16_t conn_handle, uint16_t peer_mtu, + struct ble_l2cap_chan *chan) { - struct os_mbuf *sdu_rx; + struct os_mbuf *sdu_rx; - SYS_LOG_DBG("LE CoC accepting, chan: 0x%08lx, peer_mtu %d", - (uint32_t) chan, peer_mtu); + SYS_LOG_DBG("LE CoC accepting, chan: 0x%08lx, peer_mtu %d", + (uint32_t) chan, peer_mtu); - sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); - if (!sdu_rx) { - return BLE_HS_ENOMEM; - } + sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); + if (!sdu_rx) { + return BLE_HS_ENOMEM; + } - ble_l2cap_recv_ready(chan, sdu_rx); + ble_l2cap_recv_ready(chan, sdu_rx); - return 0; + return 0; } static int tester_l2cap_event(struct ble_l2cap_event *event, void *arg) { - struct ble_l2cap_chan_info chan_info; - int accept_response; - struct ble_gap_conn_desc conn; + struct ble_l2cap_chan_info chan_info; + int accept_response; + struct ble_gap_conn_desc conn; - switch (event->type) { - case BLE_L2CAP_EVENT_COC_CONNECTED: - if (ble_l2cap_get_chan_info(event->connect.chan, &chan_info)) { - assert(0); - } + switch (event->type) { + case BLE_L2CAP_EVENT_COC_CONNECTED: + if (ble_l2cap_get_chan_info(event->connect.chan, &chan_info)) { + assert(0); + } - if (event->connect.status) { - console_printf("LE COC error: %d\n", event->connect.status); - return 0; - } + if (event->connect.status) { + console_printf("LE COC error: %d\n", event->connect.status); + return 0; + } - console_printf("LE COC connected, conn: %d, chan: 0x%08lx, " - "psm: 0x%02x, scid: 0x%04x, dcid: 0x%04x, " - "our_mps: %d, our_mtu: %d, peer_mps: %d, " - "peer_mtu: %d\n", event->connect.conn_handle, - (uint32_t) event->connect.chan, chan_info.psm, - chan_info.scid, chan_info.dcid, - chan_info.our_l2cap_mtu, chan_info.our_coc_mtu, - chan_info.peer_l2cap_mtu, chan_info.peer_coc_mtu); + console_printf("LE COC connected, conn: %d, chan: 0x%08lx, " + "psm: 0x%02x, scid: 0x%04x, dcid: 0x%04x, " + "our_mps: %d, our_mtu: %d, peer_mps: %d, " + "peer_mtu: %d\n", event->connect.conn_handle, + (uint32_t) event->connect.chan, chan_info.psm, + chan_info.scid, chan_info.dcid, + chan_info.our_l2cap_mtu, chan_info.our_coc_mtu, + chan_info.peer_l2cap_mtu, chan_info.peer_coc_mtu); - connected_cb(event->connect.conn_handle, - event->connect.chan, &chan_info, arg); + connected_cb(event->connect.conn_handle, + event->connect.chan, &chan_info, arg); - return 0; - case BLE_L2CAP_EVENT_COC_DISCONNECTED: - if (ble_l2cap_get_chan_info(event->disconnect.chan, - &chan_info)) { - assert(0); - } - console_printf("LE CoC disconnected, chan: 0x%08lx\n", - (uint32_t) event->disconnect.chan); + return 0; + case BLE_L2CAP_EVENT_COC_DISCONNECTED: + if (ble_l2cap_get_chan_info(event->disconnect.chan, + &chan_info)) { + assert(0); + } + console_printf("LE CoC disconnected, chan: 0x%08lx\n", + (uint32_t) event->disconnect.chan); - disconnected_cb(event->disconnect.conn_handle, - event->disconnect.chan, &chan_info, arg); - return 0; - case BLE_L2CAP_EVENT_COC_ACCEPT: - ble_l2cap_get_chan_info(event->accept.chan, &chan_info); - if (chan_info.psm == 0x00F2) { - /* TSPX_psm_authentication_required */ - ble_gap_conn_find(event->accept.conn_handle, &conn); - if (!conn.sec_state.authenticated) { - return BLE_HS_EAUTHEN; - } - } else if (chan_info.psm == 0x00F3) { - /* TSPX_psm_authorization_required */ - ble_gap_conn_find(event->accept.conn_handle, &conn); - if (!conn.sec_state.encrypted) { - return BLE_HS_EAUTHOR; - } - return BLE_HS_EAUTHOR; - } else if (chan_info.psm == 0x00F4) { - /* TSPX_psm_encryption_key_size_required */ - ble_gap_conn_find(event->accept.conn_handle, &conn); - if (conn.sec_state.key_size < 16) { - return BLE_HS_EENCRYPT_KEY_SZ; - } - } + disconnected_cb(event->disconnect.conn_handle, + event->disconnect.chan, &chan_info, arg); + return 0; + case BLE_L2CAP_EVENT_COC_ACCEPT: + ble_l2cap_get_chan_info(event->accept.chan, + &chan_info); + if (chan_info.psm == 0x00F2) { + /* TSPX_psm_authentication_required */ + ble_gap_conn_find(event->accept.conn_handle, &conn); + if (!conn.sec_state.authenticated) { + return BLE_HS_EAUTHEN; + } + } else if (chan_info.psm == 0x00F3) { + /* TSPX_psm_authorization_required */ + ble_gap_conn_find(event->accept.conn_handle, &conn); + if (!conn.sec_state.encrypted) { + return BLE_HS_EAUTHOR; + } + return BLE_HS_EAUTHOR; + } else if (chan_info.psm == 0x00F4) { + /* TSPX_psm_encryption_key_size_required */ + ble_gap_conn_find(event->accept.conn_handle, &conn); + if (conn.sec_state.key_size < 16) { + return BLE_HS_EENCRYPT_KEY_SZ; + } + } - accept_response = POINTER_TO_INT(arg); - if (accept_response) { - return accept_response; - } + accept_response = POINTER_TO_INT(arg); + if (accept_response) { + return accept_response; + } - console_printf("LE CoC accept, chan: 0x%08lx, handle: %u, sdu_size: %u\n", - (uint32_t) event->accept.chan, - event->accept.conn_handle, - event->accept.peer_sdu_size); + console_printf( + "LE CoC accept, chan: 0x%08lx, handle: %u, sdu_size: %u\n", + (uint32_t) event->accept.chan, + event->accept.conn_handle, + event->accept.peer_sdu_size); - return accept_cb(event->accept.conn_handle, - event->accept.peer_sdu_size, - event->accept.chan); + return accept_cb(event->accept.conn_handle, + event->accept.peer_sdu_size, + event->accept.chan); - case BLE_L2CAP_EVENT_COC_DATA_RECEIVED: - console_printf("LE CoC data received, chan: 0x%08lx, handle: %u, sdu_len: %u\n", - (uint32_t) event->receive.chan, - event->receive.conn_handle, - OS_MBUF_PKTLEN(event->receive.sdu_rx)); + case BLE_L2CAP_EVENT_COC_DATA_RECEIVED: + console_printf( + "LE CoC data received, chan: 0x%08lx, handle: %u, sdu_len: %u\n", + (uint32_t) event->receive.chan, + event->receive.conn_handle, + OS_MBUF_PKTLEN(event->receive.sdu_rx)); - recv_cb(event->receive.conn_handle, event->receive.chan, - event->receive.sdu_rx, arg); - return 0; - case BLE_L2CAP_EVENT_COC_TX_UNSTALLED: - console_printf("LE CoC tx unstalled, chan: 0x%08lx, handle: %u, status: %d\n", - (uint32_t) event->tx_unstalled.chan, - event->tx_unstalled.conn_handle, - event->tx_unstalled.status); + recv_cb(event->receive.conn_handle, event->receive.chan, + event->receive.sdu_rx, arg); + return 0; + case BLE_L2CAP_EVENT_COC_TX_UNSTALLED: + console_printf( + "LE CoC tx unstalled, chan: 0x%08lx, handle: %u, status: %d\n", + (uint32_t) event->tx_unstalled.chan, + event->tx_unstalled.conn_handle, + event->tx_unstalled.status); - unstalled_cb(event->tx_unstalled.conn_handle, - event->tx_unstalled.chan, - event->tx_unstalled.status, arg); - return 0; - case BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED: - if (ble_l2cap_get_chan_info(event->reconfigured.chan, - &chan_info)) { - assert(0); - } - console_printf("LE CoC reconfigure completed status 0x%02x, " - "chan: 0x%08lx\n", event->reconfigured.status, - (uint32_t) event->reconfigured.chan); + unstalled_cb(event->tx_unstalled.conn_handle, + event->tx_unstalled.chan, + event->tx_unstalled.status, arg); + return 0; + case BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED: + if (ble_l2cap_get_chan_info(event->reconfigured.chan, + &chan_info)) { + assert(0); + } + console_printf("LE CoC reconfigure completed status 0x%02x, " + "chan: 0x%08lx\n", event->reconfigured.status, + (uint32_t) event->reconfigured.chan); - if (event->reconfigured.status == 0) { - console_printf("\t our_mps: %d our_mtu %d\n", - chan_info.our_l2cap_mtu, chan_info.our_coc_mtu); - } + if (event->reconfigured.status == 0) { + console_printf("\t our_mps: %d our_mtu %d\n", + chan_info.our_l2cap_mtu, chan_info.our_coc_mtu); + } - reconfigured_ev(event->reconfigured.conn_handle, - event->reconfigured.chan, - &chan_info, - event->reconfigured.status); - return 0; - case BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED: - if (ble_l2cap_get_chan_info(event->reconfigured.chan, - &chan_info)) { - assert(0); - } - console_printf("LE CoC peer reconfigured status 0x%02x, " - "chan: 0x%08lx\n", event->reconfigured.status, - (uint32_t) event->reconfigured.chan); + reconfigured_ev(event->reconfigured.conn_handle, + event->reconfigured.chan, + &chan_info, + event->reconfigured.status); + return 0; + case BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED: + if (ble_l2cap_get_chan_info(event->reconfigured.chan, + &chan_info)) { + assert(0); + } + console_printf("LE CoC peer reconfigured status 0x%02x, " + "chan: 0x%08lx\n", event->reconfigured.status, + (uint32_t) event->reconfigured.chan); - if (event->reconfigured.status == 0) { - console_printf("\t peer_mps: %d peer_mtu %d\n", - chan_info.peer_l2cap_mtu, chan_info.peer_coc_mtu); - } + if (event->reconfigured.status == 0) { + console_printf("\t peer_mps: %d peer_mtu %d\n", + chan_info.peer_l2cap_mtu, + chan_info.peer_coc_mtu); + } - reconfigured_ev(event->reconfigured.conn_handle, - event->reconfigured.chan, - &chan_info, - event->reconfigured.status); - return 0; - default: - return 0; - } + reconfigured_ev(event->reconfigured.conn_handle, + event->reconfigured.chan, + &chan_info, + event->reconfigured.status); + return 0; + default: + return 0; + } } -static void connect(uint8_t *data, uint16_t len) +static void +connect(uint8_t *data, uint16_t len) { - const struct l2cap_connect_cmd *cmd = (void *) data; - uint8_t rp_buf[sizeof(struct l2cap_connect_rp) + cmd->num]; - struct l2cap_connect_rp *rp = (void *) rp_buf; - struct ble_gap_conn_desc desc; - struct channel *chan; - struct os_mbuf *sdu_rx[cmd->num]; - ble_addr_t *addr = (void *) data; - uint16_t mtu = htole16(cmd->mtu); - int rc; - int i, j; - bool ecfc = cmd->options & L2CAP_CONNECT_OPT_ECFC; - hold_credit = cmd->options & L2CAP_CONNECT_OPT_HOLD_CREDIT; + const struct l2cap_connect_cmd *cmd = (void *) data; + uint8_t rp_buf[sizeof(struct l2cap_connect_rp) + cmd->num]; + struct l2cap_connect_rp *rp = (void *) rp_buf; + struct ble_gap_conn_desc desc; + struct channel *chan; + struct os_mbuf *sdu_rx[cmd->num]; + ble_addr_t *addr = (void *) data; + uint16_t mtu = htole16(cmd->mtu); + int rc; + int i, j; + bool ecfc = cmd->options & L2CAP_CONNECT_OPT_ECFC; + hold_credit = cmd->options & L2CAP_CONNECT_OPT_HOLD_CREDIT; - SYS_LOG_DBG("connect: type: %d addr: %s", addr->type, bt_hex(addr->val, 6)); + SYS_LOG_DBG("connect: type: %d addr: %s", + addr->type, + bt_hex(addr->val, 6)); - if (mtu == 0 || mtu > TESTER_COC_MTU) { - mtu = TESTER_COC_MTU; - } + if (mtu == 0 || mtu > TESTER_COC_MTU) { + mtu = TESTER_COC_MTU; + } - rc = ble_gap_conn_find_by_addr(addr, &desc); - if (rc) { - SYS_LOG_ERR("GAP conn find failed"); - goto fail; - } + rc = ble_gap_conn_find_by_addr(addr, &desc); + if (rc) { + SYS_LOG_ERR("GAP conn find failed"); + goto fail; + } - rp->num = cmd->num; + rp->num = cmd->num; - for (i = 0; i < cmd->num; i++) { - chan = get_free_channel(); - if (!chan) { - SYS_LOG_ERR("No free channels"); - goto fail; - } - /* temporarily mark channel as used to select next one */ + for (i = 0; i < cmd->num; i++) { + chan = get_free_channel(); + if (!chan) { + SYS_LOG_ERR("No free channels"); + goto fail; + } + /* temporarily mark channel as used to select next one */ chan->state = 1; - rp->chan_ids[i] = chan->chan_id; + rp->chan_ids[i] = chan->chan_id; - sdu_rx[i] = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); - if (sdu_rx[i] == NULL) { - SYS_LOG_ERR("Failed to alloc buf"); - goto fail; - } - } + sdu_rx[i] = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); + if (sdu_rx[i] == NULL) { + SYS_LOG_ERR("Failed to alloc buf"); + goto fail; + } + } - /* mark selected channels as unused again */ - for (i = 0; i < cmd->num; i++) { - for (j = 0; j < CHANNELS; j++) { - if (rp->chan_ids[i] == channels[j].chan_id) { - channels[j].state = 0; - } - } - } + /* mark selected channels as unused again */ + for (i = 0; i < cmd->num; i++) { + for (j = 0; j < CHANNELS; j++) { + if (rp->chan_ids[i] == channels[j].chan_id) { + channels[j].state = 0; + } + } + } - if (cmd->num == 1 && !ecfc) { - rc = ble_l2cap_connect(desc.conn_handle, htole16(cmd->psm), - mtu, sdu_rx[0], - tester_l2cap_event, NULL); - } else if (ecfc) { - rc = ble_l2cap_enhanced_connect(desc.conn_handle, - htole16(cmd->psm), mtu, - cmd->num, sdu_rx, - tester_l2cap_event, NULL); - } else { - SYS_LOG_ERR("Invalid 'num' parameter value"); - goto fail; - } + if (cmd->num == 1 && !ecfc) { + rc = ble_l2cap_connect(desc.conn_handle, htole16(cmd->psm), + mtu, sdu_rx[0], + tester_l2cap_event, NULL); + } else if (ecfc) { + rc = ble_l2cap_enhanced_connect(desc.conn_handle, + htole16(cmd->psm), mtu, + cmd->num, sdu_rx, + tester_l2cap_event, NULL); + } else { + SYS_LOG_ERR("Invalid 'num' parameter value"); + goto fail; + } - if (rc) { - SYS_LOG_ERR("L2CAP connect failed\n"); - goto fail; - } + if (rc) { + SYS_LOG_ERR("L2CAP connect failed\n"); + goto fail; + } - tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX, - (uint8_t *) rp, sizeof(rp_buf)); + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX, + (uint8_t *) rp, sizeof(rp_buf)); - return; + return; fail: - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void disconnect(const uint8_t *data, uint16_t len) +static void +disconnect(const uint8_t *data, uint16_t len) { - const struct l2cap_disconnect_cmd *cmd = (void *) data; - struct channel *chan; - uint8_t status; - int err; + const struct l2cap_disconnect_cmd *cmd = (void *) data; + struct channel *chan; + uint8_t status; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - chan = get_channel(cmd->chan_id); - assert(chan != NULL); + chan = get_channel(cmd->chan_id); + assert(chan != NULL); - err = ble_l2cap_disconnect(chan->chan); - if (err) { - status = BTP_STATUS_FAILED; - goto rsp; - } + err = ble_l2cap_disconnect(chan->chan); + if (err) { + status = BTP_STATUS_FAILED; + goto rsp; + } - status = BTP_STATUS_SUCCESS; + status = BTP_STATUS_SUCCESS; rsp: - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_DISCONNECT, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_DISCONNECT, CONTROLLER_INDEX, + status); } -static void send_data(const uint8_t *data, uint16_t len) +static void +send_data(const uint8_t *data, uint16_t len) { - const struct l2cap_send_data_cmd *cmd = (void *) data; - struct os_mbuf *sdu_tx = NULL; - int rc; - uint16_t data_len = sys_le16_to_cpu(cmd->data_len); - struct channel *chan = get_channel(cmd->chan_id); + const struct l2cap_send_data_cmd *cmd = (void *) data; + struct os_mbuf *sdu_tx = NULL; + int rc; + uint16_t data_len = sys_le16_to_cpu(cmd->data_len); + struct channel *chan = get_channel(cmd->chan_id); - SYS_LOG_DBG("cmd->chan_id=%d", cmd->chan_id); + SYS_LOG_DBG("cmd->chan_id=%d", cmd->chan_id); - if (!chan) { - SYS_LOG_ERR("Invalid channel\n"); - goto fail; - } + if (!chan) { + SYS_LOG_ERR("Invalid channel\n"); + goto fail; + } - /* FIXME: For now, fail if data length exceeds buffer length */ - if (data_len > TESTER_COC_MTU) { - SYS_LOG_ERR("Data length exceeds buffer length"); - goto fail; - } + /* FIXME: For now, fail if data length exceeds buffer length */ + if (data_len > TESTER_COC_MTU) { + SYS_LOG_ERR("Data length exceeds buffer length"); + goto fail; + } - sdu_tx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); - if (sdu_tx == NULL) { - SYS_LOG_ERR("No memory in the test sdu pool\n"); - goto fail; - } + sdu_tx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); + if (sdu_tx == NULL) { + SYS_LOG_ERR("No memory in the test sdu pool\n"); + goto fail; + } - os_mbuf_append(sdu_tx, cmd->data, data_len); + os_mbuf_append(sdu_tx, cmd->data, data_len); - /* ble_l2cap_send takes ownership of the sdu */ - rc = ble_l2cap_send(chan->chan, sdu_tx); - if (rc == 0 || rc == BLE_HS_ESTALLED) { - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); - return; - } + /* ble_l2cap_send takes ownership of the sdu */ + rc = ble_l2cap_send(chan->chan, sdu_tx); + if (rc == 0 || rc == BLE_HS_ESTALLED) { + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + return; + } - SYS_LOG_ERR("Unable to send data: %d", rc); - os_mbuf_free_chain(sdu_tx); + SYS_LOG_ERR("Unable to send data: %d", rc); + os_mbuf_free_chain(sdu_tx); fail: - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } static int l2cap_coc_err2hs_err(uint16_t coc_err) { - switch (coc_err) { - case BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM: - return BLE_HS_ENOTSUP; - case BLE_L2CAP_COC_ERR_NO_RESOURCES: - return BLE_HS_ENOMEM; - case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN: - return BLE_HS_EAUTHEN; - case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR: - return BLE_HS_EAUTHOR; - case BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC: - return BLE_HS_EENCRYPT; - case BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ: - return BLE_HS_EENCRYPT_KEY_SZ; - case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS: - return BLE_HS_EINVAL; - default: - return 0; - } + switch (coc_err) { + case BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM: + return BLE_HS_ENOTSUP; + case BLE_L2CAP_COC_ERR_NO_RESOURCES: + return BLE_HS_ENOMEM; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN: + return BLE_HS_EAUTHEN; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR: + return BLE_HS_EAUTHOR; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC: + return BLE_HS_EENCRYPT; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ: + return BLE_HS_EENCRYPT_KEY_SZ; + case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS: + return BLE_HS_EINVAL; + default: + return 0; + } } static int l2cap_btp_listen_err2coc_err(uint16_t coc_err) { switch (coc_err) { - case 0x01: - return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN; - case 0x02: - return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR; - case 0x03: - return BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ; - case 0x04: - return BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC; - default: - return 0; + case 0x01: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN; + case 0x02: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR; + case 0x03: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ; + case 0x04: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC; + default: + return 0; } } -static void listen(const uint8_t *data, uint16_t len) +static void +listen(const uint8_t *data, uint16_t len) { - const struct l2cap_listen_cmd *cmd = (void *) data; - uint16_t mtu = htole16(cmd->mtu); - uint16_t rsp = htole16(cmd->response); - int rc; + const struct l2cap_listen_cmd *cmd = (void *) data; + uint16_t mtu = htole16(cmd->mtu); + uint16_t rsp = htole16(cmd->response); + int rc; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (mtu == 0 || mtu > TESTER_COC_MTU) { - mtu = TESTER_COC_MTU; - } + if (mtu == 0 || mtu > TESTER_COC_MTU) { + mtu = TESTER_COC_MTU; + } - rsp = l2cap_btp_listen_err2coc_err(rsp); - rsp = l2cap_coc_err2hs_err(rsp); + rsp = l2cap_btp_listen_err2coc_err(rsp); + rsp = l2cap_coc_err2hs_err(rsp); - /* TODO: Handle cmd->transport flag */ - rc = ble_l2cap_create_server(cmd->psm, mtu, tester_l2cap_event, - INT_TO_POINTER(rsp)); - if (rc) { - goto fail; - } + /* TODO: Handle cmd->transport flag */ + rc = ble_l2cap_create_server(cmd->psm, mtu, tester_l2cap_event, + INT_TO_POINTER(rsp)); + if (rc) { + goto fail; + } - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); - return; + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + return; fail: - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void credits(uint8_t *data, uint16_t len) +static void +credits(uint8_t *data, uint16_t len) { - const struct l2cap_credits_cmd *cmd = (void *)data; + const struct l2cap_credits_cmd *cmd = (void *) data; struct os_mbuf *sdu; int rc; @@ -645,121 +666,126 @@ fail: BTP_STATUS_FAILED); } -static void reconfigure(const uint8_t *data, uint16_t len) +static void +reconfigure(const uint8_t *data, uint16_t len) { - const struct l2cap_reconfigure_cmd *cmd = (void *) data; - uint16_t mtu = htole16(cmd->mtu); - struct ble_gap_conn_desc desc; - ble_addr_t *addr = (void *) data; - struct ble_l2cap_chan *chans[cmd->num]; - struct channel *channel; - int rc; - int i; + const struct l2cap_reconfigure_cmd *cmd = (void *) data; + uint16_t mtu = htole16(cmd->mtu); + struct ble_gap_conn_desc desc; + ble_addr_t *addr = (void *) data; + struct ble_l2cap_chan *chans[cmd->num]; + struct channel *channel; + int rc; + int i; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (mtu == 0 || mtu > TESTER_COC_MTU) { - mtu = TESTER_COC_MTU; - } + if (mtu == 0 || mtu > TESTER_COC_MTU) { + mtu = TESTER_COC_MTU; + } - rc = ble_gap_conn_find_by_addr(addr, &desc); - if (rc) { - SYS_LOG_ERR("GAP conn find failed"); - goto fail; - } + rc = ble_gap_conn_find_by_addr(addr, &desc); + if (rc) { + SYS_LOG_ERR("GAP conn find failed"); + goto fail; + } - for (i = 0; i < cmd->num; ++i) { - channel = get_channel(cmd->idxs[i]); - if (channel == NULL) { - goto fail; - } - chans[i] = channel->chan; - } + for (i = 0; i < cmd->num; ++i) { + channel = get_channel(cmd->idxs[i]); + if (channel == NULL) { + goto fail; + } + chans[i] = channel->chan; + } - rc = ble_l2cap_reconfig(chans, cmd->num, mtu); - if (rc) { - goto fail; - } + rc = ble_l2cap_reconfig(chans, cmd->num, mtu); + if (rc) { + goto fail; + } - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_RECONFIGURE, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); - return; + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_RECONFIGURE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + return; fail: - tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_RECONFIGURE, CONTROLLER_INDEX, - BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_RECONFIGURE, CONTROLLER_INDEX, + BTP_STATUS_FAILED); } -static void supported_commands(uint8_t *data, uint16_t len) +static void +supported_commands(uint8_t *data, uint16_t len) { - uint8_t cmds[1]; - struct l2cap_read_supported_commands_rp *rp = (void *) cmds; + uint8_t cmds[1]; + struct l2cap_read_supported_commands_rp *rp = (void *) cmds; - memset(cmds, 0, sizeof(cmds)); + memset(cmds, 0, sizeof(cmds)); - tester_set_bit(cmds, L2CAP_READ_SUPPORTED_COMMANDS); - tester_set_bit(cmds, L2CAP_CONNECT); - tester_set_bit(cmds, L2CAP_DISCONNECT); - tester_set_bit(cmds, L2CAP_LISTEN); - tester_set_bit(cmds, L2CAP_SEND_DATA); - tester_set_bit(cmds, L2CAP_RECONFIGURE); + tester_set_bit(cmds, L2CAP_READ_SUPPORTED_COMMANDS); + tester_set_bit(cmds, L2CAP_CONNECT); + tester_set_bit(cmds, L2CAP_DISCONNECT); + tester_set_bit(cmds, L2CAP_LISTEN); + tester_set_bit(cmds, L2CAP_SEND_DATA); + tester_set_bit(cmds, L2CAP_RECONFIGURE); - tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_READ_SUPPORTED_COMMANDS, - CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); } -void tester_handle_l2cap(uint8_t opcode, uint8_t index, uint8_t *data, - uint16_t len) +void +tester_handle_l2cap(uint8_t opcode, uint8_t index, uint8_t *data, + uint16_t len) { - switch (opcode) { - case L2CAP_READ_SUPPORTED_COMMANDS: - supported_commands(data, len); - return; - case L2CAP_CONNECT: - connect(data, len); - return; - case L2CAP_DISCONNECT: - disconnect(data, len); - return; - case L2CAP_SEND_DATA: - send_data(data, len); - return; - case L2CAP_LISTEN: - listen(data, len); - return; - case L2CAP_RECONFIGURE: - reconfigure(data, len); - return; - case L2CAP_CREDITS: - credits(data, len); - return; - default: - tester_rsp(BTP_SERVICE_ID_L2CAP, opcode, index, - BTP_STATUS_UNKNOWN_CMD); - return; - } + switch (opcode) { + case L2CAP_READ_SUPPORTED_COMMANDS: + supported_commands(data, len); + return; + case L2CAP_CONNECT: + connect(data, len); + return; + case L2CAP_DISCONNECT: + disconnect(data, len); + return; + case L2CAP_SEND_DATA: + send_data(data, len); + return; + case L2CAP_LISTEN: + listen(data, len); + return; + case L2CAP_RECONFIGURE: + reconfigure(data, len); + return; + case L2CAP_CREDITS: + credits(data, len); + return; + default: + tester_rsp(BTP_SERVICE_ID_L2CAP, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } } -uint8_t tester_init_l2cap(void) +uint8_t +tester_init_l2cap(void) { - int rc; + int rc; - /* For testing we want to support all the available channels */ - rc = os_mempool_init(&sdu_coc_mbuf_mempool, TESTER_COC_BUF_COUNT, - TESTER_COC_MTU, tester_sdu_coc_mem, - "tester_coc_sdu_pool"); - assert(rc == 0); + /* For testing we want to support all the available channels */ + rc = os_mempool_init(&sdu_coc_mbuf_mempool, TESTER_COC_BUF_COUNT, + TESTER_COC_MTU, tester_sdu_coc_mem, + "tester_coc_sdu_pool"); + assert(rc == 0); - rc = os_mbuf_pool_init(&sdu_os_mbuf_pool, &sdu_coc_mbuf_mempool, - TESTER_COC_MTU, TESTER_COC_BUF_COUNT); - assert(rc == 0); + rc = os_mbuf_pool_init(&sdu_os_mbuf_pool, &sdu_coc_mbuf_mempool, + TESTER_COC_MTU, TESTER_COC_BUF_COUNT); + assert(rc == 0); - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } -uint8_t tester_unregister_l2cap(void) +uint8_t +tester_unregister_l2cap(void) { - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } #endif diff --git a/apps/bttester/src/main.c b/apps/bttester/src/main.c index ea130805c..8cc48661a 100644 --- a/apps/bttester/src/main.c +++ b/apps/bttester/src/main.c @@ -33,40 +33,43 @@ #include "bttester.h" -static void on_reset(int reason) +static void +on_reset(int reason) { - MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason); + MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason); } -static void on_sync(void) +static void +on_sync(void) { - MODLOG_DFLT(INFO, "Bluetooth initialized\n"); + MODLOG_DFLT(INFO, "Bluetooth initialized\n"); - tester_init(); + tester_init(); } -int main(int argc, char **argv) +int +main(int argc, char **argv) { - int rc; + int rc; #ifdef ARCH_sim - mcu_sim_parse_args(argc, argv); + mcu_sim_parse_args(argc, argv); #endif - /* Initialize OS */ - sysinit(); + /* Initialize OS */ + sysinit(); - /* Initialize the NimBLE host configuration. */ - ble_hs_cfg.reset_cb = on_reset; - ble_hs_cfg.sync_cb = on_sync; - ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb, - ble_hs_cfg.store_status_cb = ble_store_util_status_rr; + /* Initialize the NimBLE host configuration. */ + ble_hs_cfg.reset_cb = on_reset; + ble_hs_cfg.sync_cb = on_sync; + ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb, + ble_hs_cfg.store_status_cb = ble_store_util_status_rr; - rc = gatt_svr_init(); - assert(rc == 0); + rc = gatt_svr_init(); + assert(rc == 0); - while (1) { - os_eventq_run(os_eventq_dflt_get()); - } - return 0; + while (1) { + os_eventq_run(os_eventq_dflt_get()); + } + return 0; } diff --git a/apps/bttester/src/mesh.c b/apps/bttester/src/mesh.c index 46bf7699c..26093f755 100644 --- a/apps/bttester/src/mesh.c +++ b/apps/bttester/src/mesh.c @@ -70,132 +70,138 @@ static uint8_t static_auth[16]; #define MODEL_BOUNDS_MAX 2 static struct model_data { - struct bt_mesh_model *model; - uint16_t addr; - uint16_t appkey_idx; + struct bt_mesh_model *model; + uint16_t addr; + uint16_t appkey_idx; } model_bound[MODEL_BOUNDS_MAX]; static struct { - uint16_t local; - uint16_t dst; - uint16_t net_idx; + uint16_t local; + uint16_t dst; + uint16_t net_idx; } net = { - .local = BT_MESH_ADDR_UNASSIGNED, - .dst = BT_MESH_ADDR_UNASSIGNED, + .local = BT_MESH_ADDR_UNASSIGNED, + .dst = BT_MESH_ADDR_UNASSIGNED, }; -static void supported_commands(uint8_t *data, uint16_t len) +static void +supported_commands(uint8_t *data, uint16_t len) { - struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE); + struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE); - net_buf_simple_init(buf, 0); + net_buf_simple_init(buf, 0); - /* 1st octet */ - memset(net_buf_simple_add(buf, 1), 0, 1); - tester_set_bit(buf->om_data, MESH_READ_SUPPORTED_COMMANDS); - tester_set_bit(buf->om_data, MESH_CONFIG_PROVISIONING); - tester_set_bit(buf->om_data, MESH_PROVISION_NODE); - tester_set_bit(buf->om_data, MESH_INIT); - tester_set_bit(buf->om_data, MESH_RESET); - tester_set_bit(buf->om_data, MESH_INPUT_NUMBER); - tester_set_bit(buf->om_data, MESH_INPUT_STRING); - /* 2nd octet */ - tester_set_bit(buf->om_data, MESH_IVU_TEST_MODE); - tester_set_bit(buf->om_data, MESH_IVU_TOGGLE_STATE); - tester_set_bit(buf->om_data, MESH_NET_SEND); - tester_set_bit(buf->om_data, MESH_HEALTH_GENERATE_FAULTS); - tester_set_bit(buf->om_data, MESH_HEALTH_CLEAR_FAULTS); - tester_set_bit(buf->om_data, MESH_LPN); - tester_set_bit(buf->om_data, MESH_LPN_POLL); - tester_set_bit(buf->om_data, MESH_MODEL_SEND); - /* 3rd octet */ - memset(net_buf_simple_add(buf, 1), 0, 1); + /* 1st octet */ + memset(net_buf_simple_add(buf, 1), 0, 1); + tester_set_bit(buf->om_data, MESH_READ_SUPPORTED_COMMANDS); + tester_set_bit(buf->om_data, MESH_CONFIG_PROVISIONING); + tester_set_bit(buf->om_data, MESH_PROVISION_NODE); + tester_set_bit(buf->om_data, MESH_INIT); + tester_set_bit(buf->om_data, MESH_RESET); + tester_set_bit(buf->om_data, MESH_INPUT_NUMBER); + tester_set_bit(buf->om_data, MESH_INPUT_STRING); + /* 2nd octet */ + tester_set_bit(buf->om_data, MESH_IVU_TEST_MODE); + tester_set_bit(buf->om_data, MESH_IVU_TOGGLE_STATE); + tester_set_bit(buf->om_data, MESH_NET_SEND); + tester_set_bit(buf->om_data, MESH_HEALTH_GENERATE_FAULTS); + tester_set_bit(buf->om_data, MESH_HEALTH_CLEAR_FAULTS); + tester_set_bit(buf->om_data, MESH_LPN); + tester_set_bit(buf->om_data, MESH_LPN_POLL); + tester_set_bit(buf->om_data, MESH_MODEL_SEND); + /* 3rd octet */ + memset(net_buf_simple_add(buf, 1), 0, 1); #if MYNEWT_VAL(BLE_MESH_TESTING) - tester_set_bit(buf->om_data, MESH_LPN_SUBSCRIBE); - tester_set_bit(buf->om_data, MESH_LPN_UNSUBSCRIBE); - tester_set_bit(buf->om_data, MESH_RPL_CLEAR); + tester_set_bit(buf->om_data, MESH_LPN_SUBSCRIBE); + tester_set_bit(buf->om_data, MESH_LPN_UNSUBSCRIBE); + tester_set_bit(buf->om_data, MESH_RPL_CLEAR); #endif /* CONFIG_BT_TESTING */ - tester_set_bit(buf->om_data, MESH_PROXY_IDENTITY); + tester_set_bit(buf->om_data, MESH_PROXY_IDENTITY); - tester_send_buf(BTP_SERVICE_ID_MESH, MESH_READ_SUPPORTED_COMMANDS, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, buf); } -static void get_faults(uint8_t *faults, uint8_t faults_size, uint8_t *dst, uint8_t *count) +static void +get_faults(uint8_t *faults, uint8_t faults_size, uint8_t *dst, uint8_t *count) { - uint8_t i, limit = *count; + uint8_t i, limit = *count; - for (i = 0, *count = 0; i < faults_size && *count < limit; i++) { - if (faults[i]) { - *dst++ = faults[i]; - (*count)++; - } - } + for (i = 0, *count = 0; i < faults_size && *count < limit; i++) { + if (faults[i]) { + *dst++ = faults[i]; + (*count)++; + } + } } -static int fault_get_cur(struct bt_mesh_model *model, uint8_t *test_id, - uint16_t *company_id, uint8_t *faults, uint8_t *fault_count) +static int +fault_get_cur(struct bt_mesh_model *model, uint8_t *test_id, + uint16_t *company_id, uint8_t *faults, uint8_t *fault_count) { - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - *test_id = HEALTH_TEST_ID; - *company_id = CID_LOCAL; + *test_id = HEALTH_TEST_ID; + *company_id = CID_LOCAL; - get_faults(cur_faults, sizeof(cur_faults), faults, fault_count); + get_faults(cur_faults, sizeof(cur_faults), faults, fault_count); - return 0; + return 0; } -static int fault_get_reg(struct bt_mesh_model *model, uint16_t company_id, - uint8_t *test_id, uint8_t *faults, uint8_t *fault_count) +static int +fault_get_reg(struct bt_mesh_model *model, uint16_t company_id, + uint8_t *test_id, uint8_t *faults, uint8_t *fault_count) { - SYS_LOG_DBG("company_id 0x%04x", company_id); + SYS_LOG_DBG("company_id 0x%04x", company_id); - if (company_id != CID_LOCAL) { - return -EINVAL; - } + if (company_id != CID_LOCAL) { + return -EINVAL; + } - *test_id = HEALTH_TEST_ID; + *test_id = HEALTH_TEST_ID; - get_faults(reg_faults, sizeof(reg_faults), faults, fault_count); + get_faults(reg_faults, sizeof(reg_faults), faults, fault_count); - return 0; + return 0; } -static int fault_clear(struct bt_mesh_model *model, uint16_t company_id) +static int +fault_clear(struct bt_mesh_model *model, uint16_t company_id) { - SYS_LOG_DBG("company_id 0x%04x", company_id); + SYS_LOG_DBG("company_id 0x%04x", company_id); - if (company_id != CID_LOCAL) { - return -EINVAL; - } + if (company_id != CID_LOCAL) { + return -EINVAL; + } - memset(reg_faults, 0, sizeof(reg_faults)); + memset(reg_faults, 0, sizeof(reg_faults)); - return 0; + return 0; } -static int fault_test(struct bt_mesh_model *model, uint8_t test_id, - uint16_t company_id) +static int +fault_test(struct bt_mesh_model *model, uint8_t test_id, + uint16_t company_id) { - SYS_LOG_DBG("test_id 0x%02x company_id 0x%04x", test_id, company_id); + SYS_LOG_DBG("test_id 0x%02x company_id 0x%04x", test_id, company_id); - if (company_id != CID_LOCAL || test_id != HEALTH_TEST_ID) { - return -EINVAL; - } + if (company_id != CID_LOCAL || test_id != HEALTH_TEST_ID) { + return -EINVAL; + } - return 0; + return 0; } static const struct bt_mesh_health_srv_cb health_srv_cb = { - .fault_get_cur = fault_get_cur, - .fault_get_reg = fault_get_reg, - .fault_clear = fault_clear, - .fault_test = fault_test, + .fault_get_cur = fault_get_cur, + .fault_get_reg = fault_get_reg, + .fault_clear = fault_clear, + .fault_test = fault_test, }; static struct bt_mesh_health_srv health_srv = { - .cb = &health_srv_cb, + .cb = &health_srv_cb, }; static struct bt_mesh_model_pub health_pub; @@ -203,791 +209,836 @@ static struct bt_mesh_model_pub health_pub; static void health_pub_init(void) { - health_pub.msg = BT_MESH_HEALTH_FAULT_MSG(CUR_FAULTS_MAX); + health_pub.msg = BT_MESH_HEALTH_FAULT_MSG(CUR_FAULTS_MAX); } static struct bt_mesh_cfg_cli cfg_cli = { }; -void show_faults(uint8_t test_id, uint16_t cid, uint8_t *faults, size_t fault_count) +void +show_faults(uint8_t test_id, uint16_t cid, uint8_t *faults, size_t fault_count) { - size_t i; + size_t i; - if (!fault_count) { - SYS_LOG_DBG("Health Test ID 0x%02x Company ID 0x%04x: " - "no faults", test_id, cid); - return; - } + if (!fault_count) { + SYS_LOG_DBG("Health Test ID 0x%02x Company ID 0x%04x: " + "no faults", test_id, cid); + return; + } - SYS_LOG_DBG("Health Test ID 0x%02x Company ID 0x%04x Fault Count %zu: ", - test_id, cid, fault_count); + SYS_LOG_DBG("Health Test ID 0x%02x Company ID 0x%04x Fault Count %zu: ", + test_id, cid, fault_count); - for (i = 0; i < fault_count; i++) { - SYS_LOG_DBG("0x%02x", faults[i]); - } + for (i = 0; i < fault_count; i++) { + SYS_LOG_DBG("0x%02x", faults[i]); + } } -static void health_current_status(struct bt_mesh_health_cli *cli, uint16_t addr, - uint8_t test_id, uint16_t cid, uint8_t *faults, - size_t fault_count) +static void +health_current_status(struct bt_mesh_health_cli *cli, uint16_t addr, + uint8_t test_id, uint16_t cid, uint8_t *faults, + size_t fault_count) { - SYS_LOG_DBG("Health Current Status from 0x%04x", addr); - show_faults(test_id, cid, faults, fault_count); + SYS_LOG_DBG("Health Current Status from 0x%04x", addr); + show_faults(test_id, cid, faults, fault_count); } static struct bt_mesh_health_cli health_cli = { - .current_status = health_current_status, + .current_status = health_current_status, }; static struct bt_mesh_model root_models[] = { - BT_MESH_MODEL_CFG_SRV, - BT_MESH_MODEL_CFG_CLI(&cfg_cli), - BT_MESH_MODEL_HEALTH_SRV(&health_srv, &health_pub), - BT_MESH_MODEL_HEALTH_CLI(&health_cli), + BT_MESH_MODEL_CFG_SRV, + BT_MESH_MODEL_CFG_CLI(&cfg_cli), + BT_MESH_MODEL_HEALTH_SRV(&health_srv, &health_pub), + BT_MESH_MODEL_HEALTH_CLI(&health_cli), }; static struct bt_mesh_model vnd_models[] = { - BT_MESH_MODEL_VND(CID_LOCAL, VND_MODEL_ID_1, BT_MESH_MODEL_NO_OPS, NULL, - NULL), + BT_MESH_MODEL_VND(CID_LOCAL, VND_MODEL_ID_1, BT_MESH_MODEL_NO_OPS, NULL, + NULL), }; static struct bt_mesh_elem elements[] = { - BT_MESH_ELEM(0, root_models, vnd_models), + BT_MESH_ELEM(0, root_models, vnd_models), }; -static void link_open(bt_mesh_prov_bearer_t bearer) +static void +link_open(bt_mesh_prov_bearer_t bearer) { - struct mesh_prov_link_open_ev ev; + struct mesh_prov_link_open_ev ev; - SYS_LOG_DBG("bearer 0x%02x", bearer); + SYS_LOG_DBG("bearer 0x%02x", bearer); - switch (bearer) { - case BT_MESH_PROV_ADV: - ev.bearer = MESH_PROV_BEARER_PB_ADV; - break; - case BT_MESH_PROV_GATT: - ev.bearer = MESH_PROV_BEARER_PB_GATT; - break; - default: - SYS_LOG_ERR("Invalid bearer"); + switch (bearer) { + case BT_MESH_PROV_ADV: + ev.bearer = MESH_PROV_BEARER_PB_ADV; + break; + case BT_MESH_PROV_GATT: + ev.bearer = MESH_PROV_BEARER_PB_GATT; + break; + default: + SYS_LOG_ERR("Invalid bearer"); - return; - } + return; + } - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_OPEN, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_OPEN, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void link_close(bt_mesh_prov_bearer_t bearer) +static void +link_close(bt_mesh_prov_bearer_t bearer) { - struct mesh_prov_link_closed_ev ev; + struct mesh_prov_link_closed_ev ev; - SYS_LOG_DBG("bearer 0x%02x", bearer); + SYS_LOG_DBG("bearer 0x%02x", bearer); - switch (bearer) { - case BT_MESH_PROV_ADV: - ev.bearer = MESH_PROV_BEARER_PB_ADV; - break; - case BT_MESH_PROV_GATT: - ev.bearer = MESH_PROV_BEARER_PB_GATT; - break; - default: - SYS_LOG_ERR("Invalid bearer"); + switch (bearer) { + case BT_MESH_PROV_ADV: + ev.bearer = MESH_PROV_BEARER_PB_ADV; + break; + case BT_MESH_PROV_GATT: + ev.bearer = MESH_PROV_BEARER_PB_GATT; + break; + default: + SYS_LOG_ERR("Invalid bearer"); - return; - } + return; + } - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_CLOSED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_CLOSED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static int output_number(bt_mesh_output_action_t action, uint32_t number) +static int +output_number(bt_mesh_output_action_t action, uint32_t number) { - struct mesh_out_number_action_ev ev; + struct mesh_out_number_action_ev ev; - SYS_LOG_DBG("action 0x%04x number 0x%08lx", action, number); + SYS_LOG_DBG("action 0x%04x number 0x%08lx", action, number); - ev.action = sys_cpu_to_le16(action); - ev.number = sys_cpu_to_le32(number); + ev.action = sys_cpu_to_le16(action); + ev.number = sys_cpu_to_le32(number); - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_OUT_NUMBER_ACTION, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_OUT_NUMBER_ACTION, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); - return 0; + return 0; } -static int output_string(const char *str) +static int +output_string(const char *str) { - struct mesh_out_string_action_ev *ev; - struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE); + struct mesh_out_string_action_ev *ev; + struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE); - SYS_LOG_DBG("str %s", str); + SYS_LOG_DBG("str %s", str); - net_buf_simple_init(buf, 0); + net_buf_simple_init(buf, 0); - ev = net_buf_simple_add(buf, sizeof(*ev)); - ev->string_len = strlen(str); + ev = net_buf_simple_add(buf, sizeof(*ev)); + ev->string_len = strlen(str); - net_buf_simple_add_mem(buf, str, ev->string_len); + net_buf_simple_add_mem(buf, str, ev->string_len); - tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_OUT_STRING_ACTION, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_OUT_STRING_ACTION, + CONTROLLER_INDEX, buf); - os_mbuf_free_chain(buf); - return 0; + os_mbuf_free_chain(buf); + return 0; } -static int input(bt_mesh_input_action_t action, uint8_t size) +static int +input(bt_mesh_input_action_t action, uint8_t size) { - struct mesh_in_action_ev ev; + struct mesh_in_action_ev ev; - SYS_LOG_DBG("action 0x%04x number 0x%02x", action, size); + SYS_LOG_DBG("action 0x%04x number 0x%02x", action, size); - input_size = size; + input_size = size; - ev.action = sys_cpu_to_le16(action); - ev.size = size; + ev.action = sys_cpu_to_le16(action); + ev.size = size; - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_IN_ACTION, CONTROLLER_INDEX, - (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_IN_ACTION, CONTROLLER_INDEX, + (uint8_t *) &ev, sizeof(ev)); - return 0; + return 0; } static uint8_t vnd_app_key[16]; static uint16_t vnd_app_key_idx = 0x000f; -static void prov_complete(uint16_t net_idx, uint16_t addr) +static void +prov_complete(uint16_t net_idx, uint16_t addr) { - SYS_LOG_DBG("net_idx 0x%04x addr 0x%04x", net_idx, addr); + SYS_LOG_DBG("net_idx 0x%04x addr 0x%04x", net_idx, addr); - net.net_idx = net_idx, - net.local = addr; - net.dst = addr; + net.net_idx = net_idx, + net.local = addr; + net.dst = addr; - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROVISIONED, CONTROLLER_INDEX, - NULL, 0); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROVISIONED, CONTROLLER_INDEX, + NULL, 0); } -static void prov_reset(void) +static void +prov_reset(void) { - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT); + bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT); } static const struct bt_mesh_comp comp = { - .cid = CID_LOCAL, - .elem = elements, - .elem_count = ARRAY_SIZE(elements), + .cid = CID_LOCAL, + .elem = elements, + .elem_count = ARRAY_SIZE(elements), }; static struct bt_mesh_prov prov = { - .uuid = dev_uuid, - .static_val = static_auth, - .static_val_len = sizeof(static_auth), - .output_number = output_number, - .output_string = output_string, - .input = input, - .link_open = link_open, - .link_close = link_close, - .complete = prov_complete, - .reset = prov_reset, + .uuid = dev_uuid, + .static_val = static_auth, + .static_val_len = sizeof(static_auth), + .output_number = output_number, + .output_string = output_string, + .input = input, + .link_open = link_open, + .link_close = link_close, + .complete = prov_complete, + .reset = prov_reset, }; -static void config_prov(uint8_t *data, uint16_t len) +static void +config_prov(uint8_t *data, uint16_t len) { - const struct mesh_config_provisioning_cmd *cmd = (void *) data; + const struct mesh_config_provisioning_cmd *cmd = (void *) data; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memcpy(dev_uuid, cmd->uuid, sizeof(dev_uuid)); - memcpy(static_auth, cmd->static_auth, sizeof(static_auth)); + memcpy(dev_uuid, cmd->uuid, sizeof(dev_uuid)); + memcpy(static_auth, cmd->static_auth, sizeof(static_auth)); - prov.output_size = cmd->out_size; - prov.output_actions = sys_le16_to_cpu(cmd->out_actions); - prov.input_size = cmd->in_size; - prov.input_actions = sys_le16_to_cpu(cmd->in_actions); + prov.output_size = cmd->out_size; + prov.output_actions = sys_le16_to_cpu(cmd->out_actions); + prov.input_size = cmd->in_size; + prov.input_actions = sys_le16_to_cpu(cmd->in_actions); - tester_rsp(BTP_SERVICE_ID_MESH, MESH_CONFIG_PROVISIONING, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_CONFIG_PROVISIONING, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); } -static void provision_node(uint8_t *data, uint16_t len) +static void +provision_node(uint8_t *data, uint16_t len) { - const struct mesh_provision_node_cmd *cmd = (void *) data; + const struct mesh_provision_node_cmd *cmd = (void *) data; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memcpy(dev_key, cmd->dev_key, sizeof(dev_key)); - memcpy(net_key, cmd->net_key, sizeof(net_key)); + memcpy(dev_key, cmd->dev_key, sizeof(dev_key)); + memcpy(net_key, cmd->net_key, sizeof(net_key)); - addr = sys_le16_to_cpu(cmd->addr); - flags = cmd->flags; - iv_index = sys_le32_to_cpu(cmd->iv_index); - net_key_idx = sys_le16_to_cpu(cmd->net_key_idx); + addr = sys_le16_to_cpu(cmd->addr); + flags = cmd->flags; + iv_index = sys_le32_to_cpu(cmd->iv_index); + net_key_idx = sys_le16_to_cpu(cmd->net_key_idx); - tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROVISION_NODE, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROVISION_NODE, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); } -static void init(uint8_t *data, uint16_t len) +static void +init(uint8_t *data, uint16_t len) { - uint8_t status = BTP_STATUS_SUCCESS; - int err; + uint8_t status = BTP_STATUS_SUCCESS; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - err = bt_mesh_init(own_addr_type, &prov, &comp); - if (err) { - status = BTP_STATUS_FAILED; + err = bt_mesh_init(own_addr_type, &prov, &comp); + if (err) { + status = BTP_STATUS_FAILED; - goto rsp; - } + goto rsp; + } - if (addr) { - err = bt_mesh_provision(net_key, net_key_idx, flags, iv_index, - addr, dev_key); - if (err) { - status = BTP_STATUS_FAILED; - } - } else { - err = bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT); - if (err) { - status = BTP_STATUS_FAILED; - } - } + if (addr) { + err = bt_mesh_provision(net_key, net_key_idx, flags, iv_index, + addr, dev_key); + if (err) { + status = BTP_STATUS_FAILED; + } + } else { + err = bt_mesh_prov_enable(BT_MESH_PROV_ADV | BT_MESH_PROV_GATT); + if (err) { + status = BTP_STATUS_FAILED; + } + } rsp: - tester_rsp(BTP_SERVICE_ID_MESH, MESH_INIT, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_INIT, CONTROLLER_INDEX, + status); } -static void reset(uint8_t *data, uint16_t len) +static void +reset(uint8_t *data, uint16_t len) { - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - bt_mesh_reset(); + bt_mesh_reset(); - tester_rsp(BTP_SERVICE_ID_MESH, MESH_RESET, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_RESET, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); } -static void input_number(uint8_t *data, uint16_t len) +static void +input_number(uint8_t *data, uint16_t len) { - const struct mesh_input_number_cmd *cmd = (void *) data; - uint8_t status = BTP_STATUS_SUCCESS; - uint32_t number; - int err; + const struct mesh_input_number_cmd *cmd = (void *) data; + uint8_t status = BTP_STATUS_SUCCESS; + uint32_t number; + int err; - number = sys_le32_to_cpu(cmd->number); + number = sys_le32_to_cpu(cmd->number); - SYS_LOG_DBG("number 0x%04lx", number); + SYS_LOG_DBG("number 0x%04lx", number); - err = bt_mesh_input_number(number); - if (err) { - status = BTP_STATUS_FAILED; - } + err = bt_mesh_input_number(number); + if (err) { + status = BTP_STATUS_FAILED; + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_NUMBER, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_NUMBER, CONTROLLER_INDEX, + status); } -static void input_string(uint8_t *data, uint16_t len) +static void +input_string(uint8_t *data, uint16_t len) { - const struct mesh_input_string_cmd *cmd = (void *) data; - uint8_t status = BTP_STATUS_SUCCESS; - uint8_t str_auth[16]; - int err; + const struct mesh_input_string_cmd *cmd = (void *) data; + uint8_t status = BTP_STATUS_SUCCESS; + uint8_t str_auth[16]; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - if (cmd->string_len > sizeof(str_auth)) { - SYS_LOG_ERR("Too long input (%u chars required)", input_size); - status = BTP_STATUS_FAILED; - goto rsp; - } else if (cmd->string_len < input_size) { - SYS_LOG_ERR("Too short input (%u chars required)", input_size); - status = BTP_STATUS_FAILED; - goto rsp; - } + if (cmd->string_len > sizeof(str_auth)) { + SYS_LOG_ERR("Too long input (%u chars required)", input_size); + status = BTP_STATUS_FAILED; + goto rsp; + } else if (cmd->string_len < input_size) { + SYS_LOG_ERR("Too short input (%u chars required)", input_size); + status = BTP_STATUS_FAILED; + goto rsp; + } - strncpy((char *)str_auth, (char *)cmd->string, cmd->string_len); + strncpy((char *) str_auth, (char *) cmd->string, cmd->string_len); - err = bt_mesh_input_string((char *)str_auth); - if (err) { - status = BTP_STATUS_FAILED; - } + err = bt_mesh_input_string((char *) str_auth); + if (err) { + status = BTP_STATUS_FAILED; + } rsp: - tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_STRING, CONTROLLER_INDEX, - status); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_STRING, CONTROLLER_INDEX, + status); } -static void ivu_test_mode(uint8_t *data, uint16_t len) +static void +ivu_test_mode(uint8_t *data, uint16_t len) { - const struct mesh_ivu_test_mode_cmd *cmd = (void *) data; + const struct mesh_ivu_test_mode_cmd *cmd = (void *) data; - SYS_LOG_DBG("enable 0x%02x", cmd->enable); + SYS_LOG_DBG("enable 0x%02x", cmd->enable); - bt_mesh_iv_update_test(cmd->enable ? true : false); + bt_mesh_iv_update_test(cmd->enable ? true : false); - tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TEST_MODE, CONTROLLER_INDEX, - BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TEST_MODE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); } -static void ivu_toggle_state(uint8_t *data, uint16_t len) +static void +ivu_toggle_state(uint8_t *data, uint16_t len) { - bool result; + bool result; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - result = bt_mesh_iv_update(); - if (!result) { - SYS_LOG_ERR("Failed to toggle the IV Update state"); - } + result = bt_mesh_iv_update(); + if (!result) { + SYS_LOG_ERR("Failed to toggle the IV Update state"); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TOGGLE_STATE, CONTROLLER_INDEX, - result ? BTP_STATUS_SUCCESS : BTP_STATUS_FAILED); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TOGGLE_STATE, CONTROLLER_INDEX, + result ? BTP_STATUS_SUCCESS : BTP_STATUS_FAILED); } -static void lpn(uint8_t *data, uint16_t len) +static void +lpn(uint8_t *data, uint16_t len) { - struct mesh_lpn_set_cmd *cmd = (void *) data; - bool enable; - int err; + struct mesh_lpn_set_cmd *cmd = (void *) data; + bool enable; + int err; - SYS_LOG_DBG("enable 0x%02x", cmd->enable); + SYS_LOG_DBG("enable 0x%02x", cmd->enable); - enable = cmd->enable ? true : false; - err = bt_mesh_lpn_set(enable); - if (err) { - SYS_LOG_ERR("Failed to toggle LPN (err %d)", err); - } + enable = cmd->enable ? true : false; + err = bt_mesh_lpn_set(enable); + if (err) { + SYS_LOG_ERR("Failed to toggle LPN (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); } -static void lpn_poll(uint8_t *data, uint16_t len) +static void +lpn_poll(uint8_t *data, uint16_t len) { - int err; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - err = bt_mesh_lpn_poll(); - if (err) { - SYS_LOG_ERR("Failed to send poll msg (err %d)", err); - } + err = bt_mesh_lpn_poll(); + if (err) { + SYS_LOG_ERR("Failed to send poll msg (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_POLL, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_POLL, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); } -static void net_send(uint8_t *data, uint16_t len) +static void +net_send(uint8_t *data, uint16_t len) { - struct mesh_net_send_cmd *cmd = (void *) data; - struct os_mbuf *msg = NET_BUF_SIMPLE(UINT8_MAX); - struct bt_mesh_msg_ctx ctx = { - .net_idx = net.net_idx, - .app_idx = vnd_app_key_idx, - .addr = sys_le16_to_cpu(cmd->dst), - .send_ttl = cmd->ttl, - }; - int err; + struct mesh_net_send_cmd *cmd = (void *) data; + struct os_mbuf *msg = NET_BUF_SIMPLE(UINT8_MAX); + struct bt_mesh_msg_ctx ctx = { + .net_idx = net.net_idx, + .app_idx = vnd_app_key_idx, + .addr = sys_le16_to_cpu(cmd->dst), + .send_ttl = cmd->ttl, + }; + int err; - SYS_LOG_DBG("ttl 0x%02x dst 0x%04x payload_len %d", ctx.send_ttl, - ctx.addr, cmd->payload_len); + SYS_LOG_DBG("ttl 0x%02x dst 0x%04x payload_len %d", ctx.send_ttl, + ctx.addr, cmd->payload_len); - if (!bt_mesh_app_key_exists(vnd_app_key_idx)) { - (void)bt_mesh_app_key_add(vnd_app_key_idx, net.net_idx, - vnd_app_key); - vnd_models[0].keys[0] = vnd_app_key_idx; - } + if (!bt_mesh_app_key_exists(vnd_app_key_idx)) { + (void) bt_mesh_app_key_add(vnd_app_key_idx, net.net_idx, + vnd_app_key); + vnd_models[0].keys[0] = vnd_app_key_idx; + } - net_buf_simple_add_mem(msg, cmd->payload, cmd->payload_len); + net_buf_simple_add_mem(msg, cmd->payload, cmd->payload_len); - err = bt_mesh_model_send(&vnd_models[0], &ctx, msg, NULL, NULL); - if (err) { - SYS_LOG_ERR("Failed to send (err %d)", err); - } + err = bt_mesh_model_send(&vnd_models[0], &ctx, msg, NULL, NULL); + if (err) { + SYS_LOG_ERR("Failed to send (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_NET_SEND, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_NET_SEND, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); - os_mbuf_free_chain(msg); + os_mbuf_free_chain(msg); } -static void health_generate_faults(uint8_t *data, uint16_t len) +static void +health_generate_faults(uint8_t *data, uint16_t len) { - struct mesh_health_generate_faults_rp *rp; - struct os_mbuf *buf = NET_BUF_SIMPLE(sizeof(*rp) + sizeof(cur_faults) + - sizeof(reg_faults)); - uint8_t some_faults[] = { 0x01, 0x02, 0x03, 0xff, 0x06 }; - uint8_t cur_faults_count, reg_faults_count; + struct mesh_health_generate_faults_rp *rp; + struct os_mbuf *buf = NET_BUF_SIMPLE(sizeof(*rp) + sizeof(cur_faults) + + sizeof(reg_faults)); + uint8_t some_faults[] = {0x01, 0x02, 0x03, 0xff, 0x06}; + uint8_t cur_faults_count, reg_faults_count; - rp = net_buf_simple_add(buf, sizeof(*rp)); + rp = net_buf_simple_add(buf, sizeof(*rp)); - cur_faults_count = min(sizeof(cur_faults), sizeof(some_faults)); - memcpy(cur_faults, some_faults, cur_faults_count); - net_buf_simple_add_mem(buf, cur_faults, cur_faults_count); - rp->cur_faults_count = cur_faults_count; + cur_faults_count = min(sizeof(cur_faults), sizeof(some_faults)); + memcpy(cur_faults, some_faults, cur_faults_count); + net_buf_simple_add_mem(buf, cur_faults, cur_faults_count); + rp->cur_faults_count = cur_faults_count; - reg_faults_count = min(sizeof(reg_faults), sizeof(some_faults)); - memcpy(reg_faults, some_faults, reg_faults_count); - net_buf_simple_add_mem(buf, reg_faults, reg_faults_count); - rp->reg_faults_count = reg_faults_count; + reg_faults_count = min(sizeof(reg_faults), sizeof(some_faults)); + memcpy(reg_faults, some_faults, reg_faults_count); + net_buf_simple_add_mem(buf, reg_faults, reg_faults_count); + rp->reg_faults_count = reg_faults_count; - bt_mesh_fault_update(&elements[0]); + bt_mesh_fault_update(&elements[0]); - tester_send_buf(BTP_SERVICE_ID_MESH, MESH_HEALTH_GENERATE_FAULTS, - CONTROLLER_INDEX, buf); + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_HEALTH_GENERATE_FAULTS, + CONTROLLER_INDEX, buf); } -static void health_clear_faults(uint8_t *data, uint16_t len) +static void +health_clear_faults(uint8_t *data, uint16_t len) { - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - memset(cur_faults, 0, sizeof(cur_faults)); - memset(reg_faults, 0, sizeof(reg_faults)); + memset(cur_faults, 0, sizeof(cur_faults)); + memset(reg_faults, 0, sizeof(reg_faults)); - bt_mesh_fault_update(&elements[0]); + bt_mesh_fault_update(&elements[0]); - tester_rsp(BTP_SERVICE_ID_MESH, MESH_HEALTH_CLEAR_FAULTS, - CONTROLLER_INDEX, BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_HEALTH_CLEAR_FAULTS, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); } -static void model_send(uint8_t *data, uint16_t len) +static void +model_send(uint8_t *data, uint16_t len) { - struct mesh_model_send_cmd *cmd = (void *) data; - struct os_mbuf *msg = NET_BUF_SIMPLE(UINT8_MAX); - struct bt_mesh_msg_ctx ctx = { - .net_idx = net.net_idx, - .app_idx = BT_MESH_KEY_DEV, - .addr = sys_le16_to_cpu(cmd->dst), - .send_ttl = BT_MESH_TTL_DEFAULT, - }; - struct bt_mesh_model *model = NULL; - int err, i; - uint16_t src = sys_le16_to_cpu(cmd->src); + struct mesh_model_send_cmd *cmd = (void *) data; + struct os_mbuf *msg = NET_BUF_SIMPLE(UINT8_MAX); + struct bt_mesh_msg_ctx ctx = { + .net_idx = net.net_idx, + .app_idx = BT_MESH_KEY_DEV, + .addr = sys_le16_to_cpu(cmd->dst), + .send_ttl = BT_MESH_TTL_DEFAULT, + }; + struct bt_mesh_model *model = NULL; + int err, i; + uint16_t src = sys_le16_to_cpu(cmd->src); - /* Lookup source address */ - for (i = 0; i < ARRAY_SIZE(model_bound); i++) { - if (bt_mesh_model_elem(model_bound[i].model)->addr == src) { - model = model_bound[i].model; - ctx.app_idx = model_bound[i].appkey_idx; + /* Lookup source address */ + for (i = 0; i < ARRAY_SIZE(model_bound); i++) { + if (bt_mesh_model_elem(model_bound[i].model)->addr == src) { + model = model_bound[i].model; + ctx.app_idx = model_bound[i].appkey_idx; - break; - } - } + break; + } + } - if (!model) { - SYS_LOG_ERR("Model not found"); - err = -EINVAL; + if (!model) { + SYS_LOG_ERR("Model not found"); + err = -EINVAL; - goto fail; - } + goto fail; + } - SYS_LOG_DBG("src 0x%04x dst 0x%04x model %p payload_len %d", src, - ctx.addr, model, cmd->payload_len); + SYS_LOG_DBG("src 0x%04x dst 0x%04x model %p payload_len %d", src, + ctx.addr, model, cmd->payload_len); - net_buf_simple_add_mem(msg, cmd->payload, cmd->payload_len); + net_buf_simple_add_mem(msg, cmd->payload, cmd->payload_len); - err = bt_mesh_model_send(model, &ctx, msg, NULL, NULL); - if (err) { - SYS_LOG_ERR("Failed to send (err %d)", err); - } + err = bt_mesh_model_send(model, &ctx, msg, NULL, NULL); + if (err) { + SYS_LOG_ERR("Failed to send (err %d)", err); + } fail: - tester_rsp(BTP_SERVICE_ID_MESH, MESH_MODEL_SEND, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_MODEL_SEND, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); - os_mbuf_free_chain(msg); + os_mbuf_free_chain(msg); } #if MYNEWT_VAL(BLE_MESH_TESTING) -static void lpn_subscribe(uint8_t *data, uint16_t len) + +static void +lpn_subscribe(uint8_t *data, uint16_t len) { - struct mesh_lpn_subscribe_cmd *cmd = (void *) data; - uint16_t address = sys_le16_to_cpu(cmd->address); - int err; + struct mesh_lpn_subscribe_cmd *cmd = (void *) data; + uint16_t address = sys_le16_to_cpu(cmd->address); + int err; - SYS_LOG_DBG("address 0x%04x", address); + SYS_LOG_DBG("address 0x%04x", address); - err = bt_test_mesh_lpn_group_add(address); - if (err) { - SYS_LOG_ERR("Failed to subscribe (err %d)", err); - } + err = bt_test_mesh_lpn_group_add(address); + if (err) { + SYS_LOG_ERR("Failed to subscribe (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_SUBSCRIBE, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_SUBSCRIBE, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); } -static void lpn_unsubscribe(uint8_t *data, uint16_t len) +static void +lpn_unsubscribe(uint8_t *data, uint16_t len) { - struct mesh_lpn_unsubscribe_cmd *cmd = (void *) data; - uint16_t address = sys_le16_to_cpu(cmd->address); - int err; + struct mesh_lpn_unsubscribe_cmd *cmd = (void *) data; + uint16_t address = sys_le16_to_cpu(cmd->address); + int err; - SYS_LOG_DBG("address 0x%04x", address); + SYS_LOG_DBG("address 0x%04x", address); - err = bt_test_mesh_lpn_group_remove(&address, 1); - if (err) { - SYS_LOG_ERR("Failed to unsubscribe (err %d)", err); - } + err = bt_test_mesh_lpn_group_remove(&address, 1); + if (err) { + SYS_LOG_ERR("Failed to unsubscribe (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_UNSUBSCRIBE, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_UNSUBSCRIBE, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); } -static void rpl_clear(uint8_t *data, uint16_t len) +static void +rpl_clear(uint8_t *data, uint16_t len) { - int err; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - err = bt_test_mesh_rpl_clear(); - if (err) { - SYS_LOG_ERR("Failed to clear RPL (err %d)", err); - } + err = bt_test_mesh_rpl_clear(); + if (err) { + SYS_LOG_ERR("Failed to clear RPL (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_RPL_CLEAR, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_RPL_CLEAR, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); } + #endif /* MYNEWT_VAL(BLE_MESH_TESTING) */ -static void proxy_identity_enable(uint8_t *data, uint16_t len) +static void +proxy_identity_enable(uint8_t *data, uint16_t len) { - int err; + int err; - SYS_LOG_DBG(""); + SYS_LOG_DBG(""); - err = bt_mesh_proxy_identity_enable(); - if (err) { - SYS_LOG_ERR("Failed to enable proxy identity (err %d)", err); - } + err = bt_mesh_proxy_identity_enable(); + if (err) { + SYS_LOG_ERR("Failed to enable proxy identity (err %d)", err); + } - tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROXY_IDENTITY, CONTROLLER_INDEX, - err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROXY_IDENTITY, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); } -void tester_handle_mesh(uint8_t opcode, uint8_t index, uint8_t *data, uint16_t len) +void +tester_handle_mesh(uint8_t opcode, uint8_t index, uint8_t *data, uint16_t len) { - switch (opcode) { - case MESH_READ_SUPPORTED_COMMANDS: - supported_commands(data, len); - break; - case MESH_CONFIG_PROVISIONING: - config_prov(data, len); - break; - case MESH_PROVISION_NODE: - provision_node(data, len); - break; - case MESH_INIT: - init(data, len); - break; - case MESH_RESET: - reset(data, len); - break; - case MESH_INPUT_NUMBER: - input_number(data, len); - break; - case MESH_INPUT_STRING: - input_string(data, len); - break; - case MESH_IVU_TEST_MODE: - ivu_test_mode(data, len); - break; - case MESH_IVU_TOGGLE_STATE: - ivu_toggle_state(data, len); - break; - case MESH_LPN: - lpn(data, len); - break; - case MESH_LPN_POLL: - lpn_poll(data, len); - break; - case MESH_NET_SEND: - net_send(data, len); - break; - case MESH_HEALTH_GENERATE_FAULTS: - health_generate_faults(data, len); - break; - case MESH_HEALTH_CLEAR_FAULTS: - health_clear_faults(data, len); - break; - case MESH_MODEL_SEND: - model_send(data, len); - break; + switch (opcode) { + case MESH_READ_SUPPORTED_COMMANDS: + supported_commands(data, len); + break; + case MESH_CONFIG_PROVISIONING: + config_prov(data, len); + break; + case MESH_PROVISION_NODE: + provision_node(data, len); + break; + case MESH_INIT: + init(data, len); + break; + case MESH_RESET: + reset(data, len); + break; + case MESH_INPUT_NUMBER: + input_number(data, len); + break; + case MESH_INPUT_STRING: + input_string(data, len); + break; + case MESH_IVU_TEST_MODE: + ivu_test_mode(data, len); + break; + case MESH_IVU_TOGGLE_STATE: + ivu_toggle_state(data, len); + break; + case MESH_LPN: + lpn(data, len); + break; + case MESH_LPN_POLL: + lpn_poll(data, len); + break; + case MESH_NET_SEND: + net_send(data, len); + break; + case MESH_HEALTH_GENERATE_FAULTS: + health_generate_faults(data, len); + break; + case MESH_HEALTH_CLEAR_FAULTS: + health_clear_faults(data, len); + break; + case MESH_MODEL_SEND: + model_send(data, len); + break; #if MYNEWT_VAL(BLE_MESH_TESTING) - case MESH_LPN_SUBSCRIBE: - lpn_subscribe(data, len); - break; - case MESH_LPN_UNSUBSCRIBE: - lpn_unsubscribe(data, len); - break; - case MESH_RPL_CLEAR: - rpl_clear(data, len); - break; + case MESH_LPN_SUBSCRIBE: + lpn_subscribe(data, len); + break; + case MESH_LPN_UNSUBSCRIBE: + lpn_unsubscribe(data, len); + break; + case MESH_RPL_CLEAR: + rpl_clear(data, len); + break; #endif /* MYNEWT_VAL(BLE_MESH_TESTING) */ - case MESH_PROXY_IDENTITY: - proxy_identity_enable(data, len); - break; - default: - tester_rsp(BTP_SERVICE_ID_MESH, opcode, index, - BTP_STATUS_UNKNOWN_CMD); - break; - } + case MESH_PROXY_IDENTITY: + proxy_identity_enable(data, len); + break; + default: + tester_rsp(BTP_SERVICE_ID_MESH, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + break; + } } -void net_recv_ev(uint8_t ttl, uint8_t ctl, uint16_t src, uint16_t dst, const void *payload, - size_t payload_len) +void +net_recv_ev(uint8_t ttl, + uint8_t ctl, + uint16_t src, + uint16_t dst, + const void *payload, + size_t payload_len) { - struct os_mbuf *buf = NET_BUF_SIMPLE(UINT8_MAX); - struct mesh_net_recv_ev *ev; + struct os_mbuf *buf = NET_BUF_SIMPLE(UINT8_MAX); + struct mesh_net_recv_ev *ev; - SYS_LOG_DBG("ttl 0x%02x ctl 0x%02x src 0x%04x dst 0x%04x " - "payload_len %d", ttl, ctl, src, dst, payload_len); + SYS_LOG_DBG("ttl 0x%02x ctl 0x%02x src 0x%04x dst 0x%04x " + "payload_len %d", ttl, ctl, src, dst, payload_len); - if (payload_len > net_buf_simple_tailroom(buf)) { - SYS_LOG_ERR("Payload size exceeds buffer size"); + if (payload_len > net_buf_simple_tailroom(buf)) { + SYS_LOG_ERR("Payload size exceeds buffer size"); - goto done; - } + goto done; + } - ev = net_buf_simple_add(buf, sizeof(*ev)); - ev->ttl = ttl; - ev->ctl = ctl; - ev->src = sys_cpu_to_le16(src); - ev->dst = sys_cpu_to_le16(dst); - ev->payload_len = payload_len; - net_buf_simple_add_mem(buf, payload, payload_len); + ev = net_buf_simple_add(buf, sizeof(*ev)); + ev->ttl = ttl; + ev->ctl = ctl; + ev->src = sys_cpu_to_le16(src); + ev->dst = sys_cpu_to_le16(dst); + ev->payload_len = payload_len; + net_buf_simple_add_mem(buf, payload, payload_len); - tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_NET_RECV, CONTROLLER_INDEX, - buf); + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_NET_RECV, CONTROLLER_INDEX, + buf); done: - os_mbuf_free_chain(buf); + os_mbuf_free_chain(buf); } -static void model_bound_cb(uint16_t addr, struct bt_mesh_model *model, - uint16_t key_idx) +static void +model_bound_cb(uint16_t addr, struct bt_mesh_model *model, + uint16_t key_idx) { - int i; + int i; - SYS_LOG_DBG("remote addr 0x%04x key_idx 0x%04x model %p", - addr, key_idx, model); + SYS_LOG_DBG("remote addr 0x%04x key_idx 0x%04x model %p", + addr, key_idx, model); - for (i = 0; i < ARRAY_SIZE(model_bound); i++) { - if (!model_bound[i].model) { - model_bound[i].model = model; - model_bound[i].addr = addr; - model_bound[i].appkey_idx = key_idx; + for (i = 0; i < ARRAY_SIZE(model_bound); i++) { + if (!model_bound[i].model) { + model_bound[i].model = model; + model_bound[i].addr = addr; + model_bound[i].appkey_idx = key_idx; - return; - } - } + return; + } + } - SYS_LOG_ERR("model_bound is full"); + SYS_LOG_ERR("model_bound is full"); } -static void model_unbound_cb(uint16_t addr, struct bt_mesh_model *model, - uint16_t key_idx) +static void +model_unbound_cb(uint16_t addr, struct bt_mesh_model *model, + uint16_t key_idx) { - int i; + int i; - SYS_LOG_DBG("remote addr 0x%04x key_idx 0x%04x model %p", - addr, key_idx, model); + SYS_LOG_DBG("remote addr 0x%04x key_idx 0x%04x model %p", + addr, key_idx, model); - for (i = 0; i < ARRAY_SIZE(model_bound); i++) { - if (model_bound[i].model == model) { - model_bound[i].model = NULL; - model_bound[i].addr = 0x0000; - model_bound[i].appkey_idx = BT_MESH_KEY_UNUSED; + for (i = 0; i < ARRAY_SIZE(model_bound); i++) { + if (model_bound[i].model == model) { + model_bound[i].model = NULL; + model_bound[i].addr = 0x0000; + model_bound[i].appkey_idx = BT_MESH_KEY_UNUSED; - return; - } - } + return; + } + } - SYS_LOG_INF("model not found"); + SYS_LOG_INF("model not found"); } -static void invalid_bearer_cb(uint8_t opcode) +static void +invalid_bearer_cb(uint8_t opcode) { - struct mesh_invalid_bearer_ev ev = { - .opcode = opcode, - }; + struct mesh_invalid_bearer_ev ev = { + .opcode = opcode, + }; - SYS_LOG_DBG("opcode 0x%02x", opcode); + SYS_LOG_DBG("opcode 0x%02x", opcode); - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INVALID_BEARER, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INVALID_BEARER, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void incomp_timer_exp_cb(void) +static void +incomp_timer_exp_cb(void) { - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INCOMP_TIMER_EXP, - CONTROLLER_INDEX, NULL, 0); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INCOMP_TIMER_EXP, + CONTROLLER_INDEX, NULL, 0); } static struct bt_test_cb bt_test_cb = { - .mesh_net_recv = net_recv_ev, - .mesh_model_bound = model_bound_cb, - .mesh_model_unbound = model_unbound_cb, - .mesh_prov_invalid_bearer = invalid_bearer_cb, - .mesh_trans_incomp_timer_exp = incomp_timer_exp_cb, + .mesh_net_recv = net_recv_ev, + .mesh_model_bound = model_bound_cb, + .mesh_model_unbound = model_unbound_cb, + .mesh_prov_invalid_bearer = invalid_bearer_cb, + .mesh_trans_incomp_timer_exp = incomp_timer_exp_cb, }; -static void lpn_established(uint16_t friend_addr) +static void +lpn_established(uint16_t friend_addr) { - struct bt_mesh_lpn *lpn = &bt_mesh.lpn; - struct mesh_lpn_established_ev ev = { lpn->sub->net_idx, friend_addr, lpn->queue_size, - lpn->recv_win }; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + struct mesh_lpn_established_ev + ev = {lpn->sub->net_idx, friend_addr, lpn->queue_size, + lpn->recv_win}; - SYS_LOG_DBG("Friendship (as LPN) established with " - "Friend 0x%04x Queue Size %d Receive Window %d", - friend_addr, lpn->queue_size, lpn->recv_win); + SYS_LOG_DBG("Friendship (as LPN) established with " + "Friend 0x%04x Queue Size %d Receive Window %d", + friend_addr, lpn->queue_size, lpn->recv_win); - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_LPN_ESTABLISHED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_LPN_ESTABLISHED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -static void lpn_terminated(uint16_t friend_addr) +static void +lpn_terminated(uint16_t friend_addr) { - struct bt_mesh_lpn *lpn = &bt_mesh.lpn; - struct mesh_lpn_terminated_ev ev = { lpn->sub->net_idx, friend_addr }; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + struct mesh_lpn_terminated_ev ev = {lpn->sub->net_idx, friend_addr}; - SYS_LOG_DBG("Friendship (as LPN) lost with Friend " - "0x%04x", friend_addr); + SYS_LOG_DBG("Friendship (as LPN) lost with Friend " + "0x%04x", friend_addr); - tester_send(BTP_SERVICE_ID_MESH, MESH_EV_LPN_TERMINATED, - CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_LPN_TERMINATED, + CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev)); } -void lpn_cb(uint16_t friend_addr, bool established) +void +lpn_cb(uint16_t friend_addr, bool established) { - if (established) { - lpn_established(friend_addr); - } else { - lpn_terminated(friend_addr); - } + if (established) { + lpn_established(friend_addr); + } else { + lpn_terminated(friend_addr); + } } -uint8_t tester_init_mesh(void) +uint8_t +tester_init_mesh(void) { - health_pub_init(); + health_pub_init(); - if (IS_ENABLED(CONFIG_BT_TESTING)) { - bt_mesh_lpn_set_cb(lpn_cb); - bt_test_cb_register(&bt_test_cb); - } + if (IS_ENABLED(CONFIG_BT_TESTING)) { + bt_mesh_lpn_set_cb(lpn_cb); + bt_test_cb_register(&bt_test_cb); + } - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } -uint8_t tester_unregister_mesh(void) +uint8_t +tester_unregister_mesh(void) { - return BTP_STATUS_SUCCESS; + return BTP_STATUS_SUCCESS; } #endif /* MYNEWT_VAL(BLE_MESH) */ diff --git a/apps/bttester/src/uart_pipe.c b/apps/bttester/src/uart_pipe.c index 1118d9af5..223a78975 100644 --- a/apps/bttester/src/uart_pipe.c +++ b/apps/bttester/src/uart_pipe.c @@ -41,7 +41,7 @@ struct uart_pipe_ring { static struct uart_dev *uart_dev; static struct uart_pipe_ring cr_tx; static uint8_t cr_tx_buf[MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)]; -typedef void (*console_write_char)(struct uart_dev*, uint8_t); +typedef void (*console_write_char)(struct uart_dev *, uint8_t); static console_write_char write_char_cb; static struct uart_pipe_ring cr_rx; @@ -217,39 +217,39 @@ bttester_pipe_send(const uint8_t *data, int len) int bttester_pipe_send_buf(struct os_mbuf *buf) { - int i, len; - struct os_mbuf *om; + int i, len; + struct os_mbuf *om; - /* Assure that there is a write cb installed; this enables to debug - * code that is faulting before the console was initialized. - */ - if (!write_char_cb) { - return -1; - } + /* Assure that there is a write cb installed; this enables to debug + * code that is faulting before the console was initialized. + */ + if (!write_char_cb) { + return -1; + } - for (om = buf; om; om = SLIST_NEXT(om, om_next)) { - len = om->om_len; - for (i = 0; i < len; ++i) { - write_char_cb(uart_dev, om->om_data[i]); - } - } + for (om = buf; om; om = SLIST_NEXT(om, om_next)) { + len = om->om_len; + for (i = 0; i < len; ++i) { + write_char_cb(uart_dev, om->om_data[i]); + } + } - uart_start_tx(uart_dev); + uart_start_tx(uart_dev); - return 0; + return 0; } int bttester_pipe_init(void) { struct uart_conf uc = { - .uc_speed = MYNEWT_VAL(CONSOLE_UART_BAUD), - .uc_databits = 8, - .uc_stopbits = 1, - .uc_parity = UART_PARITY_NONE, - .uc_flow_ctl = MYNEWT_VAL(CONSOLE_UART_FLOW_CONTROL), - .uc_tx_char = uart_console_tx_char, - .uc_rx_char = uart_console_rx_char, + .uc_speed = MYNEWT_VAL(CONSOLE_UART_BAUD), + .uc_databits = 8, + .uc_stopbits = 1, + .uc_parity = UART_PARITY_NONE, + .uc_flow_ctl = MYNEWT_VAL(CONSOLE_UART_FLOW_CONTROL), + .uc_tx_char = uart_console_tx_char, + .uc_rx_char = uart_console_rx_char, }; cr_tx.size = sizeof(cr_tx_buf); @@ -262,8 +262,9 @@ bttester_pipe_init(void) rx_ev.ev_cb = uart_console_rx_char_event; if (!uart_dev) { - uart_dev = (struct uart_dev *)os_dev_open(MYNEWT_VAL(CONSOLE_UART_DEV), - OS_TIMEOUT_NEVER, &uc); + uart_dev = + (struct uart_dev *) os_dev_open(MYNEWT_VAL(CONSOLE_UART_DEV), + OS_TIMEOUT_NEVER, &uc); if (!uart_dev) { return -1; } @@ -274,8 +275,9 @@ bttester_pipe_init(void) void bttester_pipe_register(uint8_t *buf, size_t len, bttester_pipe_recv_cb cb) { - recv_buf = buf; - recv_buf_len = len; - app_cb = cb; + recv_buf = buf; + recv_buf_len = len; + app_cb = cb; } + #endif /* MYNEWT_VAL(BTTESTER_PIPE_UART) */