diff options
author | JF <jf@codingfield.com> | 2020-04-26 10:25:59 +0200 |
---|---|---|
committer | JF <jf@codingfield.com> | 2020-04-26 10:25:59 +0200 |
commit | bdc10744fb338ae197692713a0b48a7ccc36f566 (patch) | |
tree | af7a8f2f16ddd2e5483758effec15c7683f6c453 /src/libs/mynewt-nimble/apps/bttester/src | |
parent | 032fad094c6411ad3ff4321ad61ceed95d7dc4ff (diff) |
Add Nimble in libs directory
Diffstat (limited to 'src/libs/mynewt-nimble/apps/bttester/src')
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/atomic.h | 405 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/bttester.c | 374 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/bttester.h | 1010 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h | 40 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/gap.c | 1688 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/gatt.c | 2098 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/glue.c | 129 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/glue.h | 63 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/l2cap.c | 477 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/main.c | 72 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/mesh.c | 970 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c | 136 | ||||
-rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c | 281 |
13 files changed, 7743 insertions, 0 deletions
diff --git a/src/libs/mynewt-nimble/apps/bttester/src/atomic.h b/src/libs/mynewt-nimble/apps/bttester/src/atomic.h new file mode 100644 index 00000000..66283e9a --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/atomic.h @@ -0,0 +1,405 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* atomic operations */ + +/* + * Copyright (c) 1997-2015, Wind River Systems, Inc. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __ATOMIC_H__ +#define __ATOMIC_H__ + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef int atomic_t; +typedef atomic_t atomic_val_t; + +/** + * @defgroup atomic_apis Atomic Services APIs + * @ingroup kernel_apis + * @{ + */ + +/** + * @brief Atomic compare-and-set. + * + * This routine performs an atomic compare-and-set on @a target. If the current + * value of @a target equals @a old_value, @a target is set to @a new_value. + * If the current value of @a target does not equal @a old_value, @a target + * is left unchanged. + * + * @param target Address of atomic variable. + * @param old_value Original value to compare against. + * @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) +{ + return __atomic_compare_exchange_n(target, &old_value, new_value, + 0, __ATOMIC_SEQ_CST, + __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic addition. + * + * This routine performs an atomic addition on @a target. + * + * @param target Address of atomic variable. + * @param value Value to add. + * + * @return Previous value of @a target. + */ +static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_add(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic subtraction. + * + * This routine performs an atomic subtraction on @a target. + * + * @param target Address of atomic variable. + * @param value Value to subtract. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_sub(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic increment. + * + * This routine performs an atomic increment by 1 on @a target. + * + * @param target Address of atomic variable. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_inc(atomic_t *target) +{ + return atomic_add(target, 1); +} + +/** + * + * @brief Atomic decrement. + * + * This routine performs an atomic decrement by 1 on @a target. + * + * @param target Address of atomic variable. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_dec(atomic_t *target) +{ + return atomic_sub(target, 1); +} + +/** + * + * @brief Atomic get. + * + * This routine performs an atomic read on @a target. + * + * @param target Address of atomic variable. + * + * @return Value of @a target. + */ + +static inline atomic_val_t atomic_get(const atomic_t *target) +{ + return __atomic_load_n(target, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic get-and-set. + * + * This routine atomically sets @a target to @a value and returns + * the previous value of @a target. + * + * @param target Address of atomic variable. + * @param value Value to write to @a target. + * + * @return Previous value of @a target. + */ + +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 + * writes value into *ptr, and returns the previous contents of *ptr. + */ + return __atomic_exchange_n(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic clear. + * + * This routine atomically sets @a target to zero and returns its previous + * value. (Hence, it is equivalent to atomic_set(target, 0).) + * + * @param target Address of atomic variable. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_clear(atomic_t *target) +{ + return atomic_set(target, 0); +} + +/** + * + * @brief Atomic bitwise inclusive OR. + * + * This routine atomically sets @a target to the bitwise inclusive OR of + * @a target and @a value. + * + * @param target Address of atomic variable. + * @param value Value to OR. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_or(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic bitwise exclusive OR (XOR). + * + * This routine atomically sets @a target to the bitwise exclusive OR (XOR) of + * @a target and @a value. + * + * @param target Address of atomic variable. + * @param value Value to XOR + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_xor(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic bitwise AND. + * + * This routine atomically sets @a target to the bitwise AND of @a target + * and @a value. + * + * @param target Address of atomic variable. + * @param value Value to AND. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_and(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic bitwise NAND. + * + * This routine atomically sets @a target to the bitwise NAND of @a target + * and @a value. (This operation is equivalent to target = ~(target & value).) + * + * @param target Address of atomic variable. + * @param value Value to NAND. + * + * @return Previous value of @a target. + */ + +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. + */ +#define ATOMIC_INIT(i) (i) + + /** + * @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 + */ + + /** + * @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] + + /** + * @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)))); + } + + /** + * @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); + + 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; + + old = atomic_or(ATOMIC_ELEM(target, bit), mask); + + 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); + + 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); + + atomic_or(ATOMIC_ELEM(target, bit), mask); + } + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __ATOMIC_H__ */ diff --git a/src/libs/mynewt-nimble/apps/bttester/src/bttester.c b/src/libs/mynewt-nimble/apps/bttester/src/bttester.c new file mode 100644 index 00000000..54b14daa --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/bttester.c @@ -0,0 +1,374 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* bttester.c - Bluetooth Tester */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include <stdio.h> +#include <string.h> +#include <stdlib.h> + +#include "syscfg/syscfg.h" +#include "console/console.h" + +#include "bttester_pipe.h" +#include "bttester.h" + +#define CMD_QUEUED 2 + +static struct os_eventq avail_queue; +static struct os_eventq *cmds_queue; +static struct os_event bttester_ev[CMD_QUEUED]; + +struct btp_buf { + struct os_event *ev; + union { + u8_t data[BTP_MTU]; + struct btp_hdr hdr; + }; +}; + +static struct btp_buf cmd_buf[CMD_QUEUED]; + +static void supported_commands(u8_t *data, u16_t len) +{ + u8_t buf[1]; + struct core_read_supported_commands_rp *rp = (void *) 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_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_COMMANDS, + BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf)); +} + +static void supported_services(u8_t *data, u16_t len) +{ + u8_t buf[1]; + struct core_read_supported_services_rp *rp = (void *) 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); +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) + 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); +#endif /* MYNEWT_VAL(BLE_MESH) */ + + tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_SERVICES, + BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf)); +} + +static void register_service(u8_t *data, u16_t len) +{ + struct core_register_service_cmd *cmd = (void *) data; + u8_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; +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) + 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; +#endif /* MYNEWT_VAL(BLE_MESH) */ + default: + status = BTP_STATUS_FAILED; + break; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE, + status); +} + +static void unregister_service(u8_t *data, u16_t len) +{ + struct core_unregister_service_cmd *cmd = (void *) data; + u8_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; +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) + 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; +#endif /* MYNEWT_VAL(BLE_MESH) */ + default: + status = BTP_STATUS_FAILED; + break; + } + + tester_rsp(BTP_SERVICE_ID_CORE, CORE_UNREGISTER_SERVICE, BTP_INDEX_NONE, + status); +} + +static void handle_core(u8_t opcode, u8_t index, u8_t *data, + u16_t len) +{ + 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; + } +} + +static void cmd_handler(struct os_event *ev) +{ + u16_t len; + struct btp_buf *cmd; + + if (!ev || !ev->ev_arg) { + return; + } + + 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)); + } + + /* 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; +#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; +#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; +#endif /* MYNEWT_VAL(BLE_MESH) */ + default: + tester_rsp(cmd->hdr.service, cmd->hdr.opcode, + cmd->hdr.index, BTP_STATUS_FAILED); + break; + } + + os_eventq_put(&avail_queue, ev); +} + +static u8_t *recv_cb(u8_t *buf, size_t *off) +{ + struct btp_hdr *cmd = (void *) buf; + struct os_event *new_ev; + struct btp_buf *new_buf, *old_buf; + u16_t len; + + if (*off < sizeof(*cmd)) { + 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; + } + + 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); + + new_buf = new_ev->ev_arg; + *off = 0; + return new_buf->data; +} + +static void avail_queue_init(void) +{ + int i; + + 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]; + + os_eventq_put(&avail_queue, &bttester_ev[i]); + } +} + +void bttester_evq_set(struct os_eventq *evq) +{ + cmds_queue = evq; +} + +void tester_init(void) +{ + struct os_event *ev; + struct btp_buf *buf; + + avail_queue_init(); + bttester_evq_set(os_eventq_dflt_get()); + + ev = os_eventq_get(&avail_queue); + buf = ev->ev_arg; + + if (bttester_pipe_init()) { + SYS_LOG_ERR("Failed to initialize pipe"); + return; + } + + bttester_pipe_register(buf->data, BTP_MTU, recv_cb); + + tester_send(BTP_SERVICE_ID_CORE, CORE_EV_IUT_READY, BTP_INDEX_NONE, + NULL, 0); +} + +void tester_send(u8_t service, u8_t opcode, u8_t index, u8_t *data, + size_t len) +{ + struct btp_hdr msg; + + msg.service = service; + msg.opcode = opcode; + msg.index = index; + msg.len = len; + + bttester_pipe_send((u8_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)); + } + } +} + +void tester_send_buf(u8_t service, u8_t opcode, u8_t index, + struct os_mbuf *data) +{ + struct btp_hdr msg; + + msg.service = service; + msg.opcode = opcode; + msg.index = index; + msg.len = os_mbuf_len(data); + + bttester_pipe_send((u8_t *)&msg, sizeof(msg)); + if (data && msg.len) { + bttester_pipe_send_buf(data); + } +} + +void tester_rsp(u8_t service, u8_t opcode, u8_t index, u8_t status) +{ + struct btp_status s; + + if (status == BTP_STATUS_SUCCESS) { + tester_send(service, opcode, index, NULL, 0); + return; + } + + s.code = status; + tester_send(service, BTP_STATUS, index, (u8_t *) &s, sizeof(s)); +} diff --git a/src/libs/mynewt-nimble/apps/bttester/src/bttester.h b/src/libs/mynewt-nimble/apps/bttester/src/bttester.h new file mode 100644 index 00000000..f4e66a6f --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/bttester.h @@ -0,0 +1,1010 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* bttester.h - Bluetooth tester headers */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __BTTESTER_H__ +#define __BTTESTER_H__ + +#include "syscfg/syscfg.h" +#include "host/ble_gatt.h" + +#if MYNEWT_VAL(BLE_MESH) +#include "mesh/glue.h" +#else +#include "glue.h" +#endif + +#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_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_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__); \ + } +#define SYS_LOG_INF(fmt, ...) console_printf("[INF] %s: " fmt "\n", \ + __func__, ## __VA_ARGS__); +#define SYS_LOG_ERR(fmt, ...) console_printf("[WRN] %s: " fmt "\n", \ + __func__, ## __VA_ARGS__); + +#define SYS_LOG_LEVEL SYS_LOG_LEVEL_DEBUG +#define SYS_LOG_DOMAIN "bttester" + +#define sys_cpu_to_le32 htole32 +#define sys_le32_to_cpu le32toh +#define sys_cpu_to_le16 htole16 + +struct btp_hdr { + u8_t service; + u8_t opcode; + u8_t index; + u16_t len; + u8_t data[0]; +} __packed; + +#define BTP_STATUS 0x00 +struct btp_status { + u8_t code; +} __packed; + +/* Core Service */ +#define CORE_READ_SUPPORTED_COMMANDS 0x01 +struct core_read_supported_commands_rp { + u8_t data[0]; +} __packed; + +#define CORE_READ_SUPPORTED_SERVICES 0x02 +struct core_read_supported_services_rp { + u8_t data[0]; +} __packed; + +#define CORE_REGISTER_SERVICE 0x03 +struct core_register_service_cmd { + u8_t id; +} __packed; + +#define CORE_UNREGISTER_SERVICE 0x04 +struct core_unregister_service_cmd { + u8_t id; +} __packed; + +/* events */ +#define CORE_EV_IUT_READY 0x80 + +/* GAP Service */ +/* commands */ +#define GAP_READ_SUPPORTED_COMMANDS 0x01 +struct gap_read_supported_commands_rp { + u8_t data[0]; +} __packed; + +#define GAP_READ_CONTROLLER_INDEX_LIST 0x02 +struct gap_read_controller_index_list_rp { + u8_t num; + u8_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_READ_CONTROLLER_INFO 0x03 +struct gap_read_controller_info_rp { + u8_t address[6]; + u32_t supported_settings; + u32_t current_settings; + u8_t cod[3]; + u8_t name[249]; + u8_t short_name[11]; +} __packed; + +#define GAP_RESET 0x04 +struct gap_reset_rp { + u32_t current_settings; +} __packed; + +#define GAP_SET_POWERED 0x05 +struct gap_set_powered_cmd { + u8_t powered; +} __packed; +struct gap_set_powered_rp { + u32_t current_settings; +} __packed; + +#define GAP_SET_CONNECTABLE 0x06 +struct gap_set_connectable_cmd { + u8_t connectable; +} __packed; +struct gap_set_connectable_rp { + u32_t current_settings; +} __packed; + +#define GAP_SET_FAST_CONNECTABLE 0x07 +struct gap_set_fast_connectable_cmd { + u8_t fast_connectable; +} __packed; +struct gap_set_fast_connectable_rp { + u32_t current_settings; +} __packed; + +#define GAP_NON_DISCOVERABLE 0x00 +#define GAP_GENERAL_DISCOVERABLE 0x01 +#define GAP_LIMITED_DISCOVERABLE 0x02 + +#define GAP_SET_DISCOVERABLE 0x08 +struct gap_set_discoverable_cmd { + u8_t discoverable; +} __packed; +struct gap_set_discoverable_rp { + u32_t current_settings; +} __packed; + +#define GAP_SET_BONDABLE 0x09 +struct gap_set_bondable_cmd { + u8_t bondable; +} __packed; +struct gap_set_bondable_rp { + u32_t current_settings; +} __packed; + +#define GAP_START_ADVERTISING 0x0a +struct gap_start_advertising_cmd { + u8_t adv_data_len; + u8_t scan_rsp_len; + u8_t adv_data[0]; + u8_t scan_rsp[0]; +} __packed; +struct gap_start_advertising_rp { + u32_t current_settings; +} __packed; + +#define GAP_STOP_ADVERTISING 0x0b +struct gap_stop_advertising_rp { + u32_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_START_DISCOVERY 0x0c +struct gap_start_discovery_cmd { + u8_t flags; +} __packed; + +#define GAP_STOP_DISCOVERY 0x0d + +#define GAP_CONNECT 0x0e +struct gap_connect_cmd { + u8_t address_type; + u8_t address[6]; +} __packed; + +#define GAP_DISCONNECT 0x0f +struct gap_disconnect_cmd { + u8_t address_type; + u8_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_SET_IO_CAP 0x10 +struct gap_set_io_cap_cmd { + u8_t io_cap; +} __packed; + +#define GAP_PAIR 0x11 +struct gap_pair_cmd { + u8_t address_type; + u8_t address[6]; +} __packed; + +#define GAP_UNPAIR 0x12 +struct gap_unpair_cmd { + u8_t address_type; + u8_t address[6]; +} __packed; + +#define GAP_PASSKEY_ENTRY 0x13 +struct gap_passkey_entry_cmd { + u8_t address_type; + u8_t address[6]; + u32_t passkey; +} __packed; + +#define GAP_PASSKEY_CONFIRM 0x14 +struct gap_passkey_confirm_cmd { + u8_t address_type; + u8_t address[6]; + u8_t match; +} __packed; + +#define GAP_START_DIRECT_ADV 0x15 +struct gap_start_direct_adv_cmd { + u8_t address_type; + u8_t address[6]; + u8_t high_duty; +} __packed; + +#define GAP_CONN_PARAM_UPDATE 0x16 +struct gap_conn_param_update_cmd { + u8_t address_type; + u8_t address[6]; + u16_t conn_itvl_min; + u16_t conn_itvl_max; + u16_t conn_latency; + u16_t supervision_timeout; +} __packed; + +#define GAP_PAIRING_CONSENT_RSP 0x17 +struct gap_pairing_consent_rsp_cmd { + u8_t address_type; + u8_t address[6]; + u8_t consent; +} __packed; + +#define GAP_OOB_LEGACY_SET_DATA 0x18 +struct gap_oob_legacy_set_data_cmd { + u8_t oob_data[16]; +} __packed; + +#define GAP_OOB_SC_GET_LOCAL_DATA 0x19 +struct gap_oob_sc_get_local_data_rp { + u8_t r[16]; + u8_t c[16]; +} __packed; + +#define GAP_OOB_SC_SET_REMOTE_DATA 0x1a +struct gap_oob_sc_set_remote_data_cmd { + u8_t r[16]; + u8_t c[16]; +} __packed; + +#define GAP_SET_MITM 0x1b +struct gap_set_mitm_cmd { + u8_t mitm; +} __packed; + +/* events */ +#define GAP_EV_NEW_SETTINGS 0x80 +struct gap_new_settings_ev { + u32_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_EV_DEVICE_FOUND 0x81 +struct gap_device_found_ev { + u8_t address_type; + u8_t address[6]; + s8_t rssi; + u8_t flags; + u16_t eir_data_len; + u8_t eir_data[0]; +} __packed; + +#define GAP_EV_DEVICE_CONNECTED 0x82 +struct gap_device_connected_ev { + u8_t address_type; + u8_t address[6]; + u16_t conn_itvl; + u16_t conn_latency; + u16_t supervision_timeout; +} __packed; + +#define GAP_EV_DEVICE_DISCONNECTED 0x83 +struct gap_device_disconnected_ev { + u8_t address_type; + u8_t address[6]; +} __packed; + +#define GAP_EV_PASSKEY_DISPLAY 0x84 +struct gap_passkey_display_ev { + u8_t address_type; + u8_t address[6]; + u32_t passkey; +} __packed; + +#define GAP_EV_PASSKEY_ENTRY_REQ 0x85 +struct gap_passkey_entry_req_ev { + u8_t address_type; + u8_t address[6]; +} __packed; + +#define GAP_EV_PASSKEY_CONFIRM_REQ 0x86 +struct gap_passkey_confirm_req_ev { + u8_t address_type; + u8_t address[6]; + u32_t passkey; +} __packed; + +#define GAP_EV_IDENTITY_RESOLVED 0x87 +struct gap_identity_resolved_ev { + u8_t address_type; + u8_t address[6]; + u8_t identity_address_type; + u8_t identity_address[6]; +} __packed; + +#define GAP_EV_CONN_PARAM_UPDATE 0x88 +struct gap_conn_param_update_ev { + u8_t address_type; + u8_t address[6]; + u16_t conn_itvl; + u16_t conn_latency; + u16_t supervision_timeout; +} __packed; + +#define GAP_EV_SEC_LEVEL_CHANGED 0x89 +struct gap_sec_level_changed_ev { + u8_t address_type; + u8_t address[6]; + u8_t level; +} __packed; + +#define GAP_EV_PAIRING_CONSENT_REQ 0x8a +struct gap_pairing_consent_req_ev { + u8_t address_type; + u8_t address[6]; +} __packed; + +/* GATT Service */ +/* commands */ +#define GATT_READ_SUPPORTED_COMMANDS 0x01 +struct gatt_read_supported_commands_rp { + u8_t data[0]; +} __packed; + +#define GATT_SERVICE_PRIMARY 0x00 +#define GATT_SERVICE_SECONDARY 0x01 + +#define GATT_ADD_SERVICE 0x02 +struct gatt_add_service_cmd { + u8_t type; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; +struct gatt_add_service_rp { + u16_t svc_id; +} __packed; + +#define GATT_ADD_CHARACTERISTIC 0x03 +struct gatt_add_characteristic_cmd { + u16_t svc_id; + u8_t properties; + u8_t permissions; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; +struct gatt_add_characteristic_rp { + u16_t char_id; +} __packed; + +#define GATT_ADD_DESCRIPTOR 0x04 +struct gatt_add_descriptor_cmd { + u16_t char_id; + u8_t permissions; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; +struct gatt_add_descriptor_rp { + u16_t desc_id; +} __packed; + +#define GATT_ADD_INCLUDED_SERVICE 0x05 +struct gatt_add_included_service_cmd { + u16_t svc_id; +} __packed; +struct gatt_add_included_service_rp { + u16_t included_service_id; +} __packed; + +#define GATT_SET_VALUE 0x06 + struct gatt_set_value_cmd { + u16_t attr_id; + u16_t len; + u8_t value[0]; +} __packed; + +#define GATT_START_SERVER 0x07 +struct gatt_start_server_rp { + u16_t db_attr_off; + u8_t db_attr_cnt; +} __packed; + +#define GATT_SET_ENC_KEY_SIZE 0x09 +struct gatt_set_enc_key_size_cmd { + u16_t attr_id; + u8_t key_size; +} __packed; + +/* Gatt Client */ +struct gatt_service { + u16_t start_handle; + u16_t end_handle; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; + +struct gatt_included { + u16_t included_handle; + struct gatt_service service; +} __packed; + +struct gatt_characteristic { + u16_t characteristic_handle; + u16_t value_handle; + u8_t properties; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; + +struct gatt_descriptor { + u16_t descriptor_handle; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; + +#define GATT_EXCHANGE_MTU 0x0a + +#define GATT_DISC_ALL_PRIM_SVCS 0x0b +struct gatt_disc_all_prim_svcs_cmd { + u8_t address_type; + u8_t address[6]; +} __packed; +struct gatt_disc_all_prim_svcs_rp { + u8_t services_count; + struct gatt_service services[0]; +} __packed; + +#define GATT_DISC_PRIM_UUID 0x0c +struct gatt_disc_prim_uuid_cmd { + u8_t address_type; + u8_t address[6]; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; +struct gatt_disc_prim_uuid_rp { + u8_t services_count; + struct gatt_service services[0]; +} __packed; + +#define GATT_FIND_INCLUDED 0x0d +struct gatt_find_included_cmd { + u8_t address_type; + u8_t address[6]; + u16_t start_handle; + u16_t end_handle; +} __packed; +struct gatt_find_included_rp { + u8_t services_count; + struct gatt_included included[0]; +} __packed; + +#define GATT_DISC_ALL_CHRC 0x0e +struct gatt_disc_all_chrc_cmd { + u8_t address_type; + u8_t address[6]; + u16_t start_handle; + u16_t end_handle; +} __packed; +struct gatt_disc_chrc_rp { + u8_t characteristics_count; + struct gatt_characteristic characteristics[0]; +} __packed; + +#define GATT_DISC_CHRC_UUID 0x0f +struct gatt_disc_chrc_uuid_cmd { + u8_t address_type; + u8_t address[6]; + u16_t start_handle; + u16_t end_handle; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; + +#define GATT_DISC_ALL_DESC 0x10 +struct gatt_disc_all_desc_cmd { + u8_t address_type; + u8_t address[6]; + u16_t start_handle; + u16_t end_handle; +} __packed; +struct gatt_disc_all_desc_rp { + u8_t descriptors_count; + struct gatt_descriptor descriptors[0]; +} __packed; + +#define GATT_READ 0x11 +struct gatt_read_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; +} __packed; +struct gatt_read_rp { + u8_t att_response; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_READ_UUID 0x12 +struct gatt_read_uuid_cmd { + u8_t address_type; + u8_t address[6]; + u16_t start_handle; + u16_t end_handle; + u8_t uuid_length; + u8_t uuid[0]; +} __packed; + +#define GATT_READ_LONG 0x13 +struct gatt_read_long_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; + u16_t offset; +} __packed; + +#define GATT_READ_MULTIPLE 0x14 +struct gatt_read_multiple_cmd { + u8_t address_type; + u8_t address[6]; + u8_t handles_count; + u16_t handles[0]; +} __packed; + +#define GATT_WRITE_WITHOUT_RSP 0x15 +struct gatt_write_without_rsp_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_SIGNED_WRITE_WITHOUT_RSP 0x16 +struct gatt_signed_write_without_rsp_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_WRITE 0x17 +struct gatt_write_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_WRITE_LONG 0x18 +struct gatt_write_long_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; + u16_t offset; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_RELIABLE_WRITE 0x19 +struct gatt_reliable_write_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; + u16_t offset; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_CFG_NOTIFY 0x1a +#define GATT_CFG_INDICATE 0x1b +struct gatt_cfg_notify_cmd { + u8_t address_type; + u8_t address[6]; + u8_t enable; + u16_t ccc_handle; +} __packed; + +#define GATT_GET_ATTRIBUTES 0x1c +struct gatt_get_attributes_cmd { + u16_t start_handle; + u16_t end_handle; + u8_t type_length; + u8_t type[0]; +} __packed; +struct gatt_get_attributes_rp { + u8_t attrs_count; + u8_t attrs[0]; +} __packed; +struct gatt_attr { + u16_t handle; + u8_t permission; + u8_t type_length; + u8_t type[0]; +} __packed; + +#define GATT_GET_ATTRIBUTE_VALUE 0x1d +struct gatt_get_attribute_value_cmd { + u8_t address_type; + u8_t address[6]; + u16_t handle; +} __packed; +struct gatt_get_attribute_value_rp { + u8_t att_response; + u16_t value_length; + u8_t value[0]; +} __packed; + +#define GATT_CHANGE_DATABASE 0x1e +struct gatt_change_database { + u16_t start_handle; + u16_t end_handle; + u8_t visibility; +} __packed; + +/* GATT events */ +#define GATT_EV_NOTIFICATION 0x80 +struct gatt_notification_ev { + u8_t address_type; + u8_t address[6]; + u8_t type; + u16_t handle; + u16_t data_length; + u8_t data[0]; +} __packed; + +#define GATT_EV_ATTR_VALUE_CHANGED 0x81 +struct gatt_attr_value_changed_ev { + u16_t handle; + u16_t data_length; + u8_t data[0]; +} __packed; + +static inline void tester_set_bit(u8_t *addr, unsigned int bit) +{ + u8_t *p = addr + (bit / 8); + + *p |= BIT(bit % 8); +} + +static inline u8_t tester_test_bit(const u8_t *addr, unsigned int bit) +{ + const u8_t *p = addr + (bit / 8); + + return *p & BIT(bit % 8); +} + +/* L2CAP Service */ +/* commands */ +#define L2CAP_READ_SUPPORTED_COMMANDS 0x01 +struct l2cap_read_supported_commands_rp { + u8_t data[0]; +} __packed; + +#define L2CAP_CONNECT 0x02 +struct l2cap_connect_cmd { + u8_t address_type; + u8_t address[6]; + u16_t psm; +} __packed; + +struct l2cap_connect_rp { + u8_t chan_id; +} __packed; + +#define L2CAP_DISCONNECT 0x03 +struct l2cap_disconnect_cmd { + u8_t chan_id; +} __packed; + +#define L2CAP_SEND_DATA 0x04 +struct l2cap_send_data_cmd { + u8_t chan_id; + u16_t data_len; + u8_t data[]; +} __packed; + +#define L2CAP_TRANSPORT_BREDR 0x00 +#define L2CAP_TRANSPORT_LE 0x01 + +#define L2CAP_LISTEN 0x05 +struct l2cap_listen_cmd { + u16_t psm; + u8_t transport; +} __packed; + +#define L2CAP_ACCEPT_CONNECTION 0x06 +struct l2cap_accept_connection_cmd { + u8_t chan_id; + u16_t result; +} __packed; + +/* events */ +#define L2CAP_EV_CONNECTION_REQ 0x80 +struct l2cap_connection_req_ev { + u8_t chan_id; + u16_t psm; + u8_t address_type; + u8_t address[6]; +} __packed; + +#define L2CAP_EV_CONNECTED 0x81 +struct l2cap_connected_ev { + u8_t chan_id; + u16_t psm; + u8_t address_type; + u8_t address[6]; +} __packed; + +#define L2CAP_EV_DISCONNECTED 0x82 +struct l2cap_disconnected_ev { + u16_t result; + u8_t chan_id; + u16_t psm; + u8_t address_type; + u8_t address[6]; +} __packed; + +#define L2CAP_EV_DATA_RECEIVED 0x83 +struct l2cap_data_received_ev { + u8_t chan_id; + u16_t data_length; + u8_t data[0]; +} __packed; + +/* MESH Service */ +/* commands */ +#define MESH_READ_SUPPORTED_COMMANDS 0x01 +struct mesh_read_supported_commands_rp { + u8_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_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 +struct mesh_config_provisioning_cmd { + u8_t uuid[16]; + u8_t static_auth[16]; + u8_t out_size; + u16_t out_actions; + u8_t in_size; + u16_t in_actions; +} __packed; + +#define MESH_PROVISION_NODE 0x03 +struct mesh_provision_node_cmd { + u8_t net_key[16]; + u16_t net_key_idx; + u8_t flags; + u32_t iv_index; + u32_t seq_num; + u16_t addr; + u8_t dev_key[16]; +} __packed; + +#define MESH_INIT 0x04 +#define MESH_RESET 0x05 +#define MESH_INPUT_NUMBER 0x06 +struct mesh_input_number_cmd { + u32_t number; +} __packed; + +#define MESH_INPUT_STRING 0x07 +struct mesh_input_string_cmd { + u8_t string_len; + u8_t string[0]; +} __packed; + +#define MESH_IVU_TEST_MODE 0x08 +struct mesh_ivu_test_mode_cmd { + u8_t enable; +} __packed; + +#define MESH_IVU_TOGGLE_STATE 0x09 + +#define MESH_NET_SEND 0x0a +struct mesh_net_send_cmd { + u8_t ttl; + u16_t src; + u16_t dst; + u8_t payload_len; + u8_t payload[0]; +} __packed; + +#define MESH_HEALTH_GENERATE_FAULTS 0x0b +struct mesh_health_generate_faults_rp { + u8_t test_id; + u8_t cur_faults_count; + u8_t reg_faults_count; + u8_t current_faults[0]; + u8_t registered_faults[0]; +} __packed; + +#define MESH_HEALTH_CLEAR_FAULTS 0x0c + +#define MESH_LPN 0x0d +struct mesh_lpn_set_cmd { + u8_t enable; +} __packed; + +#define MESH_LPN_POLL 0x0e + +#define MESH_MODEL_SEND 0x0f +struct mesh_model_send_cmd { + u16_t src; + u16_t dst; + u8_t payload_len; + u8_t payload[0]; +} __packed; + +#define MESH_LPN_SUBSCRIBE 0x10 +struct mesh_lpn_subscribe_cmd { + u16_t address; +} __packed; + +#define MESH_LPN_UNSUBSCRIBE 0x11 +struct mesh_lpn_unsubscribe_cmd { + u16_t address; +} __packed; + +#define MESH_RPL_CLEAR 0x12 +#define MESH_PROXY_IDENTITY 0x13 + +/* events */ +#define MESH_EV_OUT_NUMBER_ACTION 0x80 +struct mesh_out_number_action_ev { + u16_t action; + u32_t number; +} __packed; + +#define MESH_EV_OUT_STRING_ACTION 0x81 +struct mesh_out_string_action_ev { + u8_t string_len; + u8_t string[0]; +} __packed; + +#define MESH_EV_IN_ACTION 0x82 +struct mesh_in_action_ev { + u16_t action; + u8_t size; +} __packed; + +#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 +struct mesh_prov_link_open_ev { + u8_t bearer; +} __packed; + +#define MESH_EV_PROV_LINK_CLOSED 0x85 +struct mesh_prov_link_closed_ev { + u8_t bearer; +} __packed; + +#define MESH_EV_NET_RECV 0x86 +struct mesh_net_recv_ev { + u8_t ttl; + u8_t ctl; + u16_t src; + u16_t dst; + u8_t payload_len; + u8_t payload[0]; +} __packed; + +#define MESH_EV_INVALID_BEARER 0x87 +struct mesh_invalid_bearer_ev { + u8_t opcode; +} __packed; + +#define MESH_EV_INCOMP_TIMER_EXP 0x88 + +void tester_init(void); +void tester_rsp(u8_t service, u8_t opcode, u8_t index, u8_t status); +void tester_send(u8_t service, u8_t opcode, u8_t index, u8_t *data, + size_t len); +void tester_send_buf(u8_t service, u8_t opcode, u8_t index, + struct os_mbuf *buf); + +u8_t tester_init_gap(void); +u8_t tester_unregister_gap(void); +void tester_handle_gap(u8_t opcode, u8_t index, u8_t *data, + u16_t len); +u8_t tester_init_gatt(void); +u8_t tester_unregister_gatt(void); +void tester_handle_gatt(u8_t opcode, u8_t index, u8_t *data, + u16_t len); +int tester_gatt_notify_rx_ev(u16_t conn_handle, u16_t attr_handle, + u8_t indication, struct os_mbuf *om); +int tester_gatt_subscribe_ev(u16_t conn_handle, u16_t attr_handle, u8_t reason, + u8_t prev_notify, u8_t cur_notify, + u8_t prev_indicate, u8_t cur_indicate); + +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) +u8_t tester_init_l2cap(void); +u8_t tester_unregister_l2cap(void); +void tester_handle_l2cap(u8_t opcode, u8_t index, u8_t *data, + u16_t len); +#endif + +#if MYNEWT_VAL(BLE_MESH) +u8_t tester_init_mesh(void); +u8_t tester_unregister_mesh(void); +void tester_handle_mesh(u8_t opcode, u8_t index, u8_t *data, u16_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); + +#endif /* __BTTESTER_H__ */ diff --git a/src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h b/src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h new file mode 100644 index 00000000..c54d42de --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/bttester_pipe.h @@ -0,0 +1,40 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#ifndef __BTTESTER_PIPE_H__ +#define __BTTESTER_PIPE_H__ + +#include <stdlib.h> +#include "bttester.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef u8_t *(*bttester_pipe_recv_cb)(u8_t *buf, size_t *off); +void bttester_pipe_register(u8_t *buf, size_t len, bttester_pipe_recv_cb cb); +int bttester_pipe_send(const u8_t *data, int len); +int bttester_pipe_send_buf(struct os_mbuf *buf); +int bttester_pipe_init(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __BTTESTER_PIPE_H__ */ diff --git a/src/libs/mynewt-nimble/apps/bttester/src/gap.c b/src/libs/mynewt-nimble/apps/bttester/src/gap.c new file mode 100644 index 00000000..9d6de043 --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/gap.c @@ -0,0 +1,1688 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* gap.c - Bluetooth GAP Tester */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "host/ble_gap.h" +#include "host/util/util.h" +#include "console/console.h" + +#include "../../../nimble/host/src/ble_hs_pvcy_priv.h" +#include "../../../nimble/host/src/ble_hs_hci_priv.h" +#include "../../../nimble/host/src/ble_sm_priv.h" + +#include "bttester.h" + +#define CONTROLLER_INDEX 0 +#define CONTROLLER_NAME "btp_tester" + +#define BLE_AD_DISCOV_MASK (BLE_HS_ADV_F_DISC_LTD | BLE_HS_ADV_F_DISC_GEN) +#define ADV_BUF_LEN (sizeof(struct gap_device_found_ev) + 2 * 31) + +const uint8_t irk[16] = { + 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, + 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, +}; + +static uint8_t oob[16]; +static struct ble_sm_sc_oob_data oob_data_local; +static struct ble_sm_sc_oob_data oob_data_remote; + +static uint16_t current_settings; +u8_t own_addr_type; +static ble_addr_t peer_id_addr; +static ble_addr_t peer_ota_addr; +static bool encrypted = false; + +static struct os_callout update_params_co; +static struct gap_conn_param_update_cmd update_params; + +static struct os_callout connected_ev_co; +static struct gap_device_connected_ev connected_ev; +#define CONNECTED_EV_DELAY_MS(itvl) 8 * BLE_HCI_CONN_ITVL * itvl / 1000 +static int connection_attempts; + +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, +}; + +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) +{ + 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 (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; + } + + 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); + } +} + +static int gap_event_cb(struct ble_gap_event *event, void *arg); + +static void supported_commands(u8_t *data, u16_t len) +{ + u8_t cmds[3]; + struct gap_read_supported_commands_rp *rp = (void *) &cmds; + + SYS_LOG_DBG(""); + + 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_send(BTP_SERVICE_ID_GAP, GAP_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (u8_t *) rp, sizeof(cmds)); +} + +static void controller_index_list(u8_t *data, u16_t len) +{ + struct gap_read_controller_index_list_rp *rp; + u8_t buf[sizeof(*rp) + 1]; + + SYS_LOG_DBG(""); + + rp = (void *) buf; + + rp->num = 1; + rp->index[0] = CONTROLLER_INDEX; + + tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INDEX_LIST, + BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf)); +} + +static int check_pub_addr_unassigned(void) +{ +#ifdef ARCH_sim + return 0; +#else + uint8_t zero_addr[BLE_DEV_ADDR_LEN] = { 0 }; + + return memcmp(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR), + zero_addr, BLE_DEV_ADDR_LEN) == 0; +#endif +} + +static void controller_info(u8_t *data, u16_t len) +{ + struct gap_read_controller_info_rp rp; + u32_t supported_settings = 0; + ble_addr_t addr; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_hs_pvcy_set_our_irk(irk); + assert(rc == 0); + + 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); + + 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 { + if (check_pub_addr_unassigned()) { + 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; + memcpy(rp.address, MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR), + sizeof(rp.address)); + } + } + + 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); + } + + 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)); + + tester_send(BTP_SERVICE_ID_GAP, GAP_READ_CONTROLLER_INFO, + CONTROLLER_INDEX, (u8_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, +}; + +static void set_connectable(u8_t *data, u16_t len) +{ + const struct gap_set_connectable_cmd *cmd = (void *) data; + struct gap_set_connectable_rp rp; + + 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; + } + + rp.current_settings = sys_cpu_to_le32(current_settings); + + tester_send(BTP_SERVICE_ID_GAP, GAP_SET_CONNECTABLE, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); +} + +static u8_t ad_flags = BLE_HS_ADV_F_BREDR_UNSUP; + +static void set_discoverable(u8_t *data, u16_t len) +{ + const struct gap_set_discoverable_cmd *cmd = (void *) data; + struct gap_set_discoverable_rp rp; + + 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; + } + + rp.current_settings = sys_cpu_to_le32(current_settings); + + tester_send(BTP_SERVICE_ID_GAP, GAP_SET_DISCOVERABLE, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); +} + +static void set_bondable(const u8_t *data, u16_t len) +{ + const struct gap_set_bondable_cmd *cmd = (void *) data; + struct gap_set_bondable_rp rp; + + 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); + } + + rp.current_settings = sys_cpu_to_le32(current_settings); + + tester_send(BTP_SERVICE_ID_GAP, GAP_SET_BONDABLE, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); +} + +static struct bt_data ad[10] = { + 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, + u8_t *buf, u8_t *buf_len) +{ + int i; + + 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; + } + + return 0; +} + +static void start_advertising(const u8_t *data, u16_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; + u8_t adv_len, sd_len; + int err; + + int i; + + 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; + } + + 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; + } + + 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 = ble_gap_adv_set_data(buf, buf_len); + if (err != 0) { + goto fail; + } + + 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 = 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); + } + + 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); + + tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); + return; +fail: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void stop_advertising(const u8_t *data, u16_t len) +{ + struct gap_stop_advertising_rp rp; + + SYS_LOG_DBG(""); + + 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); + + tester_send(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); +} + +static u8_t get_ad_flags(const u8_t *data, u8_t data_len) +{ + u8_t len, i; + + /* 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; + } + + switch (data[i++]) { + case BLE_HS_ADV_TYPE_FLAGS: + return data[i]; + default: + break; + } + } + + return 0; +} + +static u8_t discovery_flags; +static struct os_mbuf *adv_buf; + +static void store_adv(const ble_addr_t *addr, s8_t rssi, + const u8_t *data, u8_t len) +{ + struct gap_device_found_ev *ev; + + /* cleanup */ + net_buf_simple_init(adv_buf, 0); + + 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); +} + +static void device_found(ble_addr_t *addr, s8_t rssi, u8_t evtype, + const u8_t *data, u8_t len) +{ + 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)) { + u8_t flags = get_ad_flags(data, len); + + /* 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; + } + } + + /* 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)); + + /* + * 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; + + memcpy(net_buf_simple_add(adv_buf, len), data, len); + + 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); + } + + 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; + } +done: + 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) +{ + 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; +} + +static void start_discovery(const u8_t *data, u16_t len) +{ + const struct gap_start_discovery_cmd *cmd = (void *) data; + struct ble_gap_disc_params params = {0}; + u8_t status; + + SYS_LOG_DBG(""); + + /* 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; + + 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; + + status = BTP_STATUS_SUCCESS; +reply: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DISCOVERY, CONTROLLER_INDEX, + status); +} + +static void stop_discovery(const u8_t *data, u16_t len) +{ + u8_t status = BTP_STATUS_SUCCESS; + + SYS_LOG_DBG(""); + + if (ble_gap_disc_cancel() != 0) { + status = BTP_STATUS_FAILED; + } + + 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 + * the resolvable private address [...] + * If the resolution is successful, the Host may accept the connection. + * 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) +{ +#if !MYNEWT_VAL(BTTESTER_PRIVACY_MODE) + return; +#endif + int count; + + 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); + } +} + +static void device_connected_ev_send(struct os_event *ev) +{ + struct ble_gap_conn_desc desc; + int rc; + + 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; + } + + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_CONNECTED, + CONTROLLER_INDEX, (u8_t *) &connected_ev, + sizeof(connected_ev)); + + periph_privacy(desc); +} + +static void le_connected(u16_t conn_handle, int status) +{ + struct ble_gap_conn_desc desc; + ble_addr_t *addr; + int rc; + + SYS_LOG_DBG(""); + + if (status != 0) { + 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; + + 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; + +#if MYNEWT_VAL(BTTESTER_CONN_RETRY) + 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, (u8_t *) &connected_ev, + sizeof(connected_ev)); +#endif +} + +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; + + SYS_LOG_DBG(""); + +#if MYNEWT_VAL(BTTESTER_CONN_RETRY) + 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); + + /* 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; + } +#endif + + connection_attempts = 0; + memset(&connected_ev, 0, sizeof(connected_ev)); + + 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, (u8_t *) &ev, sizeof(ev)); +} + +static void auth_passkey_oob(u16_t conn_handle) +{ + struct ble_gap_conn_desc desc; + struct ble_sm_io pk; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } + + memcpy(pk.oob, oob, sizeof(oob)); + pk.action = BLE_SM_IOACT_OOB; + + rc = ble_sm_inject_io(conn_handle, &pk); + assert(rc == 0); +} + +static void auth_passkey_display(u16_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; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } + + rc = ble_hs_hci_util_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); + + 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); + + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_DISPLAY, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static void auth_passkey_entry(u16_t conn_handle) +{ + struct ble_gap_conn_desc desc; + struct gap_passkey_entry_req_ev ev; + ble_addr_t *addr; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } + + addr = &desc.peer_ota_addr; + + 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, (u8_t *) &ev, sizeof(ev)); +} + +static void auth_passkey_numcmp(u16_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; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find(conn_handle, &desc); + if (rc) { + return; + } + + 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); + + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_CONFIRM_REQ, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static void auth_passkey_oob_sc(u16_t conn_handle) +{ + int rc; + struct ble_sm_io pk; + + SYS_LOG_DBG(""); + + memset(&pk, 0, sizeof(pk)); + + pk.oob_sc_data.local = &oob_data_local; + + 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); + } +} + +static void le_passkey_action(u16_t conn_handle, + struct ble_gap_passkey_params *params) +{ + 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); + } +} + +static void le_identity_resolved(u16_t conn_handle) +{ + struct ble_gap_conn_desc desc; + struct gap_identity_resolved_ev ev; + int rc; + + SYS_LOG_DBG(""); + + 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; + + 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)); + + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_IDENTITY_RESOLVED, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static void le_conn_param_update(struct ble_gap_conn_desc *desc) +{ + struct gap_conn_param_update_ev ev; + + SYS_LOG_DBG(""); + + 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; + + tester_send(BTP_SERVICE_ID_GAP, GAP_EV_CONN_PARAM_UPDATE, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static void le_encryption_changed(struct ble_gap_conn_desc *desc) +{ + struct gap_sec_level_changed_ev ev; + + SYS_LOG_DBG(""); + + 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; + + 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, (u8_t *) &ev, sizeof(ev)); +} + +static void print_bytes(const uint8_t *bytes, int len) +{ + int i; + + for (i = 0; i < len; i++) { + console_printf("%s0x%02x", i != 0 ? ":" : "", bytes[i]); + } +} + +static void print_mbuf(const struct os_mbuf *om) +{ + 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); + } +} + +static void print_addr(const void *addr) +{ + 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]); +} + +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); +} + +static void adv_complete(void) +{ + struct gap_new_settings_ev ev; + + 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, + (u8_t *) &ev, sizeof(ev)); +} + +static int gap_event_cb(struct ble_gap_event *event, void *arg) +{ + 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); + } + + 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); + 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_gatt_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); + 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; + default: + break; + } + + return 0; +} + +static void connect(const u8_t *data, u16_t len) +{ + u8_t status = BTP_STATUS_SUCCESS; + + SYS_LOG_DBG(""); + + if (ble_gap_connect(own_addr_type, (ble_addr_t *) data, 0, + &dflt_conn_params, gap_event_cb, NULL)) { + status = BTP_STATUS_FAILED; + } + + tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONNECT, CONTROLLER_INDEX, status); +} + +static void disconnect(const u8_t *data, u16_t len) +{ + struct ble_gap_conn_desc desc; + u8_t status; + int rc; + + SYS_LOG_DBG(""); + + 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; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_DISCONNECT, CONTROLLER_INDEX, + status); +} + +static void set_io_cap(const u8_t *data, u16_t len) +{ + const struct gap_set_io_cap_cmd *cmd = (void *) data; + u8_t status; + + 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; + } + + status = BTP_STATUS_SUCCESS; + +rsp: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_IO_CAP, CONTROLLER_INDEX, + status); +} + +static void pair(const u8_t *data, u16_t len) +{ + struct ble_gap_conn_desc desc; + u8_t status; + int rc; + + SYS_LOG_DBG(""); + + 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; + } + + status = BTP_STATUS_SUCCESS; + +rsp: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_PAIR, CONTROLLER_INDEX, status); +} + +static void unpair(const u8_t *data, u16_t len) +{ + u8_t status; + int err; + + 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); +} + +static void passkey_entry(const u8_t *data, u16_t len) +{ + const struct gap_passkey_entry_cmd *cmd = (void *) data; + struct ble_gap_conn_desc desc; + struct ble_sm_io pk; + u8_t status; + int rc; + + SYS_LOG_DBG(""); + + 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); + + rc = ble_sm_inject_io(desc.conn_handle, &pk); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + status = BTP_STATUS_SUCCESS; + +rsp: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_ENTRY, CONTROLLER_INDEX, + status); +} + +static void passkey_confirm(const u8_t *data, u16_t len) +{ + const struct gap_passkey_confirm_cmd *cmd = (void *) data; + struct ble_gap_conn_desc desc; + struct ble_sm_io pk; + u8_t status; + int rc; + + SYS_LOG_DBG(""); + + 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; + + 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; + +rsp: + tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_CONFIRM, CONTROLLER_INDEX, + status); +} + +static void start_direct_adv(const u8_t *data, u16_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; + + SYS_LOG_DBG(""); + + 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; + } + + 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, + (u8_t *) &rp, sizeof(rp)); + return; +fail: + 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) +{ + console_printf("conn param update complete; conn_handle=%d status=%d\n", + conn_handle, status); +} + +static int conn_param_update_slave(u16_t conn_handle, + const struct gap_conn_param_update_cmd *cmd) +{ + 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; + + 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; +} + +static int conn_param_update_master(u16_t conn_handle, + const struct gap_conn_param_update_cmd *cmd) +{ + 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); + } + + return rc; +} + +static void conn_param_update(struct os_event *ev) +{ + struct ble_gap_conn_desc desc; + int rc; + + SYS_LOG_DBG(""); + + 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.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; + } + +rsp: + SYS_LOG_ERR("Conn param update fail; rc=%d", rc); +} + +static void conn_param_update_async(const u8_t *data, u16_t len) +{ + const struct gap_conn_param_update_cmd *cmd = (void *) data; + update_params = *cmd; + + os_callout_reset(&update_params_co, 0); + + tester_rsp(BTP_SERVICE_ID_GAP, GAP_CONN_PARAM_UPDATE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); +} + +static void oob_legacy_set_data(const u8_t *data, u16_t len) +{ + 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)); + + tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_LEGACY_SET_DATA, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); +} + +static void oob_sc_get_local_data(const u8_t *data, u16_t len) +{ + 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); + + tester_send(BTP_SERVICE_ID_GAP, GAP_OOB_SC_GET_LOCAL_DATA, + CONTROLLER_INDEX, (u8_t *) &rp, sizeof(rp)); +} + +static void oob_sc_set_remote_data(const u8_t *data, u16_t len) +{ + 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); + + tester_rsp(BTP_SERVICE_ID_GAP, GAP_OOB_SC_SET_REMOTE_DATA, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); +} + +static void set_mitm(const u8_t *data, u16_t len) +{ + const struct gap_set_mitm_cmd *cmd = (void *) data; + + ble_hs_cfg.sm_mitm = cmd->mitm; + + tester_rsp(BTP_SERVICE_ID_GAP, GAP_SET_MITM, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); +} + +void tester_handle_gap(u8_t opcode, u8_t index, u8_t *data, + u16_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: + 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; + default: + tester_rsp(BTP_SERVICE_ID_GAP, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } +} + +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; + } + + 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(&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); +} + +u8_t tester_init_gap(void) +{ +#if MYNEWT_VAL(BLE_SM_SC) + 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; + } +#endif + + adv_buf = NET_BUF_SIMPLE(ADV_BUF_LEN); + + tester_init_gap_cb(BTP_STATUS_SUCCESS); + return BTP_STATUS_SUCCESS; +} + +u8_t tester_unregister_gap(void) +{ + return BTP_STATUS_SUCCESS; +} diff --git a/src/libs/mynewt-nimble/apps/bttester/src/gatt.c b/src/libs/mynewt-nimble/apps/bttester/src/gatt.c new file mode 100644 index 00000000..7e7d1d3b --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/gatt.c @@ -0,0 +1,2098 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* gatt.c - Bluetooth GATT Server Tester */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include <string.h> +#include <errno.h> +#include <assert.h> + +#include "host/ble_gap.h" +#include "host/ble_gatt.h" +#include "console/console.h" +#include "services/gatt/ble_svc_gatt.h" +#include "../../../nimble/host/src/ble_att_priv.h" +#include "../../../nimble/host/src/ble_gatt_priv.h" + +#include "bttester.h" + +#define CONTROLLER_INDEX 0 +#define MAX_BUFFER_SIZE 2048 + +/* 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 \ + ))) + +/* 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 \ + ))) + +#define PTS_SVC 0x0001 +#define PTS_CHR_READ 0x0002 +#define PTS_CHR_WRITE 0x0003 +#define PTS_CHR_RELIABLE_WRITE 0x0004 +#define PTS_CHR_WRITE_NO_RSP 0x0005 +#define PTS_CHR_READ_WRITE 0x0006 +#define PTS_CHR_READ_WRITE_ENC 0x0007 +#define PTS_CHR_READ_WRITE_AUTHEN 0x0008 +#define PTS_DSC_READ 0x0009 +#define PTS_DSC_WRITE 0x000a +#define PTS_DSC_READ_WRITE 0x000b +#define PTS_CHR_NOTIFY 0x0025 +#define PTS_LONG_CHR_READ_WRITE 0x0015 +#define PTS_LONG_CHR_READ_WRITE_ALT 0x0016 +#define PTS_LONG_DSC_READ_WRITE 0x001b +#define PTS_INC_SVC 0x001e +#define PTS_CHR_READ_WRITE_ALT 0x001f + +static uint8_t gatt_svr_pts_static_long_val[300]; +static uint8_t gatt_svr_pts_static_val[30]; +static uint8_t gatt_svr_pts_static_short_val; +static u8_t notify_state; +static u8_t indicate_state; +static uint16_t myconn_handle; +static struct os_callout notify_tx_timer; +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; +}; + +static int +gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle, + 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); + +static int +gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle, + 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); + +static int +gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle, + 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); + +static int +gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, + 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); + +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); + +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, + } }, + }, + + { + 0, /* No more services. */ + }, +}; + +static const struct ble_gatt_svc_def *inc_svcs[] = { + &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. */ + }, +}; + +static void attr_value_changed_ev(u16_t handle, struct os_mbuf *data) +{ + struct gatt_attr_value_changed_ev *ev; + struct os_mbuf *buf = os_msys_get(0, 0); + + SYS_LOG_DBG(""); + + 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)); + + 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) +{ + 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; + } + + 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); + + return 0; +} + +static uint16_t +extract_uuid16_from_pts_uuid128(const ble_uuid_t *uuid) +{ + const uint8_t *u8ptr; + uint16_t 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) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +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) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } +} + +static int +gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + 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; + } + + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static void start_server(u8_t *data, u16_t len) +{ + struct gatt_start_server_rp rp; + + SYS_LOG_DBG(""); + + ble_gatts_show_local(); + + ble_svc_gatt_changed(0x0001, 0xffff); + + rp.db_attr_off = 0; + rp.db_attr_cnt = 0; + + tester_send(BTP_SERVICE_ID_GATT, GATT_START_SERVER, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); +} + +/* Convert UUID from BTP command to bt_uuid */ +static u8_t btp2bt_uuid(const u8_t *uuid, u8_t len, + ble_uuid_any_t *bt_uuid) +{ + u16_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; +} + +/* + * gatt_buf - cache used by a gatt client (to cache data read/discovered) + * and gatt server (to store attribute user_data). + * It is not intended to be used by client and server at the same time. + */ +static struct { + u16_t len; + u8_t buf[MAX_BUFFER_SIZE]; +} gatt_buf; + +static void *gatt_buf_add(const void *data, size_t len) +{ + void *ptr = gatt_buf.buf + gatt_buf.len; + + if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) { + return NULL; + } + + if (data) { + memcpy(ptr, data, len); + } else { + (void)memset(ptr, 0, len); + } + + gatt_buf.len += len; + + SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); + + return ptr; +} + +static void *gatt_buf_reserve(size_t len) +{ + return gatt_buf_add(NULL, len); +} + +static void gatt_buf_clear(void) +{ + (void)memset(&gatt_buf, 0, sizeof(gatt_buf)); +} + +static void discover_destroy(void) +{ + gatt_buf_clear(); +} + +static void read_destroy() +{ + gatt_buf_clear(); +} + +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; + u8_t btp_opcode = (uint8_t) (int) arg; + + 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 (!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(); + + return 0; +} + +static void read(u8_t *data, u16_t len) +{ + const struct gatt_read_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + 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; + } + + return; + +fail: + 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) +{ + struct gatt_read_rp *rp = (void *) gatt_buf.buf; + u8_t btp_opcode = (uint8_t) (int) arg; + + 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 == 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; + } + + rp->data_length += attr->om->om_len; + + return 0; +} + +static void read_long(u8_t *data, u16_t len) +{ + const struct gatt_read_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + 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; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void read_multiple(u8_t *data, u16_t len) +{ + const struct gatt_read_multiple_cmd *cmd = (void *) data; + u16_t handles[cmd->handles_count]; + struct ble_gap_conn_desc conn; + 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) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + 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; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void write_without_rsp(u8_t *data, u16_t len, u8_t op, bool sign) +{ + const struct gatt_write_without_rsp_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + u8_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_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) +{ + uint8_t err = (uint8_t) error->status; + u8_t btp_opcode = (uint8_t) (int) arg; + + SYS_LOG_DBG(""); + + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, &err, sizeof(err)); + return 0; +} + +static void write(u8_t *data, u16_t len) +{ + const struct gatt_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + 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; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void write_long(u8_t *data, u16_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; + + 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"); + 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; + } + +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); +} + +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; + + SYS_LOG_DBG("Reliable write status %d", err); + + tester_send(BTP_SERVICE_ID_GATT, GATT_RELIABLE_WRITE, + CONTROLLER_INDEX, &err, sizeof(err)); + return 0; +} + +static void reliable_write(u8_t *data, u16_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; + + 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)); + /* 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; + + if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, + reliable_write_rsp, NULL)) { + goto fail; + } + + return; + +fail: + os_mbuf_free_chain(om); + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static struct bt_gatt_subscribe_params { + u16_t ccc_handle; + u16_t value; + u16_t value_handle; +} subscribe_params; + +static void read_uuid(u8_t *data, u16_t len) +{ + 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(""); + + 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; + } + + /* Clear buffer */ + read_destroy(); + + 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; + } + + return; + +fail: + 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) +{ + struct gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf; + struct gatt_service *service; + const ble_uuid_any_t *uuid; + u8_t uuid_length; + u8_t opcode = (u8_t) (int) arg; + + 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 == 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); + + 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; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + u16_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++; + + 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) +{ + struct gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf; + struct gatt_descriptor *dsc; + const ble_uuid_any_t *uuid; + u8_t uuid_length; + + 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 == 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); + + 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; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + u16_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++; + + return 0; +} + +static void disc_all_prim_svcs(u8_t *data, u16_t len) +{ + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + 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 (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_uuid_cb, + (void *) GATT_DISC_ALL_PRIM_SVCS)) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_PRIM_SVCS, + CONTROLLER_INDEX, BTP_STATUS_FAILED); +} + +static void disc_all_desc(u8_t *data, u16_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; + + SYS_LOG_DBG(""); + + 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; + } + + 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); + + SYS_LOG_DBG("rc=%d", rc); + + if (rc) { + discover_destroy(); + goto fail; + } + + return; + +fail: + 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) +{ + 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; + u8_t uuid_length; + + 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 == 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); + + + 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; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + u16_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++; + + 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) +{ + struct gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf; + struct gatt_characteristic *chrc; + const ble_uuid_any_t *uuid; + u8_t btp_opcode = (uint8_t) (int) arg; + u8_t uuid_length; + + 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 == 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); + + 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; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + u16_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++; + + return 0; +} + +static void disc_chrc_uuid(u8_t *data, u16_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; + + SYS_LOG_DBG(""); + + 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 (!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); + + 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; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_CHRC_UUID, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void disc_prim_uuid(u8_t *data, u16_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; + + SYS_LOG_DBG(""); + + 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 (!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; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void disc_all_chrc(u8_t *data, u16_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; + + 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; + } + + 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); + + 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; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_CHRC, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void find_included(u8_t *data, u16_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; + + SYS_LOG_DBG(""); + + 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; + } + + 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; + } + + return; + +fail: + 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) +{ + SYS_LOG_DBG(""); + + if (error->status) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + + return 0; +} + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + + return 0; +} + +static void exchange_mtu(u8_t *data, u16_t len) +{ + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + 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; + } + + return; +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, + CONTROLLER_INDEX, BTP_STATUS_FAILED); +} + +static int enable_subscription(u16_t conn_handle, u16_t ccc_handle, + u16_t value) +{ + u8_t op; + + SYS_LOG_DBG(""); + + 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; + } + + subscribe_params.ccc_handle = value; + + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + return 0; +} + +static int disable_subscription(u16_t conn_handle, u16_t ccc_handle) +{ + u16_t value = 0x00; + + 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; + } + + if (ble_gattc_write_no_rsp_flat(conn_handle, ccc_handle, + &value, sizeof(value))) { + return -EINVAL; + } + + subscribe_params.ccc_handle = 0; + return 0; +} + +static void config_subscription(u8_t *data, u16_t len, u8_t op) +{ + const struct gatt_cfg_notify_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + u16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); + u8_t status; + int rc; + + 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; + } + + if (cmd->enable) { + u16_t value; + + 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; + } + + 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); + + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); +} + +#define BTP_PERM_F_READ 0x01 +#define BTP_PERM_F_WRITE 0x02 +#define BTP_PERM_F_READ_ENC 0x04 +#define BTP_PERM_F_WRITE_ENC 0x08 +#define BTP_PERM_F_READ_AUTHEN 0x10 +#define BTP_PERM_F_WRITE_AUTHEN 0x20 +#define BTP_PERM_F_READ_AUTHOR 0x40 +#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, +}; + +static u8_t flags_hs2btp(u8_t flags) +{ + int i; + u8_t ret = 0; + + for (i = 0; i < 8; ++i) { + if (flags & BIT(i)) { + ret |= flags_hs2btp_map[i]; + } + } + + return ret; +} + +static void get_attrs(u8_t *data, u16_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); + u16_t start_handle, end_handle; + struct ble_att_svr_entry *entry = NULL; + ble_uuid_any_t uuid; + ble_uuid_t *uuid_ptr = NULL; + u8_t count = 0; + char str[BLE_UUID_STR_LEN]; + + 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); + + 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); + + 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)); + + 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; + } + + 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); + } + + count++; + + entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); + } + + rp->attrs_count = count; + + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, + CONTROLLER_INDEX, buf); + + goto free; +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +free: + os_mbuf_free_chain(buf); +} + +static void get_attr_val(u8_t *data, u16_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); + u16_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); + + 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); + + 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); + + 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); + + 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); + + goto free; + } + +free: + os_mbuf_free_chain(buf); +} + +static void change_database(u8_t *data, u16_t len) +{ + const struct gatt_change_database *cmd = (void *) data; + + SYS_LOG_DBG("") + + ble_gatts_show_local(); + + ble_svc_gatt_changed(cmd->start_handle, cmd->end_handle); + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_CHANGE_DATABASE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + + return; +} + +static void supported_commands(u8_t *data, u16_t len) +{ + u8_t cmds[4]; + struct gatt_read_supported_commands_rp *rp = (void *) cmds; + + SYS_LOG_DBG(""); + + 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); +#if 0 + 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_send(BTP_SERVICE_ID_GATT, GATT_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (u8_t *) rp, sizeof(cmds)); +} + +enum attr_type { + BLE_GATT_ATTR_SVC = 0, + BLE_GATT_ATTR_CHR, + BLE_GATT_ATTR_DSC, +}; + +void tester_handle_gatt(u8_t opcode, u8_t index, u8_t *data, + u16_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; +#if 0 + 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; + } +} + +int tester_gatt_notify_rx_ev(u16_t conn_handle, u16_t attr_handle, + u8_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; + + SYS_LOG_DBG(""); + + if (!subscribe_params.ccc_handle) { + 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)); + + addr = &conn.peer_ota_addr; + + ev->address_type = addr->type; + memcpy(ev->address, addr->val, sizeof(ev->address)); + ev->type = (u8_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); + +fail: + os_mbuf_free_chain(buf); + return 0; +} + +void notify_test_stop(void) +{ + os_callout_stop(¬ify_tx_timer); +} + +void notify_test_reset(void) +{ + int rc; + + rc = os_callout_reset(¬ify_tx_timer, OS_TICKS_PER_SEC); + assert(rc == 0); +} + +void notify_test(struct os_event *ev) +{ + static uint8_t ntf[1]; + struct os_mbuf *om; + int rc; + + if (!notify_state && !indicate_state) { + notify_test_stop(); + notify_value = 90; + return; + } + + ntf[0] = notify_value; + + notify_value++; + if (notify_value == 160) { + notify_value = 90; + } + + om = ble_hs_mbuf_from_flat(ntf, sizeof(ntf)); + + if (notify_state) { + rc = ble_gattc_notify_custom(myconn_handle, notify_handle, om); + assert(rc == 0); + } + + if (indicate_state) { + rc = ble_gattc_indicate_custom(myconn_handle, notify_handle, om); + assert(rc == 0); + } +} + +int tester_gatt_subscribe_ev(u16_t conn_handle, u16_t attr_handle, u8_t reason, + u8_t prev_notify, u8_t cur_notify, + u8_t prev_indicate, u8_t cur_indicate) +{ + 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) { + 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 (notify_state || indicate_state) { + notify_test_reset(); + } else { + notify_test_stop(); + } + + return 0; +} + +void +gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) +{ + 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; + + 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; + + default: + assert(0); + break; + } +} + +int gatt_svr_init(void) +{ + int 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_count_cfg(gatt_svr_svcs); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_add_svcs(gatt_svr_svcs); + if (rc != 0) { + return rc; + } + + return 0; +} + +u8_t tester_init_gatt(void) +{ + os_callout_init(¬ify_tx_timer, os_eventq_dflt_get(), + notify_test, NULL); + + return BTP_STATUS_SUCCESS; +} + +u8_t tester_unregister_gatt(void) +{ + return BTP_STATUS_SUCCESS; +} diff --git a/src/libs/mynewt-nimble/apps/bttester/src/glue.c b/src/libs/mynewt-nimble/apps/bttester/src/glue.c new file mode 100644 index 00000000..6cd7643c --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/glue.c @@ -0,0 +1,129 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include "syscfg/syscfg.h" + +#if !MYNEWT_VAL(BLE_MESH) +#include <assert.h> +#include <string.h> +#include "os/os.h" +#include "os/os_mbuf.h" +#include "glue.h" + + +#define ASSERT_NOT_CHAIN(om) assert(SLIST_NEXT(om, om_next) == NULL) + +const char *bt_hex(const void *buf, size_t len) +{ + static const char hex[] = "0123456789abcdef"; + static char hexbufs[4][137]; + static u8_t curbuf; + const u8_t *b = buf; + char *str; + int i; + + str = hexbufs[curbuf++]; + curbuf %= ARRAY_SIZE(hexbufs); + + 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]; + } + + str[i * 2] = '\0'; + + return str; +} + +struct os_mbuf * NET_BUF_SIMPLE(uint16_t size) +{ + struct os_mbuf *buf; + + buf = os_msys_get(size, 0); + assert(buf); + + return buf; +} + +/* This is by purpose */ +void net_buf_simple_init(struct os_mbuf *buf, + 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; +} + +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); +} + +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); +} + +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); +} + +void +net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val) +{ + os_mbuf_append(om, &val, 1); + ASSERT_NOT_CHAIN(om); +} + +void* +net_buf_simple_add(struct os_mbuf *om, uint8_t len) +{ + void * tmp; + + tmp = os_mbuf_extend(om, len); + ASSERT_NOT_CHAIN(om); + + 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]; + + assert(headroom >= len); + om->om_data -= len; + om->om_len += len; + + return om->om_data; +} +#endif diff --git a/src/libs/mynewt-nimble/apps/bttester/src/glue.h b/src/libs/mynewt-nimble/apps/bttester/src/glue.h new file mode 100644 index 00000000..e563331e --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/glue.h @@ -0,0 +1,63 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#ifndef __GLUE_H__ +#define __GLUE_H__ + +#include "os/endian.h" + +#define u8_t uint8_t +#define s8_t int8_t +#define u16_t uint16_t +#define u32_t uint32_t +#define s32_t int32_t + +#ifndef BIT +#define BIT(n) (1UL << (n)) +#endif + +#define __packed __attribute__((__packed__)) + +#define sys_le16_to_cpu le16toh + +struct bt_data { + u8_t type; + u8_t data_len; + const u8_t *data; +}; + +#define BT_DATA(_type, _data, _data_len) \ + { \ + .type = (_type), \ + .data_len = (_data_len), \ + .data = (const u8_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); + +#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); + +#endif /* __GLUE_H__ */ diff --git a/src/libs/mynewt-nimble/apps/bttester/src/l2cap.c b/src/libs/mynewt-nimble/apps/bttester/src/l2cap.c new file mode 100644 index 00000000..45b904a1 --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/l2cap.c @@ -0,0 +1,477 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* l2cap.c - Bluetooth L2CAP Tester */ + +/* + * Copyright (c) 2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) + +#include "console/console.h" +#include "host/ble_gap.h" +#include "host/ble_l2cap.h" + +#include "bttester.h" + +#define CONTROLLER_INDEX 0 +#define CHANNELS MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) +#define TESTER_COC_MTU (230) +#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) +]; + +struct os_mbuf_pool sdu_os_mbuf_pool; +static struct os_mempool sdu_coc_mbuf_mempool; + +static struct channel { + u8_t chan_id; /* Internal number that identifies L2CAP channel. */ + u8_t state; + struct ble_l2cap_chan *chan; +} channels[CHANNELS]; + +static u8_t recv_cb_buf[TESTER_COC_MTU + sizeof(struct l2cap_data_received_ev)]; + +struct channel *find_channel(struct ble_l2cap_chan *chan) { + int i; + + for (i = 0; i < CHANNELS; ++i) { + if (channels[i].chan == chan) { + return &channels[i]; + } + } + + return NULL; +} + +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)); + + os_mbuf_free_chain(sdu); + sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); + assert(sdu != NULL); + + 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) +{ + struct l2cap_data_received_ev *ev = (void *) recv_cb_buf; + struct channel *channel = arg; + + ev->chan_id = channel->chan_id; + ev->data_length = buf->om_len; + memcpy(ev->data, buf->om_data, buf->om_len); + + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DATA_RECEIVED, + CONTROLLER_INDEX, recv_cb_buf, sizeof(*ev) + buf->om_len); + + tester_l2cap_coc_recv(chan, buf); +} + +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); + } +} + +static struct channel *get_free_channel(void) +{ + u8_t i; + struct channel *chan; + + for (i = 0; i < CHANNELS; i++) { + if (channels[i].state) { + continue; + } + + chan = &channels[i]; + chan->chan_id = i; + + return chan; + } + + return NULL; +} + +static void connected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, + void *arg) +{ + struct l2cap_connected_ev ev; + struct ble_gap_conn_desc desc; + struct channel *channel; + + channel = get_free_channel(); + if (!channel) { + assert(0); + } + + channel->chan = chan; + channel->state = 0; + + ev.chan_id = channel->chan_id; + channel->state = 1; + channel->chan = chan; + /* TODO: ev.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)); + } + + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_CONNECTED, CONTROLLER_INDEX, + (u8_t *) &ev, sizeof(ev)); +} + +static void disconnected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan, + void *arg) +{ + struct l2cap_disconnected_ev ev; + struct ble_gap_conn_desc desc; + struct channel *channel; + + memset(&ev, 0, sizeof(struct l2cap_disconnected_ev)); + + channel = find_channel(chan); + if (channel != NULL) { + channel->state = 0; + channel->chan = chan; + + ev.chan_id = channel->chan_id; + /* TODO: ev.result */ + /* TODO: ev.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)); + } + + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DISCONNECTED, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static int accept_cb(uint16_t conn_handle, uint16_t peer_mtu, + struct ble_l2cap_chan *chan) +{ + struct os_mbuf *sdu_rx; + + 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; + } + + ble_l2cap_recv_ready(chan, sdu_rx); + + return 0; +} + +static int +tester_l2cap_event(struct ble_l2cap_event *event, void *arg) +{ + struct ble_l2cap_chan_info chan_info; + + switch (event->type) { + case BLE_L2CAP_EVENT_COC_CONNECTED: + if (event->connect.status) { + console_printf("LE COC error: %d\n", event->connect.status); + disconnected_cb(event->connect.conn_handle, + event->connect.chan, arg); + return 0; + } + + ble_l2cap_get_chan_info(event->connect.chan, &chan_info); + + console_printf("LE COC connected, conn: %d, chan: 0x%08lx, scid: 0x%04x, " + "dcid: 0x%04x, our_mtu: 0x%04x, peer_mtu: 0x%04x\n", + event->connect.conn_handle, + (uint32_t) event->connect.chan, + chan_info.scid, + chan_info.dcid, + chan_info.our_l2cap_mtu, + chan_info.peer_l2cap_mtu); + + connected_cb(event->connect.conn_handle, + event->connect.chan, arg); + + return 0; + case BLE_L2CAP_EVENT_COC_DISCONNECTED: + console_printf("LE CoC disconnected, chan: 0x%08lx\n", + (uint32_t) event->disconnect.chan); + + disconnected_cb(event->disconnect.conn_handle, + event->disconnect.chan, arg); + return 0; + case BLE_L2CAP_EVENT_COC_ACCEPT: + 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); + + 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, + event->receive.sdu_rx->om_len); + 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; + default: + return 0; + } +} + +static void connect(u8_t *data, u16_t len) +{ + const struct l2cap_connect_cmd *cmd = (void *) data; + struct l2cap_connect_rp rp; + struct ble_gap_conn_desc desc; + struct channel *chan; + struct os_mbuf *sdu_rx; + ble_addr_t *addr = (void *) data; + int rc; + + SYS_LOG_DBG("connect: type: %d addr: %s", addr->type, bt_hex(addr->val, 6)); + + rc = ble_gap_conn_find_by_addr(addr, &desc); + if (rc) { + SYS_LOG_ERR("GAP conn find failed"); + goto fail; + } + + chan = get_free_channel(); + if (!chan) { + SYS_LOG_ERR("No free channels"); + goto fail; + } + + sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0); + if (sdu_rx == NULL) { + SYS_LOG_ERR("Failed to alloc buf"); + goto fail; + } + + rc = ble_l2cap_connect(desc.conn_handle, htole16(cmd->psm), + TESTER_COC_MTU, sdu_rx, + tester_l2cap_event, chan); + if (rc) { + SYS_LOG_ERR("L2CAP connect failed\n"); + goto fail; + } + + rp.chan_id = chan->chan_id; + + tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX, + (u8_t *) &rp, sizeof(rp)); + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void disconnect(u8_t *data, u16_t len) +{ + const struct l2cap_disconnect_cmd *cmd = (void *) data; + struct channel *chan; + u8_t status; + int err; + + SYS_LOG_DBG(""); + + chan = &channels[cmd->chan_id]; + + err = ble_l2cap_disconnect(chan->chan); + if (err) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + status = BTP_STATUS_SUCCESS; + +rsp: + tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_DISCONNECT, CONTROLLER_INDEX, + status); +} + +static void send_data(u8_t *data, u16_t len) +{ + const struct l2cap_send_data_cmd *cmd = (void *) data; + struct channel *chan = &channels[cmd->chan_id]; + struct os_mbuf *sdu_tx = NULL; + int rc; + u16_t data_len = sys_le16_to_cpu(cmd->data_len); + + SYS_LOG_DBG("cmd->chan_id=%d", cmd->chan_id); + + /* 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; + } + + 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; + } + + 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); +} + +static void listen(u8_t *data, u16_t len) +{ + const struct l2cap_listen_cmd *cmd = (void *) data; + int rc; + + SYS_LOG_DBG(""); + + /* TODO: Handle cmd->transport flag */ + rc = ble_l2cap_create_server(cmd->psm, TESTER_COC_MTU, + tester_l2cap_event, NULL); + if (rc) { + goto fail; + } + + 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); +} + +static void supported_commands(u8_t *data, u16_t len) +{ + u8_t cmds[1]; + struct l2cap_read_supported_commands_rp *rp = (void *) 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_send(BTP_SERVICE_ID_L2CAP, L2CAP_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (u8_t *) rp, sizeof(cmds)); +} + +void tester_handle_l2cap(u8_t opcode, u8_t index, u8_t *data, + u16_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; + default: + tester_rsp(BTP_SERVICE_ID_L2CAP, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } +} + +u8_t tester_init_l2cap(void) +{ + 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); + + 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; +} + +u8_t tester_unregister_l2cap(void) +{ + return BTP_STATUS_SUCCESS; +} + +#endif diff --git a/src/libs/mynewt-nimble/apps/bttester/src/main.c b/src/libs/mynewt-nimble/apps/bttester/src/main.c new file mode 100644 index 00000000..ea130805 --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/main.c @@ -0,0 +1,72 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* main.c - Application main entry point */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "sysinit/sysinit.h" + +#include "modlog/modlog.h" +#include "host/ble_uuid.h" +#include "host/ble_hs.h" + +#include "bttester.h" + +static void on_reset(int reason) +{ + MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason); +} + +static void on_sync(void) +{ + MODLOG_DFLT(INFO, "Bluetooth initialized\n"); + + tester_init(); +} + +int main(int argc, char **argv) +{ + int rc; + +#ifdef ARCH_sim + mcu_sim_parse_args(argc, argv); +#endif + + /* 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; + + rc = gatt_svr_init(); + assert(rc == 0); + + while (1) { + os_eventq_run(os_eventq_dflt_get()); + } + return 0; +} diff --git a/src/libs/mynewt-nimble/apps/bttester/src/mesh.c b/src/libs/mynewt-nimble/apps/bttester/src/mesh.c new file mode 100644 index 00000000..e18a2a4e --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/mesh.c @@ -0,0 +1,970 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +/* mesh.c - Bluetooth Mesh Tester */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_MESH) + +#include <errno.h> + +#include "mesh/mesh.h" +#include "mesh/glue.h" +#include "mesh/testing.h" +#include "console/console.h" + +#include "bttester.h" + +extern u8_t own_addr_type; + +#define CONTROLLER_INDEX 0 +#define CID_LOCAL 0xffff + +/* Health server data */ +#define CUR_FAULTS_MAX 4 +#define HEALTH_TEST_ID 0x00 + +static u8_t cur_faults[CUR_FAULTS_MAX]; +static u8_t reg_faults[CUR_FAULTS_MAX * 2]; + +/* Provision node data */ +static u8_t net_key[16]; +static u16_t net_key_idx; +static u8_t flags; +static u32_t iv_index; +static u16_t addr; +static u8_t dev_key[16]; +static u8_t input_size; + +/* Configured provisioning data */ +static u8_t dev_uuid[16]; +static u8_t static_auth[16]; + +/* Vendor Model data */ +#define VND_MODEL_ID_1 0x1234 + +/* Model send data */ +#define MODEL_BOUNDS_MAX 2 + +static struct model_data { + struct bt_mesh_model *model; + u16_t addr; + u16_t appkey_idx; +} model_bound[MODEL_BOUNDS_MAX]; + +static struct { + u16_t local; + u16_t dst; + u16_t net_idx; +} net = { + .local = BT_MESH_ADDR_UNASSIGNED, + .dst = BT_MESH_ADDR_UNASSIGNED, +}; + +static void supported_commands(u8_t *data, u16_t len) +{ + struct os_mbuf *buf = NET_BUF_SIMPLE(BTP_DATA_MAX_SIZE); + + 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); +#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); +#endif /* CONFIG_BT_TESTING */ + tester_set_bit(buf->om_data, MESH_PROXY_IDENTITY); + + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, buf); +} + +static struct bt_mesh_cfg_srv cfg_srv = { + .relay = BT_MESH_RELAY_ENABLED, + .beacon = BT_MESH_BEACON_ENABLED, +#if MYNEWT_VAL(BLE_MESH_FRIEND) + .frnd = BT_MESH_FRIEND_ENABLED, +#else + .frnd = BT_MESH_FRIEND_NOT_SUPPORTED, +#endif +#if MYNEWT_VAL(BLE_MESH_GATT_PROXY) + .gatt_proxy = BT_MESH_GATT_PROXY_ENABLED, +#else + .gatt_proxy = BT_MESH_GATT_PROXY_NOT_SUPPORTED, +#endif + .default_ttl = 7, + + /* 3 transmissions with 20ms interval */ + .net_transmit = BT_MESH_TRANSMIT(2, 20), + .relay_retransmit = BT_MESH_TRANSMIT(2, 20), +}; + +static void get_faults(u8_t *faults, u8_t faults_size, u8_t *dst, u8_t *count) +{ + u8_t i, limit = *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, u8_t *test_id, + u16_t *company_id, u8_t *faults, u8_t *fault_count) +{ + SYS_LOG_DBG(""); + + *test_id = HEALTH_TEST_ID; + *company_id = CID_LOCAL; + + get_faults(cur_faults, sizeof(cur_faults), faults, fault_count); + + return 0; +} + +static int fault_get_reg(struct bt_mesh_model *model, u16_t company_id, + u8_t *test_id, u8_t *faults, u8_t *fault_count) +{ + SYS_LOG_DBG("company_id 0x%04x", company_id); + + if (company_id != CID_LOCAL) { + return -EINVAL; + } + + *test_id = HEALTH_TEST_ID; + + get_faults(reg_faults, sizeof(reg_faults), faults, fault_count); + + return 0; +} + +static int fault_clear(struct bt_mesh_model *model, uint16_t company_id) +{ + SYS_LOG_DBG("company_id 0x%04x", company_id); + + if (company_id != CID_LOCAL) { + return -EINVAL; + } + + memset(reg_faults, 0, sizeof(reg_faults)); + + return 0; +} + +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); + + if (company_id != CID_LOCAL || test_id != HEALTH_TEST_ID) { + return -EINVAL; + } + + 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, +}; + +static struct bt_mesh_health_srv health_srv = { + .cb = &health_srv_cb, +}; + +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); +} + +static struct bt_mesh_cfg_cli cfg_cli = { +}; + +void show_faults(u8_t test_id, u16_t cid, u8_t *faults, size_t fault_count) +{ + size_t i; + + 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); + + 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, u16_t addr, + u8_t test_id, u16_t cid, u8_t *faults, + size_t 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, +}; + +static struct bt_mesh_model root_models[] = { + BT_MESH_MODEL_CFG_SRV(&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), +}; + +static struct bt_mesh_elem elements[] = { + BT_MESH_ELEM(0, root_models, vnd_models), +}; + +static void link_open(bt_mesh_prov_bearer_t bearer) +{ + struct mesh_prov_link_open_ev ev; + + 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"); + + return; + } + + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_OPEN, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static void link_close(bt_mesh_prov_bearer_t bearer) +{ + struct mesh_prov_link_closed_ev ev; + + 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"); + + return; + } + + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROV_LINK_CLOSED, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static int output_number(bt_mesh_output_action_t action, u32_t number) +{ + struct mesh_out_number_action_ev ev; + + 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); + + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_OUT_NUMBER_ACTION, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); + + return 0; +} + +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); + + SYS_LOG_DBG("str %s", str); + + net_buf_simple_init(buf, 0); + + ev = net_buf_simple_add(buf, sizeof(*ev)); + ev->string_len = strlen(str); + + 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); + + os_mbuf_free_chain(buf); + return 0; +} + +static int input(bt_mesh_input_action_t action, u8_t size) +{ + struct mesh_in_action_ev ev; + + SYS_LOG_DBG("action 0x%04x number 0x%02x", action, size); + + input_size = size; + + ev.action = sys_cpu_to_le16(action); + ev.size = size; + + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_IN_ACTION, CONTROLLER_INDEX, + (u8_t *) &ev, sizeof(ev)); + + return 0; +} + +static void prov_complete(u16_t net_idx, u16_t 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; + + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_PROVISIONED, CONTROLLER_INDEX, + NULL, 0); +} + +static void prov_reset(void) +{ + SYS_LOG_DBG(""); + + 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), +}; + +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, +}; + +static void config_prov(u8_t *data, u16_t len) +{ + const struct mesh_config_provisioning_cmd *cmd = (void *) data; + + SYS_LOG_DBG(""); + + 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); + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_CONFIG_PROVISIONING, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); +} + +static void provision_node(u8_t *data, u16_t len) +{ + const struct mesh_provision_node_cmd *cmd = (void *) data; + + SYS_LOG_DBG(""); + + 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); + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_PROVISION_NODE, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); +} + +static void init(u8_t *data, u16_t len) +{ + u8_t status = BTP_STATUS_SUCCESS; + int err; + + SYS_LOG_DBG(""); + + err = bt_mesh_init(own_addr_type, &prov, &comp); + if (err) { + status = BTP_STATUS_FAILED; + + 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; + } + } + + /* Set device key for vendor model */ + vnd_models[0].keys[0] = BT_MESH_KEY_DEV; + +rsp: + tester_rsp(BTP_SERVICE_ID_MESH, MESH_INIT, CONTROLLER_INDEX, + status); +} + +static void reset(u8_t *data, u16_t len) +{ + SYS_LOG_DBG(""); + + bt_mesh_reset(); + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_RESET, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); +} + +static void input_number(u8_t *data, u16_t len) +{ + const struct mesh_input_number_cmd *cmd = (void *) data; + u8_t status = BTP_STATUS_SUCCESS; + u32_t number; + int err; + + number = sys_le32_to_cpu(cmd->number); + + SYS_LOG_DBG("number 0x%04lx", number); + + err = bt_mesh_input_number(number); + if (err) { + status = BTP_STATUS_FAILED; + } + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_INPUT_NUMBER, CONTROLLER_INDEX, + status); +} + +static void input_string(u8_t *data, u16_t len) +{ + const struct mesh_input_string_cmd *cmd = (void *) data; + u8_t status = BTP_STATUS_SUCCESS; + u8_t str_auth[16]; + int err; + + 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; + } + + strncpy((char *)str_auth, (char *)cmd->string, cmd->string_len); + + 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); +} + +static void ivu_test_mode(u8_t *data, u16_t len) +{ + const struct mesh_ivu_test_mode_cmd *cmd = (void *) data; + + SYS_LOG_DBG("enable 0x%02x", cmd->enable); + + bt_mesh_iv_update_test(cmd->enable ? true : false); + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_IVU_TEST_MODE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); +} + +static void ivu_toggle_state(u8_t *data, u16_t len) +{ + bool result; + + SYS_LOG_DBG(""); + + 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); +} + +static void lpn(u8_t *data, u16_t len) +{ + struct mesh_lpn_set_cmd *cmd = (void *) data; + bool enable; + int err; + + 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); + } + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); +} + +static void lpn_poll(u8_t *data, u16_t len) +{ + int err; + + SYS_LOG_DBG(""); + + 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); +} + +static void net_send(u8_t *data, u16_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 = BT_MESH_KEY_DEV, + .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); + + 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); + } + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_NET_SEND, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); + + os_mbuf_free_chain(msg); +} + +static void health_generate_faults(u8_t *data, u16_t len) +{ + struct mesh_health_generate_faults_rp *rp; + struct os_mbuf *buf = NET_BUF_SIMPLE(sizeof(*rp) + sizeof(cur_faults) + + sizeof(reg_faults)); + u8_t some_faults[] = { 0x01, 0x02, 0x03, 0xff, 0x06 }; + u8_t cur_faults_count, reg_faults_count; + + 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; + + 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]); + + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_HEALTH_GENERATE_FAULTS, + CONTROLLER_INDEX, buf); +} + +static void health_clear_faults(u8_t *data, u16_t len) +{ + SYS_LOG_DBG(""); + + memset(cur_faults, 0, sizeof(cur_faults)); + memset(reg_faults, 0, sizeof(reg_faults)); + + bt_mesh_fault_update(&elements[0]); + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_HEALTH_CLEAR_FAULTS, + CONTROLLER_INDEX, BTP_STATUS_SUCCESS); +} + +static void model_send(u8_t *data, u16_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; + u16_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; + + break; + } + } + + if (!model) { + SYS_LOG_ERR("Model not found"); + err = -EINVAL; + + goto fail; + } + + 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); + + 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); + + os_mbuf_free_chain(msg); +} + +#if MYNEWT_VAL(BLE_MESH_TESTING) +static void lpn_subscribe(u8_t *data, u16_t len) +{ + struct mesh_lpn_subscribe_cmd *cmd = (void *) data; + u16_t address = sys_le16_to_cpu(cmd->address); + int err; + + 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); + } + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_SUBSCRIBE, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); +} + +static void lpn_unsubscribe(u8_t *data, u16_t len) +{ + struct mesh_lpn_unsubscribe_cmd *cmd = (void *) data; + u16_t address = sys_le16_to_cpu(cmd->address); + int err; + + 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); + } + + tester_rsp(BTP_SERVICE_ID_MESH, MESH_LPN_UNSUBSCRIBE, CONTROLLER_INDEX, + err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); +} + +static void rpl_clear(u8_t *data, u16_t len) +{ + int err; + + SYS_LOG_DBG(""); + + 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); +} +#endif /* MYNEWT_VAL(BLE_MESH_TESTING) */ + +static void proxy_identity_enable(u8_t *data, u16_t len) +{ + int err; + + SYS_LOG_DBG(""); + + 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); +} + +void tester_handle_mesh(u8_t opcode, u8_t index, u8_t *data, u16_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; +#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; +#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; + } +} + +void net_recv_ev(u8_t ttl, u8_t ctl, u16_t src, u16_t dst, const void *payload, + size_t payload_len) +{ + 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); + + if (payload_len > net_buf_simple_tailroom(buf)) { + SYS_LOG_ERR("Payload size exceeds buffer size"); + + 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); + + tester_send_buf(BTP_SERVICE_ID_MESH, MESH_EV_NET_RECV, CONTROLLER_INDEX, + buf); +done: + os_mbuf_free_chain(buf); +} + +static void model_bound_cb(u16_t addr, struct bt_mesh_model *model, + u16_t key_idx) +{ + int i; + + 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; + + return; + } + } + + SYS_LOG_ERR("model_bound is full"); +} + +static void model_unbound_cb(u16_t addr, struct bt_mesh_model *model, + u16_t key_idx) +{ + int i; + + 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; + + return; + } + } + + SYS_LOG_INF("model not found"); +} + +static void invalid_bearer_cb(u8_t opcode) +{ + struct mesh_invalid_bearer_ev ev = { + .opcode = opcode, + }; + + SYS_LOG_DBG("opcode 0x%02x", opcode); + + tester_send(BTP_SERVICE_ID_MESH, MESH_EV_INVALID_BEARER, + CONTROLLER_INDEX, (u8_t *) &ev, sizeof(ev)); +} + +static void incomp_timer_exp_cb(void) +{ + 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, +}; + +u8_t tester_init_mesh(void) +{ + health_pub_init(); + + if (IS_ENABLED(CONFIG_BT_TESTING)) { + bt_test_cb_register(&bt_test_cb); + } + + return BTP_STATUS_SUCCESS; +} + +u8_t tester_unregister_mesh(void) +{ + return BTP_STATUS_SUCCESS; +} + +#endif /* MYNEWT_VAL(BLE_MESH) */ diff --git a/src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c b/src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c new file mode 100644 index 00000000..379345a0 --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/rtt_pipe.c @@ -0,0 +1,136 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BTTESTER_PIPE_RTT) + +#include "os/mynewt.h" +#include "console/console.h" +#include "rtt/SEGGER_RTT.h" + +#include "bttester_pipe.h" + +static struct hal_timer rtt_timer; + +static bttester_pipe_recv_cb app_cb; + +static u8_t *recv_buf; +static size_t recv_buf_len; +static size_t recv_off; + +static uint8_t rtt_buf_up[MYNEWT_VAL(BTTESTER_RTT_BUFFER_SIZE_UP)]; +static uint8_t rtt_buf_down[MYNEWT_VAL(BTTESTER_RTT_BUFFER_SIZE_DOWN)]; +static int rtt_index_up, rtt_index_down; + +#define RTT_INPUT_POLL_INTERVAL_MIN 10 /* ms */ +#define RTT_INPUT_POLL_INTERVAL_STEP 10 /* ms */ +#define RTT_INPUT_POLL_INTERVAL_MAX 250 /* ms */ + +static int rtt_pipe_get_char(unsigned int index) +{ + char c; + int r; + + r = (int)SEGGER_RTT_Read(index, &c, 1u); + if (r == 1) { + r = (int)(unsigned char)c; + } else { + r = -1; + } + + return r; +} + +static void +rtt_pipe_poll_func(void *arg) +{ + static uint32_t itvl_ms = RTT_INPUT_POLL_INTERVAL_MIN; + static int key = -1; + int avail = recv_buf_len - recv_off; + + if (key < 0) { + key = rtt_pipe_get_char((unsigned int) rtt_index_down); + } + + if (key < 0) { + itvl_ms += RTT_INPUT_POLL_INTERVAL_STEP; + itvl_ms = min(itvl_ms, RTT_INPUT_POLL_INTERVAL_MAX); + } else { + while (key >= 0 && avail > 0) { + recv_buf[recv_off] = (u8_t) key; + recv_off++; + avail = recv_buf_len - recv_off; + key = rtt_pipe_get_char((unsigned int) rtt_index_down); + } + + /* + * Call application callback with received data. Application + * may provide new buffer or alter data offset. + */ + recv_buf = app_cb(recv_buf, &recv_off); + + itvl_ms = RTT_INPUT_POLL_INTERVAL_MIN; + } + + os_cputime_timer_relative(&rtt_timer, itvl_ms * 1000); +} + +int +bttester_pipe_send(const u8_t *data, int len) +{ + SEGGER_RTT_Write((unsigned int) rtt_index_up, data, (unsigned int) len); + return 0; +} + +void +bttester_pipe_register(u8_t *buf, size_t len, bttester_pipe_recv_cb cb) +{ + recv_buf = buf; + recv_buf_len = len; + app_cb = cb; +} + +int +bttester_pipe_init(void) +{ + rtt_index_up = SEGGER_RTT_AllocUpBuffer(MYNEWT_VAL(BTTESTER_RTT_BUFFER_NAME), + rtt_buf_up, sizeof(rtt_buf_up), + SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL); + + if (rtt_index_up < 0) { + return -1; + } + + rtt_index_down = SEGGER_RTT_AllocDownBuffer(MYNEWT_VAL(BTTESTER_RTT_BUFFER_NAME), + rtt_buf_down, sizeof(rtt_buf_down), + SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL); + + if (rtt_index_down < 0) { + return -1; + } + + console_printf("Using up-buffer #%d\n", rtt_index_up); + console_printf("Using down-buffer #%d\n", rtt_index_down); + + os_cputime_timer_init(&rtt_timer, rtt_pipe_poll_func, NULL); + os_cputime_timer_relative(&rtt_timer, 200000); + return 0; +} +#endif /* MYNEWT_VAL(BTTESTER_PIPE_RTT) */ diff --git a/src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c b/src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c new file mode 100644 index 00000000..ecbefa02 --- /dev/null +++ b/src/libs/mynewt-nimble/apps/bttester/src/uart_pipe.c @@ -0,0 +1,281 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BTTESTER_PIPE_UART) + +#include "os/mynewt.h" +#include "uart/uart.h" + +#include "bttester_pipe.h" + +static u8_t *recv_buf; +static size_t recv_buf_len; +static bttester_pipe_recv_cb app_cb; +static size_t recv_off; + +struct uart_pipe_ring { + uint8_t head; + uint8_t tail; + uint16_t size; + uint8_t *buf; +}; + +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); +static console_write_char write_char_cb; + +static struct uart_pipe_ring cr_rx; +static uint8_t cr_rx_buf[MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)]; +static volatile bool uart_console_rx_stalled; + +struct os_event rx_ev; + +static inline int +inc_and_wrap(int i, int max) +{ + return (i + 1) & (max - 1); +} + +static void +uart_pipe_ring_add_char(struct uart_pipe_ring *cr, char ch) +{ + cr->buf[cr->head] = ch; + cr->head = inc_and_wrap(cr->head, cr->size); +} + +static uint8_t +uart_pipe_ring_pull_char(struct uart_pipe_ring *cr) +{ + uint8_t ch; + + ch = cr->buf[cr->tail]; + cr->tail = inc_and_wrap(cr->tail, cr->size); + return ch; +} + +static bool +uart_pipe_ring_is_full(const struct uart_pipe_ring *cr) +{ + return inc_and_wrap(cr->head, cr->size) == cr->tail; +} + +static bool +uart_pipe_ring_is_empty(const struct uart_pipe_ring *cr) +{ + return cr->head == cr->tail; +} + +static void +uart_pipe_queue_char(struct uart_dev *uart_dev, uint8_t ch) +{ + int sr; + + if ((uart_dev->ud_dev.od_flags & OS_DEV_F_STATUS_OPEN) == 0) { + return; + } + + OS_ENTER_CRITICAL(sr); + while (uart_pipe_ring_is_full(&cr_tx)) { + /* TX needs to drain */ + uart_start_tx(uart_dev); + OS_EXIT_CRITICAL(sr); + if (os_started()) { + os_time_delay(1); + } + OS_ENTER_CRITICAL(sr); + } + uart_pipe_ring_add_char(&cr_tx, ch); + OS_EXIT_CRITICAL(sr); +} + +/* + * Interrupts disabled when console_tx_char/console_rx_char are called. + * Characters sent only in blocking mode. + */ +static int +uart_console_tx_char(void *arg) +{ + if (uart_pipe_ring_is_empty(&cr_tx)) { + return -1; + } + return uart_pipe_ring_pull_char(&cr_tx); +} + +/* + * Interrupts disabled when console_tx_char/console_rx_char are called. + */ +static int +uart_console_rx_char(void *arg, uint8_t byte) +{ + if (uart_pipe_ring_is_full(&cr_rx)) { + uart_console_rx_stalled = true; + return -1; + } + + uart_pipe_ring_add_char(&cr_rx, byte); + + if (!rx_ev.ev_queued) { + os_eventq_put(os_eventq_dflt_get(), &rx_ev); + } + + return 0; +} + +static int +uart_pipe_handle_char(int key) +{ + recv_buf[recv_off] = (u8_t) key; + recv_off++; + + return 0; +} + +static void +uart_console_rx_char_event(struct os_event *ev) +{ + static int b = -1; + int sr; + int ret; + + /* We may have unhandled character - try it first */ + if (b >= 0) { + ret = uart_pipe_handle_char(b); + if (ret < 0) { + return; + } + } + + while (!uart_pipe_ring_is_empty(&cr_rx)) { + OS_ENTER_CRITICAL(sr); + b = uart_pipe_ring_pull_char(&cr_rx); + OS_EXIT_CRITICAL(sr); + + /* If UART RX was stalled due to a full receive buffer, restart RX now + * that we have removed a byte from the buffer. + */ + if (uart_console_rx_stalled) { + uart_console_rx_stalled = false; + uart_start_rx(uart_dev); + } + + ret = uart_pipe_handle_char(b); + if (ret < 0) { + return; + } + } + + /* + * Call application callback with received data. Application + * may provide new buffer or alter data offset. + */ + recv_buf = app_cb(recv_buf, &recv_off); + + b = -1; +} + +int +bttester_pipe_send(const u8_t *data, int len) +{ + int i; + + /* 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 (i = 0; i < len; ++i) { + write_char_cb(uart_dev, data[i]); + } + + uart_start_tx(uart_dev); + + return 0; +} + +int +bttester_pipe_send_buf(struct os_mbuf *buf) +{ + 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; + } + + 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); + + 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, + }; + + cr_tx.size = sizeof(cr_tx_buf); + cr_tx.buf = cr_tx_buf; + write_char_cb = uart_pipe_queue_char; + + cr_rx.size = sizeof(cr_rx_buf); + cr_rx.buf = cr_rx_buf; + + 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); + if (!uart_dev) { + return -1; + } + } + return 0; +} + +void +bttester_pipe_register(u8_t *buf, size_t len, bttester_pipe_recv_cb cb) +{ + recv_buf = buf; + recv_buf_len = len; + app_cb = cb; +} +#endif /* MYNEWT_VAL(BTTESTER_PIPE_UART) */ |