diff options
Diffstat (limited to 'src/libs/mynewt-nimble/apps/bttester/src/gap.c')
| -rw-r--r-- | src/libs/mynewt-nimble/apps/bttester/src/gap.c | 1688 |
1 files changed, 1688 insertions, 0 deletions
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; +} |