void tester_rsp(uint8_t service, uint8_t opcode, uint8_t index, uint8_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, (uint8_t *) &s, sizeof(s));
}
static void set_discoverable(uint8_t *data, uint16_t len)
{
const struct gap_set_discoverable_cmd *cmd = (void *) data;
struct gap_set_discoverable_rp rp;
switch (cmd->discoverable) {
case GAP_NON_DISCOVERABLE:
ad_flags &= ~(BT_LE_AD_GENERAL | BT_LE_AD_LIMITED);
atomic_clear_bit(¤t_settings, GAP_SETTINGS_DISCOVERABLE);
break;
case GAP_GENERAL_DISCOVERABLE:
ad_flags &= ~BT_LE_AD_LIMITED;
ad_flags |= BT_LE_AD_GENERAL;
atomic_set_bit(¤t_settings, GAP_SETTINGS_DISCOVERABLE);
break;
case GAP_LIMITED_DISCOVERABLE:
ad_flags &= ~BT_LE_AD_GENERAL;
ad_flags |= BT_LE_AD_LIMITED;
atomic_set_bit(¤t_settings, 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,
(uint8_t *) &rp, sizeof(rp));
}
static void supported_commands(uint8_t *data, uint16_t len)
{
uint8_t cmds[3];
struct gap_read_supported_commands_rp *rp = (void *) &cmds;
memset(cmds, 0, sizeof(cmds));
tester_set_bit(cmds, GAP_READ_SUPPORTED_COMMANDS);
tester_set_bit(cmds, GAP_READ_CONTROLLER_INDEX_LIST);
tester_set_bit(cmds, GAP_READ_CONTROLLER_INFO);
tester_set_bit(cmds, GAP_SET_CONNECTABLE);
tester_set_bit(cmds, GAP_SET_DISCOVERABLE);
tester_set_bit(cmds, GAP_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_PASSKEY_ENTRY);
tester_send(BTP_SERVICE_ID_GAP, GAP_READ_SUPPORTED_COMMANDS,
CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds));
}
static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t evtype,
const uint8_t *ad, uint8_t len)
{
struct gap_device_found_ev *ev;
uint8_t buf[sizeof(*ev) + len];
ev = (void*) buf;
memcpy(ev->address, addr->a.val, sizeof(ev->address));
ev->address_type = addr->type;
ev->flags = GAP_DEVICE_FOUND_FLAG_RSSI;
ev->rssi = rssi;
if (evtype == BT_LE_ADV_SCAN_RSP) {
ev->flags |= GAP_DEVICE_FOUND_FLAG_SD;
} else {
ev->flags |= GAP_DEVICE_FOUND_FLAG_AD;
}
ev->eir_data_len = len;
if (len) {
memcpy(ev->eir_data, ad, len);
}
tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_FOUND, CONTROLLER_INDEX,
buf, sizeof(buf));
}
static void auth_passkey_entry(struct bt_conn *conn)
{
struct gap_passkey_entry_req_ev ev;
const bt_addr_le_t *addr = bt_conn_get_dst(conn);
memcpy(ev.address, addr->a.val, sizeof(ev.address));
ev.address_type = addr->type;
tester_send(BTP_SERVICE_ID_GAP, GAP_EV_PASSKEY_ENTRY_REQ,
CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev));
}
static void le_disconnected(struct bt_conn *conn, uint8_t reason)
{
struct gap_device_disconnected_ev ev;
const bt_addr_le_t *addr = bt_conn_get_dst(conn);
memcpy(ev.address, addr->a.val, sizeof(ev.address));
ev.address_type = addr->type;
tester_send(BTP_SERVICE_ID_GAP, GAP_EV_DEVICE_DISCONNECTED,
CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev));
}
static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
{
struct gap_passkey_display_ev ev;
const bt_addr_le_t *addr = bt_conn_get_dst(conn);
memcpy(ev.address, addr->a.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_DISPLAY,
CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev));
}
static void controller_index_list(uint8_t *data, uint16_t len)
{
struct gap_read_controller_index_list_rp *rp;
uint8_t buf[sizeof(*rp) + 1];
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, (uint8_t *) rp, sizeof(buf));
}
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_len, sd_len;
bool adv_conn;
int i;
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;
}
for (i = 0, sd_len = 0; i < cmd->scan_rsp_len; sd_len++) {
if (sd_len >= ARRAY_SIZE(sd)) {
BTTESTER_DBG("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;
}
adv_conn = atomic_test_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
/* BTP API don't allow to set empty scan response data. */
if (bt_le_adv_start(adv_conn ? BT_LE_ADV_CONN : BT_LE_ADV_NCONN,
ad, adv_len, sd_len ? sd : NULL, sd_len) < 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 supported_commands(uint8_t *data, uint16_t len)
{
uint8_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_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_COMMANDS,
BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf));
}
static void supported_services(uint8_t *data, uint16_t len)
{
uint8_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);
tester_set_bit(buf, BTP_SERVICE_ID_L2CAP);
tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_SERVICES,
BTP_INDEX_NONE, (uint8_t *) rp, sizeof(buf));
}
static void set_connectable(uint8_t *data, uint16_t len)
{
const struct gap_set_connectable_cmd *cmd = (void *) data;
struct gap_set_connectable_rp rp;
if (cmd->connectable) {
atomic_set_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
} else {
atomic_clear_bit(¤t_settings, GAP_SETTINGS_CONNECTABLE);
}
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_SET_CONNECTABLE, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
}
static void stop_advertising(const uint8_t *data, uint16_t len)
{
struct gap_stop_advertising_rp rp;
if (bt_le_adv_stop() < 0) {
tester_rsp(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING,
CONTROLLER_INDEX, BTP_STATUS_FAILED);
return;
}
atomic_clear_bit(¤t_settings, GAP_SETTINGS_ADVERTISING);
rp.current_settings = sys_cpu_to_le32(current_settings);
tester_send(BTP_SERVICE_ID_GAP, GAP_STOP_ADVERTISING, CONTROLLER_INDEX,
(uint8_t *) &rp, sizeof(rp));
}
void tester_init(void)
{
int i;
for (i = 0; i < CMD_QUEUED; i++) {
k_fifo_put(&avail_queue, &cmd_buf[i * BTP_MTU]);
}
k_thread_create(&cmd_thread, stack, STACKSIZE, cmd_handler,
NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
uart_pipe_register(k_fifo_get(&avail_queue, K_NO_WAIT),
BTP_MTU, recv_cb);
tester_send(BTP_SERVICE_ID_CORE, CORE_EV_IUT_READY, BTP_INDEX_NONE,
NULL, 0);
}
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 defined(CONFIG_BLUETOOTH_L2CAP_DYNAMIC_CHANNEL)
tester_set_bit(buf, BTP_SERVICE_ID_L2CAP);
#endif /* CONFIG_BLUETOOTH_L2CAP_DYNAMIC_CHANNEL */
tester_send(BTP_SERVICE_ID_CORE, CORE_READ_SUPPORTED_SERVICES,
BTP_INDEX_NONE, (u8_t *) rp, sizeof(buf));
}
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 supported_commands(uint8_t *data, uint16_t len)
{
uint16_t cmds;
struct gap_read_supported_commands_rp *rp = (void *) &cmds;
cmds = 1 << GAP_READ_SUPPORTED_COMMANDS;
cmds |= 1 << GAP_READ_CONTROLLER_INDEX_LIST;
cmds |= 1 << GAP_READ_CONTROLLER_INFO;
cmds |= 1 << GAP_SET_CONNECTABLE;
cmds |= 1 << GAP_SET_DISCOVERABLE;
cmds |= 1 << GAP_START_ADVERTISING;
cmds |= 1 << GAP_STOP_ADVERTISING;
cmds |= 1 << GAP_START_DISCOVERY;
cmds |= 1 << GAP_STOP_DISCOVERY;
cmds |= 1 << GAP_DISCONNECT;
cmds |= 1 << GAP_SET_IO_CAP;
cmds |= 1 << GAP_PAIR;
cmds |= 1 << GAP_PASSKEY_ENTRY;
tester_send(BTP_SERVICE_ID_GAP, GAP_READ_SUPPORTED_COMMANDS,
CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds));
}
static void controller_info(uint8_t *data, uint16_t len)
{
struct gap_read_controller_info_rp rp;
uint32_t supported_settings;
memset(&rp, 0, sizeof(rp));
memcpy(rp.address, CONTROLLER_ADDR, sizeof(bt_addr_t));
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);
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, (uint8_t *) &rp, sizeof(rp));
}