static void set_io_cap(const uint8_t *data, uint16_t len)
{
const struct gap_set_io_cap_cmd *cmd = (void *) data;
uint8_t status;
/* Reset io cap requirements */
memset(&cb, 0, sizeof(cb));
bt_conn_auth_cb_register(NULL);
switch (cmd->io_cap) {
case GAP_IO_CAP_DISPLAY_ONLY:
cb.cancel = auth_cancel;
cb.passkey_display = auth_passkey_display;
break;
case GAP_IO_CAP_KEYBOARD_DISPLAY:
cb.cancel = auth_cancel;
cb.passkey_display = auth_passkey_display;
cb.passkey_entry = auth_passkey_entry;
break;
case GAP_IO_CAP_NO_INPUT_OUTPUT:
cb.cancel = auth_cancel;
break;
case GAP_IO_CAP_KEYBOARD_ONLY:
cb.cancel = auth_cancel;
cb.passkey_entry = auth_passkey_entry;
break;
case GAP_IO_CAP_DISPLAY_YESNO:
default:
status = BTP_STATUS_FAILED;
goto rsp;
}
if (bt_conn_auth_cb_register(&cb)) {
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 register_service(uint8_t *data, uint16_t len)
{
struct core_register_service_cmd *cmd = (void *) data;
uint8_t status;
switch (cmd->id) {
case BTP_SERVICE_ID_GAP:
status = tester_init_gap();
break;
case BTP_SERVICE_ID_GATT:
status = tester_init_gatt();
break;
default:
status = BTP_STATUS_FAILED;
break;
}
tester_rsp(BTP_SERVICE_ID_CORE, CORE_REGISTER_SERVICE, BTP_INDEX_NONE,
status);
}
static void cmd_handler(void *p1, void *p2, void *p3)
{
while (1) {
struct btp_hdr *cmd;
u16_t len;
cmd = k_fifo_get(&cmds_queue, K_FOREVER);
len = sys_le16_to_cpu(cmd->len);
/* TODO
* verify if service is registered before calling handler
*/
switch (cmd->service) {
case BTP_SERVICE_ID_CORE:
handle_core(cmd->opcode, cmd->index, cmd->data, len);
break;
case BTP_SERVICE_ID_GAP:
tester_handle_gap(cmd->opcode, cmd->index, cmd->data,
len);
break;
case BTP_SERVICE_ID_GATT:
tester_handle_gatt(cmd->opcode, cmd->index, cmd->data,
len);
break;
#if defined(CONFIG_BLUETOOTH_L2CAP_DYNAMIC_CHANNEL)
case BTP_SERVICE_ID_L2CAP:
tester_handle_l2cap(cmd->opcode, cmd->index, cmd->data,
len);
#endif /* CONFIG_BLUETOOTH_L2CAP_DYNAMIC_CHANNEL */
break;
default:
tester_rsp(cmd->service, cmd->opcode, cmd->index,
BTP_STATUS_FAILED);
break;
}
k_fifo_put(&avail_queue, cmd);
}
}
static void passkey_entry(const uint8_t *data, uint16_t len)
{
const struct gap_passkey_entry_cmd *cmd = (void *) data;
struct bt_conn *conn;
uint8_t status;
conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data);
if (!conn) {
status = BTP_STATUS_FAILED;
goto rsp;
}
bt_conn_auth_passkey_entry(conn, sys_le32_to_cpu(cmd->passkey));
bt_conn_unref(conn);
status = BTP_STATUS_SUCCESS;
rsp:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_PASSKEY_ENTRY, CONTROLLER_INDEX,
status);
}
static void start_discovery(const uint8_t *data, uint16_t len)
{
const struct gap_start_discovery_cmd *cmd = (void *) data;
uint8_t status;
/* only LE scan is supported */
if (cmd->flags & (~GAP_DISCOVERY_FLAG_LE)) {
status = BTP_STATUS_FAILED;
goto reply;
}
if (bt_le_scan_start(BT_LE_SCAN_ACTIVE, device_found) < 0) {
status = BTP_STATUS_FAILED;
goto reply;
}
status = BTP_STATUS_SUCCESS;
reply:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_DISCOVERY, CONTROLLER_INDEX,
status);
}
static void start_advertising(const uint8_t *data, uint16_t len)
{
const struct gap_start_advertising_cmd *cmd = (void *) data;
struct gap_start_advertising_rp rp;
uint8_t adv_type, adv_len;
int i;
if (atomic_test_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE)) {
adv_type = BT_LE_ADV_IND;
} else {
adv_type = BT_LE_ADV_NONCONN_IND;
}
for (i = 0, adv_len = 1; i < cmd->adv_data_len; adv_len++) {
if (adv_len >= ARRAY_SIZE(ad)) {
BTTESTER_DBG("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;
}
if (bt_le_adv_start(BT_LE_ADV(adv_type), ad, adv_len, NULL, 0) < 0) {
BTTESTER_DBG("Failed to start advertising");
goto fail;
}
atomic_set_bit(¤t_settings, GAP_SETTINGS_ADVERTISING);
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
return;
fail:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_START_ADVERTISING, CONTROLLER_INDEX,
BTP_STATUS_FAILED);
}
static void cmd_handler(int arg1, int arg2)
{
while (1) {
struct btp_hdr *cmd;
uint16_t len;
cmd = nano_fiber_fifo_get(&cmds_queue, TICKS_UNLIMITED);
len = sys_le16_to_cpu(cmd->len);
/* TODO
* verify if service is registered before calling handler
*/
switch (cmd->service) {
case BTP_SERVICE_ID_CORE:
handle_core(cmd->opcode, cmd->index, cmd->data, len);
break;
case BTP_SERVICE_ID_GAP:
tester_handle_gap(cmd->opcode, cmd->index, cmd->data,
len);
break;
case BTP_SERVICE_ID_GATT:
tester_handle_gatt(cmd->opcode, cmd->index, cmd->data,
len);
break;
case BTP_SERVICE_ID_L2CAP:
tester_handle_l2cap(cmd->opcode, cmd->index, cmd->data,
len);
break;
default:
tester_rsp(cmd->service, cmd->opcode, cmd->index,
BTP_STATUS_FAILED);
break;
}
nano_fiber_fifo_put(&avail_queue, cmd);
}
}
static void pair(const uint8_t *data, uint16_t len)
{
struct bt_conn *conn;
uint8_t status;
conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data);
if (!conn) {
status = BTP_STATUS_FAILED;
goto rsp;
}
if (bt_conn_security(conn, BT_SECURITY_MEDIUM)) {
status = BTP_STATUS_FAILED;
bt_conn_unref(conn);
goto rsp;
}
bt_conn_unref(conn);
status = BTP_STATUS_SUCCESS;
rsp:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_PAIR, CONTROLLER_INDEX, status);
}
static void disconnect(const uint8_t *data, uint16_t len)
{
struct bt_conn *conn;
uint8_t status;
conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data);
if (!conn) {
status = BTP_STATUS_FAILED;
goto rsp;
}
if (bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN)) {
status = BTP_STATUS_FAILED;
} else {
status = BTP_STATUS_SUCCESS;
}
bt_conn_unref(conn);
rsp:
tester_rsp(BTP_SERVICE_ID_GAP, GAP_DISCONNECT, CONTROLLER_INDEX,
status);
}
void tester_handle_gap(uint8_t opcode, uint8_t index, uint8_t *data,
uint16_t len)
{
switch (opcode) {
case GAP_READ_SUPPORTED_COMMANDS:
case GAP_READ_CONTROLLER_INDEX_LIST:
if (index != BTP_INDEX_NONE){
tester_rsp(BTP_SERVICE_ID_GAP, opcode, index,
BTP_STATUS_FAILED);
return;
}
break;
default:
if (index != CONTROLLER_INDEX){
tester_rsp(BTP_SERVICE_ID_GAP, opcode, index,
BTP_STATUS_FAILED);
return;
}
break;
}
switch (opcode) {
case GAP_READ_SUPPORTED_COMMANDS:
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_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_PASSKEY_ENTRY:
passkey_entry(data, len);
return;
default:
tester_rsp(BTP_SERVICE_ID_GAP, opcode, index,
BTP_STATUS_UNKNOWN_CMD);
return;
}
}
