mirror of
https://git.kernel.org/pub/scm/bluetooth/bluez.git
synced 2024-11-23 12:14:26 +08:00
8002 lines
204 KiB
C
8002 lines
204 KiB
C
// SPDX-License-Identifier: LGPL-2.1-or-later
|
||
/*
|
||
*
|
||
* BlueZ - Bluetooth protocol stack for Linux
|
||
*
|
||
* Copyright (C) 2011-2012 Intel Corporation
|
||
* Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
|
||
* Copyright 2023-2024 NXP
|
||
*
|
||
*
|
||
*/
|
||
|
||
#ifdef HAVE_CONFIG_H
|
||
#include <config.h>
|
||
#endif
|
||
|
||
#include <stdio.h>
|
||
#include <ctype.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <alloca.h>
|
||
#include <sys/uio.h>
|
||
#include <stdint.h>
|
||
#include <errno.h>
|
||
#include <fcntl.h>
|
||
#include <unistd.h>
|
||
|
||
#include "lib/bluetooth.h"
|
||
#include "lib/hci.h"
|
||
|
||
#include "src/shared/util.h"
|
||
#include "src/shared/timeout.h"
|
||
#include "src/shared/crypto.h"
|
||
#include "src/shared/ecc.h"
|
||
#include "src/shared/queue.h"
|
||
#include "monitor/bt.h"
|
||
#include "monitor/msft.h"
|
||
#include "monitor/emulator.h"
|
||
#include "btdev.h"
|
||
|
||
#define AL_SIZE 16
|
||
#define RL_SIZE 16
|
||
#define CIS_SIZE 3
|
||
#define BIS_SIZE 3
|
||
#define CIG_SIZE 3
|
||
|
||
#define MAX_PA_DATA_LEN 252
|
||
|
||
#define has_bredr(btdev) (!((btdev)->features[4] & 0x20))
|
||
#define has_le(btdev) (!!((btdev)->features[4] & 0x40))
|
||
|
||
#define ACL_HANDLE 42
|
||
#define ISO_HANDLE 257
|
||
#define SCO_HANDLE 257
|
||
#define SYNC_HANDLE 1
|
||
#define INV_HANDLE 0xffff
|
||
|
||
struct hook {
|
||
btdev_hook_func handler;
|
||
void *user_data;
|
||
enum btdev_hook_type type;
|
||
uint16_t opcode;
|
||
};
|
||
|
||
#define MAX_HOOK_ENTRIES 16
|
||
#define MAX_EXT_ADV_SETS 3
|
||
#define MAX_PENDING_CONN 16
|
||
|
||
struct btdev_conn {
|
||
uint16_t handle;
|
||
uint8_t type;
|
||
struct btdev *dev;
|
||
struct btdev_conn *link;
|
||
void *data;
|
||
};
|
||
|
||
struct btdev_al {
|
||
uint8_t type;
|
||
bdaddr_t addr;
|
||
};
|
||
|
||
struct btdev_rl {
|
||
uint8_t type;
|
||
bdaddr_t addr;
|
||
uint8_t mode;
|
||
uint8_t peer_irk[16];
|
||
uint8_t local_irk[16];
|
||
};
|
||
|
||
struct le_ext_adv {
|
||
struct btdev *dev;
|
||
uint8_t handle;
|
||
uint8_t enable;
|
||
uint8_t type; /* evt_properties */
|
||
uint8_t own_addr_type; /* own_addr_type */
|
||
uint8_t direct_addr_type; /* peer_addr_type */
|
||
uint8_t direct_addr[6]; /* peer_addr */
|
||
uint8_t filter_policy; /* filter_policy */
|
||
uint8_t random_addr[6];
|
||
bool rpa;
|
||
uint8_t adv_data[252];
|
||
uint8_t adv_data_len;
|
||
uint8_t scan_data[252];
|
||
uint8_t scan_data_len;
|
||
unsigned int id;
|
||
};
|
||
|
||
struct le_per_adv {
|
||
struct btdev *dev;
|
||
uint8_t addr_type;
|
||
uint8_t addr[6];
|
||
uint16_t sync_handle;
|
||
};
|
||
|
||
struct le_big {
|
||
struct btdev *dev;
|
||
uint8_t handle;
|
||
struct queue *bis;
|
||
};
|
||
|
||
struct le_cig {
|
||
struct bt_hci_cmd_le_set_cig_params params;
|
||
struct bt_hci_cis_params cis[CIS_SIZE];
|
||
bool activated;
|
||
} __attribute__ ((packed));
|
||
|
||
struct btdev {
|
||
enum btdev_type type;
|
||
uint16_t id;
|
||
|
||
struct queue *conns;
|
||
|
||
bool auth_init;
|
||
uint8_t link_key[16];
|
||
uint16_t pin[16];
|
||
uint8_t pin_len;
|
||
uint8_t io_cap;
|
||
uint8_t auth_req;
|
||
bool ssp_auth_complete;
|
||
uint8_t ssp_status;
|
||
|
||
btdev_command_func command_handler;
|
||
void *command_data;
|
||
|
||
btdev_send_func send_handler;
|
||
void *send_data;
|
||
|
||
unsigned int inquiry_id;
|
||
unsigned int inquiry_timeout_id;
|
||
|
||
struct hook *hook_list[MAX_HOOK_ENTRIES];
|
||
|
||
struct bt_crypto *crypto;
|
||
|
||
uint16_t manufacturer;
|
||
uint8_t version;
|
||
uint16_t revision;
|
||
uint8_t commands[64];
|
||
uint8_t max_page;
|
||
uint8_t features[8];
|
||
uint8_t feat_page_2[8];
|
||
uint16_t acl_mtu;
|
||
uint16_t acl_max_pkt;
|
||
uint16_t sco_mtu;
|
||
uint16_t sco_max_pkt;
|
||
uint16_t iso_mtu;
|
||
uint16_t iso_max_pkt;
|
||
uint8_t country_code;
|
||
uint8_t bdaddr[6];
|
||
uint8_t random_addr[6];
|
||
uint8_t le_features[8];
|
||
uint8_t le_states[8];
|
||
const struct btdev_cmd *cmds;
|
||
uint16_t msft_opcode;
|
||
const struct btdev_cmd *msft_cmds;
|
||
uint16_t emu_opcode;
|
||
const struct btdev_cmd *emu_cmds;
|
||
bool aosp_capable;
|
||
|
||
uint16_t default_link_policy;
|
||
uint8_t event_mask[8];
|
||
uint8_t event_mask_page2[8];
|
||
uint8_t event_filter;
|
||
uint8_t name[248];
|
||
uint8_t dev_class[3];
|
||
uint16_t voice_setting;
|
||
uint16_t conn_accept_timeout;
|
||
uint16_t page_timeout;
|
||
uint8_t scan_enable;
|
||
uint16_t page_scan_interval;
|
||
uint16_t page_scan_window;
|
||
uint16_t page_scan_type;
|
||
uint8_t auth_enable;
|
||
uint16_t inquiry_scan_interval;
|
||
uint16_t inquiry_scan_window;
|
||
uint8_t inquiry_mode;
|
||
uint8_t afh_assessment_mode;
|
||
uint8_t ext_inquiry_fec;
|
||
uint8_t ext_inquiry_rsp[240];
|
||
uint8_t simple_pairing_mode;
|
||
uint8_t ssp_debug_mode;
|
||
uint8_t secure_conn_support;
|
||
uint8_t host_flow_control;
|
||
uint8_t le_supported;
|
||
uint8_t le_simultaneous;
|
||
uint8_t le_event_mask[8];
|
||
uint8_t le_adv_data[31];
|
||
uint8_t le_adv_data_len;
|
||
uint8_t le_adv_type;
|
||
uint8_t le_adv_own_addr;
|
||
uint8_t le_adv_direct_addr_type;
|
||
uint8_t le_adv_direct_addr[6];
|
||
uint8_t le_adv_filter_policy;
|
||
uint8_t le_scan_data[31];
|
||
uint8_t le_scan_data_len;
|
||
uint8_t le_scan_enable;
|
||
uint8_t le_scan_type;
|
||
uint8_t le_scan_own_addr_type;
|
||
uint8_t le_scan_filter_policy;
|
||
uint8_t le_filter_dup;
|
||
uint8_t le_adv_enable;
|
||
uint8_t le_pa_enable;
|
||
uint16_t le_pa_properties;
|
||
uint16_t le_pa_min_interval;
|
||
uint16_t le_pa_max_interval;
|
||
uint8_t le_pa_data_len;
|
||
uint8_t le_pa_data[MAX_PA_DATA_LEN];
|
||
uint8_t le_ltk[16];
|
||
struct le_cig le_cig[CIG_SIZE];
|
||
uint8_t le_iso_path[2];
|
||
|
||
/* Real time length of AL array */
|
||
uint8_t le_al_len;
|
||
/* Real time length of RL array */
|
||
uint8_t le_rl_len;
|
||
struct btdev_al le_al[AL_SIZE];
|
||
struct btdev_rl le_rl[RL_SIZE];
|
||
uint8_t le_rl_enable;
|
||
uint16_t le_rl_timeout;
|
||
|
||
struct btdev *pending_conn[MAX_PENDING_CONN];
|
||
|
||
uint8_t le_local_sk256[32];
|
||
|
||
uint16_t sync_train_interval;
|
||
uint32_t sync_train_timeout;
|
||
uint8_t sync_train_service_data;
|
||
|
||
uint16_t le_ext_adv_type;
|
||
|
||
struct queue *le_ext_adv;
|
||
struct queue *le_per_adv;
|
||
struct queue *le_big;
|
||
|
||
btdev_debug_func_t debug_callback;
|
||
btdev_destroy_func_t debug_destroy;
|
||
void *debug_data;
|
||
};
|
||
|
||
struct inquiry_data {
|
||
struct btdev *btdev;
|
||
int num_resp;
|
||
|
||
int sent_count;
|
||
int iter;
|
||
};
|
||
|
||
#define DEFAULT_INQUIRY_INTERVAL 100 /* 100 miliseconds */
|
||
|
||
#define MAX_BTDEV_ENTRIES 16
|
||
|
||
static const uint8_t LINK_KEY_NONE[16] = { 0 };
|
||
static const uint8_t LINK_KEY_DUMMY[16] = { 0, 1, 2, 3, 4, 5, 6, 7,
|
||
8, 9, 0, 1, 2, 3, 4, 5 };
|
||
|
||
static struct btdev *btdev_list[MAX_BTDEV_ENTRIES] = { };
|
||
|
||
static int get_hook_index(struct btdev *btdev, enum btdev_hook_type type,
|
||
uint16_t opcode)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
|
||
if (btdev->hook_list[i] == NULL)
|
||
continue;
|
||
|
||
if (btdev->hook_list[i]->type == type &&
|
||
btdev->hook_list[i]->opcode == opcode)
|
||
return i;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
static bool run_hooks(struct btdev *btdev, enum btdev_hook_type type,
|
||
uint16_t opcode, const void *data, uint16_t len)
|
||
{
|
||
int index = get_hook_index(btdev, type, opcode);
|
||
if (index < 0)
|
||
return true;
|
||
|
||
return btdev->hook_list[index]->handler(data, len,
|
||
btdev->hook_list[index]->user_data);
|
||
}
|
||
|
||
static inline int add_btdev(struct btdev *btdev)
|
||
{
|
||
int i, index = -1;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (btdev_list[i] == NULL) {
|
||
index = i;
|
||
btdev_list[index] = btdev;
|
||
break;
|
||
}
|
||
}
|
||
|
||
return index;
|
||
}
|
||
|
||
static inline int del_btdev(struct btdev *btdev)
|
||
{
|
||
int i, index = -1;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (btdev_list[i] == btdev) {
|
||
index = i;
|
||
btdev_list[index] = NULL;
|
||
break;
|
||
}
|
||
}
|
||
|
||
return index;
|
||
}
|
||
|
||
static inline bool valid_btdev(struct btdev *btdev)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (btdev_list[i] == btdev)
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
static inline struct btdev *find_btdev_by_bdaddr(const uint8_t *bdaddr)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (btdev_list[i] && !memcmp(btdev_list[i]->bdaddr, bdaddr, 6))
|
||
return btdev_list[i];
|
||
}
|
||
|
||
return NULL;
|
||
}
|
||
|
||
static bool match_adv_addr(const void *data, const void *match_data)
|
||
{
|
||
const struct le_ext_adv *adv = data;
|
||
const uint8_t *bdaddr = match_data;
|
||
|
||
return !memcmp(adv->random_addr, bdaddr, 6);
|
||
}
|
||
|
||
static inline struct btdev *find_btdev_by_bdaddr_type(const uint8_t *bdaddr,
|
||
uint8_t bdaddr_type)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
struct btdev *dev = btdev_list[i];
|
||
int cmp;
|
||
struct le_ext_adv *adv;
|
||
|
||
if (!dev)
|
||
continue;
|
||
|
||
if (bdaddr_type == 0x01)
|
||
cmp = memcmp(dev->random_addr, bdaddr, 6);
|
||
else
|
||
cmp = memcmp(dev->bdaddr, bdaddr, 6);
|
||
|
||
if (!cmp)
|
||
return dev;
|
||
|
||
/* Check for instance own Random addresses */
|
||
if (bdaddr_type == 0x01) {
|
||
adv = queue_find(dev->le_ext_adv, match_adv_addr,
|
||
bdaddr);
|
||
if (adv)
|
||
return dev;
|
||
}
|
||
}
|
||
|
||
return NULL;
|
||
}
|
||
|
||
static void get_bdaddr(uint16_t id, uint8_t index, uint8_t *bdaddr)
|
||
{
|
||
bdaddr[0] = id & 0xff;
|
||
bdaddr[1] = id >> 8;
|
||
bdaddr[2] = index;
|
||
bdaddr[3] = 0x01;
|
||
bdaddr[4] = 0xaa;
|
||
bdaddr[5] = 0x00;
|
||
}
|
||
|
||
struct btdev_cmd {
|
||
uint16_t opcode;
|
||
int (*func)(struct btdev *dev, const void *data, uint8_t len);
|
||
int (*complete)(struct btdev *dev, const void *data, uint8_t len);
|
||
};
|
||
|
||
#define CMD(_opcode, _func, _complete) \
|
||
{ \
|
||
.opcode = _opcode, \
|
||
.func = _func, \
|
||
.complete = _complete, \
|
||
}
|
||
|
||
static void send_packet(struct btdev *btdev, const struct iovec *iov,
|
||
int iovlen)
|
||
{
|
||
int i;
|
||
|
||
if (!btdev->send_handler)
|
||
return;
|
||
|
||
for (i = 0; i < iovlen; i++) {
|
||
if (!i)
|
||
util_hexdump('<', iov[i].iov_base, iov[i].iov_len,
|
||
btdev->debug_callback, btdev->debug_data);
|
||
else
|
||
util_hexdump(' ', iov[i].iov_base, iov[i].iov_len,
|
||
btdev->debug_callback, btdev->debug_data);
|
||
}
|
||
|
||
btdev->send_handler(iov, iovlen, btdev->send_data);
|
||
}
|
||
|
||
static void send_cmd(struct btdev *btdev, uint8_t evt, uint16_t opcode,
|
||
const struct iovec *iov, int iovlen)
|
||
{
|
||
struct bt_hci_evt_hdr hdr;
|
||
struct iovec iov2[2 + iovlen];
|
||
uint8_t pkt = BT_H4_EVT_PKT;
|
||
int i;
|
||
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"event 0x%02x opcode 0x%04x", evt, opcode);
|
||
|
||
iov2[0].iov_base = &pkt;
|
||
iov2[0].iov_len = sizeof(pkt);
|
||
|
||
hdr.evt = evt;
|
||
hdr.plen = 0;
|
||
|
||
iov2[1].iov_base = &hdr;
|
||
iov2[1].iov_len = sizeof(hdr);
|
||
|
||
for (i = 0; i < iovlen; i++) {
|
||
hdr.plen += iov[i].iov_len;
|
||
iov2[2 + i].iov_base = iov[i].iov_base;
|
||
iov2[2 + i].iov_len = iov[i].iov_len;
|
||
}
|
||
|
||
if (run_hooks(btdev, BTDEV_HOOK_POST_CMD, opcode, iov[i -1].iov_base,
|
||
iov[i -1].iov_len))
|
||
send_packet(btdev, iov2, 2 + iovlen);
|
||
}
|
||
|
||
static void cmd_complete(struct btdev *btdev, uint16_t opcode,
|
||
const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_evt_cmd_complete cc;
|
||
struct iovec iov[2];
|
||
|
||
cc.ncmd = 0x01;
|
||
cc.opcode = cpu_to_le16(opcode);
|
||
|
||
iov[0].iov_base = &cc;
|
||
iov[0].iov_len = sizeof(cc);
|
||
|
||
iov[1].iov_base = (void *) data;
|
||
iov[1].iov_len = len;
|
||
|
||
send_cmd(btdev, BT_HCI_EVT_CMD_COMPLETE, opcode, iov, 2);
|
||
}
|
||
|
||
static int cmd_set_event_mask(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_set_event_mask *cmd = data;
|
||
uint8_t status;
|
||
|
||
memcpy(dev->event_mask, cmd->mask, 8);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void al_reset(struct btdev_al *al)
|
||
{
|
||
al->type = 0xff;
|
||
bacpy(&al->addr, BDADDR_ANY);
|
||
}
|
||
|
||
static void al_clear(struct btdev *dev)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < AL_SIZE; i++)
|
||
al_reset(&dev->le_al[i]);
|
||
}
|
||
|
||
static void rl_reset(struct btdev_rl *rl)
|
||
{
|
||
rl->type = 0xff;
|
||
bacpy(&rl->addr, BDADDR_ANY);
|
||
memset(rl->peer_irk, 0, 16);
|
||
memset(rl->local_irk, 0, 16);
|
||
}
|
||
|
||
static void rl_clear(struct btdev *dev)
|
||
{
|
||
int i;
|
||
|
||
for (i = 0; i < RL_SIZE; i++)
|
||
rl_reset(&dev->le_rl[i]);
|
||
}
|
||
|
||
/* Set the real time length of AL array */
|
||
void btdev_set_al_len(struct btdev *btdev, uint8_t len)
|
||
{
|
||
btdev->le_al_len = len;
|
||
}
|
||
|
||
/* Set the real time length of RL array */
|
||
void btdev_set_rl_len(struct btdev *btdev, uint8_t len)
|
||
{
|
||
btdev->le_rl_len = len;
|
||
}
|
||
|
||
static void conn_unlink(struct btdev_conn *conn1, struct btdev_conn *conn2)
|
||
{
|
||
conn1->link = NULL;
|
||
conn2->link = NULL;
|
||
}
|
||
|
||
static void conn_remove(void *data)
|
||
{
|
||
struct btdev_conn *conn = data;
|
||
|
||
if (conn->link) {
|
||
struct btdev_conn *link = conn->link;
|
||
|
||
conn_unlink(conn, conn->link);
|
||
conn_remove(link);
|
||
}
|
||
|
||
queue_remove(conn->dev->conns, conn);
|
||
|
||
free(conn->data);
|
||
free(conn);
|
||
}
|
||
|
||
static void le_ext_adv_free(void *data)
|
||
{
|
||
struct le_ext_adv *ext_adv = data;
|
||
|
||
/* Remove to queue */
|
||
queue_remove(ext_adv->dev->le_ext_adv, ext_adv);
|
||
|
||
if (ext_adv->id)
|
||
timeout_remove(ext_adv->id);
|
||
|
||
free(ext_adv);
|
||
}
|
||
|
||
static void le_big_free(void *data)
|
||
{
|
||
struct le_big *big = data;
|
||
|
||
queue_destroy(big->bis, NULL);
|
||
|
||
free(big);
|
||
}
|
||
|
||
static void btdev_reset(struct btdev *btdev)
|
||
{
|
||
/* FIXME: include here clearing of all states that should be
|
||
* cleared upon HCI_Reset
|
||
*/
|
||
|
||
btdev->le_scan_enable = 0x00;
|
||
btdev->le_adv_enable = 0x00;
|
||
btdev->le_pa_enable = 0x00;
|
||
|
||
al_clear(btdev);
|
||
rl_clear(btdev);
|
||
|
||
btdev->le_al_len = AL_SIZE;
|
||
btdev->le_rl_len = RL_SIZE;
|
||
|
||
queue_remove_all(btdev->conns, NULL, NULL, conn_remove);
|
||
queue_remove_all(btdev->le_ext_adv, NULL, NULL, le_ext_adv_free);
|
||
queue_remove_all(btdev->le_per_adv, NULL, NULL, free);
|
||
queue_remove_all(btdev->le_big, NULL, NULL, le_big_free);
|
||
}
|
||
|
||
static int cmd_reset(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
uint8_t status;
|
||
|
||
btdev_reset(dev);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_RESET, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_version(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_version rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.hci_ver = dev->version;
|
||
rsp.hci_rev = cpu_to_le16(dev->revision);
|
||
rsp.lmp_ver = dev->version;
|
||
rsp.manufacturer = cpu_to_le16(dev->manufacturer);
|
||
rsp.lmp_subver = cpu_to_le16(dev->revision);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_commands(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_commands rsp;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.commands, dev->commands, 64);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_features rsp;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.features, dev->features, 8);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_buffer_size(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_buffer_size rsp;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.acl_mtu = cpu_to_le16(dev->acl_mtu);
|
||
rsp.sco_mtu = cpu_to_le16(dev->sco_mtu);
|
||
rsp.acl_max_pkt = cpu_to_le16(dev->acl_max_pkt);
|
||
rsp.sco_max_pkt = cpu_to_le16(dev->sco_max_pkt);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_COMMON_ALL \
|
||
CMD(BT_HCI_CMD_SET_EVENT_MASK, cmd_set_event_mask, NULL), \
|
||
CMD(BT_HCI_CMD_RESET, cmd_reset, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_VERSION, cmd_read_local_version, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_COMMANDS, cmd_read_local_commands, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_FEATURES, cmd_read_local_features, NULL), \
|
||
CMD(BT_HCI_CMD_READ_BUFFER_SIZE, cmd_read_buffer_size, NULL)
|
||
|
||
static void set_common_commands_all(struct btdev *btdev)
|
||
{
|
||
btdev->commands[5] |= 0x40; /* Set Event Mask */
|
||
btdev->commands[5] |= 0x80; /* Reset */
|
||
btdev->commands[14] |= 0x08; /* Read Local Version */
|
||
btdev->commands[14] |= 0x10; /* Read Local Supported Commands */
|
||
btdev->commands[14] |= 0x20; /* Read Local Supported Features */
|
||
btdev->commands[14] |= 0x80; /* Read Buffer Size */
|
||
}
|
||
|
||
static void cmd_status(struct btdev *btdev, uint8_t status, uint16_t opcode)
|
||
{
|
||
struct bt_hci_evt_cmd_status cs;
|
||
struct iovec iov;
|
||
|
||
cs.status = status;
|
||
cs.ncmd = 0x01;
|
||
cs.opcode = cpu_to_le16(opcode);
|
||
|
||
iov.iov_base = &cs;
|
||
iov.iov_len = sizeof(cs);
|
||
|
||
send_cmd(btdev, BT_HCI_EVT_CMD_STATUS, opcode, &iov, 1);
|
||
}
|
||
|
||
static int cmd_disconnect(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_DISCONNECT);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void send_event(struct btdev *btdev, uint8_t event,
|
||
const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_evt_hdr hdr;
|
||
struct iovec iov[3];
|
||
uint8_t pkt = BT_H4_EVT_PKT;
|
||
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"event 0x%02x", event);
|
||
|
||
iov[0].iov_base = &pkt;
|
||
iov[0].iov_len = sizeof(pkt);
|
||
|
||
hdr.evt = event;
|
||
hdr.plen = len;
|
||
|
||
iov[1].iov_base = &hdr;
|
||
iov[1].iov_len = sizeof(hdr);
|
||
|
||
if (len > 0) {
|
||
iov[2].iov_base = (void *) data;
|
||
iov[2].iov_len = len;
|
||
}
|
||
|
||
if (run_hooks(btdev, BTDEV_HOOK_POST_EVT, event, data, len))
|
||
send_packet(btdev, iov, len > 0 ? 3 : 2);
|
||
}
|
||
|
||
static bool match_handle(const void *data, const void *match_data)
|
||
{
|
||
const struct btdev_conn *conn = data;
|
||
uint16_t handle = PTR_TO_UINT(match_data);
|
||
|
||
return conn->handle == handle;
|
||
}
|
||
|
||
static void disconnect_complete(struct btdev *dev, uint16_t handle,
|
||
uint8_t status, uint8_t reason)
|
||
{
|
||
struct bt_hci_evt_disconnect_complete rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = status;
|
||
rsp.handle = cpu_to_le16(handle);
|
||
rsp.reason = reason;
|
||
|
||
send_event(dev, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp, sizeof(rsp));
|
||
}
|
||
|
||
static int cmd_disconnect_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_disconnect *cmd = data;
|
||
struct bt_hci_evt_disconnect_complete rsp;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
conn = queue_remove_if(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (!conn) {
|
||
disconnect_complete(dev, 0x0000, BT_HCI_ERR_UNKNOWN_CONN_ID,
|
||
0x00);
|
||
return 0;
|
||
}
|
||
|
||
/* Local host has different reason (Core v5.3 Vol 4 Part E Sec 7.1.6) */
|
||
disconnect_complete(dev, conn->handle, BT_HCI_ERR_SUCCESS,
|
||
BT_HCI_ERR_LOCAL_HOST_TERM);
|
||
|
||
if (conn->link)
|
||
disconnect_complete(conn->link->dev, conn->link->handle,
|
||
BT_HCI_ERR_SUCCESS, cmd->reason);
|
||
|
||
conn_remove(conn);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remote_version(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_READ_REMOTE_VERSION);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remote_version_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_remote_version *cmd = data;
|
||
struct bt_hci_evt_remote_version_complete ev;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (conn) {
|
||
ev.status = BT_HCI_ERR_SUCCESS;
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.lmp_ver = conn->link->dev->version;
|
||
ev.manufacturer = cpu_to_le16(conn->link->dev->manufacturer);
|
||
ev.lmp_subver = cpu_to_le16(conn->link->dev->revision);
|
||
} else {
|
||
ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.lmp_ver = 0x00;
|
||
ev.manufacturer = cpu_to_le16(0);
|
||
ev.lmp_subver = cpu_to_le16(0);
|
||
}
|
||
|
||
send_event(dev, BT_HCI_EVT_REMOTE_VERSION_COMPLETE, &ev, sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_host_flowctl(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_set_host_flow_control *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (cmd->enable > 0x03) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
} else {
|
||
dev->host_flow_control = cmd->enable;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_SET_HOST_FLOW_CONTROL, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_host_buffer_size(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_HOST_BUFFER_SIZE, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_host_num_completed_pkts(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* This command is special in the sense that no event is
|
||
* normally generated after the command has completed.
|
||
*/
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_bdaddr(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_bd_addr rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, dev->bdaddr, 6);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_BD_ADDR, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_COMMON_BREDR_LE \
|
||
CMD(BT_HCI_CMD_DISCONNECT, cmd_disconnect, cmd_disconnect_complete), \
|
||
CMD(BT_HCI_CMD_READ_REMOTE_VERSION, cmd_remote_version, \
|
||
cmd_remote_version_complete), \
|
||
CMD(BT_HCI_CMD_SET_HOST_FLOW_CONTROL, cmd_set_host_flowctl, NULL), \
|
||
CMD(BT_HCI_CMD_HOST_BUFFER_SIZE, cmd_host_buffer_size, NULL), \
|
||
CMD(BT_HCI_CMD_HOST_NUM_COMPLETED_PACKETS, \
|
||
cmd_host_num_completed_pkts, NULL), \
|
||
CMD(BT_HCI_CMD_READ_BD_ADDR, cmd_read_bdaddr, NULL)
|
||
|
||
static void set_common_commands_bredrle(struct btdev *btdev)
|
||
{
|
||
btdev->commands[0] |= 0x20; /* Disconnect */
|
||
btdev->commands[2] |= 0x80; /* Read Remote Version Information */
|
||
btdev->commands[10] |= 0x20; /* Set Host Flow Control */
|
||
btdev->commands[10] |= 0x40; /* Host Buffer Size */
|
||
btdev->commands[15] |= 0x02; /* Read BD ADDR */
|
||
}
|
||
|
||
static int cmd_inquiry(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_INQUIRY);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static bool inquiry_callback(void *user_data)
|
||
{
|
||
struct inquiry_data *data = user_data;
|
||
struct btdev *btdev = data->btdev;
|
||
struct bt_hci_evt_inquiry_complete ic;
|
||
int sent = data->sent_count;
|
||
int i;
|
||
|
||
/*Report devices only once and wait for inquiry timeout*/
|
||
if (data->iter == MAX_BTDEV_ENTRIES)
|
||
return true;
|
||
|
||
for (i = data->iter; i < MAX_BTDEV_ENTRIES; i++) {
|
||
/*Lets sent 10 inquiry results at once */
|
||
if (sent + 10 == data->sent_count)
|
||
break;
|
||
|
||
if (!btdev_list[i] || btdev_list[i] == btdev)
|
||
continue;
|
||
|
||
if (!(btdev_list[i]->scan_enable & 0x02))
|
||
continue;
|
||
|
||
if (btdev->inquiry_mode == 0x02 &&
|
||
btdev_list[i]->ext_inquiry_rsp[0]) {
|
||
struct bt_hci_evt_ext_inquiry_result ir;
|
||
|
||
ir.num_resp = 0x01;
|
||
memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
|
||
ir.pscan_rep_mode = 0x00;
|
||
ir.pscan_period_mode = 0x00;
|
||
memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
|
||
ir.clock_offset = 0x0000;
|
||
ir.rssi = -60;
|
||
memcpy(ir.data, btdev_list[i]->ext_inquiry_rsp, 240);
|
||
|
||
send_event(btdev, BT_HCI_EVT_EXT_INQUIRY_RESULT,
|
||
&ir, sizeof(ir));
|
||
data->sent_count++;
|
||
continue;
|
||
}
|
||
|
||
if (btdev->inquiry_mode > 0x00) {
|
||
struct bt_hci_evt_inquiry_result_with_rssi ir;
|
||
|
||
ir.num_resp = 0x01;
|
||
memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
|
||
ir.pscan_rep_mode = 0x00;
|
||
ir.pscan_period_mode = 0x00;
|
||
memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
|
||
ir.clock_offset = 0x0000;
|
||
ir.rssi = -60;
|
||
|
||
send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT_WITH_RSSI,
|
||
&ir, sizeof(ir));
|
||
data->sent_count++;
|
||
} else {
|
||
struct bt_hci_evt_inquiry_result ir;
|
||
|
||
ir.num_resp = 0x01;
|
||
memcpy(ir.bdaddr, btdev_list[i]->bdaddr, 6);
|
||
ir.pscan_rep_mode = 0x00;
|
||
ir.pscan_period_mode = 0x00;
|
||
ir.pscan_mode = 0x00;
|
||
memcpy(ir.dev_class, btdev_list[i]->dev_class, 3);
|
||
ir.clock_offset = 0x0000;
|
||
|
||
send_event(btdev, BT_HCI_EVT_INQUIRY_RESULT,
|
||
&ir, sizeof(ir));
|
||
data->sent_count++;
|
||
}
|
||
}
|
||
data->iter = i;
|
||
|
||
/* Check if we sent already required amount of responses*/
|
||
if (data->num_resp && data->sent_count == data->num_resp)
|
||
goto finish;
|
||
|
||
return true;
|
||
|
||
finish:
|
||
/* Note that destroy will be called */
|
||
ic.status = BT_HCI_ERR_SUCCESS;
|
||
send_event(btdev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
|
||
|
||
return false;
|
||
}
|
||
|
||
static void inquiry_destroy(void *user_data)
|
||
{
|
||
struct inquiry_data *data = user_data;
|
||
struct btdev *btdev = data->btdev;
|
||
|
||
if (!btdev)
|
||
goto finish;
|
||
|
||
btdev->inquiry_id = 0;
|
||
|
||
if (btdev->inquiry_timeout_id > 0) {
|
||
timeout_remove(btdev->inquiry_timeout_id);
|
||
btdev->inquiry_timeout_id = 0;
|
||
}
|
||
|
||
finish:
|
||
free(data);
|
||
}
|
||
|
||
static bool inquiry_timeout(void *user_data)
|
||
{
|
||
struct inquiry_data *data = user_data;
|
||
struct btdev *btdev = data->btdev;
|
||
struct bt_hci_evt_inquiry_complete ic;
|
||
|
||
timeout_remove(btdev->inquiry_id);
|
||
btdev->inquiry_timeout_id = 0;
|
||
|
||
/* Inquiry is stopped, send Inquiry complete event. */
|
||
ic.status = BT_HCI_ERR_SUCCESS;
|
||
send_event(btdev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
|
||
|
||
return false;
|
||
}
|
||
|
||
static int cmd_inquiry_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_inquiry *cmd = data;
|
||
struct inquiry_data *idata;
|
||
struct bt_hci_evt_inquiry_complete ic;
|
||
int status = BT_HCI_ERR_HARDWARE_FAILURE;
|
||
unsigned int inquiry_len_ms;
|
||
|
||
if (dev->inquiry_id > 0) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto failed;
|
||
}
|
||
|
||
idata = malloc0(sizeof(*idata));
|
||
if (!idata)
|
||
goto failed;
|
||
|
||
idata->btdev = dev;
|
||
idata->num_resp = cmd->num_resp;
|
||
|
||
/* Add timeout to cancel inquiry */
|
||
inquiry_len_ms = 1280 * cmd->length;
|
||
if (inquiry_len_ms)
|
||
dev->inquiry_timeout_id = timeout_add(inquiry_len_ms,
|
||
inquiry_timeout,
|
||
idata, NULL);
|
||
|
||
dev->inquiry_id = timeout_add(DEFAULT_INQUIRY_INTERVAL,
|
||
inquiry_callback, idata,
|
||
inquiry_destroy);
|
||
/* Return if success */
|
||
if (dev->inquiry_id > 0)
|
||
return 0;
|
||
|
||
failed:
|
||
ic.status = status;
|
||
send_event(dev, BT_HCI_EVT_INQUIRY_COMPLETE, &ic, sizeof(ic));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_inquiry_cancel(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
uint8_t status;
|
||
|
||
if (!dev->inquiry_id) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
timeout_remove(dev->inquiry_timeout_id);
|
||
dev->inquiry_timeout_id = 0;
|
||
timeout_remove(dev->inquiry_id);
|
||
dev->inquiry_id = 0;
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_INQUIRY_CANCEL, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_create_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_CREATE_CONN);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static struct btdev_conn *conn_new(struct btdev *dev, uint16_t handle,
|
||
uint8_t type)
|
||
{
|
||
struct btdev_conn *conn;
|
||
|
||
while ((conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(handle))))
|
||
handle++;
|
||
|
||
conn = new0(struct btdev_conn, 1);
|
||
conn->handle = handle;
|
||
conn->type = type;
|
||
conn->dev = dev;
|
||
|
||
if (!queue_push_tail(dev->conns, conn)) {
|
||
free(conn);
|
||
return NULL;
|
||
}
|
||
|
||
return conn;
|
||
}
|
||
|
||
static struct btdev_conn *conn_link(struct btdev *dev, struct btdev *remote,
|
||
uint16_t handle, uint8_t type)
|
||
{
|
||
struct btdev_conn *conn1, *conn2;
|
||
|
||
conn1 = conn_new(dev, handle, type);
|
||
if (!conn1)
|
||
return NULL;
|
||
|
||
conn2 = conn_new(remote, handle, type);
|
||
if (!conn2) {
|
||
free(conn1);
|
||
return NULL;
|
||
}
|
||
|
||
conn1->link = conn2;
|
||
conn2->link = conn1;
|
||
|
||
util_debug(dev->debug_callback, dev->debug_data,
|
||
"conn1 %p handle 0x%04x", conn1, conn1->handle);
|
||
util_debug(dev->debug_callback, dev->debug_data,
|
||
"conn2 %p handle 0x%04x", conn2, conn2->handle);
|
||
|
||
return conn1;
|
||
}
|
||
|
||
static struct btdev_conn *conn_add(struct btdev *dev,
|
||
const uint8_t *bdaddr, uint8_t bdaddr_type,
|
||
uint16_t handle, uint8_t type)
|
||
{
|
||
struct btdev *remote;
|
||
|
||
remote = find_btdev_by_bdaddr_type(bdaddr, bdaddr_type);
|
||
if (!remote)
|
||
return NULL;
|
||
|
||
return conn_link(dev, remote, handle, type);
|
||
}
|
||
|
||
static struct btdev_conn *conn_add_acl(struct btdev *dev,
|
||
const uint8_t *bdaddr, uint8_t bdaddr_type)
|
||
{
|
||
return conn_add(dev, bdaddr, bdaddr_type, ACL_HANDLE, HCI_ACLDATA_PKT);
|
||
}
|
||
|
||
static struct btdev_conn *conn_add_sco(struct btdev_conn *acl)
|
||
{
|
||
return conn_link(acl->dev, acl->link->dev, SCO_HANDLE, HCI_SCODATA_PKT);
|
||
}
|
||
|
||
static struct btdev_conn *conn_add_cis(struct btdev_conn *acl, uint16_t handle)
|
||
{
|
||
return conn_link(acl->dev, acl->link->dev, handle, HCI_ISODATA_PKT);
|
||
}
|
||
|
||
static struct btdev_conn *conn_add_bis(struct btdev *dev, uint16_t handle,
|
||
const struct bt_hci_bis *bis)
|
||
{
|
||
struct btdev_conn *conn;
|
||
|
||
conn = conn_new(dev, handle, HCI_ISODATA_PKT);
|
||
if (!conn)
|
||
return conn;
|
||
|
||
conn->data = util_memdup(bis, sizeof(*bis));
|
||
|
||
return conn;
|
||
}
|
||
|
||
static struct btdev_conn *find_bis_index(const struct btdev *remote,
|
||
uint8_t index)
|
||
{
|
||
struct btdev_conn *conn;
|
||
const struct queue_entry *entry;
|
||
|
||
for (entry = queue_get_entries(remote->conns); entry;
|
||
entry = entry->next) {
|
||
conn = entry->data;
|
||
|
||
/* Skip if not a broadcast */
|
||
if (conn->type != HCI_ISODATA_PKT || conn->link)
|
||
continue;
|
||
|
||
if (!index)
|
||
return conn;
|
||
|
||
index--;
|
||
}
|
||
|
||
return NULL;
|
||
}
|
||
|
||
static struct btdev_conn *conn_link_bis(struct btdev *dev, struct btdev *remote,
|
||
uint8_t index)
|
||
{
|
||
struct btdev_conn *conn;
|
||
struct btdev_conn *bis;
|
||
|
||
bis = find_bis_index(remote, index);
|
||
if (!bis)
|
||
return NULL;
|
||
|
||
conn = conn_add_bis(dev, ISO_HANDLE, bis->data);
|
||
if (!conn)
|
||
return NULL;
|
||
|
||
bis->link = conn;
|
||
conn->link = bis;
|
||
|
||
util_debug(dev->debug_callback, dev->debug_data,
|
||
"bis %p handle 0x%04x", bis, bis->handle);
|
||
util_debug(dev->debug_callback, dev->debug_data,
|
||
"conn %p handle 0x%04x", conn, conn->handle);
|
||
|
||
return conn;
|
||
}
|
||
|
||
static void pending_conn_add(struct btdev *btdev, struct btdev *remote)
|
||
{
|
||
unsigned int i;
|
||
|
||
for (i = 0; i < ARRAY_SIZE(btdev->pending_conn); ++i) {
|
||
if (!btdev->pending_conn[i]) {
|
||
btdev->pending_conn[i] = remote;
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
static bool pending_conn_del(struct btdev *btdev, struct btdev *remote)
|
||
{
|
||
unsigned int i;
|
||
|
||
for (i = 0; i < ARRAY_SIZE(btdev->pending_conn); ++i) {
|
||
if (btdev->pending_conn[i] == remote) {
|
||
btdev->pending_conn[i] = NULL;
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
static void conn_complete(struct btdev *btdev,
|
||
const uint8_t *bdaddr, uint8_t status)
|
||
{
|
||
struct bt_hci_evt_conn_complete cc;
|
||
struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
|
||
|
||
if (!remote)
|
||
return;
|
||
|
||
if (!status) {
|
||
struct btdev_conn *conn;
|
||
|
||
conn = conn_add_acl(btdev, bdaddr, BDADDR_BREDR);
|
||
if (!conn)
|
||
return;
|
||
|
||
pending_conn_del(conn->link->dev, btdev);
|
||
|
||
cc.status = status;
|
||
memcpy(cc.bdaddr, btdev->bdaddr, 6);
|
||
cc.encr_mode = 0x00;
|
||
|
||
cc.handle = cpu_to_le16(conn->link->handle);
|
||
cc.link_type = 0x01;
|
||
|
||
send_event(conn->link->dev, BT_HCI_EVT_CONN_COMPLETE, &cc,
|
||
sizeof(cc));
|
||
|
||
cc.handle = cpu_to_le16(conn->handle);
|
||
cc.link_type = 0x01;
|
||
} else {
|
||
cc.handle = cpu_to_le16(0x0000);
|
||
cc.link_type = 0x01;
|
||
}
|
||
|
||
pending_conn_del(btdev, remote);
|
||
|
||
cc.status = status;
|
||
memcpy(cc.bdaddr, bdaddr, 6);
|
||
cc.encr_mode = 0x00;
|
||
|
||
send_event(btdev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
|
||
}
|
||
|
||
struct page_timeout_data {
|
||
struct btdev *btdev;
|
||
uint8_t bdaddr[6];
|
||
unsigned int timeout_id;
|
||
};
|
||
|
||
static bool page_timeout(void *user_data)
|
||
{
|
||
struct page_timeout_data *pt_data = user_data;
|
||
struct btdev *btdev = pt_data->btdev;
|
||
const uint8_t *bdaddr = pt_data->bdaddr;
|
||
|
||
timeout_remove(pt_data->timeout_id);
|
||
pt_data->timeout_id = 0;
|
||
|
||
if (valid_btdev(btdev))
|
||
conn_complete(btdev, bdaddr, BT_HCI_ERR_PAGE_TIMEOUT);
|
||
|
||
free(pt_data);
|
||
return false;
|
||
}
|
||
|
||
static int cmd_create_conn_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_create_conn *cmd = data;
|
||
struct btdev *remote = find_btdev_by_bdaddr(cmd->bdaddr);
|
||
|
||
if (remote && remote->scan_enable & 0x02) {
|
||
struct bt_hci_evt_conn_request cr;
|
||
|
||
memcpy(cr.bdaddr, dev->bdaddr, 6);
|
||
memcpy(cr.dev_class, dev->dev_class, 3);
|
||
cr.link_type = 0x01;
|
||
|
||
pending_conn_add(dev, remote);
|
||
|
||
send_event(remote, BT_HCI_EVT_CONN_REQUEST, &cr, sizeof(cr));
|
||
} else {
|
||
struct page_timeout_data *pt_data =
|
||
new0(struct page_timeout_data, 1);
|
||
|
||
pt_data->btdev = dev;
|
||
memcpy(pt_data->bdaddr, cmd->bdaddr, 6);
|
||
|
||
/* Send page timeout after 5.12 seconds to emulate real
|
||
* paging.
|
||
*/
|
||
pt_data->timeout_id = timeout_add(5120,
|
||
page_timeout,
|
||
pt_data, NULL);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_add_sco_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_add_sco_conn *cmd = data;
|
||
struct bt_hci_evt_conn_complete cc;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&cc, 0, sizeof(cc));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (!conn) {
|
||
cc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
conn = conn_add_sco(conn);
|
||
if (!conn) {
|
||
cc.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
cc.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(cc.bdaddr, conn->link->dev->bdaddr, 6);
|
||
cc.handle = cpu_to_le16(conn->handle);
|
||
cc.link_type = 0x00;
|
||
cc.encr_mode = 0x00;
|
||
|
||
done:
|
||
pending_conn_del(dev, conn->link->dev);
|
||
|
||
send_event(dev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static bool match_bdaddr(const void *data, const void *match_data)
|
||
{
|
||
const struct btdev_conn *conn = data;
|
||
const uint8_t *bdaddr = match_data;
|
||
|
||
return !memcmp(conn->link->dev->bdaddr, bdaddr, 6);
|
||
}
|
||
|
||
static int cmd_create_conn_cancel(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_create_conn_cancel_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_create_conn_cancel *cmd = data;
|
||
struct bt_hci_rsp_create_conn_cancel rp;
|
||
struct btdev *remote = find_btdev_by_bdaddr(cmd->bdaddr);
|
||
struct btdev_conn *conn;
|
||
|
||
/* BLUETOOTH CORE SPECIFICATION Version 5.4 | Vol 4, Part E page 1848
|
||
*
|
||
* If the connection is already established, and the
|
||
* HCI_Connection_Complete event has been sent, then the Controller
|
||
* shall return an HCI_Command_Complete event with the error code
|
||
* Connection Already Exists (0x0B). If the HCI_Create_Connection_Cancel
|
||
* command is sent to the Controller without a preceding
|
||
* HCI_Create_Connection command to the same device, the BR/EDR
|
||
* Controller shall return an HCI_Command_Complete event with the error
|
||
* code Unknown Connection Identifier (0x02).
|
||
*/
|
||
if (pending_conn_del(dev, remote)) {
|
||
rp.status = BT_HCI_ERR_SUCCESS;
|
||
} else {
|
||
conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
|
||
if (conn)
|
||
rp.status = BT_HCI_ERR_CONN_ALREADY_EXISTS;
|
||
else
|
||
rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
}
|
||
|
||
memcpy(rp.bdaddr, cmd->bdaddr, sizeof(rp.bdaddr));
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_CREATE_CONN_CANCEL, &rp, sizeof(rp));
|
||
|
||
if (!rp.status)
|
||
conn_complete(dev, cmd->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_accept_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_ACCEPT_CONN_REQUEST);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_accept_conn_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_accept_conn_request *cmd = data;
|
||
struct btdev *remote = find_btdev_by_bdaddr(cmd->bdaddr);
|
||
|
||
if (!remote)
|
||
return 0;
|
||
|
||
if (dev->auth_enable || remote->auth_enable)
|
||
send_event(remote, BT_HCI_EVT_LINK_KEY_REQUEST, dev->bdaddr, 6);
|
||
else
|
||
conn_complete(dev, cmd->bdaddr, BT_HCI_ERR_SUCCESS);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_reject_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_REJECT_CONN_REQUEST);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_reject_conn_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_reject_conn_request *cmd = data;
|
||
|
||
conn_complete(dev, cmd->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_link_key_reply(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_link_key_request_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, data, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_LINK_KEY_REQUEST_REPLY, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void auth_complete(struct btdev_conn *conn, uint8_t status)
|
||
{
|
||
struct bt_hci_evt_auth_complete ev;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
ev.handle = conn ? cpu_to_le16(conn->handle) : 0x0000;
|
||
ev.status = status;
|
||
|
||
send_event(conn->dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
|
||
|
||
conn->dev->ssp_status = 0;
|
||
conn->dev->ssp_auth_complete = false;
|
||
conn->link->dev->ssp_status = 0;
|
||
conn->link->dev->ssp_auth_complete = false;
|
||
}
|
||
|
||
static int cmd_link_key_reply_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_link_key_request_reply *cmd = data;
|
||
struct btdev_conn *conn;
|
||
uint8_t status;
|
||
|
||
conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
|
||
if (!conn) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
memcpy(dev->link_key, cmd->link_key, 16);
|
||
|
||
if (!memcmp(conn->link->dev->link_key, LINK_KEY_NONE, 16)) {
|
||
send_event(conn->link->dev, BT_HCI_EVT_LINK_KEY_REQUEST,
|
||
dev->bdaddr, 6);
|
||
return 0;
|
||
}
|
||
|
||
if (!memcmp(dev->link_key, conn->link->dev->link_key, 16))
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
else
|
||
status = BT_HCI_ERR_AUTH_FAILURE;
|
||
|
||
done:
|
||
auth_complete(conn, status);
|
||
|
||
if (conn)
|
||
auth_complete(conn->link, status);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_link_key_neg_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_link_key_request_neg_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, data, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static bool use_ssp(struct btdev *btdev1, struct btdev *btdev2)
|
||
{
|
||
if (btdev1->auth_enable || btdev2->auth_enable)
|
||
return false;
|
||
|
||
return (btdev1->simple_pairing_mode && btdev2->simple_pairing_mode);
|
||
}
|
||
|
||
static int cmd_link_key_neg_reply_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_link_key_request_neg_reply *cmd = data;
|
||
struct btdev *remote;
|
||
|
||
remote = find_btdev_by_bdaddr(cmd->bdaddr);
|
||
if (!remote)
|
||
return 0;
|
||
|
||
if (use_ssp(dev, remote)) {
|
||
struct bt_hci_evt_io_capability_request io_req;
|
||
|
||
memcpy(io_req.bdaddr, cmd->bdaddr, 6);
|
||
send_event(dev, BT_HCI_EVT_IO_CAPABILITY_REQUEST, &io_req,
|
||
sizeof(io_req));
|
||
} else {
|
||
struct bt_hci_evt_pin_code_request pin_req;
|
||
|
||
memcpy(pin_req.bdaddr, cmd->bdaddr, 6);
|
||
send_event(dev, BT_HCI_EVT_PIN_CODE_REQUEST, &pin_req,
|
||
sizeof(pin_req));
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pin_code_reply(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_pin_code_request_neg_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, data, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_PIN_CODE_REQUEST_REPLY, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static uint8_t get_link_key_type(struct btdev *btdev, const uint8_t *bdaddr)
|
||
{
|
||
struct btdev_conn *conn;
|
||
uint8_t auth, unauth;
|
||
|
||
conn = queue_find(btdev->conns, match_bdaddr, bdaddr);
|
||
if (!conn)
|
||
return 0x00;
|
||
|
||
if (!btdev->simple_pairing_mode)
|
||
return 0x00;
|
||
|
||
if (btdev->ssp_debug_mode || conn->link->dev->ssp_debug_mode)
|
||
return 0x03;
|
||
|
||
if (btdev->secure_conn_support &&
|
||
conn->link->dev->secure_conn_support) {
|
||
unauth = 0x07;
|
||
auth = 0x08;
|
||
} else {
|
||
unauth = 0x04;
|
||
auth = 0x05;
|
||
}
|
||
|
||
if (btdev->io_cap == 0x03 || conn->link->dev->io_cap == 0x03)
|
||
return unauth;
|
||
|
||
if (!(btdev->auth_req & 0x01) && !(conn->link->dev->auth_req & 0x01))
|
||
return unauth;
|
||
|
||
/* DisplayOnly only produces authenticated with KeyboardOnly */
|
||
if (btdev->io_cap == 0x00 && conn->link->dev->io_cap != 0x02)
|
||
return unauth;
|
||
|
||
/* DisplayOnly only produces authenticated with KeyboardOnly */
|
||
if (conn->link->dev->io_cap == 0x00 && btdev->io_cap != 0x02)
|
||
return unauth;
|
||
|
||
return auth;
|
||
}
|
||
|
||
static void link_key_notify(struct btdev *btdev, const uint8_t *bdaddr,
|
||
const uint8_t *key)
|
||
{
|
||
struct bt_hci_evt_link_key_notify ev;
|
||
|
||
memcpy(btdev->link_key, key, 16);
|
||
|
||
memcpy(ev.bdaddr, bdaddr, 6);
|
||
memcpy(ev.link_key, key, 16);
|
||
ev.key_type = get_link_key_type(btdev, bdaddr);
|
||
|
||
send_event(btdev, BT_HCI_EVT_LINK_KEY_NOTIFY, &ev, sizeof(ev));
|
||
}
|
||
|
||
static int cmd_pin_code_reply_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_pin_code_request_reply *cmd = data;
|
||
struct btdev *remote;
|
||
struct btdev_conn *conn;
|
||
uint8_t status;
|
||
|
||
conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
|
||
if (!conn) {
|
||
remote = find_btdev_by_bdaddr(cmd->bdaddr);
|
||
if (!remote)
|
||
return 0;
|
||
} else
|
||
remote = conn->link->dev;
|
||
|
||
memcpy(dev->pin, cmd->pin_code, cmd->pin_len);
|
||
dev->pin_len = cmd->pin_len;
|
||
|
||
if (!remote->pin_len) {
|
||
struct bt_hci_evt_pin_code_request pin_req;
|
||
|
||
memcpy(pin_req.bdaddr, dev->bdaddr, 6);
|
||
send_event(remote, BT_HCI_EVT_PIN_CODE_REQUEST,
|
||
&pin_req, sizeof(pin_req));
|
||
return 0;
|
||
}
|
||
|
||
if (dev->pin_len == remote->pin_len &&
|
||
!memcmp(dev->pin, remote->pin, dev->pin_len)) {
|
||
link_key_notify(dev, remote->bdaddr, LINK_KEY_DUMMY);
|
||
link_key_notify(remote, dev->bdaddr, LINK_KEY_DUMMY);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
} else {
|
||
status = BT_HCI_ERR_AUTH_FAILURE;
|
||
}
|
||
|
||
if (conn)
|
||
auth_complete(conn->link, status);
|
||
else
|
||
conn_complete(remote, dev->bdaddr, status);
|
||
|
||
dev->pin_len = 0;
|
||
remote->pin_len = 0;
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pin_code_neg_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_pin_code_request_neg_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, data, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pin_code_neg_reply_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_pin_code_request_neg_reply *cmd = data;
|
||
struct btdev *remote;
|
||
struct btdev_conn *conn;
|
||
uint8_t status;
|
||
|
||
remote = find_btdev_by_bdaddr(cmd->bdaddr);
|
||
if (!remote)
|
||
return 0;
|
||
|
||
status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
|
||
|
||
conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
|
||
if (conn)
|
||
auth_complete(conn, status);
|
||
else
|
||
conn_complete(dev, cmd->bdaddr, BT_HCI_ERR_PIN_OR_KEY_MISSING);
|
||
|
||
if (conn) {
|
||
if (remote->pin_len)
|
||
auth_complete(conn->link, status);
|
||
} else {
|
||
conn_complete(remote, dev->bdaddr,
|
||
BT_HCI_ERR_PIN_OR_KEY_MISSING);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_auth_requested(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_AUTH_REQUESTED);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_auth_requested_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_auth_requested *cmd = data;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn) {
|
||
struct bt_hci_evt_auth_complete ev;
|
||
|
||
ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
|
||
send_event(dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
dev->auth_init = true;
|
||
|
||
send_event(dev, BT_HCI_EVT_LINK_KEY_REQUEST, conn->link->dev->bdaddr,
|
||
sizeof(conn->link->dev->bdaddr));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_conn_encrypt(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_SET_CONN_ENCRYPT);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void encrypt_change(struct btdev_conn *conn, uint8_t mode,
|
||
uint8_t status)
|
||
{
|
||
struct bt_hci_evt_encrypt_change ev;
|
||
|
||
if (!conn)
|
||
return;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
ev.status = status;
|
||
ev.handle = cpu_to_le16(conn->handle);
|
||
ev.encr_mode = mode;
|
||
|
||
send_event(conn->dev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
|
||
}
|
||
|
||
static int cmd_set_conn_encrypt_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_set_conn_encrypt *cmd = data;
|
||
struct btdev_conn *conn;
|
||
uint8_t mode;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn)
|
||
return 0;
|
||
|
||
if (!cmd->encr_mode)
|
||
mode = 0x00;
|
||
else if (dev->secure_conn_support &&
|
||
conn->link->dev->secure_conn_support)
|
||
mode = 0x02;
|
||
else
|
||
mode = 0x01;
|
||
|
||
encrypt_change(conn, mode, BT_HCI_ERR_SUCCESS);
|
||
encrypt_change(conn->link, mode, BT_HCI_ERR_SUCCESS);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remote_name(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_REMOTE_NAME_REQUEST);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void name_request_complete(struct btdev *btdev,
|
||
const uint8_t *bdaddr, uint8_t status)
|
||
{
|
||
struct bt_hci_evt_remote_name_request_complete nc;
|
||
|
||
nc.status = status;
|
||
memcpy(nc.bdaddr, bdaddr, 6);
|
||
memset(nc.name, 0, 248);
|
||
|
||
if (!status) {
|
||
struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
|
||
|
||
if (remote)
|
||
memcpy(nc.name, remote->name, 248);
|
||
else
|
||
nc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
}
|
||
|
||
send_event(btdev, BT_HCI_EVT_REMOTE_NAME_REQUEST_COMPLETE,
|
||
&nc, sizeof(nc));
|
||
}
|
||
|
||
static int cmd_remote_name_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_remote_name_request *cmd = data;
|
||
|
||
name_request_complete(dev, cmd->bdaddr, BT_HCI_ERR_SUCCESS);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remote_name_cancel(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_remote_name_request_cancel *cmd = data;
|
||
struct bt_hci_rsp_remote_name_request_cancel rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, cmd->bdaddr, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remote_name_cancel_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_remote_name_request_cancel *cmd = data;
|
||
|
||
name_request_complete(dev, cmd->bdaddr, BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_remote_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_READ_REMOTE_FEATURES);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_remote_features_complete(struct btdev *dev,
|
||
const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_remote_features *cmd = data;
|
||
struct bt_hci_evt_remote_features_complete rfc;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (conn) {
|
||
rfc.status = BT_HCI_ERR_SUCCESS;
|
||
rfc.handle = cpu_to_le16(cmd->handle);
|
||
memcpy(rfc.features, conn->link->dev->features, 8);
|
||
} else {
|
||
rfc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
rfc.handle = cpu_to_le16(cmd->handle);
|
||
memset(rfc.features, 0, 8);
|
||
}
|
||
|
||
send_event(dev, BT_HCI_EVT_REMOTE_FEATURES_COMPLETE, &rfc, sizeof(rfc));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_remote_ext_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS,
|
||
BT_HCI_CMD_READ_REMOTE_EXT_FEATURES);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void btdev_get_host_features(struct btdev *btdev, uint8_t features[8])
|
||
{
|
||
memset(features, 0, 8);
|
||
if (btdev->simple_pairing_mode)
|
||
features[0] |= 0x01;
|
||
if (btdev->le_supported)
|
||
features[0] |= 0x02;
|
||
if (btdev->le_simultaneous)
|
||
features[0] |= 0x04;
|
||
if (btdev->secure_conn_support)
|
||
features[0] |= 0x08;
|
||
}
|
||
|
||
static int cmd_read_remote_ext_features_compl(struct btdev *dev,
|
||
const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_remote_ext_features *cmd = data;
|
||
struct bt_hci_evt_remote_ext_features_complete ev;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (conn && cmd->page < 0x02) {
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.page = cmd->page;
|
||
ev.max_page = 0x01;
|
||
|
||
switch (cmd->page) {
|
||
case 0x00:
|
||
ev.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(ev.features, conn->link->dev->features, 8);
|
||
break;
|
||
case 0x01:
|
||
ev.status = BT_HCI_ERR_SUCCESS;
|
||
btdev_get_host_features(conn->link->dev, ev.features);
|
||
break;
|
||
default:
|
||
ev.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
memset(ev.features, 0, 8);
|
||
break;
|
||
}
|
||
} else {
|
||
ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.page = cmd->page;
|
||
ev.max_page = 0x01;
|
||
memset(ev.features, 0, 8);
|
||
}
|
||
|
||
send_event(dev, BT_HCI_EVT_REMOTE_EXT_FEATURES_COMPLETE, &ev,
|
||
sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_clock_offset(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_READ_CLOCK_OFFSET);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_clock_offset_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_clock_offset *cmd = data;
|
||
struct bt_hci_evt_clock_offset_complete ev;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (conn) {
|
||
ev.status = BT_HCI_ERR_SUCCESS;
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.clock_offset = 0;
|
||
} else {
|
||
ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.clock_offset = 0;
|
||
}
|
||
|
||
send_event(dev, BT_HCI_EVT_CLOCK_OFFSET_COMPLETE, &ev, sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_link_policy(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_default_link_policy rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.policy = cpu_to_le16(dev->default_link_policy);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_DEFAULT_LINK_POLICY, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_link_policy(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_default_link_policy *cmd = data;
|
||
uint8_t status;
|
||
|
||
dev->default_link_policy = le16_to_cpu(cmd->policy);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_DEFAULT_LINK_POLICY, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_event_filter(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_set_event_filter *cmd = data;
|
||
uint8_t status;
|
||
|
||
dev->event_filter = cmd->type;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_SET_EVENT_FILTER, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_link_key(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_stored_link_key rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.max_num_keys = cpu_to_le16(0);
|
||
rsp.num_keys = cpu_to_le16(0);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_STORED_LINK_KEY, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_link_key(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_write_stored_link_key rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.num_keys = 0;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_STORED_LINK_KEY, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_delete_link_key(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_delete_stored_link_key rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.num_keys = cpu_to_le16(0);
|
||
cmd_complete(dev, BT_HCI_CMD_DELETE_STORED_LINK_KEY, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_name(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_name rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.name, dev->name, 248);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_NAME, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_local_name(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_local_name *cmd = data;
|
||
uint8_t status;
|
||
|
||
memcpy(dev->name, cmd->name, 248);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_LOCAL_NAME, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_accept_timeout(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_conn_accept_timeout rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.timeout = cpu_to_le16(dev->conn_accept_timeout);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_CONN_ACCEPT_TIMEOUT, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_accept_timeout(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_conn_accept_timeout *cmd = data;
|
||
uint8_t status;
|
||
|
||
dev->conn_accept_timeout = le16_to_cpu(cmd->timeout);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_CONN_ACCEPT_TIMEOUT, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_page_timeout(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_page_timeout rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.timeout = cpu_to_le16(dev->page_timeout);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_PAGE_TIMEOUT, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_page_timeout(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_page_timeout *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->page_timeout = le16_to_cpu(cmd->timeout);
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_PAGE_TIMEOUT, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_scan_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_scan_enable rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.enable = dev->scan_enable;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_SCAN_ENABLE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_scan_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_scan_enable *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->scan_enable = cmd->enable;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_SCAN_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_page_scan(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_page_scan_activity rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.interval = cpu_to_le16(dev->page_scan_interval);
|
||
rsp.window = cpu_to_le16(dev->page_scan_window);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_PAGE_SCAN_ACTIVITY, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_page_scan(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_page_scan_activity *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->page_scan_interval = le16_to_cpu(cmd->interval);
|
||
dev->page_scan_window = le16_to_cpu(cmd->window);
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_PAGE_SCAN_ACTIVITY, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_inquiry_scan(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_inquiry_scan_activity rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.interval = cpu_to_le16(dev->inquiry_scan_interval);
|
||
rsp.window = cpu_to_le16(dev->inquiry_scan_window);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_INQUIRY_SCAN_ACTIVITY, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_inquiry_scan(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_inquiry_scan_activity *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->inquiry_scan_interval = le16_to_cpu(cmd->interval);
|
||
dev->inquiry_scan_window = le16_to_cpu(cmd->window);
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_INQUIRY_SCAN_ACTIVITY, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_auth_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_auth_enable rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.enable = dev->auth_enable;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_AUTH_ENABLE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_auth_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_auth_enable *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->auth_enable = cmd->enable;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_AUTH_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_class(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_class_of_dev rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.dev_class, dev->dev_class, 3);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_CLASS_OF_DEV, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_class(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_class_of_dev *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
memcpy(dev->dev_class, cmd->dev_class, 3);
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_CLASS_OF_DEV, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_voice(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_voice_setting rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.setting = cpu_to_le16(dev->voice_setting);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_VOICE_SETTING, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_voice(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_voice_setting *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->voice_setting = le16_to_cpu(cmd->setting);
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_VOICE_SETTING, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_tx_power_level(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_tx_power *cmd = data;
|
||
struct bt_hci_rsp_read_tx_power rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.handle = le16_to_cpu(cmd->handle);
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
if (cmd->type)
|
||
rsp.level = 4;
|
||
else
|
||
rsp.level = -1;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_TX_POWER, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_num_iac(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_num_supported_iac rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.num_iac = 0x01;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_NUM_SUPPORTED_IAC, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_current_iac_lap(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_current_iac_lap *rsp;
|
||
|
||
rsp = alloca(sizeof(*rsp) + 3);
|
||
rsp->status = BT_HCI_ERR_SUCCESS;
|
||
rsp->num_iac = 0x01;
|
||
rsp->iac_lap[0] = 0x33;
|
||
rsp->iac_lap[1] = 0x8b;
|
||
rsp->iac_lap[2] = 0x9e;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_CURRENT_IAC_LAP, rsp,
|
||
sizeof(*rsp) + 3);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_current_iac_lap(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_CURRENT_IAC_LAP, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_inquiry_mode(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_inquiry_mode rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.mode = dev->inquiry_mode;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_INQUIRY_MODE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_inquiry_mode(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_inquiry_mode *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->inquiry_mode = cmd->mode;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_INQUIRY_MODE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_page_scan_type(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_page_scan_type rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.type = dev->page_scan_type;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_PAGE_SCAN_TYPE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_page_scan_type(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_page_scan_type *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->page_scan_type = cmd->type;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_PAGE_SCAN_TYPE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_afh_mode(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_afh_assessment_mode rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.mode = dev->afh_assessment_mode;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_AFH_ASSESSMENT_MODE, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_afh_mode(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_afh_assessment_mode *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->afh_assessment_mode = cmd->mode;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_AFH_ASSESSMENT_MODE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_ext_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_ext_features rsp;
|
||
uint8_t page = ((const uint8_t *) data)[0];
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.page = page;
|
||
rsp.max_page = dev->max_page;
|
||
|
||
if (page > dev->max_page) {
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
|
||
switch (page) {
|
||
case 0x00:
|
||
memcpy(rsp.features, dev->features, 8);
|
||
break;
|
||
case 0x01:
|
||
btdev_get_host_features(dev, rsp.features);
|
||
break;
|
||
case 0x02:
|
||
memcpy(rsp.features, dev->feat_page_2, 8);
|
||
break;
|
||
default:
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
break;
|
||
}
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_EXT_FEATURES, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_country_code(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_country_code rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.code = dev->country_code;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_COUNTRY_CODE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_rssi(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_rssi *cmd = data;
|
||
struct bt_hci_rsp_read_rssi rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.handle = le16_to_cpu(cmd->handle);
|
||
rsp.rssi = -1;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_RSSI, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_clock(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_clock *cmd = data;
|
||
struct bt_hci_rsp_read_clock rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.handle = le16_to_cpu(cmd->handle);
|
||
rsp.clock = 0x11223344;
|
||
rsp.accuracy = 0x5566;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_CLOCK, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_enable_dut_mode(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_ENABLE_DUT_MODE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_COMMON_BREDR_20 \
|
||
CMD(BT_HCI_CMD_INQUIRY, cmd_inquiry, cmd_inquiry_complete), \
|
||
CMD(BT_HCI_CMD_INQUIRY_CANCEL, cmd_inquiry_cancel, NULL), \
|
||
CMD(BT_HCI_CMD_CREATE_CONN, cmd_create_conn, \
|
||
cmd_create_conn_complete), \
|
||
CMD(BT_HCI_CMD_ADD_SCO_CONN, cmd_add_sco_conn, NULL), \
|
||
CMD(BT_HCI_CMD_CREATE_CONN_CANCEL, cmd_create_conn_cancel, \
|
||
cmd_create_conn_cancel_complete), \
|
||
CMD(BT_HCI_CMD_ACCEPT_CONN_REQUEST, cmd_accept_conn, \
|
||
cmd_accept_conn_complete), \
|
||
CMD(BT_HCI_CMD_REJECT_CONN_REQUEST, cmd_reject_conn, \
|
||
cmd_reject_conn_complete), \
|
||
CMD(BT_HCI_CMD_LINK_KEY_REQUEST_REPLY, cmd_link_key_reply, \
|
||
cmd_link_key_reply_complete), \
|
||
CMD(BT_HCI_CMD_LINK_KEY_REQUEST_NEG_REPLY, \
|
||
cmd_link_key_neg_reply, \
|
||
cmd_link_key_neg_reply_complete), \
|
||
CMD(BT_HCI_CMD_PIN_CODE_REQUEST_REPLY, cmd_pin_code_reply, \
|
||
cmd_pin_code_reply_complete), \
|
||
CMD(BT_HCI_CMD_PIN_CODE_REQUEST_NEG_REPLY, \
|
||
cmd_pin_code_neg_reply, \
|
||
cmd_pin_code_neg_reply_complete), \
|
||
CMD(BT_HCI_CMD_AUTH_REQUESTED, cmd_auth_requested, \
|
||
cmd_auth_requested_complete), \
|
||
CMD(BT_HCI_CMD_SET_CONN_ENCRYPT, cmd_set_conn_encrypt, \
|
||
cmd_set_conn_encrypt_complete), \
|
||
CMD(BT_HCI_CMD_REMOTE_NAME_REQUEST, cmd_remote_name, \
|
||
cmd_remote_name_complete), \
|
||
CMD(BT_HCI_CMD_REMOTE_NAME_REQUEST_CANCEL, cmd_remote_name_cancel, \
|
||
cmd_remote_name_cancel_complete), \
|
||
CMD(BT_HCI_CMD_READ_REMOTE_FEATURES, cmd_read_remote_features, \
|
||
cmd_read_remote_features_complete), \
|
||
CMD(BT_HCI_CMD_READ_REMOTE_EXT_FEATURES, \
|
||
cmd_read_remote_ext_features, \
|
||
cmd_read_remote_ext_features_compl), \
|
||
CMD(BT_HCI_CMD_READ_CLOCK_OFFSET, cmd_read_clock_offset, \
|
||
cmd_read_clock_offset_complete), \
|
||
CMD(BT_HCI_CMD_READ_DEFAULT_LINK_POLICY, cmd_read_link_policy, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_DEFAULT_LINK_POLICY, cmd_write_link_policy, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_SET_EVENT_FILTER, cmd_set_event_filter, NULL), \
|
||
CMD(BT_HCI_CMD_READ_STORED_LINK_KEY, cmd_read_link_key, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_STORED_LINK_KEY, cmd_write_link_key, NULL), \
|
||
CMD(BT_HCI_CMD_DELETE_STORED_LINK_KEY, cmd_delete_link_key, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_NAME, cmd_read_local_name, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_LOCAL_NAME, cmd_write_local_name, NULL), \
|
||
CMD(BT_HCI_CMD_READ_CONN_ACCEPT_TIMEOUT, cmd_read_accept_timeout, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_CONN_ACCEPT_TIMEOUT, cmd_write_accept_timeout, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_PAGE_TIMEOUT, cmd_read_page_timeout, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_PAGE_TIMEOUT, cmd_write_page_timeout, NULL), \
|
||
CMD(BT_HCI_CMD_READ_SCAN_ENABLE, cmd_read_scan_enable, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_SCAN_ENABLE, cmd_write_scan_enable, NULL), \
|
||
CMD(BT_HCI_CMD_READ_PAGE_SCAN_ACTIVITY, cmd_read_page_scan, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_PAGE_SCAN_ACTIVITY, cmd_write_page_scan, NULL), \
|
||
CMD(BT_HCI_CMD_READ_INQUIRY_SCAN_ACTIVITY, cmd_read_inquiry_scan, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_INQUIRY_SCAN_ACTIVITY, cmd_write_inquiry_scan, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_AUTH_ENABLE, cmd_read_auth_enable, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_AUTH_ENABLE, cmd_write_auth_enable, NULL), \
|
||
CMD(BT_HCI_CMD_READ_CLASS_OF_DEV, cmd_read_class, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_CLASS_OF_DEV, cmd_write_class, NULL), \
|
||
CMD(BT_HCI_CMD_READ_VOICE_SETTING, cmd_read_voice, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_VOICE_SETTING, cmd_write_voice, NULL), \
|
||
CMD(BT_HCI_CMD_READ_TX_POWER, cmd_read_tx_power_level, NULL), \
|
||
CMD(BT_HCI_CMD_READ_NUM_SUPPORTED_IAC, cmd_read_num_iac, NULL), \
|
||
CMD(BT_HCI_CMD_READ_CURRENT_IAC_LAP, cmd_read_current_iac_lap, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_CURRENT_IAC_LAP, cmd_write_current_iac_lap, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_INQUIRY_MODE, cmd_read_inquiry_mode, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_INQUIRY_MODE, cmd_write_inquiry_mode, NULL), \
|
||
CMD(BT_HCI_CMD_READ_PAGE_SCAN_TYPE, cmd_read_page_scan_type, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_PAGE_SCAN_TYPE, cmd_write_page_scan_type, NULL), \
|
||
CMD(BT_HCI_CMD_READ_AFH_ASSESSMENT_MODE, cmd_read_afh_mode, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_AFH_ASSESSMENT_MODE, cmd_write_afh_mode, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_EXT_FEATURES, cmd_read_local_ext_features, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_COUNTRY_CODE, cmd_read_country_code, NULL), \
|
||
CMD(BT_HCI_CMD_READ_RSSI, cmd_read_rssi, NULL), \
|
||
CMD(BT_HCI_CMD_READ_CLOCK, cmd_read_clock, NULL), \
|
||
CMD(BT_HCI_CMD_ENABLE_DUT_MODE, cmd_enable_dut_mode, NULL)
|
||
|
||
static void set_common_commands_bredr20(struct btdev *btdev)
|
||
{
|
||
btdev->commands[0] |= 0x01; /* Inquiry */
|
||
btdev->commands[0] |= 0x02; /* Inquiry Cancel */
|
||
btdev->commands[0] |= 0x10; /* Create Connection */
|
||
btdev->commands[0] |= 0x40; /* Add SCO Connection */
|
||
btdev->commands[0] |= 0x80; /* Cancel Create Connection */
|
||
btdev->commands[1] |= 0x01; /* Accept Connection Request */
|
||
btdev->commands[1] |= 0x02; /* Reject Connection Request */
|
||
btdev->commands[1] |= 0x04; /* Link Key Request Reply */
|
||
btdev->commands[1] |= 0x08; /* Link Key Request Negative Reply */
|
||
btdev->commands[1] |= 0x10; /* PIN Code Request Reply */
|
||
btdev->commands[1] |= 0x20; /* PIN Code Request Negative Reply */
|
||
btdev->commands[1] |= 0x80; /* Authentication Requested */
|
||
btdev->commands[2] |= 0x01; /* Set Connection Encryption */
|
||
btdev->commands[2] |= 0x08; /* Remote Name Request */
|
||
btdev->commands[2] |= 0x10; /* Cancel Remote Name Request */
|
||
btdev->commands[2] |= 0x20; /* Read Remote Supported Features */
|
||
btdev->commands[2] |= 0x40; /* Read Remote Extended Features */
|
||
btdev->commands[3] |= 0x01; /* Read Clock Offset */
|
||
btdev->commands[5] |= 0x08; /* Read Default Link Policy */
|
||
btdev->commands[5] |= 0x10; /* Write Default Link Policy */
|
||
btdev->commands[6] |= 0x01; /* Set Event Filter */
|
||
btdev->commands[6] |= 0x20; /* Read Stored Link Key */
|
||
btdev->commands[6] |= 0x40; /* Write Stored Link Key */
|
||
btdev->commands[6] |= 0x80; /* Delete Stored Link Key */
|
||
btdev->commands[7] |= 0x01; /* Write Local Name */
|
||
btdev->commands[7] |= 0x02; /* Read Local Name */
|
||
btdev->commands[7] |= 0x04; /* Read Connection Accept Timeout */
|
||
btdev->commands[7] |= 0x08; /* Write Connection Accept Timeout */
|
||
btdev->commands[7] |= 0x10; /* Read Page Timeout */
|
||
btdev->commands[7] |= 0x20; /* Write Page Timeout */
|
||
btdev->commands[7] |= 0x40; /* Read Scan Enable */
|
||
btdev->commands[7] |= 0x80; /* Write Scan Enable */
|
||
btdev->commands[8] |= 0x01; /* Read Page Scan Activity */
|
||
btdev->commands[8] |= 0x02; /* Write Page Scan Activity */
|
||
btdev->commands[8] |= 0x04; /* Read Inquiry Scan Activity */
|
||
btdev->commands[8] |= 0x08; /* Write Inquiry Scan Activity */
|
||
btdev->commands[8] |= 0x10; /* Read Authentication Enable */
|
||
btdev->commands[8] |= 0x20; /* Write Authentication Enable */
|
||
btdev->commands[9] |= 0x01; /* Read Class Of Device */
|
||
btdev->commands[9] |= 0x02; /* Write Class Of Device */
|
||
btdev->commands[9] |= 0x04; /* Read Voice Setting */
|
||
btdev->commands[9] |= 0x08; /* Write Voice Setting */
|
||
btdev->commands[10] |= 0x04; /* Read TX Power Level */
|
||
btdev->commands[11] |= 0x04; /* Read Number of Supported IAC */
|
||
btdev->commands[11] |= 0x08; /* Read Current IAC LAP */
|
||
btdev->commands[11] |= 0x10; /* Write Current IAC LAP */
|
||
btdev->commands[12] |= 0x40; /* Read Inquiry Mode */
|
||
btdev->commands[12] |= 0x80; /* Write Inquiry Mode */
|
||
btdev->commands[13] |= 0x01; /* Read Page Scan Type */
|
||
btdev->commands[13] |= 0x02; /* Write Page Scan Type */
|
||
btdev->commands[13] |= 0x04; /* Read AFH Assess Mode */
|
||
btdev->commands[13] |= 0x08; /* Write AFH Assess Mode */
|
||
btdev->commands[14] |= 0x40; /* Read Local Extended Features */
|
||
btdev->commands[15] |= 0x01; /* Read Country Code */
|
||
btdev->commands[15] |= 0x20; /* Read RSSI */
|
||
btdev->commands[15] |= 0x80; /* Read Clock */
|
||
btdev->commands[16] |= 0x04; /* Enable Device Under Test Mode */
|
||
}
|
||
|
||
static int cmd_enhanced_setup_sync_conn(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_enhanced_setup_sync_conn *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
if (cmd->tx_coding_format[0] > 5)
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
|
||
cmd_status(dev, status, BT_HCI_CMD_ENHANCED_SETUP_SYNC_CONN);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_enhanced_setup_sync_conn_complete(struct btdev *dev,
|
||
const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_enhanced_setup_sync_conn *cmd = data;
|
||
struct bt_hci_evt_sync_conn_complete cc;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&cc, 0, sizeof(cc));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn) {
|
||
cc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
conn = conn_add_sco(conn);
|
||
if (!conn) {
|
||
cc.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
/* TODO: HCI_Connection_Request connection flow */
|
||
|
||
cc.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(cc.bdaddr, conn->link->dev->bdaddr, 6);
|
||
|
||
cc.handle = cpu_to_le16(conn->handle);
|
||
cc.link_type = 0x02;
|
||
cc.tx_interval = 0x000c;
|
||
cc.retrans_window = 0x06;
|
||
cc.rx_pkt_len = 60;
|
||
cc.tx_pkt_len = 60;
|
||
cc.air_mode = cmd->tx_coding_format[0];
|
||
|
||
done:
|
||
send_event(dev, BT_HCI_EVT_SYNC_CONN_COMPLETE, &cc, sizeof(cc));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_setup_sync_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_SETUP_SYNC_CONN);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_setup_sync_conn_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_setup_sync_conn *cmd = data;
|
||
struct bt_hci_evt_sync_conn_complete cc;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&cc, 0, sizeof(cc));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn) {
|
||
cc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
conn = conn_add_sco(conn);
|
||
if (!conn) {
|
||
cc.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
cc.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(cc.bdaddr, conn->link->dev->bdaddr, 6);
|
||
|
||
cc.handle = cpu_to_le16(conn->handle);
|
||
cc.link_type = 0x02;
|
||
cc.tx_interval = 0x000c;
|
||
cc.retrans_window = 0x06;
|
||
cc.rx_pkt_len = 60;
|
||
cc.tx_pkt_len = 60;
|
||
cc.air_mode = (cmd->voice_setting == 0x0060) ? 0x02 : 0x03;
|
||
|
||
done:
|
||
send_event(dev, BT_HCI_EVT_SYNC_CONN_COMPLETE, &cc, sizeof(cc));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_ext_inquiry(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_ext_inquiry_response rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.fec = dev->ext_inquiry_fec;
|
||
memcpy(rsp.data, dev->ext_inquiry_rsp, 240);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_EXT_INQUIRY_RESPONSE, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_ext_inquiry(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_ext_inquiry_response *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->ext_inquiry_fec = cmd->fec;
|
||
memcpy(dev->ext_inquiry_rsp, cmd->data, 240);
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_EXT_INQUIRY_RESPONSE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_ssp_mode(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_simple_pairing_mode rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.mode = dev->simple_pairing_mode;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_SIMPLE_PAIRING_MODE, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_ssp_mode(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_simple_pairing_mode *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->simple_pairing_mode = cmd->mode;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_oob_data(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_oob_data rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_OOB_DATA, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_inquiry_tx_power(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_inquiry_resp_tx_power rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.level = 0;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_INQUIRY_RESP_TX_POWER, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_inquiry_tx_power(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_io_cap_reply(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_io_capability_request_reply *cmd = data;
|
||
struct bt_hci_evt_io_capability_response ev;
|
||
struct bt_hci_rsp_io_capability_request_reply rsp;
|
||
struct btdev_conn *conn;
|
||
uint8_t status;
|
||
|
||
conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
|
||
if (!conn) {
|
||
status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->io_cap = cmd->capability;
|
||
dev->auth_req = cmd->authentication;
|
||
|
||
memcpy(ev.bdaddr, dev->bdaddr, 6);
|
||
ev.capability = cmd->capability;
|
||
ev.oob_data = cmd->oob_data;
|
||
ev.authentication = cmd->authentication;
|
||
|
||
send_event(conn->link->dev, BT_HCI_EVT_IO_CAPABILITY_RESPONSE, &ev,
|
||
sizeof(ev));
|
||
|
||
if (conn->link->dev->io_cap) {
|
||
struct bt_hci_evt_user_confirm_request cfm;
|
||
|
||
memcpy(cfm.bdaddr, dev->bdaddr, 6);
|
||
cfm.passkey = 0;
|
||
|
||
send_event(conn->link->dev, BT_HCI_EVT_USER_CONFIRM_REQUEST,
|
||
&cfm, sizeof(cfm));
|
||
|
||
memcpy(cfm.bdaddr, cmd->bdaddr, 6);
|
||
send_event(dev, BT_HCI_EVT_USER_CONFIRM_REQUEST,
|
||
&cfm, sizeof(cfm));
|
||
} else {
|
||
send_event(conn->link->dev, BT_HCI_EVT_IO_CAPABILITY_REQUEST,
|
||
dev->bdaddr, 6);
|
||
}
|
||
|
||
done:
|
||
rsp.status = status;
|
||
memcpy(rsp.bdaddr, cmd->bdaddr, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY,
|
||
&rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void ssp_complete(struct btdev *btdev, const uint8_t *bdaddr,
|
||
uint8_t status, bool wait)
|
||
{
|
||
struct bt_hci_evt_simple_pairing_complete iev, aev;
|
||
struct btdev_conn *conn;
|
||
struct btdev_conn *init, *accp;
|
||
|
||
conn = queue_find(btdev->conns, match_bdaddr, bdaddr);
|
||
if (!conn)
|
||
return;
|
||
|
||
btdev->ssp_status = status;
|
||
btdev->ssp_auth_complete = true;
|
||
|
||
if (!conn->link->dev->ssp_auth_complete && wait)
|
||
return;
|
||
|
||
if (status == BT_HCI_ERR_SUCCESS &&
|
||
conn->link->dev->ssp_status != BT_HCI_ERR_SUCCESS)
|
||
status = conn->link->dev->ssp_status;
|
||
|
||
iev.status = status;
|
||
aev.status = status;
|
||
|
||
if (btdev->auth_init) {
|
||
init = conn;
|
||
accp = conn->link;
|
||
memcpy(iev.bdaddr, bdaddr, 6);
|
||
memcpy(aev.bdaddr, btdev->bdaddr, 6);
|
||
} else {
|
||
init = conn->link;
|
||
accp = conn;
|
||
memcpy(iev.bdaddr, btdev->bdaddr, 6);
|
||
memcpy(aev.bdaddr, bdaddr, 6);
|
||
}
|
||
|
||
send_event(init->dev, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &iev,
|
||
sizeof(iev));
|
||
send_event(accp->dev, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &aev,
|
||
sizeof(aev));
|
||
|
||
if (status == BT_HCI_ERR_SUCCESS) {
|
||
link_key_notify(init->dev, iev.bdaddr, LINK_KEY_DUMMY);
|
||
link_key_notify(accp->dev, aev.bdaddr, LINK_KEY_DUMMY);
|
||
}
|
||
|
||
auth_complete(init, status);
|
||
}
|
||
|
||
static int cmd_user_confirm_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_user_confirm_request_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, data, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY, &rsp,
|
||
sizeof(rsp));
|
||
ssp_complete(dev, data, BT_HCI_ERR_SUCCESS, true);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_user_confirm_negative_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_user_confirm_request_neg_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, data, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY,
|
||
&rsp, sizeof(rsp));
|
||
ssp_complete(dev, data, BT_HCI_ERR_AUTH_FAILURE, true);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_user_passkey_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_user_passkey_negative_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_io_cap_negative_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_io_capability_request_neg_reply *cmd = data;
|
||
struct bt_hci_rsp_io_capability_request_neg_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.bdaddr, cmd->bdaddr, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_IO_CAPABILITY_REQUEST_NEG_REPLY,
|
||
&rsp, sizeof(rsp));
|
||
|
||
ssp_complete(dev, cmd->bdaddr, BT_HCI_ERR_AUTH_FAILURE, false);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_encrypt_key_size(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_encrypt_key_size *cmd = data;
|
||
struct bt_hci_rsp_read_encrypt_key_size rsp;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.handle = cmd->handle;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (conn) {
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.key_size = 16;
|
||
} else {
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
rsp.key_size = 0;
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_ENCRYPT_KEY_SIZE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_data_block_size(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_data_block_size rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.max_acl_len = cpu_to_le16(dev->acl_mtu);
|
||
rsp.block_len = cpu_to_le16(dev->acl_mtu);
|
||
rsp.num_blocks = cpu_to_le16(dev->acl_max_pkt);
|
||
cmd_complete(dev, BT_HCI_CMD_READ_DATA_BLOCK_SIZE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_codecs(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_codecs *rsp;
|
||
|
||
rsp = alloca(sizeof(*rsp) + 7);
|
||
rsp->status = BT_HCI_ERR_SUCCESS;
|
||
rsp->num_codecs = 0x06;
|
||
rsp->codec[0] = 0x00;
|
||
rsp->codec[1] = 0x01;
|
||
rsp->codec[2] = 0x02;
|
||
rsp->codec[3] = 0x03;
|
||
rsp->codec[4] = 0x04;
|
||
rsp->codec[5] = 0x05;
|
||
rsp->codec[6] = 0x00;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_CODECS, rsp, sizeof(*rsp) + 7);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_get_mws_transport_config(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_get_mws_transport_config *rsp;
|
||
|
||
rsp = alloca(sizeof(*rsp));
|
||
rsp->status = BT_HCI_ERR_SUCCESS;
|
||
rsp->num_transports = 0x00;
|
||
cmd_complete(dev, BT_HCI_CMD_GET_MWS_TRANSPORT_CONFIG, rsp,
|
||
sizeof(*rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_BREDR \
|
||
CMD(BT_HCI_CMD_SETUP_SYNC_CONN, cmd_setup_sync_conn, \
|
||
cmd_setup_sync_conn_complete), \
|
||
CMD(BT_HCI_CMD_READ_EXT_INQUIRY_RESPONSE, cmd_read_ext_inquiry, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_EXT_INQUIRY_RESPONSE, cmd_write_ext_inquiry, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_SIMPLE_PAIRING_MODE, cmd_read_ssp_mode, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_SIMPLE_PAIRING_MODE, cmd_write_ssp_mode, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_OOB_DATA, cmd_read_oob_data, NULL), \
|
||
CMD(BT_HCI_CMD_READ_INQUIRY_RESP_TX_POWER, cmd_read_inquiry_tx_power, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_INQUIRY_TX_POWER, cmd_write_inquiry_tx_power, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_IO_CAPABILITY_REQUEST_REPLY, cmd_io_cap_reply, NULL), \
|
||
CMD(BT_HCI_CMD_USER_CONFIRM_REQUEST_REPLY, cmd_user_confirm_reply, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_USER_CONFIRM_REQUEST_NEG_REPLY, \
|
||
cmd_user_confirm_negative_reply, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_USER_PASSKEY_REQUEST_NEG_REPLY, cmd_user_passkey_reply, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_USER_PASSKEY_REQUEST_NEG_REPLY, \
|
||
cmd_user_passkey_negative_reply, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_IO_CAPABILITY_REQUEST_NEG_REPLY, \
|
||
cmd_io_cap_negative_reply, NULL), \
|
||
CMD(BT_HCI_CMD_READ_ENCRYPT_KEY_SIZE, cmd_read_encrypt_key_size, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_DATA_BLOCK_SIZE, cmd_read_data_block_size, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_CODECS, cmd_read_local_codecs, NULL), \
|
||
CMD(BT_HCI_CMD_GET_MWS_TRANSPORT_CONFIG, cmd_get_mws_transport_config, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_ENHANCED_SETUP_SYNC_CONN, cmd_enhanced_setup_sync_conn, \
|
||
cmd_enhanced_setup_sync_conn_complete)
|
||
|
||
static const struct btdev_cmd cmd_bredr[] = {
|
||
CMD_COMMON_ALL,
|
||
CMD_COMMON_BREDR_LE,
|
||
CMD_COMMON_BREDR_20,
|
||
CMD_BREDR,
|
||
{}
|
||
};
|
||
|
||
static void set_bredr_commands(struct btdev *btdev)
|
||
{
|
||
set_common_commands_all(btdev);
|
||
set_common_commands_bredrle(btdev);
|
||
set_common_commands_bredr20(btdev);
|
||
|
||
btdev->commands[16] |= 0x08; /* Setup Synchronous Connection */
|
||
btdev->commands[17] |= 0x01; /* Read Extended Inquiry Response */
|
||
btdev->commands[17] |= 0x02; /* Write Extended Inquiry Response */
|
||
btdev->commands[17] |= 0x20; /* Read Simple Pairing Mode */
|
||
btdev->commands[17] |= 0x40; /* Write Simple Pairing Mode */
|
||
btdev->commands[17] |= 0x80; /* Read Local OOB Data */
|
||
btdev->commands[18] |= 0x01; /* Read Inquiry Response TX Power */
|
||
btdev->commands[18] |= 0x02; /* Write Inquiry Response TX Power */
|
||
btdev->commands[18] |= 0x80; /* IO Capability Request Reply */
|
||
btdev->commands[19] |= 0x01; /* User Confirmation Request Reply */
|
||
btdev->commands[19] |= 0x02; /* User Confirmation Request N Reply */
|
||
btdev->commands[19] |= 0x04; /* User Passkey Request Reply */
|
||
btdev->commands[19] |= 0x08; /* User Passkey Request N Reply */
|
||
btdev->commands[20] |= 0x08; /* IO Capability Request N Reply */
|
||
btdev->commands[20] |= 0x10; /* Read Encryption Key Size */
|
||
btdev->commands[23] |= 0x04; /* Read Data Block Size */
|
||
btdev->commands[29] |= 0x20; /* Read Local Supported Codecs */
|
||
btdev->commands[29] |= 0x08; /* Enhanced Setup Synchronous Conn */
|
||
btdev->commands[30] |= 0x08; /* Get MWS Transport Layer Config */
|
||
btdev->cmds = cmd_bredr;
|
||
}
|
||
|
||
static const struct btdev_cmd cmd_bredr_20[] = {
|
||
CMD_COMMON_ALL,
|
||
CMD_COMMON_BREDR_LE,
|
||
CMD_COMMON_BREDR_20,
|
||
{}
|
||
};
|
||
|
||
static void set_bredr20_commands(struct btdev *btdev)
|
||
{
|
||
set_common_commands_all(btdev);
|
||
set_common_commands_bredrle(btdev);
|
||
set_common_commands_bredr20(btdev);
|
||
btdev->cmds = cmd_bredr_20;
|
||
}
|
||
|
||
static int cmd_read_le_host_supported(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_le_host_supported rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.supported = dev->le_supported;
|
||
rsp.simultaneous = dev->le_simultaneous;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LE_HOST_SUPPORTED, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_le_host_supported(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_le_host_supported *cmd = data;
|
||
uint8_t status;
|
||
|
||
dev->le_supported = cmd->supported;
|
||
dev->le_simultaneous = cmd->simultaneous;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_set_event_mask(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_event_mask *cmd = data;
|
||
uint8_t status;
|
||
|
||
memcpy(dev->le_event_mask, cmd->mask, 8);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EVENT_MASK, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_read_buffer_size(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_buffer_size rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.le_mtu = cpu_to_le16(dev->acl_mtu);
|
||
rsp.le_max_pkt = dev->acl_max_pkt;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_read_local_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_local_features rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.features, dev->le_features, 8);
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_random_address(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_random_address *cmd = data;
|
||
uint8_t status;
|
||
|
||
/* If the Host issues this command when any of advertising
|
||
* (created using legacy advertising commands), scanning, or initiating
|
||
* are enabled, the Controller shall return the error code
|
||
* Command Disallowed (0x0C).
|
||
*/
|
||
if (dev->le_scan_enable || (dev->le_adv_enable &&
|
||
queue_isempty(dev->le_ext_adv))) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
memcpy(dev->random_addr, cmd->addr, 6);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static uint16_t ext_legacy_adv_type(uint8_t type)
|
||
{
|
||
switch (type) {
|
||
case 0x00:
|
||
/* Connectable undirected - ADV_IND" */
|
||
return 0x0013;
|
||
case 0x01:
|
||
/* Connectable directed - ADV_DIRECT_IND */
|
||
return 0x0015;
|
||
case 0x02:
|
||
/* Scannable undirected - ADV_SCAN_IND */
|
||
return 0x0012;
|
||
case 0x03:
|
||
/* Non connectable undirected - ADV_NONCONN_IND */
|
||
return 0x0010;
|
||
case 0x04:
|
||
/* Scan response - SCAN_RSP */
|
||
return 0x0012;
|
||
}
|
||
|
||
return 0x0000;
|
||
}
|
||
|
||
static int cmd_set_adv_params(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_adv_parameters *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (dev->le_adv_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
dev->le_adv_type = cmd->type;
|
||
/* Use Legacy PDU if the remote is using EXT Scan */
|
||
dev->le_ext_adv_type = ext_legacy_adv_type(cmd->type);
|
||
dev->le_adv_own_addr = cmd->own_addr_type;
|
||
dev->le_adv_direct_addr_type = cmd->direct_addr_type;
|
||
memcpy(dev->le_adv_direct_addr, cmd->direct_addr, 6);
|
||
dev->le_adv_filter_policy = cmd->filter_policy;
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_ADV_PARAMETERS, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_adv_tx_power(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_adv_tx_power rsp;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.level = 0;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_ADV_TX_POWER, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_adv_data(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_adv_data *cmd = data;
|
||
uint8_t status;
|
||
|
||
dev->le_adv_data_len = cmd->len;
|
||
memcpy(dev->le_adv_data, cmd->data, 31);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_ADV_DATA, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_scan_rsp_data(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_scan_rsp_data *cmd = data;
|
||
uint8_t status;
|
||
|
||
dev->le_scan_data_len = cmd->len;
|
||
memcpy(dev->le_scan_data, cmd->data, 31);
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_SCAN_RSP_DATA, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static uint8_t get_ext_adv_type(uint8_t ext_adv_type)
|
||
{
|
||
/*
|
||
* If legacy bit is not set then just reset high duty cycle directed
|
||
* bit.
|
||
*/
|
||
if (!(ext_adv_type & 0x10))
|
||
return (ext_adv_type & 0xf7);
|
||
|
||
/*
|
||
* Connectable low duty cycle directed advertising creates a
|
||
* connectable directed advertising report type.
|
||
*/
|
||
if (ext_adv_type == 0x001d)
|
||
return 0x0015;
|
||
|
||
return ext_adv_type;
|
||
}
|
||
|
||
static const uint8_t *scan_addr(const struct btdev *btdev)
|
||
{
|
||
if (btdev->le_scan_own_addr_type == 0x01)
|
||
return btdev->random_addr;
|
||
|
||
return btdev->bdaddr;
|
||
}
|
||
|
||
static const uint8_t *adv_addr(const struct btdev *btdev)
|
||
{
|
||
if (btdev->le_adv_own_addr == 0x01)
|
||
return btdev->random_addr;
|
||
|
||
return btdev->bdaddr;
|
||
}
|
||
|
||
static void le_send_adv_report(struct btdev *btdev, const struct btdev *remote,
|
||
uint8_t type)
|
||
{
|
||
struct __packed {
|
||
uint8_t subevent;
|
||
union {
|
||
struct bt_hci_evt_le_adv_report lar;
|
||
uint8_t raw[10 + 31 + 1];
|
||
};
|
||
} meta_event;
|
||
|
||
meta_event.subevent = BT_HCI_EVT_LE_ADV_REPORT;
|
||
|
||
memset(&meta_event.lar, 0, sizeof(meta_event.lar));
|
||
meta_event.lar.num_reports = 1;
|
||
meta_event.lar.event_type = type;
|
||
meta_event.lar.addr_type = remote->le_adv_own_addr;
|
||
memcpy(meta_event.lar.addr, adv_addr(remote), 6);
|
||
|
||
/* Scan or advertising response */
|
||
if (type == 0x04) {
|
||
meta_event.lar.data_len = remote->le_scan_data_len;
|
||
memcpy(meta_event.lar.data, remote->le_scan_data,
|
||
meta_event.lar.data_len);
|
||
} else {
|
||
meta_event.lar.data_len = remote->le_adv_data_len;
|
||
memcpy(meta_event.lar.data, remote->le_adv_data,
|
||
meta_event.lar.data_len);
|
||
}
|
||
/* Not available */
|
||
meta_event.raw[10 + meta_event.lar.data_len] = 127;
|
||
send_event(btdev, BT_HCI_EVT_LE_META_EVENT, &meta_event,
|
||
1 + 10 + meta_event.lar.data_len + 1);
|
||
}
|
||
|
||
static uint8_t get_adv_report_type(uint8_t adv_type)
|
||
{
|
||
/*
|
||
* Connectable low duty cycle directed advertising creates a
|
||
* connectable directed advertising report type.
|
||
*/
|
||
if (adv_type == 0x04)
|
||
return 0x01;
|
||
|
||
return adv_type;
|
||
}
|
||
|
||
static bool adv_match(struct btdev *scan, struct btdev *adv)
|
||
{
|
||
/* Match everything if this is not directed advertising */
|
||
if (adv->le_adv_type != 0x01 && adv->le_adv_type != 0x04)
|
||
return true;
|
||
|
||
if (scan->le_scan_own_addr_type != adv->le_adv_direct_addr_type)
|
||
return false;
|
||
|
||
return !memcmp(scan_addr(scan), adv->le_adv_direct_addr, 6);
|
||
}
|
||
|
||
static void le_set_adv_enable_complete(struct btdev *btdev)
|
||
{
|
||
uint8_t report_type;
|
||
int i;
|
||
|
||
report_type = get_adv_report_type(btdev->le_adv_type);
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (!btdev_list[i] || btdev_list[i] == btdev)
|
||
continue;
|
||
|
||
if (!btdev_list[i]->le_scan_enable)
|
||
continue;
|
||
|
||
if (!adv_match(btdev_list[i], btdev))
|
||
continue;
|
||
|
||
le_send_adv_report(btdev_list[i], btdev, report_type);
|
||
|
||
if (btdev_list[i]->le_scan_type != 0x01)
|
||
continue;
|
||
|
||
/* ADV_IND & ADV_SCAN_IND generate a scan response */
|
||
if (btdev->le_adv_type == 0x00 || btdev->le_adv_type == 0x02)
|
||
le_send_adv_report(btdev_list[i], btdev, 0x04);
|
||
}
|
||
}
|
||
|
||
#define RL_ADDR_EQUAL(_rl, _type, _addr) \
|
||
(_rl->type == _type && !bacmp(&_rl->addr, (bdaddr_t *)_addr))
|
||
|
||
static const struct btdev_rl *rl_find(const struct btdev *dev, uint8_t type,
|
||
const uint8_t *addr)
|
||
{
|
||
unsigned int i;
|
||
|
||
for (i = 0; i < ARRAY_SIZE(dev->le_rl); i++) {
|
||
const struct btdev_rl *rl = &dev->le_rl[i];
|
||
|
||
if (RL_ADDR_EQUAL(rl, type, addr))
|
||
return rl;
|
||
}
|
||
|
||
return NULL;
|
||
}
|
||
|
||
static int cmd_set_adv_enable(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_adv_enable *cmd = data;
|
||
uint8_t status;
|
||
bool random_addr;
|
||
|
||
if (dev->le_adv_enable == cmd->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
dev->le_adv_enable = cmd->enable;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
if (!cmd->enable)
|
||
goto done;
|
||
|
||
random_addr = bacmp((bdaddr_t *)dev->random_addr, BDADDR_ANY);
|
||
|
||
/* If Advertising_Enable is set to 0x01, the advertising parameters'
|
||
* Own_Address_Type parameter is set to 0x01, and the random address for
|
||
* the device has not been initialized, the Controller shall return the
|
||
* error code Invalid HCI Command Parameters (0x12).
|
||
*/
|
||
if (dev->le_adv_own_addr == 0x01 && !random_addr) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
/* If Advertising_Enable is set to 0x01, the advertising parameters'
|
||
* Own_Address_Type parameter is set to 0x03, the controller's resolving
|
||
* list did not contain a matching entry, and the random address for the
|
||
* device has not been initialized, the Controller shall return the
|
||
* error code Invalid HCI Command Parameters (0x12).
|
||
*/
|
||
if (dev->le_adv_own_addr == 0x03 && !random_addr) {
|
||
if (!dev->le_rl_enable ||
|
||
!rl_find(dev, dev->le_adv_direct_addr_type,
|
||
dev->le_adv_direct_addr)) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
}
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_ADV_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
if (!status && dev->le_adv_enable)
|
||
le_set_adv_enable_complete(dev);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_scan_params(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_scan_parameters *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (dev->le_scan_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
dev->le_scan_type = cmd->type;
|
||
dev->le_scan_own_addr_type = cmd->own_addr_type;
|
||
dev->le_scan_filter_policy = cmd->filter_policy;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_scan_enable(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_scan_enable *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (dev->le_scan_enable == cmd->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* If LE_Scan_Enable is set to 0x01, the scanning parameters'
|
||
* Own_Address_Type parameter is set to 0x01 or 0x03, and the random
|
||
* address for the device has not been initialized, the Controller shall
|
||
* return the error code Invalid HCI Command Parameters (0x12).
|
||
*/
|
||
if ((dev->le_scan_own_addr_type == 0x01 ||
|
||
dev->le_scan_own_addr_type == 0x03) &&
|
||
!bacmp((bdaddr_t *)dev->random_addr, BDADDR_ANY)) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
dev->le_scan_enable = cmd->enable;
|
||
dev->le_filter_dup = cmd->filter_dup;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_SCAN_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_scan_enable_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_scan_enable *cmd = data;
|
||
int i;
|
||
|
||
if (!dev->le_scan_enable || !cmd->enable)
|
||
return 0;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
uint8_t report_type;
|
||
|
||
if (!btdev_list[i] || btdev_list[i] == dev)
|
||
continue;
|
||
|
||
if (!btdev_list[i]->le_adv_enable)
|
||
continue;
|
||
|
||
if (!adv_match(dev, btdev_list[i]))
|
||
continue;
|
||
|
||
report_type = get_adv_report_type(btdev_list[i]->le_adv_type);
|
||
le_send_adv_report(dev, btdev_list[i], report_type);
|
||
|
||
if (dev->le_scan_type != 0x01)
|
||
continue;
|
||
|
||
/* ADV_IND & ADV_SCAN_IND generate a scan response */
|
||
if (btdev_list[i]->le_adv_type == 0x00 ||
|
||
btdev_list[i]->le_adv_type == 0x02)
|
||
le_send_adv_report(dev, btdev_list[i], 0x04);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static bool adv_connectable(struct btdev *btdev)
|
||
{
|
||
if (!btdev->le_adv_enable)
|
||
return false;
|
||
|
||
return btdev->le_adv_type != 0x03;
|
||
}
|
||
|
||
static void le_meta_event(struct btdev *btdev, uint8_t event,
|
||
void *data, uint8_t len)
|
||
{
|
||
void *pkt_data;
|
||
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"meta event 0x%02x", event);
|
||
|
||
pkt_data = alloca(1 + len);
|
||
if (!pkt_data)
|
||
return;
|
||
|
||
((uint8_t *) pkt_data)[0] = event;
|
||
|
||
if (len > 0)
|
||
memcpy(pkt_data + 1, data, len);
|
||
|
||
send_event(btdev, BT_HCI_EVT_LE_META_EVENT, pkt_data, 1 + len);
|
||
}
|
||
|
||
static void le_conn_complete(struct btdev *btdev,
|
||
const struct bt_hci_cmd_le_create_conn *lecc,
|
||
uint8_t status)
|
||
{
|
||
struct bt_hci_evt_le_conn_complete cc;
|
||
|
||
memset(&cc, 0, sizeof(cc));
|
||
|
||
if (!status) {
|
||
struct btdev_conn *conn;
|
||
|
||
conn = conn_add_acl(btdev, lecc->peer_addr,
|
||
lecc->peer_addr_type);
|
||
if (!conn)
|
||
return;
|
||
|
||
btdev->le_adv_enable = 0;
|
||
conn->link->dev->le_adv_enable = 0;
|
||
|
||
cc.status = status;
|
||
cc.peer_addr_type = btdev->le_scan_own_addr_type;
|
||
if (cc.peer_addr_type == 0x01)
|
||
memcpy(cc.peer_addr, btdev->random_addr, 6);
|
||
else
|
||
memcpy(cc.peer_addr, btdev->bdaddr, 6);
|
||
|
||
cc.role = 0x01;
|
||
cc.handle = cpu_to_le16(conn->handle);
|
||
cc.interval = lecc->max_interval;
|
||
cc.latency = lecc->latency;
|
||
cc.supv_timeout = lecc->supv_timeout;
|
||
le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_COMPLETE,
|
||
&cc, sizeof(cc));
|
||
}
|
||
|
||
cc.status = status;
|
||
cc.peer_addr_type = lecc->peer_addr_type;
|
||
memcpy(cc.peer_addr, lecc->peer_addr, 6);
|
||
cc.role = 0x00;
|
||
|
||
le_meta_event(btdev, BT_HCI_EVT_LE_CONN_COMPLETE, &cc, sizeof(cc));
|
||
}
|
||
|
||
static int cmd_le_create_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CREATE_CONN);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_create_conn_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_create_conn *cmd = data;
|
||
struct btdev *remote;
|
||
|
||
dev->le_scan_own_addr_type = cmd->own_addr_type;
|
||
|
||
remote = find_btdev_by_bdaddr_type(cmd->peer_addr, cmd->peer_addr_type);
|
||
if (remote && adv_connectable(remote) && adv_match(dev, remote) &&
|
||
remote->le_adv_own_addr == cmd->peer_addr_type)
|
||
le_conn_complete(dev, cmd, 0);
|
||
else
|
||
le_conn_complete(dev, cmd, BT_HCI_ERR_CONN_FAILED_TO_ESTABLISH);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_create_conn_cancel(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CREATE_CONN_CANCEL, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_al_size(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_accept_list_size rsp;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.size = dev->le_al_len;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_ACCEPT_LIST_SIZE, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static bool al_can_change(struct btdev *dev)
|
||
{
|
||
/* filter policy uses the Accept List and advertising is enable. */
|
||
if (dev->le_adv_enable && dev->le_adv_filter_policy)
|
||
return false;
|
||
|
||
/* scan filter policy uses the Accept List and scanning is enabled */
|
||
if (dev->le_scan_enable) {
|
||
switch (dev->le_scan_filter_policy) {
|
||
case 0x00:
|
||
return true;
|
||
case 0x01:
|
||
return false;
|
||
case 0x02:
|
||
return true;
|
||
case 0x03:
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
static int cmd_al_clear(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
uint8_t status;
|
||
|
||
/* This command shall not be used when:
|
||
* • any advertising filter policy uses the Accept List and
|
||
* advertising is enabled,
|
||
* • the scanning filter policy uses the Accept List and scanning is
|
||
* enabled, or
|
||
* • the initiator filter policy uses the Accept List and an
|
||
* HCI_LE_Create_Connection or HCI_LE_Extended_Create_Connection
|
||
* command is outstanding.
|
||
*/
|
||
if (!al_can_change(dev))
|
||
return -EPERM;
|
||
|
||
al_clear(dev);
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_ACCEPT_LIST, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define AL_ADDR_EQUAL(_al, _type, _addr) \
|
||
(_al->type == _type && !bacmp(&_al->addr, (bdaddr_t *)_addr))
|
||
|
||
static void al_add(struct btdev_al *al, uint8_t type, bdaddr_t *addr)
|
||
{
|
||
al->type = type;
|
||
bacpy(&al->addr, addr);
|
||
}
|
||
|
||
static int cmd_add_al(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_add_to_accept_list *cmd = data;
|
||
uint8_t status;
|
||
bool exists = false;
|
||
int i, pos = -1;
|
||
|
||
/* This command shall not be used when:
|
||
* • any advertising filter policy uses the Accept List and
|
||
* advertising is enabled,
|
||
* • the scanning filter policy uses the Accept List and scanning is
|
||
* enabled, or
|
||
* • the initiator filter policy uses the Accept List and an
|
||
* HCI_LE_Create_Connection or HCI_LE_Extended_Create_Connection
|
||
* command is outstanding.
|
||
*/
|
||
if (!al_can_change(dev)) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* Valid range for address type is 0x00 to 0x01 */
|
||
if (cmd->addr_type > 0x01) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
for (i = 0; i < dev->le_al_len; i++) {
|
||
struct btdev_al *al = &dev->le_al[i];
|
||
|
||
if (AL_ADDR_EQUAL(al, cmd->addr_type, &cmd->addr)) {
|
||
exists = true;
|
||
break;
|
||
} else if (pos < 0 && al->type == 0xff)
|
||
pos = i;
|
||
}
|
||
|
||
/* If the device is already in the Filter Accept List, the Controller
|
||
* should not add the device to the Filter Accept List again and should
|
||
* return success.
|
||
*/
|
||
if (exists) {
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
goto done;
|
||
}
|
||
|
||
if (pos < 0) {
|
||
status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
al_add(&dev->le_al[pos], cmd->addr_type, (bdaddr_t *)&cmd->addr);
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remove_al(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_remove_from_accept_list *cmd = data;
|
||
uint8_t status;
|
||
int i;
|
||
char addr[18];
|
||
|
||
/* This command shall not be used when:
|
||
* • any advertising filter policy uses the Accept List and
|
||
* advertising is enabled,
|
||
* • the scanning filter policy uses the Accept List and scanning is
|
||
* enabled, or
|
||
* • the initiator filter policy uses the Accept List and an
|
||
* HCI_LE_Create_Connection or HCI_LE_Extended_Create_Connection
|
||
* command is outstanding.
|
||
*/
|
||
if (!al_can_change(dev)) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* Valid range for address type is 0x00 to 0x01 */
|
||
if (cmd->addr_type > 0x01) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
for (i = 0; i < dev->le_al_len; i++) {
|
||
struct btdev_al *al = &dev->le_al[i];
|
||
|
||
ba2str(&al->addr, addr);
|
||
|
||
util_debug(dev->debug_callback, dev->debug_data,
|
||
"type 0x%02x addr %s", dev->le_al[i].type,
|
||
addr);
|
||
|
||
if (AL_ADDR_EQUAL(al, cmd->addr_type, &cmd->addr)) {
|
||
al_reset(al);
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (i == dev->le_al_len) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_FROM_ACCEPT_LIST,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_add_rl(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_add_to_resolv_list *cmd = data;
|
||
uint8_t status;
|
||
bool exists = false;
|
||
int i, pos = -1;
|
||
|
||
/* This command shall not be used when address resolution is enabled in
|
||
* the Controller and:
|
||
* • Advertising (other than periodic advertising) is enabled,
|
||
* • Scanning is enabled, or
|
||
* • an HCI_LE_Create_Connection, HCI_LE_Extended_Create_Connection,
|
||
* or HCI_LE_Periodic_Advertising_Create_Sync command is outstanding.
|
||
*/
|
||
if (dev->le_adv_enable || dev->le_scan_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* Valid range for address type is 0x00 to 0x01 */
|
||
if (cmd->addr_type > 0x01) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
for (i = 0; i < dev->le_rl_len; i++) {
|
||
struct btdev_rl *rl = &dev->le_rl[i];
|
||
|
||
if (RL_ADDR_EQUAL(rl, cmd->addr_type, &cmd->addr)) {
|
||
exists = true;
|
||
break;
|
||
} else if (pos < 0 && rl->type == 0xff)
|
||
pos = i;
|
||
}
|
||
|
||
/* If an entry already exists in the resolving list with the same four
|
||
* parameter values, the Controller shall either reject the command or
|
||
* not add the device to the resolving list again and return success.
|
||
*/
|
||
if (exists) {
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
goto done;
|
||
}
|
||
|
||
if (pos < 0) {
|
||
status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
dev->le_rl[pos].type = cmd->addr_type;
|
||
bacpy(&dev->le_rl[pos].addr, (bdaddr_t *)&cmd->addr);
|
||
memcpy(dev->le_rl[pos].peer_irk, cmd->peer_irk, 16);
|
||
memcpy(dev->le_rl[pos].local_irk, cmd->local_irk, 16);
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remove_rl(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_remove_from_resolv_list *cmd = data;
|
||
uint8_t status;
|
||
int i;
|
||
|
||
/* This command shall not be used when address resolution is enabled in
|
||
* the Controller and:
|
||
* • Advertising (other than periodic advertising) is enabled,
|
||
* • Scanning is enabled, or
|
||
* • an HCI_LE_Create_Connection, HCI_LE_Extended_Create_Connection,
|
||
* or HCI_LE_Periodic_Advertising_Create_Sync command is outstanding.
|
||
*/
|
||
if (dev->le_adv_enable || dev->le_scan_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* Valid range for address type is 0x00 to 0x01 */
|
||
if (cmd->addr_type > 0x01) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
for (i = 0; i < dev->le_rl_len; i++) {
|
||
struct btdev_rl *rl = &dev->le_rl[i];
|
||
|
||
if (RL_ADDR_EQUAL(rl, cmd->addr_type, &cmd->addr)) {
|
||
rl_reset(rl);
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (i == dev->le_rl_len) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_clear_rl(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
uint8_t status;
|
||
|
||
/* This command shall not be used when address resolution is enabled in
|
||
* the Controller and:
|
||
* • Advertising (other than periodic advertising) is enabled,
|
||
* • Scanning is enabled, or
|
||
* • an HCI_LE_Create_Connection, HCI_LE_Extended_Create_Connection,
|
||
* or HCI_LE_Periodic_Advertising_Create_Sync command is outstanding.
|
||
*/
|
||
if (dev->le_adv_enable || dev->le_scan_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
rl_clear(dev);
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_RESOLV_LIST,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_rl_size(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_resolv_list_size rsp;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.size = dev->le_rl_len;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE,
|
||
&rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_peer_rl_addr(struct btdev *dev, const void *data,
|
||
uint8_t size)
|
||
{
|
||
const struct bt_hci_cmd_le_read_peer_resolv_addr *cmd = data;
|
||
struct bt_hci_rsp_le_read_peer_resolv_addr rsp;
|
||
|
||
/* Valid range for address type is 0x00 to 0x01 */
|
||
if (cmd->addr_type > 0x01) {
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
memset(rsp.addr, 0, 6);
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR,
|
||
&rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_rl_addr(struct btdev *dev, const void *data,
|
||
uint8_t size)
|
||
{
|
||
const struct bt_hci_cmd_le_read_local_resolv_addr *cmd = data;
|
||
struct bt_hci_rsp_le_read_local_resolv_addr rsp;
|
||
|
||
/* Valid range for address type is 0x00 to 0x01 */
|
||
if (cmd->addr_type > 0x01) {
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
memset(rsp.addr, 0, 6);
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR,
|
||
&rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_rl_enable(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_resolv_enable *cmd = data;
|
||
uint8_t status;
|
||
|
||
/* This command shall not be used when address resolution is enabled in
|
||
* the Controller and:
|
||
* • Advertising (other than periodic advertising) is enabled,
|
||
* • Scanning is enabled, or
|
||
* • an HCI_LE_Create_Connection, HCI_LE_Extended_Create_Connection,
|
||
* or HCI_LE_Periodic_Advertising_Create_Sync command is outstanding.
|
||
*/
|
||
if (dev->le_adv_enable || dev->le_scan_enable)
|
||
return -EPERM;
|
||
|
||
/* Valid range for address resolution enable is 0x00 to 0x01 */
|
||
if (cmd->enable > 0x01)
|
||
return -EINVAL;
|
||
|
||
dev->le_rl_enable = cmd->enable;
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_rl_timeout(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_resolv_timeout *cmd = data;
|
||
uint16_t timeout;
|
||
uint8_t status;
|
||
|
||
timeout = le16_to_cpu(cmd->timeout);
|
||
|
||
/* Valid range for RPA timeout is 0x0001 to 0xa1b8 */
|
||
if (timeout < 0x0001 || timeout > 0xa1b8)
|
||
return -EINVAL;
|
||
|
||
dev->le_rl_timeout = timeout;
|
||
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_conn_update(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CONN_UPDATE);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void le_conn_update(struct btdev *btdev, uint16_t handle,
|
||
uint16_t min_interval, uint16_t max_interval,
|
||
uint16_t latency, uint16_t supv_timeout,
|
||
uint16_t min_length, uint16_t max_length)
|
||
{
|
||
struct bt_hci_evt_le_conn_update_complete ev;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
ev.handle = cpu_to_le16(handle);
|
||
ev.interval = cpu_to_le16(min_interval);
|
||
ev.latency = cpu_to_le16(latency);
|
||
ev.supv_timeout = cpu_to_le16(supv_timeout);
|
||
|
||
conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
|
||
if (conn)
|
||
ev.status = BT_HCI_ERR_SUCCESS;
|
||
else
|
||
ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
|
||
le_meta_event(btdev, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
|
||
sizeof(ev));
|
||
|
||
if (conn)
|
||
le_meta_event(conn->link->dev,
|
||
BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE,
|
||
&ev, sizeof(ev));
|
||
}
|
||
|
||
static void le_conn_param_req(struct btdev *btdev, uint16_t handle,
|
||
uint16_t min_interval, uint16_t max_interval,
|
||
uint16_t latency, uint16_t supv_timeout,
|
||
uint16_t min_length, uint16_t max_length)
|
||
{
|
||
struct bt_hci_evt_le_conn_param_request ev;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
|
||
if (!conn)
|
||
return;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
ev.handle = cpu_to_le16(handle);
|
||
ev.min_interval = cpu_to_le16(min_interval);
|
||
ev.max_interval = cpu_to_le16(max_interval);
|
||
ev.latency = cpu_to_le16(latency);
|
||
ev.supv_timeout = cpu_to_le16(supv_timeout);
|
||
|
||
le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_PARAM_REQUEST, &ev,
|
||
sizeof(ev));
|
||
}
|
||
|
||
static int cmd_conn_update_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_conn_update *cmd = data;
|
||
|
||
if (dev->le_features[0] & 0x02)
|
||
le_conn_param_req(dev, le16_to_cpu(cmd->handle),
|
||
le16_to_cpu(cmd->min_interval),
|
||
le16_to_cpu(cmd->max_interval),
|
||
le16_to_cpu(cmd->latency),
|
||
le16_to_cpu(cmd->supv_timeout),
|
||
le16_to_cpu(cmd->min_length),
|
||
le16_to_cpu(cmd->max_length));
|
||
else
|
||
le_conn_update(dev, le16_to_cpu(cmd->handle),
|
||
le16_to_cpu(cmd->min_interval),
|
||
le16_to_cpu(cmd->max_interval),
|
||
le16_to_cpu(cmd->latency),
|
||
le16_to_cpu(cmd->supv_timeout),
|
||
le16_to_cpu(cmd->min_length),
|
||
le16_to_cpu(cmd->max_length));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_read_remote_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_remote_features *cmd = data;
|
||
struct bt_hci_evt_le_remote_features_complete ev;
|
||
struct btdev_conn *conn;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn)
|
||
status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
|
||
cmd_status(dev, status, BT_HCI_CMD_LE_READ_REMOTE_FEATURES);
|
||
|
||
if (status)
|
||
return 0;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
ev.status = BT_HCI_ERR_SUCCESS;
|
||
ev.handle = cpu_to_le16(conn->handle);
|
||
memcpy(ev.features, conn->link->dev->le_features, 8);
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_REMOTE_FEATURES_COMPLETE, &ev,
|
||
sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_encrypt(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_encrypt *cmd = data;
|
||
struct bt_hci_rsp_le_encrypt rsp;
|
||
|
||
if (!bt_crypto_e(dev->crypto, cmd->key, cmd->plaintext, rsp.data)) {
|
||
cmd_status(dev, BT_HCI_ERR_COMMAND_DISALLOWED,
|
||
BT_HCI_CMD_LE_ENCRYPT);
|
||
return 0;
|
||
}
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_ENCRYPT, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_rand(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_rand rsp;
|
||
|
||
if (!bt_crypto_random_bytes(dev->crypto,
|
||
(uint8_t *)&rsp.number, 8)) {
|
||
cmd_status(dev, BT_HCI_ERR_COMMAND_DISALLOWED,
|
||
BT_HCI_CMD_LE_RAND);
|
||
return 0;
|
||
}
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_RAND, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_start_encrypt(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_start_encrypt *cmd = data;
|
||
struct bt_hci_evt_le_long_term_key_request ev;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn) {
|
||
cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
|
||
BT_HCI_CMD_LE_START_ENCRYPT);
|
||
return 0;
|
||
}
|
||
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_START_ENCRYPT);
|
||
|
||
memcpy(dev->le_ltk, cmd->ltk, 16);
|
||
|
||
ev.handle = cpu_to_le16(conn->handle);
|
||
ev.ediv = cmd->ediv;
|
||
ev.rand = cmd->rand;
|
||
|
||
le_meta_event(conn->link->dev, BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST, &ev,
|
||
sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_ltk_reply(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_ltk_req_reply *cmd = data;
|
||
struct bt_hci_rsp_le_ltk_req_reply rp;
|
||
struct btdev_conn *conn;
|
||
uint8_t mode, status;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn) {
|
||
rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_REPLY, &rp, sizeof(rp));
|
||
return 0;
|
||
}
|
||
|
||
memcpy(dev->le_ltk, cmd->ltk, 16);
|
||
|
||
memset(&rp, 0, sizeof(rp));
|
||
rp.handle = cpu_to_le16(conn->handle);
|
||
|
||
rp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_REPLY, &rp, sizeof(rp));
|
||
|
||
if (memcmp(dev->le_ltk, conn->link->dev->le_ltk, 16)) {
|
||
status = BT_HCI_ERR_AUTH_FAILURE;
|
||
mode = 0x00;
|
||
} else {
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
mode = 0x01;
|
||
}
|
||
|
||
encrypt_change(conn, mode, status);
|
||
encrypt_change(conn->link, mode, status);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_ltk_neg_reply(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_ltk_req_neg_reply *cmd = data;
|
||
struct bt_hci_rsp_le_ltk_req_neg_reply rp;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn) {
|
||
rp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, &rp,
|
||
sizeof(rp));
|
||
return 0;
|
||
}
|
||
|
||
memset(&rp, 0, sizeof(rp));
|
||
rp.handle = cpu_to_le16(conn->handle);
|
||
rp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, &rp, sizeof(rp));
|
||
|
||
encrypt_change(conn->link, 0x00, BT_HCI_ERR_PIN_OR_KEY_MISSING);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_le_read_supported_states(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_supported_states rsp;
|
||
|
||
memset(&rsp, 0, sizeof(0));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
memcpy(rsp.states, dev->le_states, 8);
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_SUPPORTED_STATES, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_rx_test(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_tx_test(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_test_end(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_conn_param_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CONN_PARAM_REQ_REPLY, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_conn_param_reply_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_conn_param_req_reply *cmd = data;
|
||
|
||
le_conn_update(dev, le16_to_cpu(cmd->handle),
|
||
le16_to_cpu(cmd->min_interval),
|
||
le16_to_cpu(cmd->max_interval),
|
||
le16_to_cpu(cmd->latency),
|
||
le16_to_cpu(cmd->supv_timeout),
|
||
le16_to_cpu(cmd->min_length),
|
||
le16_to_cpu(cmd->max_length));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_conn_param_neg_reply(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_conn_param_req_neg_reply *cmd = data;
|
||
struct bt_hci_rsp_le_conn_param_req_neg_reply rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.handle = cmd->handle;
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CONN_PARAM_REQ_NEG_REPLY, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_conn_param_neg_reply_complete(struct btdev *dev,
|
||
const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_conn_param_req_neg_reply *cmd = data;
|
||
struct btdev_conn *conn;
|
||
struct bt_hci_evt_le_conn_update_complete ev;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn)
|
||
return 0;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
ev.handle = cpu_to_le16(cmd->handle);
|
||
ev.status = cpu_to_le16(cmd->reason);
|
||
|
||
le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
|
||
sizeof(ev));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_pk256(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_gen_dhkey(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
#define CMD_LE \
|
||
CMD(BT_HCI_CMD_READ_LE_HOST_SUPPORTED, cmd_read_le_host_supported, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_LE_HOST_SUPPORTED, cmd_write_le_host_supported, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EVENT_MASK, cmd_le_set_event_mask, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_BUFFER_SIZE, cmd_le_read_buffer_size, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_LOCAL_FEATURES, cmd_le_read_local_features, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_RANDOM_ADDRESS, cmd_set_random_address, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_ADV_PARAMETERS, cmd_set_adv_params, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_ADV_TX_POWER, cmd_read_adv_tx_power, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_ADV_DATA, cmd_set_adv_data, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_SCAN_RSP_DATA, cmd_set_scan_rsp_data, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_ADV_ENABLE, cmd_set_adv_enable, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_SCAN_PARAMETERS, cmd_set_scan_params, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_SCAN_ENABLE, cmd_set_scan_enable, \
|
||
cmd_set_scan_enable_complete), \
|
||
CMD(BT_HCI_CMD_LE_CREATE_CONN, cmd_le_create_conn, \
|
||
cmd_le_create_conn_complete), \
|
||
CMD(BT_HCI_CMD_LE_CREATE_CONN_CANCEL, cmd_le_create_conn_cancel, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_ACCEPT_LIST_SIZE, cmd_read_al_size, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CLEAR_ACCEPT_LIST, cmd_al_clear, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ADD_TO_ACCEPT_LIST, cmd_add_al, NULL), \
|
||
CMD(BT_HCI_CMD_LE_REMOVE_FROM_ACCEPT_LIST, cmd_remove_al, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CONN_UPDATE, cmd_conn_update, \
|
||
cmd_conn_update_complete), \
|
||
CMD(BT_HCI_CMD_LE_READ_REMOTE_FEATURES, cmd_le_read_remote_features, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_ENCRYPT, cmd_encrypt, NULL), \
|
||
CMD(BT_HCI_CMD_LE_RAND, cmd_rand, NULL), \
|
||
CMD(BT_HCI_CMD_LE_START_ENCRYPT, cmd_start_encrypt, NULL), \
|
||
CMD(BT_HCI_CMD_LE_LTK_REQ_REPLY, cmd_ltk_reply, NULL), \
|
||
CMD(BT_HCI_CMD_LE_LTK_REQ_NEG_REPLY, cmd_ltk_neg_reply, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_SUPPORTED_STATES, cmd_le_read_supported_states, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_RECEIVER_TEST, cmd_rx_test, NULL), \
|
||
CMD(BT_HCI_CMD_LE_TRANSMITTER_TEST, cmd_tx_test, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ISO_TEST_END, cmd_test_end, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CONN_PARAM_REQ_REPLY, cmd_conn_param_reply, \
|
||
cmd_conn_param_reply_complete), \
|
||
CMD(BT_HCI_CMD_LE_CONN_PARAM_REQ_NEG_REPLY, cmd_conn_param_neg_reply, \
|
||
cmd_conn_param_neg_reply_complete), \
|
||
CMD(BT_HCI_CMD_LE_READ_LOCAL_PK256, cmd_read_local_pk256, NULL), \
|
||
CMD(BT_HCI_CMD_LE_GENERATE_DHKEY, cmd_gen_dhkey, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST, cmd_add_rl, NULL), \
|
||
CMD(BT_HCI_CMD_LE_REMOVE_FROM_RESOLV_LIST, cmd_remove_rl, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CLEAR_RESOLV_LIST, cmd_clear_rl, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_RESOLV_LIST_SIZE, cmd_read_rl_size, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_PEER_RESOLV_ADDR, cmd_read_peer_rl_addr, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_LOCAL_RESOLV_ADDR, cmd_read_local_rl_addr, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_RESOLV_ENABLE, cmd_set_rl_enable, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT, cmd_set_rl_timeout, NULL)
|
||
|
||
static int cmd_set_default_phy(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_default_phy *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (cmd->all_phys > 0x03 || (!(cmd->all_phys & 0x01) &&
|
||
(!cmd->tx_phys || cmd->tx_phys > 0x07)) ||
|
||
(!(cmd->all_phys & 0x02) &&
|
||
(!cmd->rx_phys || cmd->rx_phys > 0x07)))
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
else
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_DEFAULT_PHY, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static const uint8_t *ext_adv_gen_rpa(const struct btdev *dev,
|
||
struct le_ext_adv *adv)
|
||
{
|
||
const struct btdev_rl *rl;
|
||
|
||
if (adv->rpa)
|
||
return adv->random_addr;
|
||
|
||
/* Lookup for Local IRK in the resolving list */
|
||
rl = rl_find(dev, adv->direct_addr_type, adv->direct_addr);
|
||
if (rl) {
|
||
uint8_t rpa[6];
|
||
|
||
bt_crypto_random_bytes(dev->crypto, rpa + 3, 3);
|
||
rpa[5] &= 0x3f; /* Clear two most significant bits */
|
||
rpa[5] |= 0x40; /* Set second most significant bit */
|
||
bt_crypto_ah(dev->crypto, rl->peer_irk, rpa + 3, rpa);
|
||
|
||
memcpy(adv->random_addr, rpa, sizeof(rpa));
|
||
adv->rpa = true;
|
||
}
|
||
|
||
return adv->random_addr;
|
||
}
|
||
|
||
static const uint8_t *ext_adv_addr(const struct btdev *btdev,
|
||
struct le_ext_adv *ext_adv)
|
||
{
|
||
if (ext_adv->own_addr_type == 0x01)
|
||
return ext_adv->random_addr;
|
||
|
||
if (ext_adv->own_addr_type == 0x03)
|
||
return ext_adv_gen_rpa(btdev, ext_adv);
|
||
|
||
return btdev->bdaddr;
|
||
}
|
||
|
||
static bool ext_adv_match_addr(const struct btdev *btdev,
|
||
struct le_ext_adv *ext_adv)
|
||
{
|
||
/* Match everything if this is not directed advertising */
|
||
if (!(ext_adv->type & 0x04))
|
||
return true;
|
||
|
||
if (btdev->le_scan_own_addr_type != ext_adv->direct_addr_type)
|
||
return false;
|
||
|
||
return !memcmp(scan_addr(btdev), ext_adv->direct_addr, 6);
|
||
}
|
||
|
||
static bool match_ext_adv_handle(const void *data, const void *match_data)
|
||
{
|
||
const struct le_ext_adv *ext_adv = data;
|
||
uint8_t handle = PTR_TO_UINT(match_data);
|
||
|
||
return ext_adv->handle == handle;
|
||
}
|
||
|
||
static void ext_adv_disable(void *data, void *user_data)
|
||
{
|
||
struct le_ext_adv *ext_adv = data;
|
||
uint8_t handle = PTR_TO_UINT(user_data);
|
||
|
||
if (handle && ext_adv->handle != handle)
|
||
return;
|
||
|
||
if (ext_adv->id) {
|
||
timeout_remove(ext_adv->id);
|
||
ext_adv->id = 0;
|
||
}
|
||
|
||
ext_adv->enable = 0x00;
|
||
}
|
||
|
||
static bool ext_adv_is_connectable(struct le_ext_adv *ext_adv)
|
||
{
|
||
if (!ext_adv->enable)
|
||
return false;
|
||
|
||
return ext_adv->type & 0x01;
|
||
}
|
||
|
||
static struct le_ext_adv *le_ext_adv_new(struct btdev *btdev, uint8_t handle)
|
||
{
|
||
struct le_ext_adv *ext_adv;
|
||
|
||
ext_adv = new0(struct le_ext_adv, 1);
|
||
ext_adv->dev = btdev;
|
||
ext_adv->handle = handle;
|
||
|
||
/* Add to queue */
|
||
if (!queue_push_tail(btdev->le_ext_adv, ext_adv)) {
|
||
free(ext_adv);
|
||
return NULL;
|
||
}
|
||
|
||
return ext_adv;
|
||
}
|
||
|
||
static int cmd_set_adv_rand_addr(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_adv_set_rand_addr *cmd = data;
|
||
struct le_ext_adv *ext_adv;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
/* Check if Ext Adv is already existed */
|
||
ext_adv = queue_find(dev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(cmd->handle));
|
||
if (!ext_adv) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_ADV_SET_RAND_ADDR, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
if (ext_adv_is_connectable(ext_adv)) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_ADV_SET_RAND_ADDR, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
memcpy(ext_adv->random_addr, cmd->bdaddr, 6);
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_ADV_SET_RAND_ADDR, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_ext_adv_params(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_adv_params *cmd = data;
|
||
struct bt_hci_rsp_le_set_ext_adv_params rsp;
|
||
struct le_ext_adv *ext_adv;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
/* Check if Ext Adv is already existed */
|
||
ext_adv = queue_find(dev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(cmd->handle));
|
||
if (!ext_adv) {
|
||
/* No more than maximum number */
|
||
if (queue_length(dev->le_ext_adv) >= MAX_EXT_ADV_SETS) {
|
||
rsp.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_PARAMS,
|
||
&rsp, sizeof(rsp));
|
||
return 0;
|
||
}
|
||
|
||
/* Create new set */
|
||
ext_adv = le_ext_adv_new(dev, cmd->handle);
|
||
if (!ext_adv) {
|
||
rsp.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_PARAMS,
|
||
&rsp, sizeof(rsp));
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
if (ext_adv->enable) {
|
||
rsp.status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_PARAMS, &rsp,
|
||
sizeof(rsp));
|
||
return 0;
|
||
}
|
||
|
||
ext_adv->type = le16_to_cpu(cmd->evt_properties);
|
||
ext_adv->own_addr_type = cmd->own_addr_type;
|
||
ext_adv->direct_addr_type = cmd->peer_addr_type;
|
||
memcpy(ext_adv->direct_addr, cmd->peer_addr, 6);
|
||
ext_adv->filter_policy = cmd->filter_policy;
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.tx_power = 0;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_PARAMS, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_ext_adv_data(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_adv_data *cmd = data;
|
||
struct le_ext_adv *ext_adv;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
ext_adv = queue_find(dev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(cmd->handle));
|
||
if (!ext_adv) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_DATA, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
ext_adv->adv_data_len = cmd->data_len;
|
||
memcpy(ext_adv->adv_data, cmd->data, cmd->data_len);
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_DATA, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_ext_scan_rsp_data(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_scan_rsp_data *cmd = data;
|
||
struct le_ext_adv *ext_adv;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
ext_adv = queue_find(dev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(cmd->handle));
|
||
if (!ext_adv) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_DATA, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
ext_adv->scan_data_len = cmd->data_len;
|
||
memcpy(ext_adv->scan_data, cmd->data, cmd->data_len);
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_SCAN_RSP_DATA, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static uint8_t ext_adv_addr_type(struct le_ext_adv *adv)
|
||
{
|
||
/* Converts the address type on advertising params to advertising
|
||
* report.
|
||
*/
|
||
switch (adv->own_addr_type) {
|
||
/* LL RPAs shall be advertised as random type or they need to be
|
||
* resolved depending on the filter policy.
|
||
*/
|
||
case 0x02:
|
||
case 0x03:
|
||
return 0x01;
|
||
}
|
||
|
||
return adv->own_addr_type;
|
||
}
|
||
|
||
static void send_ext_adv(struct btdev *btdev, const struct btdev *remote,
|
||
struct le_ext_adv *ext_adv,
|
||
uint16_t type, bool is_scan_rsp)
|
||
{
|
||
|
||
struct __packed {
|
||
uint8_t num_reports;
|
||
union {
|
||
struct bt_hci_le_ext_adv_report lear;
|
||
uint8_t raw[24 + 31];
|
||
};
|
||
} meta_event;
|
||
|
||
memset(&meta_event.lear, 0, sizeof(meta_event.lear));
|
||
meta_event.num_reports = 1;
|
||
meta_event.lear.event_type = cpu_to_le16(type);
|
||
meta_event.lear.addr_type = ext_adv_addr_type(ext_adv);
|
||
memcpy(meta_event.lear.addr, ext_adv_addr(remote, ext_adv), 6);
|
||
meta_event.lear.rssi = 127;
|
||
meta_event.lear.tx_power = 127;
|
||
/* Right now we dont care about phy in adv report */
|
||
meta_event.lear.primary_phy = 0x01;
|
||
meta_event.lear.secondary_phy = 0x01;
|
||
|
||
/* Scan or advertising response */
|
||
if (is_scan_rsp) {
|
||
meta_event.lear.data_len = ext_adv->scan_data_len;
|
||
memcpy(meta_event.lear.data, ext_adv->scan_data,
|
||
meta_event.lear.data_len);
|
||
} else {
|
||
meta_event.lear.data_len = ext_adv->adv_data_len;
|
||
memcpy(meta_event.lear.data, ext_adv->adv_data,
|
||
meta_event.lear.data_len);
|
||
}
|
||
|
||
le_meta_event(btdev, BT_HCI_EVT_LE_EXT_ADV_REPORT, &meta_event,
|
||
1 + 24 + meta_event.lear.data_len);
|
||
}
|
||
|
||
static void le_set_ext_adv_enable_complete(struct btdev *btdev,
|
||
struct le_ext_adv *ext_adv)
|
||
{
|
||
uint16_t report_type;
|
||
int i;
|
||
|
||
report_type = get_ext_adv_type(ext_adv->type);
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (!btdev_list[i] || btdev_list[i] == btdev)
|
||
continue;
|
||
|
||
if (!btdev_list[i]->le_scan_enable)
|
||
continue;
|
||
|
||
if (!ext_adv_match_addr(btdev_list[i], ext_adv))
|
||
continue;
|
||
|
||
send_ext_adv(btdev_list[i], btdev, ext_adv, report_type, false);
|
||
|
||
if (btdev_list[i]->le_scan_type != 0x01)
|
||
continue;
|
||
|
||
/* if scannable bit is set the send scan response */
|
||
if (ext_adv->type & 0x02) {
|
||
if (ext_adv->type == 0x13)
|
||
report_type = 0x1b;
|
||
else if (ext_adv->type == 0x12)
|
||
report_type = 0x1a;
|
||
else if (!(ext_adv->type & 0x10))
|
||
report_type &= 0x08;
|
||
else
|
||
continue;
|
||
|
||
send_ext_adv(btdev_list[i], btdev, ext_adv,
|
||
report_type, true);
|
||
}
|
||
}
|
||
}
|
||
static void adv_set_terminate(struct btdev *dev, uint8_t status, uint8_t handle,
|
||
uint16_t conn_handle, uint8_t num_evts)
|
||
{
|
||
struct bt_hci_evt_le_adv_set_term ev;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
ev.status = status;
|
||
ev.handle = handle;
|
||
ev.conn_handle = cpu_to_le16(conn_handle);
|
||
ev.num_evts = num_evts;
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_ADV_SET_TERM, &ev, sizeof(ev));
|
||
}
|
||
|
||
static bool ext_adv_timeout(void *user_data)
|
||
{
|
||
struct le_ext_adv *adv = user_data;
|
||
|
||
adv->id = 0;
|
||
adv_set_terminate(adv->dev, BT_HCI_ERR_ADV_TIMEOUT, adv->handle,
|
||
0x0000, 0x00);
|
||
le_ext_adv_free(adv);
|
||
|
||
return false;
|
||
}
|
||
|
||
static int cmd_set_ext_adv_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_adv_enable *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
int i;
|
||
|
||
/* Num of set is zero */
|
||
if (!cmd->num_of_sets) {
|
||
if (cmd->enable) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto exit_complete;
|
||
}
|
||
|
||
/* Disable all advertising sets */
|
||
queue_foreach(dev->le_ext_adv, ext_adv_disable, NULL);
|
||
|
||
dev->le_adv_enable = 0x00;
|
||
|
||
goto exit_complete;
|
||
}
|
||
|
||
/* Process each sets */
|
||
for (i = 0; i < cmd->num_of_sets; i++) {
|
||
const struct bt_hci_cmd_ext_adv_set *eas;
|
||
struct le_ext_adv *ext_adv;
|
||
bool random_addr;
|
||
|
||
eas = data + sizeof(*cmd) + (sizeof(*eas) * i);
|
||
|
||
ext_adv = queue_find(dev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(eas->handle));
|
||
if (!ext_adv) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
goto exit_complete;
|
||
}
|
||
|
||
if (ext_adv->enable == cmd->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto exit_complete;
|
||
}
|
||
|
||
random_addr = bacmp((bdaddr_t *)ext_adv->random_addr,
|
||
BDADDR_ANY);
|
||
|
||
/* If the advertising set's Own_Address_Type parameter
|
||
* is set to 0x01 and the random address for
|
||
* the advertising set has not been initialized, the
|
||
* Controller shall return the error code Invalid HCI
|
||
* Command Parameters (0x12).
|
||
*/
|
||
if (ext_adv->own_addr_type == 0x01 && !random_addr) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto exit_complete;
|
||
}
|
||
|
||
/* If the advertising set's Own_Address_Type parameter is set
|
||
* to 0x03, the controller's resolving list did not contain a
|
||
* matching entry, and the random address for the advertising
|
||
* set has not been initialized, the Controller shall return the
|
||
* error code Invalid HCI Command Parameters (0x12).
|
||
*/
|
||
if (ext_adv->own_addr_type == 0x03 && !random_addr) {
|
||
if (!dev->le_rl_enable ||
|
||
!rl_find(dev, ext_adv->direct_addr_type,
|
||
ext_adv->direct_addr)) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto exit_complete;
|
||
}
|
||
}
|
||
|
||
ext_adv->enable = cmd->enable;
|
||
|
||
dev->le_adv_enable = 0x01;
|
||
|
||
if (!cmd->enable)
|
||
ext_adv_disable(ext_adv, NULL);
|
||
else if (eas->duration)
|
||
ext_adv->id = timeout_add(eas->duration * 10,
|
||
ext_adv_timeout,
|
||
ext_adv, NULL);
|
||
}
|
||
|
||
exit_complete:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_ADV_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
if (status == BT_HCI_ERR_SUCCESS && cmd->enable) {
|
||
/* Go through each sets and send adv event to peer device */
|
||
for (i = 0; i < cmd->num_of_sets; i++) {
|
||
const struct bt_hci_cmd_ext_adv_set *eas;
|
||
struct le_ext_adv *ext_adv;
|
||
|
||
eas = data + sizeof(*cmd) + (sizeof(*eas) * i);
|
||
|
||
ext_adv = queue_find(dev->le_ext_adv,
|
||
match_ext_adv_handle,
|
||
UINT_TO_PTR(eas->handle));
|
||
if (ext_adv)
|
||
le_set_ext_adv_enable_complete(dev, ext_adv);
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_max_adv_data_len(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_read_num_adv_sets(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_num_supported_adv_sets rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.num_of_sets = MAX_EXT_ADV_SETS;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_NUM_SUPPORTED_ADV_SETS, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remove_adv_set(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_remove_adv_set *cmd = data;
|
||
struct le_ext_adv *ext_adv;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
ext_adv = queue_find(dev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(cmd->handle));
|
||
if (!ext_adv) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_ADV_SET, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
if (ext_adv->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_ADV_SET, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
queue_remove(dev->le_ext_adv, ext_adv);
|
||
free(ext_adv);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_ADV_SET, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_clear_adv_sets(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct queue_entry *entry;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
for (entry = queue_get_entries(dev->le_ext_adv); entry;
|
||
entry = entry->next) {
|
||
struct le_ext_adv *ext_adv = entry->data;
|
||
|
||
if (ext_adv->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_ADV_SETS, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
queue_remove_all(dev->le_ext_adv, NULL, NULL, le_ext_adv_free);
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_CLEAR_ADV_SETS, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_pa_params(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_pa_params *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (dev->le_pa_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
} else {
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
dev->le_pa_properties = le16_to_cpu(cmd->properties);
|
||
dev->le_pa_min_interval = cmd->min_interval;
|
||
dev->le_pa_max_interval = cmd->max_interval;
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_PA_PARAMS, &status,
|
||
sizeof(status));
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_pa_data(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_pa_data *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
uint8_t data_len = cmd->data_len;
|
||
|
||
if (data_len > MAX_PA_DATA_LEN)
|
||
data_len = MAX_PA_DATA_LEN;
|
||
|
||
dev->le_pa_data_len = data_len;
|
||
memcpy(dev->le_pa_data, cmd->data, data_len);
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_PA_DATA, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void send_biginfo(struct btdev *dev, const struct btdev *remote,
|
||
uint16_t sync_handle)
|
||
{
|
||
struct bt_hci_evt_le_big_info_adv_report ev;
|
||
const struct btdev_conn *conn;
|
||
struct bt_hci_bis *bis;
|
||
|
||
conn = find_bis_index(remote, 0);
|
||
if (!conn)
|
||
return;
|
||
|
||
bis = conn->data;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
ev.sync_handle = cpu_to_le16(sync_handle);
|
||
ev.num_bis = 1;
|
||
|
||
while (find_bis_index(remote, ev.num_bis))
|
||
ev.num_bis++;
|
||
|
||
ev.nse = 0x01;
|
||
ev.iso_interval = bis->latency / 1.25;
|
||
ev.bn = 0x01;
|
||
ev.pto = 0x00;
|
||
ev.irc = 0x01;
|
||
ev.max_pdu = bis->sdu;
|
||
memcpy(ev.sdu_interval, bis->sdu_interval, sizeof(ev.sdu_interval));
|
||
ev.max_sdu = bis->sdu;
|
||
ev.phy = bis->phy;
|
||
ev.framing = bis->framing;
|
||
ev.encryption = bis->encryption;
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_INFO_ADV_REPORT, &ev, sizeof(ev));
|
||
}
|
||
|
||
static void send_pa(struct btdev *dev, const struct btdev *remote,
|
||
uint8_t offset, uint16_t sync_handle)
|
||
{
|
||
struct __packed {
|
||
struct bt_hci_le_pa_report ev;
|
||
uint8_t data[247];
|
||
} pdu;
|
||
|
||
memset(&pdu.ev, 0, sizeof(pdu.ev));
|
||
pdu.ev.handle = cpu_to_le16(sync_handle);
|
||
pdu.ev.tx_power = 127;
|
||
pdu.ev.rssi = 127;
|
||
pdu.ev.cte_type = 0x0ff;
|
||
|
||
if ((size_t) remote->le_pa_data_len - offset > sizeof(pdu.data)) {
|
||
pdu.ev.data_status = 0x01;
|
||
pdu.ev.data_len = sizeof(pdu.data);
|
||
} else {
|
||
pdu.ev.data_status = 0x00;
|
||
pdu.ev.data_len = remote->le_pa_data_len - offset;
|
||
}
|
||
|
||
memcpy(pdu.data, remote->le_pa_data + offset, pdu.ev.data_len);
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_PA_REPORT, &pdu,
|
||
sizeof(pdu.ev) + pdu.ev.data_len);
|
||
|
||
if (pdu.ev.data_status == 0x01) {
|
||
offset += pdu.ev.data_len;
|
||
send_pa(dev, remote, offset, sync_handle);
|
||
return;
|
||
}
|
||
|
||
send_biginfo(dev, remote, sync_handle);
|
||
}
|
||
|
||
static bool match_sync_handle(const void *data, const void *match_data)
|
||
{
|
||
const struct le_per_adv *per_adv = data;
|
||
uint16_t sync_handle = PTR_TO_UINT(match_data);
|
||
|
||
return per_adv->sync_handle == sync_handle;
|
||
}
|
||
|
||
static bool match_big_handle(const void *data, const void *match_data)
|
||
{
|
||
const struct le_big *big = data;
|
||
uint8_t handle = PTR_TO_UINT(match_data);
|
||
|
||
return big->handle == handle;
|
||
}
|
||
|
||
static bool match_dev(const void *data, const void *match_data)
|
||
{
|
||
const struct le_per_adv *per_adv = data;
|
||
const struct btdev *dev = match_data;
|
||
|
||
return dev == find_btdev_by_bdaddr_type(per_adv->addr,
|
||
per_adv->addr_type);
|
||
}
|
||
|
||
static void le_pa_sync_estabilished(struct btdev *dev, struct btdev *remote,
|
||
uint8_t status)
|
||
{
|
||
struct bt_hci_evt_le_per_sync_established ev;
|
||
struct le_per_adv *per_adv;
|
||
uint16_t sync_handle = SYNC_HANDLE;
|
||
|
||
per_adv = queue_find(dev->le_per_adv, match_dev, remote);
|
||
if (!per_adv)
|
||
return;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
ev.status = status;
|
||
|
||
if (status) {
|
||
queue_remove(dev->le_per_adv, per_adv);
|
||
free(per_adv);
|
||
le_meta_event(dev, BT_HCI_EVT_LE_PA_SYNC_ESTABLISHED, &ev,
|
||
sizeof(ev));
|
||
return;
|
||
}
|
||
|
||
while (queue_find(dev->le_per_adv, match_sync_handle,
|
||
UINT_TO_PTR(sync_handle)))
|
||
sync_handle++;
|
||
|
||
per_adv->sync_handle = sync_handle;
|
||
|
||
ev.handle = cpu_to_le16(per_adv->sync_handle);
|
||
ev.addr_type = per_adv->addr_type;
|
||
memcpy(ev.addr, per_adv->addr, sizeof(ev.addr));
|
||
ev.phy = 0x01;
|
||
ev.interval = remote->le_pa_min_interval;
|
||
ev.clock_accuracy = 0x07;
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_PA_SYNC_ESTABLISHED, &ev, sizeof(ev));
|
||
send_pa(dev, remote, 0, per_adv->sync_handle);
|
||
}
|
||
|
||
static int cmd_set_pa_enable(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_pa_enable *cmd = data;
|
||
uint8_t status;
|
||
int i;
|
||
|
||
if (dev->le_pa_enable == cmd->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
} else {
|
||
dev->le_pa_enable = cmd->enable;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_PA_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
struct btdev *remote = btdev_list[i];
|
||
|
||
if (!remote || remote == dev)
|
||
continue;
|
||
|
||
if (remote->le_scan_enable &&
|
||
queue_find(remote->le_per_adv, match_sync_handle,
|
||
UINT_TO_PTR(INV_HANDLE)))
|
||
le_pa_sync_estabilished(remote, dev,
|
||
BT_HCI_ERR_SUCCESS);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_ext_scan_params(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_scan_params *cmd = data;
|
||
const struct bt_hci_le_scan_phy *scan = (void *)cmd->data;
|
||
uint8_t status;
|
||
|
||
if (dev->le_scan_enable)
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
else if (cmd->num_phys == 0)
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
else {
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
/* Currently we dont support multiple types in single
|
||
* command So just take the first one will do.
|
||
*/
|
||
dev->le_scan_type = scan->type;
|
||
dev->le_scan_own_addr_type = cmd->own_addr_type;
|
||
dev->le_scan_filter_policy = cmd->filter_policy;
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_SCAN_PARAMS, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_ext_scan_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_scan_enable *cmd = data;
|
||
uint8_t status;
|
||
|
||
if (dev->le_scan_enable == cmd->enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* If Enable is set to 0x01, the scanning parameters' Own_Address_Type
|
||
* parameter is set to 0x01 or 0x03, and the random address for the
|
||
* device has not been initialized, the Controller shall return the
|
||
* error code Invalid HCI Command Parameters (0x12).
|
||
*/
|
||
if ((dev->le_scan_own_addr_type == 0x01 ||
|
||
dev->le_scan_own_addr_type == 0x03) &&
|
||
!bacmp((bdaddr_t *)dev->random_addr, BDADDR_ANY)) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
dev->le_scan_enable = cmd->enable;
|
||
dev->le_filter_dup = cmd->filter_dup;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_EXT_SCAN_ENABLE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void scan_ext_adv(struct btdev *dev, struct btdev *remote)
|
||
{
|
||
const struct queue_entry *entry;
|
||
|
||
for (entry = queue_get_entries(remote->le_ext_adv); entry;
|
||
entry = entry->next) {
|
||
struct le_ext_adv *ext_adv = entry->data;
|
||
uint16_t report_type;
|
||
|
||
if (!ext_adv->enable)
|
||
continue;
|
||
|
||
if (!ext_adv_match_addr(dev, ext_adv))
|
||
continue;
|
||
|
||
report_type = get_ext_adv_type(ext_adv->type);
|
||
send_ext_adv(dev, remote, ext_adv, report_type, false);
|
||
|
||
if (dev->le_scan_type != 0x01)
|
||
continue;
|
||
|
||
/* if scannable bit is set the send scan response */
|
||
if (ext_adv->type & 0x02) {
|
||
if (ext_adv->type == 0x13)
|
||
report_type = 0x1b;
|
||
else if (ext_adv->type == 0x12)
|
||
report_type = 0x1a;
|
||
else if (!(ext_adv->type & 0x10))
|
||
report_type &= 0x08;
|
||
else
|
||
continue;
|
||
|
||
send_ext_adv(dev, remote, ext_adv, report_type, true);
|
||
}
|
||
}
|
||
}
|
||
|
||
static void scan_pa(struct btdev *dev, struct btdev *remote)
|
||
{
|
||
struct le_per_adv *per_adv = queue_find(dev->le_per_adv,
|
||
match_sync_handle, UINT_TO_PTR(INV_HANDLE));
|
||
|
||
if (!per_adv || !remote->le_pa_enable)
|
||
return;
|
||
|
||
if (remote != find_btdev_by_bdaddr_type(per_adv->addr,
|
||
per_adv->addr_type))
|
||
return;
|
||
|
||
le_pa_sync_estabilished(dev, remote, BT_HCI_ERR_SUCCESS);
|
||
}
|
||
|
||
static int cmd_set_ext_scan_enable_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_ext_scan_enable *cmd = data;
|
||
int i;
|
||
|
||
if (!dev->le_scan_enable || !cmd->enable)
|
||
return 0;
|
||
|
||
for (i = 0; i < MAX_BTDEV_ENTRIES; i++) {
|
||
if (!btdev_list[i] || btdev_list[i] == dev)
|
||
continue;
|
||
|
||
scan_ext_adv(dev, btdev_list[i]);
|
||
scan_pa(dev, btdev_list[i]);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_ext_create_conn(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_EXT_CREATE_CONN);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static void ext_adv_term(void *data, void *user_data)
|
||
{
|
||
struct le_ext_adv *adv = data;
|
||
struct btdev_conn *conn = user_data;
|
||
|
||
/* if connectable bit is set the send adv terminate */
|
||
if (conn && adv->type & 0x01) {
|
||
adv_set_terminate(adv->dev, 0x00, adv->handle, conn->handle,
|
||
0x00);
|
||
ext_adv_disable(adv, NULL);
|
||
}
|
||
}
|
||
|
||
static void le_ext_conn_complete(struct btdev *btdev,
|
||
const struct bt_hci_cmd_le_ext_create_conn *cmd,
|
||
struct le_ext_adv *ext_adv,
|
||
uint8_t status)
|
||
{
|
||
struct btdev_conn *conn = NULL;
|
||
struct bt_hci_evt_le_enhanced_conn_complete ev;
|
||
struct bt_hci_le_ext_create_conn *lecc = (void *)cmd->data;
|
||
|
||
memset(&ev, 0, sizeof(ev));
|
||
|
||
if (!status) {
|
||
conn = conn_add_acl(btdev, cmd->peer_addr, cmd->peer_addr_type);
|
||
if (!conn)
|
||
return;
|
||
|
||
ev.status = status;
|
||
ev.peer_addr_type = btdev->le_scan_own_addr_type;
|
||
if (ev.peer_addr_type == 0x01 || ev.peer_addr_type == 0x03) {
|
||
ev.peer_addr_type = 0x01;
|
||
memcpy(ev.peer_addr, btdev->random_addr, 6);
|
||
} else
|
||
memcpy(ev.peer_addr, btdev->bdaddr, 6);
|
||
|
||
ev.role = 0x01;
|
||
ev.handle = cpu_to_le16(conn->handle);
|
||
ev.interval = lecc->max_interval;
|
||
ev.latency = lecc->latency;
|
||
ev.supv_timeout = lecc->supv_timeout;
|
||
|
||
/* Set Local RPA if an RPA was generated for the advertising */
|
||
if (ext_adv->rpa)
|
||
memcpy(ev.local_rpa, ext_adv->random_addr,
|
||
sizeof(ev.local_rpa));
|
||
|
||
le_meta_event(conn->link->dev,
|
||
BT_HCI_EVT_LE_ENHANCED_CONN_COMPLETE, &ev,
|
||
sizeof(ev));
|
||
|
||
/* Disable EXT ADV */
|
||
queue_foreach(conn->link->dev->le_ext_adv, ext_adv_term, conn);
|
||
}
|
||
|
||
ev.status = status;
|
||
ev.peer_addr_type = cmd->peer_addr_type;
|
||
memcpy(ev.peer_addr, cmd->peer_addr, 6);
|
||
ev.role = 0x00;
|
||
|
||
/* Use random address as Local RPA if Create Connection own_addr_type
|
||
* is 0x03 since that expects the controller to generate the RPA.
|
||
*/
|
||
if (btdev->le_scan_own_addr_type == 0x03)
|
||
memcpy(ev.local_rpa, btdev->random_addr, 6);
|
||
else
|
||
memset(ev.local_rpa, 0, sizeof(ev.local_rpa));
|
||
|
||
le_meta_event(btdev, BT_HCI_EVT_LE_ENHANCED_CONN_COMPLETE, &ev,
|
||
sizeof(ev));
|
||
|
||
/* Disable EXT ADV */
|
||
if (conn)
|
||
queue_foreach(btdev->le_ext_adv, ext_adv_term, conn);
|
||
}
|
||
|
||
static int cmd_ext_create_conn_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_ext_create_conn *cmd = data;
|
||
const struct queue_entry *entry;
|
||
struct btdev *remote;
|
||
|
||
dev->le_scan_own_addr_type = cmd->own_addr_type;
|
||
|
||
remote = find_btdev_by_bdaddr_type(cmd->peer_addr, cmd->peer_addr_type);
|
||
if (!remote) {
|
||
le_ext_conn_complete(dev, cmd, NULL,
|
||
BT_HCI_ERR_CONN_FAILED_TO_ESTABLISH);
|
||
return 0;
|
||
}
|
||
|
||
for (entry = queue_get_entries(remote->le_ext_adv); entry;
|
||
entry = entry->next) {
|
||
struct le_ext_adv *ext_adv = entry->data;
|
||
|
||
if (ext_adv_is_connectable(ext_adv) &&
|
||
ext_adv_match_addr(dev, ext_adv) &&
|
||
ext_adv_addr_type(ext_adv) == cmd->peer_addr_type) {
|
||
le_ext_conn_complete(dev, cmd, ext_adv, 0);
|
||
return 0;
|
||
}
|
||
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
static struct le_per_adv *le_per_adv_new(struct btdev *btdev,
|
||
uint8_t addr_type, const uint8_t *addr)
|
||
{
|
||
struct le_per_adv *per_adv;
|
||
|
||
per_adv = new0(struct le_per_adv, 1);
|
||
|
||
per_adv->dev = btdev;
|
||
per_adv->addr_type = addr_type;
|
||
memcpy(per_adv->addr, addr, 6);
|
||
per_adv->sync_handle = INV_HANDLE;
|
||
|
||
/* Add to queue */
|
||
if (!queue_push_tail(btdev->le_per_adv, per_adv)) {
|
||
free(per_adv);
|
||
return NULL;
|
||
}
|
||
|
||
return per_adv;
|
||
}
|
||
|
||
static int cmd_pa_create_sync(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_pa_create_sync *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
struct le_per_adv *per_adv;
|
||
|
||
/* Create new train */
|
||
per_adv = le_per_adv_new(dev, cmd->addr_type, cmd->addr);
|
||
if (!per_adv)
|
||
status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
|
||
cmd_status(dev, status, BT_HCI_CMD_LE_PA_CREATE_SYNC);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pa_create_sync_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_pa_create_sync *cmd = data;
|
||
struct btdev *remote;
|
||
|
||
/* This command may be issued whether or not scanning is enabled and
|
||
* scanning may be enabled and disabled (see the LE Set Extended Scan
|
||
* Enable command) while this command is pending. However,
|
||
* synchronization can only occur when scanning is enabled. While
|
||
* scanning is disabled, no attempt to synchronize will take place.
|
||
*/
|
||
if (!dev->scan_enable)
|
||
return 0;
|
||
|
||
remote = find_btdev_by_bdaddr_type(cmd->addr, cmd->addr_type);
|
||
if (!remote || !remote->le_pa_enable)
|
||
return 0;
|
||
|
||
le_pa_sync_estabilished(dev, remote, BT_HCI_ERR_SUCCESS);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pa_create_sync_cancel(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
/* If the Host issues this command while no
|
||
* HCI_LE_Periodic_Advertising_Create_Sync command is pending, the
|
||
* Controller shall return the error code Command Disallowed (0x0C).
|
||
*/
|
||
if (!queue_find(dev->le_per_adv, match_sync_handle,
|
||
UINT_TO_PTR(INV_HANDLE)))
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_PA_CREATE_SYNC_CANCEL,
|
||
&status, sizeof(status));
|
||
|
||
/* After the HCI_Command_Complete is sent and if the cancellation was
|
||
* successful, the Controller sends an
|
||
* HCI_LE_Periodic_Advertising_Sync_Established event to the Host with
|
||
* the error code Operation Cancelled by Host (0x44).
|
||
*/
|
||
if (!status)
|
||
le_pa_sync_estabilished(dev, NULL, BT_HCI_ERR_CANCELLED);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pa_term_sync(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_pa_term_sync *cmd = data;
|
||
struct le_per_adv *per_adv;
|
||
uint16_t sync_handle = le16_to_cpu(cmd->sync_handle);
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
per_adv = queue_find(dev->le_per_adv, match_sync_handle,
|
||
UINT_TO_PTR(sync_handle));
|
||
|
||
/* If the periodic advertising train corresponding to the Sync_Handle
|
||
* parameter does not exist, then the Controller shall return the error
|
||
* code Unknown Advertising Identifier (0x42).
|
||
*/
|
||
if (!per_adv) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
} else {
|
||
queue_remove(dev->le_per_adv, per_adv);
|
||
free(per_adv);
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_PA_TERM_SYNC,
|
||
&status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_pa_add(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_pa_remove(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_pa_clear(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_read_pa_list_size(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_read_tx_power(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_tx_power rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
/* a random default value */
|
||
rsp.max_tx_power = 0x07;
|
||
rsp.min_tx_power = 0xDE;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_TX_POWER, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_privacy_mode(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_priv_mode *cmd = data;
|
||
const struct btdev_rl *rl;
|
||
uint8_t status;
|
||
|
||
/* This command shall not be used when address resolution is enabled in
|
||
* the Controller and:
|
||
* • Advertising (other than periodic advertising) is enabled,
|
||
* • Scanning is enabled, or
|
||
* • an HCI_LE_Create_Connection, HCI_LE_Extended_Create_Connection,
|
||
* or HCI_LE_Periodic_Advertising_Create_Sync command is pending.
|
||
*/
|
||
if (dev->le_rl_enable || dev->le_adv_enable || dev->le_scan_enable) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* If the device is not on the resolving list, the Controller shall
|
||
* return the error code Unknown Connection Identifier (0x02).
|
||
*/
|
||
rl = rl_find(dev, cmd->peer_id_addr_type, cmd->peer_id_addr);
|
||
if (!rl) {
|
||
status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
if (cmd->priv_mode > 0x01) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
((struct btdev_rl *)rl)->mode = cmd->priv_mode;
|
||
status = BT_HCI_ERR_SUCCESS;
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_PRIV_MODE, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_LE_50 \
|
||
CMD(BT_HCI_CMD_LE_SET_DEFAULT_PHY, cmd_set_default_phy, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_ADV_SET_RAND_ADDR, cmd_set_adv_rand_addr, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EXT_ADV_PARAMS, cmd_set_ext_adv_params, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EXT_ADV_DATA, cmd_set_ext_adv_data, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EXT_SCAN_RSP_DATA, cmd_set_ext_scan_rsp_data, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EXT_ADV_ENABLE, cmd_set_ext_adv_enable, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_MAX_ADV_DATA_LEN, cmd_read_max_adv_data_len, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_NUM_SUPPORTED_ADV_SETS, cmd_read_num_adv_sets, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_REMOVE_ADV_SET, cmd_remove_adv_set, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CLEAR_ADV_SETS, cmd_clear_adv_sets, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_PA_PARAMS, cmd_set_pa_params, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_PA_DATA, cmd_set_pa_data, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_PA_ENABLE, cmd_set_pa_enable, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EXT_SCAN_PARAMS, cmd_set_ext_scan_params, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EXT_SCAN_ENABLE, cmd_set_ext_scan_enable, \
|
||
cmd_set_ext_scan_enable_complete), \
|
||
CMD(BT_HCI_CMD_LE_EXT_CREATE_CONN, cmd_ext_create_conn, \
|
||
cmd_ext_create_conn_complete), \
|
||
CMD(BT_HCI_CMD_LE_PA_CREATE_SYNC, cmd_pa_create_sync, \
|
||
cmd_pa_create_sync_complete), \
|
||
CMD(BT_HCI_CMD_LE_PA_CREATE_SYNC_CANCEL, cmd_pa_create_sync_cancel, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_PA_TERM_SYNC, cmd_pa_term_sync, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ADD_DEV_PA_LIST, cmd_pa_add, NULL), \
|
||
CMD(BT_HCI_CMD_LE_REMOVE_DEV_PA_LIST, cmd_pa_remove, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CLEAR_PA_LIST, cmd_pa_clear, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_PA_LIST_SIZE, cmd_read_pa_list_size, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_TX_POWER, cmd_read_tx_power, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_PRIV_MODE, cmd_set_privacy_mode, NULL)
|
||
|
||
static const struct btdev_cmd cmd_le_5_0[] = {
|
||
CMD_COMMON_ALL,
|
||
CMD_COMMON_BREDR_LE,
|
||
CMD_LE,
|
||
CMD_LE_50,
|
||
{}
|
||
};
|
||
|
||
static void set_le_50_commands(struct btdev *btdev)
|
||
{
|
||
btdev->commands[35] |= 0x20; /* LE Set Default PHY */
|
||
btdev->commands[36] |= 0x02; /* LE Set Adv Set Random Address */
|
||
btdev->commands[36] |= 0x04; /* LE Set Ext Adv Parameters */
|
||
btdev->commands[36] |= 0x08; /* LE Set Ext Adv Data */
|
||
btdev->commands[36] |= 0x10; /* LE Set Ext Scan Response Data */
|
||
btdev->commands[36] |= 0x20; /* LE Set Ext Adv Enable */
|
||
btdev->commands[36] |= 0x40; /* LE Read Maximum Adv Data Length */
|
||
btdev->commands[36] |= 0x80; /* LE Read Num of Supported Adv Sets */
|
||
btdev->commands[37] |= 0x01; /* LE Remove Adv Set */
|
||
btdev->commands[37] |= 0x02; /* LE Clear Adv Sets */
|
||
btdev->commands[37] |= 0x04; /* LE Set Periodic Adv Parameters */
|
||
btdev->commands[37] |= 0x08; /* LE Set Periodic Adv Data */
|
||
btdev->commands[37] |= 0x10; /* LE Set Periodic Adv Enable */
|
||
btdev->commands[37] |= 0x20; /* LE Set Ext Scan Parameters */
|
||
btdev->commands[37] |= 0x40; /* LE Set Ext Scan Enable */
|
||
btdev->commands[37] |= 0x80; /* LE Ext Create Connection */
|
||
btdev->commands[38] |= 0x01; /* LE Periodic Adv Create Sync */
|
||
btdev->commands[38] |= 0x02; /* LE Periodic Adv Create Sync Cancel */
|
||
btdev->commands[38] |= 0x04; /* LE Periodic Adv Terminate Sync */
|
||
btdev->commands[38] |= 0x08; /* LE Add Device To Periodic Adv List */
|
||
btdev->commands[38] |= 0x10; /* LE Remove Periodic Adv List */
|
||
btdev->commands[38] |= 0x20; /* LE Clear Periodic Adv List */
|
||
btdev->commands[38] |= 0x40; /* LE Read Periodic Adv List Size */
|
||
btdev->commands[38] |= 0x80; /* LE Read Transmit Power */
|
||
btdev->commands[39] |= 0x04; /* LE Set Privacy Mode */
|
||
btdev->cmds = cmd_le_5_0;
|
||
}
|
||
|
||
static int cmd_read_size_v2(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_le_read_buffer_size_v2 rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.acl_mtu = cpu_to_le16(dev->acl_mtu);
|
||
rsp.acl_max_pkt = dev->acl_max_pkt;
|
||
rsp.iso_mtu = cpu_to_le16(dev->iso_mtu);
|
||
rsp.iso_max_pkt = dev->iso_max_pkt;
|
||
cmd_complete(dev, BT_HCI_CMD_LE_READ_BUFFER_SIZE_V2, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_iso_tx_sync(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int find_cig(struct btdev *dev, uint8_t cig_id)
|
||
{
|
||
unsigned int i;
|
||
|
||
for (i = 0; i < ARRAY_SIZE(dev->le_cig); ++i)
|
||
if (dev->le_cig[i].params.cig_id == cig_id)
|
||
return i;
|
||
return -1;
|
||
}
|
||
|
||
static uint16_t make_cis_handle(uint8_t cig_idx, uint8_t cis_idx)
|
||
{
|
||
/* Put CIG+CIS idxs to handle so don't need to track separately */
|
||
return ISO_HANDLE + cig_idx*CIS_SIZE + cis_idx;
|
||
}
|
||
|
||
static int parse_cis_handle(uint16_t handle, int *cis_idx)
|
||
{
|
||
int cig_idx;
|
||
|
||
if (handle < ISO_HANDLE || handle >= ISO_HANDLE + CIS_SIZE*CIG_SIZE)
|
||
return -1;
|
||
|
||
cig_idx = (handle - ISO_HANDLE) / CIS_SIZE;
|
||
if (cis_idx)
|
||
*cis_idx = (handle - ISO_HANDLE) % CIS_SIZE;
|
||
|
||
return cig_idx;
|
||
}
|
||
|
||
static int cmd_set_cig_params(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_set_cig_params *cmd = data;
|
||
struct lescp {
|
||
struct bt_hci_rsp_le_set_cig_params params;
|
||
uint16_t handle[CIS_SIZE];
|
||
} __attribute__ ((packed)) rsp;
|
||
int i = 0;
|
||
int cig_idx;
|
||
uint32_t interval;
|
||
uint16_t latency;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.params.cig_id = cmd->cig_id;
|
||
|
||
if (cmd->num_cis > ARRAY_SIZE(dev->le_cig[0].cis)) {
|
||
rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
if (cmd->cig_id > 0xef) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
interval = get_le24(cmd->c_interval);
|
||
if (interval < 0x0000ff || interval > 0x0fffff) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
interval = get_le24(cmd->p_interval);
|
||
if (interval < 0x0000ff || interval > 0x0fffff) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
if (cmd->sca > 0x07) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
if (cmd->packing > 0x01) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
if (cmd->framing > 0x01) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
latency = cpu_to_le16(cmd->c_latency);
|
||
if (latency < 0x0005 || latency > 0x0fa0) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
latency = cpu_to_le16(cmd->p_latency);
|
||
if (latency < 0x0005 || latency > 0x0fa0) {
|
||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
cig_idx = find_cig(dev, cmd->cig_id);
|
||
if (cig_idx < 0)
|
||
cig_idx = find_cig(dev, 0xff);
|
||
if (cig_idx < 0) {
|
||
rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
/* BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 4, Part E
|
||
* page 2553
|
||
*
|
||
* If the Host issues this command when the CIG is not in the
|
||
* configurable state, the Controller shall return the error
|
||
* code Command Disallowed (0x0C).
|
||
*/
|
||
if (dev->le_cig[cig_idx].activated) {
|
||
rsp.params.status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
rsp.params.status = BT_HCI_ERR_SUCCESS;
|
||
|
||
for (i = 0; i < cmd->num_cis; i++) {
|
||
rsp.params.num_handles++;
|
||
rsp.handle[i] = cpu_to_le16(make_cis_handle(cig_idx, i));
|
||
}
|
||
|
||
memcpy(&dev->le_cig[cig_idx], data, len);
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_CIG_PARAMS, &rsp,
|
||
sizeof(rsp.params) + (i * sizeof(uint16_t)));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_set_cig_params_test(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_create_cis(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_create_cis *cmd = data;
|
||
int i, j;
|
||
|
||
for (i = 0; i < cmd->num_cis; i++) {
|
||
const struct bt_hci_cis *cis = &cmd->cis[i];
|
||
struct btdev_conn *acl;
|
||
struct btdev_conn *iso;
|
||
int cig_idx, cis_idx;
|
||
|
||
/* Check for errors (Core v5.3 Vol 4 Part E Sec. 7.8.99) */
|
||
for (j = 0; j < i; j++)
|
||
if (cis->cis_handle == cmd->cis[j].cis_handle)
|
||
return -EINVAL;
|
||
|
||
cig_idx = parse_cis_handle(le16_to_cpu(cis->cis_handle),
|
||
&cis_idx);
|
||
if (cig_idx < 0)
|
||
return -ENOENT;
|
||
if (cis_idx >= dev->le_cig[cig_idx].params.num_cis)
|
||
return -ENOENT;
|
||
|
||
acl = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cis->acl_handle)));
|
||
if (!acl)
|
||
return -ENOENT;
|
||
|
||
iso = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cis->cis_handle)));
|
||
if (iso)
|
||
return -EEXIST;
|
||
}
|
||
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CREATE_CIS);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static uint8_t le_cis_interval(uint8_t sdu_interval[3])
|
||
{
|
||
/* ISO Interval (slots of 1.25 ms) = SDU_Interval (us) */
|
||
return get_le24(sdu_interval) / 1250;
|
||
}
|
||
|
||
static uint32_t le_cis_latecy(uint8_t ft, uint8_t iso_interval,
|
||
uint8_t sdu_interval[3])
|
||
{
|
||
uint32_t latency;
|
||
uint32_t interval = get_le24(sdu_interval);
|
||
|
||
/* Transport_Latency = FT × ISO_Interval - SDU_Interval */
|
||
latency = ft * (iso_interval * 1250) - interval;
|
||
|
||
return latency >= interval ? latency : interval;
|
||
}
|
||
|
||
static void le_cis_estabilished(struct btdev *dev, struct btdev_conn *conn,
|
||
uint8_t status)
|
||
{
|
||
struct bt_hci_evt_le_cis_established evt;
|
||
int cig_idx, cis_idx = -1;
|
||
|
||
memset(&evt, 0, sizeof(evt));
|
||
|
||
evt.status = status;
|
||
evt.conn_handle = conn ? cpu_to_le16(conn->handle) : 0x0000;
|
||
|
||
cig_idx = conn ? parse_cis_handle(conn->link->handle, &cis_idx) : -1;
|
||
if (cig_idx < 0 && !evt.status)
|
||
evt.status = BT_HCI_ERR_UNSPECIFIED_ERROR;
|
||
|
||
if (!evt.status) {
|
||
struct btdev *remote = conn->link->dev;
|
||
struct le_cig *le_cig = &remote->le_cig[cig_idx];
|
||
|
||
memset(evt.cig_sync_delay, 0, sizeof(evt.cig_sync_delay));
|
||
memset(evt.cis_sync_delay, 0, sizeof(evt.cis_sync_delay));
|
||
|
||
evt.c_phy = le_cig->cis[cis_idx].c_phy;
|
||
evt.p_phy = le_cig->cis[cis_idx].p_phy;
|
||
evt.nse = 0x01;
|
||
evt.c_bn = 0x01;
|
||
evt.p_bn = 0x01;
|
||
evt.c_ft = 0x02;
|
||
evt.p_ft = 0x02;
|
||
evt.c_mtu = le_cig->cis[cis_idx].c_sdu;
|
||
evt.p_mtu = le_cig->cis[cis_idx].p_sdu;
|
||
evt.interval = le_cis_interval(le_cig->params.c_interval);
|
||
|
||
/* BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 6, Part G
|
||
* page 3050:
|
||
*
|
||
* Transport_Latency_C_To_P = CIG_Sync_Delay + FT_C_To_P ×
|
||
* ISO_Interval - SDU_Interval_C_To_P
|
||
* Transport_Latency_P_To_C = CIG_Sync_Delay + FT_P_To_C ×
|
||
* ISO_Interval - SDU_Interval_P_To_C
|
||
*/
|
||
put_le24(le_cis_latecy(evt.c_ft, evt.interval,
|
||
le_cig->params.c_interval), evt.c_latency);
|
||
put_le24(le_cis_latecy(evt.p_ft, evt.interval,
|
||
le_cig->params.p_interval), evt.p_latency);
|
||
}
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt, sizeof(evt));
|
||
|
||
if (conn)
|
||
le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CIS_ESTABLISHED,
|
||
&evt, sizeof(evt));
|
||
}
|
||
|
||
static int cmd_create_cis_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_create_cis *cmd = data;
|
||
int i;
|
||
|
||
for (i = 0; i < cmd->num_cis; i++) {
|
||
const struct bt_hci_cis *cis = &cmd->cis[i];
|
||
struct btdev_conn *acl;
|
||
struct btdev_conn *iso;
|
||
struct bt_hci_evt_le_cis_req evt;
|
||
struct le_cig *le_cig;
|
||
int cig_idx, cis_idx;
|
||
|
||
cig_idx = parse_cis_handle(le16_to_cpu(cis->cis_handle),
|
||
&cis_idx);
|
||
if (cig_idx < 0) {
|
||
le_cis_estabilished(dev, NULL,
|
||
BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||
break;
|
||
}
|
||
le_cig = &dev->le_cig[cig_idx];
|
||
|
||
acl = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cis->acl_handle)));
|
||
if (!acl) {
|
||
le_cis_estabilished(dev, NULL,
|
||
BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||
break;
|
||
}
|
||
|
||
iso = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cis->cis_handle)));
|
||
if (!iso) {
|
||
iso = conn_add_cis(acl, le16_to_cpu(cis->cis_handle));
|
||
if (!iso) {
|
||
le_cis_estabilished(dev, NULL,
|
||
BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||
break;
|
||
}
|
||
}
|
||
|
||
evt.acl_handle = cpu_to_le16(acl->handle);
|
||
evt.cis_handle = cpu_to_le16(iso->handle);
|
||
evt.cig_id = le_cig->params.cig_id;
|
||
evt.cis_id = le_cig->cis[cis_idx].cis_id;
|
||
|
||
le_cig->activated = true;
|
||
|
||
le_meta_event(iso->link->dev, BT_HCI_EVT_LE_CIS_REQ, &evt,
|
||
sizeof(evt));
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
static bool match_handle_cig_idx(const void *data, const void *match_data)
|
||
{
|
||
const struct btdev_conn *conn = data;
|
||
int cig_idx = PTR_TO_INT(match_data);
|
||
|
||
return cig_idx >= 0 && parse_cis_handle(conn->handle, NULL) == cig_idx;
|
||
}
|
||
|
||
static int cmd_remove_cig(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_remove_cig *cmd = data;
|
||
struct bt_hci_rsp_le_remove_cig rsp;
|
||
struct btdev_conn *conn = NULL;
|
||
int idx;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.cig_id = cmd->cig_id;
|
||
|
||
idx = find_cig(dev, cmd->cig_id);
|
||
conn = queue_find(dev->conns, match_handle_cig_idx, INT_TO_PTR(idx));
|
||
|
||
if (idx >= 0 && !conn) {
|
||
memset(&dev->le_cig[idx], 0, sizeof(struct le_cig));
|
||
dev->le_cig[idx].params.cig_id = 0xff;
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
} else if (conn) {
|
||
rsp.status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
} else {
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_CIG, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_accept_cis(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_accept_cis *cmd = data;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (!conn) {
|
||
cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
|
||
BT_HCI_CMD_LE_ACCEPT_CIS);
|
||
return 0;
|
||
}
|
||
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_ACCEPT_CIS);
|
||
le_cis_estabilished(dev, conn, BT_HCI_ERR_SUCCESS);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_reject_cis(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_reject_cis *cmd = data;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (!conn) {
|
||
cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
|
||
BT_HCI_CMD_LE_REJECT_CIS);
|
||
return 0;
|
||
}
|
||
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_REJECT_CIS);
|
||
le_cis_estabilished(dev, conn, cmd->reason);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_create_big(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_CREATE_BIG);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_create_big_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_create_big *cmd = data;
|
||
const struct bt_hci_bis *bis = &cmd->bis;
|
||
int i;
|
||
struct bt_hci_evt_le_big_complete evt;
|
||
uint16_t *bis_handle;
|
||
uint8_t *pdu;
|
||
uint8_t pdu_len;
|
||
|
||
pdu_len = sizeof(evt) + cmd->num_bis * sizeof(*bis_handle);
|
||
|
||
pdu = malloc(pdu_len);
|
||
if (!pdu)
|
||
return -ENOMEM;
|
||
|
||
bis_handle = (uint16_t *)(pdu + sizeof(evt));
|
||
|
||
memset(&evt, 0, sizeof(evt));
|
||
|
||
for (i = 0; i < cmd->num_bis; i++) {
|
||
struct btdev_conn *conn;
|
||
|
||
conn = conn_add_bis(dev, ISO_HANDLE + i, bis);
|
||
if (!conn) {
|
||
evt.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
goto done;
|
||
}
|
||
|
||
*bis_handle = cpu_to_le16(conn->handle);
|
||
bis_handle++;
|
||
}
|
||
|
||
evt.handle = cmd->handle;
|
||
evt.phy = bis->phy;
|
||
evt.max_pdu = bis->sdu;
|
||
memcpy(evt.sync_delay, bis->sdu_interval, 3);
|
||
memcpy(evt.latency, bis->sdu_interval, 3);
|
||
evt.interval = bis->latency / 1.25;
|
||
evt.num_bis = cmd->num_bis;
|
||
|
||
done:
|
||
memcpy(pdu, &evt, sizeof(evt));
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_COMPLETE, pdu,
|
||
pdu_len);
|
||
|
||
free(pdu);
|
||
|
||
return 0;
|
||
}
|
||
static int cmd_create_big_test(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_term_big(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_TERM_BIG);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_term_big_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_term_big *cmd = data;
|
||
struct bt_hci_evt_le_big_terminate rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.reason = cmd->reason;
|
||
rsp.handle = cmd->handle;
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_TERMINATE, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_big_create_sync(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_big_create_sync *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
uint16_t sync_handle = le16_to_cpu(cmd->sync_handle);
|
||
|
||
/* If the Sync_Handle does not exist, the Controller shall return the
|
||
* error code Unknown Advertising Identifier (0x42).
|
||
*/
|
||
if (!queue_find(dev->le_per_adv, match_sync_handle,
|
||
UINT_TO_PTR(sync_handle))) {
|
||
status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
goto done;
|
||
}
|
||
|
||
/* If the Host sends this command with a BIG_Handle that is already
|
||
* allocated, the Controller shall return the error code Command
|
||
* Disallowed (0x0C).
|
||
*/
|
||
if (queue_find(dev->le_big, match_big_handle,
|
||
UINT_TO_PTR(cmd->handle))) {
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
goto done;
|
||
}
|
||
|
||
/* If the Num_BIS parameter is greater than the total number of BISes
|
||
* in the BIG, the Controller shall return the error code Unsupported
|
||
* Feature or Parameter Value (0x11).
|
||
*/
|
||
if (cmd->num_bis != len - sizeof(*cmd))
|
||
status = BT_HCI_ERR_UNSUPPORTED_FEATURE;
|
||
|
||
done:
|
||
cmd_status(dev, status, BT_HCI_CMD_LE_BIG_CREATE_SYNC);
|
||
|
||
return 0;
|
||
}
|
||
|
||
static struct le_big *le_big_new(struct btdev *btdev, uint8_t handle)
|
||
{
|
||
struct le_big *big;
|
||
|
||
big = new0(struct le_big, 1);
|
||
|
||
big->dev = btdev;
|
||
big->handle = handle;
|
||
big->bis = queue_new();
|
||
|
||
/* Add to queue */
|
||
if (!queue_push_tail(btdev->le_big, big)) {
|
||
le_big_free(big);
|
||
return NULL;
|
||
}
|
||
|
||
return big;
|
||
}
|
||
|
||
static int cmd_big_create_sync_complete(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_big_create_sync *cmd = data;
|
||
struct __packed {
|
||
struct bt_hci_evt_le_big_sync_estabilished ev;
|
||
uint16_t bis[BIS_SIZE];
|
||
} pdu;
|
||
struct btdev *remote;
|
||
struct btdev_conn *conn = NULL;
|
||
struct bt_hci_bis *bis;
|
||
int i;
|
||
uint16_t sync_handle = le16_to_cpu(cmd->sync_handle);
|
||
struct le_per_adv *per_adv = queue_find(dev->le_per_adv,
|
||
match_sync_handle, UINT_TO_PTR(sync_handle));
|
||
struct le_big *big;
|
||
|
||
if (!per_adv)
|
||
return 0;
|
||
|
||
remote = find_btdev_by_bdaddr_type(per_adv->addr,
|
||
per_adv->addr_type);
|
||
if (!remote)
|
||
return 0;
|
||
|
||
big = le_big_new(dev, cmd->handle);
|
||
if (!big) {
|
||
pdu.ev.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_SYNC_ESTABILISHED, &pdu,
|
||
sizeof(pdu.ev));
|
||
return 0;
|
||
}
|
||
|
||
memset(&pdu.ev, 0, sizeof(pdu.ev));
|
||
|
||
for (i = 0; i < cmd->num_bis; i++) {
|
||
conn = conn_link_bis(dev, remote, i);
|
||
if (!conn)
|
||
break;
|
||
|
||
pdu.bis[i] = cpu_to_le16(conn->handle);
|
||
|
||
queue_push_tail(big->bis, conn);
|
||
}
|
||
|
||
if (i != cmd->num_bis || !conn) {
|
||
pdu.ev.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_SYNC_ESTABILISHED, &pdu,
|
||
sizeof(pdu.ev));
|
||
return 0;
|
||
}
|
||
|
||
bis = conn->data;
|
||
|
||
if (bis->encryption != cmd->encryption) {
|
||
pdu.ev.status = BT_HCI_ERR_ENC_MODE_NOT_ACCEPTABLE;
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_SYNC_ESTABILISHED, &pdu,
|
||
sizeof(pdu.ev));
|
||
return 0;
|
||
}
|
||
|
||
pdu.ev.handle = cmd->handle;
|
||
memcpy(pdu.ev.latency, bis->sdu_interval, sizeof(pdu.ev.interval));
|
||
pdu.ev.nse = 0x01;
|
||
pdu.ev.bn = 0x01;
|
||
pdu.ev.pto = 0x00;
|
||
pdu.ev.irc = 0x01;
|
||
pdu.ev.max_pdu = bis->sdu;
|
||
pdu.ev.interval = bis->latency / 1.25;
|
||
pdu.ev.num_bis = cmd->num_bis;
|
||
|
||
le_meta_event(dev, BT_HCI_EVT_LE_BIG_SYNC_ESTABILISHED, &pdu,
|
||
sizeof(pdu.ev) + (cmd->num_bis * sizeof(uint16_t)));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_big_term_sync(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_big_term_sync *cmd = data;
|
||
struct bt_hci_rsp_le_big_term_sync rsp;
|
||
struct btdev_conn *conn;
|
||
struct le_big *big = queue_find(dev->le_big, match_big_handle,
|
||
UINT_TO_PTR(cmd->handle));
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
/* If the Host issues this command with a BIG_Handle that does not
|
||
* exist, the Controller shall return the error code Unknown
|
||
* Advertising Identifier (0x42).
|
||
*/
|
||
if (!big) {
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_ADVERTISING_ID;
|
||
goto done;
|
||
}
|
||
|
||
rsp.status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
rsp.handle = cmd->handle;
|
||
|
||
/* Cleanup existing connections */
|
||
while ((conn = queue_pop_head(big->bis))) {
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
conn_remove(conn);
|
||
}
|
||
|
||
done:
|
||
if (rsp.status == BT_HCI_ERR_SUCCESS) {
|
||
queue_remove(dev->le_big, big);
|
||
le_big_free(big);
|
||
}
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_BIG_TERM_SYNC, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_req_peer_sca(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_setup_iso_path(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_setup_iso_path *cmd = data;
|
||
struct bt_hci_rsp_le_setup_iso_path rsp;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (!conn) {
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
/* Only support HCI or disabled paths */
|
||
if (cmd->path && cmd->path != 0xff) {
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
switch (cmd->direction) {
|
||
case 0x00:
|
||
dev->le_iso_path[0] = cmd->path;
|
||
rsp.handle = cpu_to_le16(conn->handle);
|
||
break;
|
||
case 0x01:
|
||
dev->le_iso_path[1] = cmd->path;
|
||
rsp.handle = cpu_to_le16(conn->handle);
|
||
break;
|
||
default:
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
}
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SETUP_ISO_PATH, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_remove_iso_path(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_le_remove_iso_path *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(cpu_to_le16(cmd->handle)));
|
||
if (!conn) {
|
||
status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
goto done;
|
||
}
|
||
|
||
switch (cmd->direction) {
|
||
case 0x00:
|
||
dev->le_iso_path[0] = 0x00;
|
||
break;
|
||
case 0x01:
|
||
dev->le_iso_path[1] = 0x00;
|
||
break;
|
||
default:
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
}
|
||
|
||
done:
|
||
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_ISO_PATH, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_iso_tx_test(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_iso_rx_test(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_iso_read_test_counter(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_iso_test_end(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_set_host_feature(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_LE_SET_HOST_FEATURE, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_codecs_v2(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct {
|
||
struct bt_hci_rsp_read_local_codecs rsp;
|
||
struct bt_hci_codec codec[6];
|
||
uint8_t num_vnd_codecs;
|
||
} pdu;
|
||
|
||
memset(&pdu, 0, sizeof(pdu));
|
||
|
||
pdu.rsp.status = BT_HCI_ERR_SUCCESS;
|
||
pdu.rsp.num_codecs = 0x06;
|
||
pdu.codec[0].id = 0x00;
|
||
pdu.codec[0].transport = BT_HCI_LOCAL_CODEC_BREDR_SCO;
|
||
pdu.codec[1].id = 0x01;
|
||
pdu.codec[1].transport = BT_HCI_LOCAL_CODEC_BREDR_SCO;
|
||
pdu.codec[2].id = 0x02;
|
||
pdu.codec[2].transport = BT_HCI_LOCAL_CODEC_BREDR_SCO;
|
||
pdu.codec[3].id = 0x03;
|
||
pdu.codec[3].transport = BT_HCI_LOCAL_CODEC_BREDR_SCO;
|
||
pdu.codec[4].id = 0x04;
|
||
pdu.codec[4].transport = BT_HCI_LOCAL_CODEC_BREDR_SCO;
|
||
pdu.codec[5].id = 0x05;
|
||
pdu.codec[5].transport = BT_HCI_LOCAL_CODEC_BREDR_SCO;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_CODECS_V2, &pdu, sizeof(pdu));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_codec_caps(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_local_codec_caps *cmd = data;
|
||
struct bt_hci_rsp_read_local_codec_caps rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
if (cmd->codec.id > 0x05)
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_CODEC_CAPS, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_local_ctrl_delay(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_read_local_ctrl_delay *cmd = data;
|
||
struct bt_hci_rsp_read_local_ctrl_delay rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
if (cmd->codec.id > 0x05)
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_CTRL_DELAY, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_config_data_path(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_config_data_path *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
if (cmd->id > 0x05)
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
|
||
cmd_complete(dev, BT_HCI_CMD_CONFIG_DATA_PATH, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_LE_52 \
|
||
CMD(BT_HCI_CMD_LE_READ_BUFFER_SIZE_V2, cmd_read_size_v2, NULL), \
|
||
CMD(BT_HCI_CMD_LE_READ_ISO_TX_SYNC, cmd_read_iso_tx_sync, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_EVENT_MASK, cmd_le_set_event_mask, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_CIG_PARAMS, cmd_set_cig_params, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_CIG_PARAMS_TEST, cmd_set_cig_params_test, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CREATE_CIS, cmd_create_cis, \
|
||
cmd_create_cis_complete), \
|
||
CMD(BT_HCI_CMD_LE_REMOVE_CIG, cmd_remove_cig, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ACCEPT_CIS, cmd_accept_cis, NULL), \
|
||
CMD(BT_HCI_CMD_LE_REJECT_CIS, cmd_reject_cis, NULL), \
|
||
CMD(BT_HCI_CMD_LE_CREATE_BIG, cmd_create_big, \
|
||
cmd_create_big_complete), \
|
||
CMD(BT_HCI_CMD_LE_CREATE_BIG_TEST, cmd_create_big_test, NULL), \
|
||
CMD(BT_HCI_CMD_LE_TERM_BIG, cmd_term_big, cmd_term_big_complete), \
|
||
CMD(BT_HCI_CMD_LE_BIG_CREATE_SYNC, cmd_big_create_sync, \
|
||
cmd_big_create_sync_complete), \
|
||
CMD(BT_HCI_CMD_LE_BIG_TERM_SYNC, cmd_big_term_sync, NULL), \
|
||
CMD(BT_HCI_CMD_LE_REQ_PEER_SCA, cmd_req_peer_sca, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SETUP_ISO_PATH, cmd_setup_iso_path, NULL), \
|
||
CMD(BT_HCI_CMD_LE_REMOVE_ISO_PATH, cmd_remove_iso_path, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ISO_TX_TEST, cmd_iso_tx_test, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ISO_RX_TEST, cmd_iso_rx_test, NULL), \
|
||
CMD(BT_HCI_CMD_LE_ISO_READ_TEST_COUNTER, cmd_iso_read_test_counter, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_LE_ISO_TEST_END, cmd_iso_test_end, NULL), \
|
||
CMD(BT_HCI_CMD_LE_SET_HOST_FEATURE, cmd_set_host_feature, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_CODECS_V2, cmd_read_local_codecs_v2, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_CODEC_CAPS, cmd_read_local_codec_caps, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_CTRL_DELAY, cmd_read_local_ctrl_delay, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_CONFIG_DATA_PATH, cmd_config_data_path, NULL)
|
||
|
||
static const struct btdev_cmd cmd_le_5_2[] = {
|
||
CMD_COMMON_ALL,
|
||
CMD_COMMON_BREDR_LE,
|
||
CMD_LE,
|
||
CMD_LE_50,
|
||
CMD_LE_52,
|
||
{}
|
||
};
|
||
|
||
static void set_le_52_commands(struct btdev *btdev)
|
||
{
|
||
btdev->commands[41] |= 0x20; /* LE Read Buffer Size v2 */
|
||
btdev->commands[41] |= 0x40; /* LE Read ISO TX Sync */
|
||
btdev->commands[41] |= 0x80; /* LE Set CIG Parameters */
|
||
btdev->commands[42] |= 0x01; /* LE Set CIG Parameters Test */
|
||
btdev->commands[42] |= 0x02; /* LE Create CIS */
|
||
btdev->commands[42] |= 0x04; /* LE Remove CIG */
|
||
btdev->commands[42] |= 0x08; /* LE Accept CIS */
|
||
btdev->commands[42] |= 0x10; /* LE Reject CIS */
|
||
btdev->commands[42] |= 0x20; /* LE Create BIG */
|
||
btdev->commands[42] |= 0x40; /* LE Create BIG Test */
|
||
btdev->commands[42] |= 0x80; /* LE Terminate BIG */
|
||
btdev->commands[43] |= 0x01; /* LE BIG Create Sync */
|
||
btdev->commands[43] |= 0x02; /* LE BIG Terminate Sync */
|
||
btdev->commands[43] |= 0x04; /* LE Request Peer SCA */
|
||
btdev->commands[43] |= 0x08; /* LE Setup ISO Path */
|
||
btdev->commands[43] |= 0x10; /* LE Remove ISO Path */
|
||
btdev->commands[43] |= 0x20; /* LE ISO TX Test */
|
||
btdev->commands[43] |= 0x40; /* LE ISO RX Test */
|
||
btdev->commands[43] |= 0x80; /* LE ISO Read Test Counter */
|
||
btdev->commands[44] |= 0x01; /* LE ISO Test End */
|
||
btdev->commands[44] |= 0x02; /* LE ISO Set Host Feature */
|
||
btdev->commands[45] |= 0x04; /* Read Local Supported Codecs v2 */
|
||
btdev->commands[45] |= 0x08; /* Read Local Supported Codecs Caps */
|
||
btdev->commands[45] |= 0x10; /* Read Local Supported Ctrl Delay */
|
||
btdev->commands[45] |= 0x20; /* Config Data Path */
|
||
btdev->cmds = cmd_le_5_2;
|
||
}
|
||
|
||
static const struct btdev_cmd cmd_le[] = {
|
||
CMD_COMMON_ALL,
|
||
CMD_COMMON_BREDR_LE,
|
||
CMD_LE,
|
||
{}
|
||
};
|
||
|
||
static void set_le_commands(struct btdev *btdev)
|
||
{
|
||
set_common_commands_all(btdev);
|
||
set_common_commands_bredrle(btdev);
|
||
|
||
btdev->commands[24] |= 0x20; /* Read LE Host Supported */
|
||
btdev->commands[24] |= 0x20; /* Write LE Host Supported */
|
||
btdev->commands[25] |= 0x01; /* LE Set Event Mask */
|
||
btdev->commands[25] |= 0x02; /* LE Read Buffer Size */
|
||
btdev->commands[25] |= 0x04; /* LE Read Local Features */
|
||
btdev->commands[25] |= 0x10; /* LE Set Random Address */
|
||
btdev->commands[25] |= 0x20; /* LE Set Adv Parameters */
|
||
btdev->commands[25] |= 0x40; /* LE Read Adv TX Power */
|
||
btdev->commands[25] |= 0x80; /* LE Set Adv Data */
|
||
btdev->commands[26] |= 0x01; /* LE Set Scan Response Data */
|
||
btdev->commands[26] |= 0x02; /* LE Set Adv Enable */
|
||
btdev->commands[26] |= 0x04; /* LE Set Scan Parameters */
|
||
btdev->commands[26] |= 0x08; /* LE Set Scan Enable */
|
||
btdev->commands[26] |= 0x10; /* LE Create Connection */
|
||
btdev->commands[26] |= 0x20; /* LE Create Connection Cancel */
|
||
btdev->commands[26] |= 0x40; /* LE Read Accept List Size */
|
||
btdev->commands[26] |= 0x80; /* LE Clear Accept List */
|
||
btdev->commands[27] |= 0x01; /* LE Add Device to Accept List */
|
||
btdev->commands[27] |= 0x02; /* LE Remove Device from Accept List */
|
||
btdev->commands[27] |= 0x04; /* LE Connection Update */
|
||
btdev->commands[27] |= 0x20; /* LE Read Remote Used Features */
|
||
btdev->commands[27] |= 0x40; /* LE Encrypt */
|
||
btdev->commands[27] |= 0x80; /* LE Rand */
|
||
btdev->commands[28] |= 0x01; /* LE Start Encryption */
|
||
btdev->commands[28] |= 0x02; /* LE Long Term Key Request Reply */
|
||
btdev->commands[28] |= 0x04; /* LE Long Term Key Request Neg Reply */
|
||
btdev->commands[28] |= 0x08; /* LE Read Supported States */
|
||
btdev->commands[28] |= 0x10; /* LE Receiver Test */
|
||
btdev->commands[28] |= 0x20; /* LE Transmitter Test */
|
||
btdev->commands[28] |= 0x40; /* LE Test End */
|
||
|
||
/* Extra LE commands for >= 4.1 adapters */
|
||
btdev->commands[33] |= 0x10; /* LE Remote Conn Param Req Reply */
|
||
btdev->commands[33] |= 0x20; /* LE Remote Conn Param Req Neg Reply */
|
||
|
||
/* Extra LE commands for >= 4.2 adapters */
|
||
btdev->commands[34] |= 0x02; /* LE Read Local P-256 Public Key */
|
||
btdev->commands[34] |= 0x04; /* LE Generate DHKey */
|
||
btdev->commands[34] |= 0x08; /* LE Add Device To Resolving List */
|
||
btdev->commands[34] |= 0x10; /* LE Remove Dev From Resolving List */
|
||
btdev->commands[34] |= 0x20; /* LE Clear Resolving List */
|
||
btdev->commands[34] |= 0x40; /* LE Read Resolving List Size */
|
||
btdev->commands[34] |= 0x80; /* LE Read Peer Resolvable Address */
|
||
btdev->commands[35] |= 0x01; /* LE Read Local Resolvable Address */
|
||
btdev->commands[35] |= 0x02; /* LE Set Address Resolution Enable */
|
||
btdev->commands[35] |= 0x04; /* LE Set RPA Timeout */
|
||
|
||
btdev->cmds = cmd_le;
|
||
|
||
/* Extra LE commands for >= 5.0 adapters */
|
||
if (btdev->type >= BTDEV_TYPE_BREDRLE50) {
|
||
set_le_50_commands(btdev);
|
||
btdev->cmds = cmd_le_5_0;
|
||
}
|
||
|
||
/* Extra LE commands for >= 5.2 adapters */
|
||
if (btdev->type >= BTDEV_TYPE_BREDRLE52) {
|
||
set_le_52_commands(btdev);
|
||
btdev->cmds = cmd_le_5_2;
|
||
}
|
||
}
|
||
|
||
static int cmd_set_event_mask_2(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_set_event_mask_page2 *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
memcpy(dev->event_mask_page2, cmd->mask, 8);
|
||
cmd_complete(dev, BT_HCI_CMD_SET_EVENT_MASK_PAGE2, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_sync_train_params(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_sync_train_params rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.interval = cpu_to_le16(dev->sync_train_interval);
|
||
rsp.timeout = cpu_to_le32(dev->sync_train_timeout);
|
||
rsp.service_data = dev->sync_train_service_data;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_SYNC_TRAIN_PARAMS, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_sc_support(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_secure_conn_support rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.support = dev->secure_conn_support;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_SECURE_CONN_SUPPORT, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_write_sc_support(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct bt_hci_cmd_write_secure_conn_support *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
dev->secure_conn_support = cmd->support;
|
||
cmd_complete(dev, BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT, &status,
|
||
sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_read_auth_payload_timeout(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_write_auth_payload_timeout(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
/* TODO */
|
||
return -ENOTSUP;
|
||
}
|
||
|
||
static int cmd_read_local_oob_ext_data(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct bt_hci_rsp_read_local_oob_ext_data rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
cmd_complete(dev, BT_HCI_CMD_READ_LOCAL_OOB_EXT_DATA, &rsp,
|
||
sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define BT_BREDR_LE \
|
||
CMD(BT_HCI_CMD_SET_EVENT_MASK_PAGE2, cmd_set_event_mask_2, NULL), \
|
||
CMD(BT_HCI_CMD_READ_SYNC_TRAIN_PARAMS, cmd_read_sync_train_params, \
|
||
NULL), \
|
||
CMD(BT_HCI_CMD_READ_SECURE_CONN_SUPPORT, cmd_read_sc_support, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_SECURE_CONN_SUPPORT, cmd_write_sc_support, NULL), \
|
||
CMD(BT_HCI_CMD_READ_AUTH_PAYLOAD_TIMEOUT, \
|
||
cmd_read_auth_payload_timeout, NULL), \
|
||
CMD(BT_HCI_CMD_WRITE_AUTH_PAYLOAD_TIMEOUT, \
|
||
cmd_write_auth_payload_timeout, NULL), \
|
||
CMD(BT_HCI_CMD_READ_LOCAL_OOB_EXT_DATA, \
|
||
cmd_read_local_oob_ext_data, NULL)
|
||
|
||
static const struct btdev_cmd cmd_bredr_le[] = {
|
||
CMD_COMMON_ALL,
|
||
CMD_COMMON_BREDR_LE,
|
||
CMD_COMMON_BREDR_20,
|
||
CMD_BREDR,
|
||
CMD_LE,
|
||
CMD_LE_50,
|
||
CMD_LE_52,
|
||
BT_BREDR_LE,
|
||
{}
|
||
};
|
||
|
||
static void set_bredrle_commands(struct btdev *btdev)
|
||
{
|
||
set_bredr_commands(btdev);
|
||
set_le_commands(btdev);
|
||
|
||
/* Extra BR/EDR commands we want to only support for >= 4.0
|
||
* adapters.
|
||
*/
|
||
btdev->commands[22] |= 0x04; /* Set Event Mask Page 2 */
|
||
btdev->commands[31] |= 0x80; /* Read Sync Train Parameters */
|
||
btdev->commands[32] |= 0x04; /* Read Secure Connections Support */
|
||
btdev->commands[32] |= 0x08; /* Write Secure Connections Support */
|
||
btdev->commands[32] |= 0x10; /* Read Auth Payload Timeout */
|
||
btdev->commands[32] |= 0x20; /* Write Auth Payload Timeout */
|
||
btdev->commands[32] |= 0x40; /* Read Local OOB Extended Data */
|
||
btdev->cmds = cmd_bredr_le;
|
||
}
|
||
|
||
static void set_amp_commands(struct btdev *btdev)
|
||
{
|
||
set_common_commands_all(btdev);
|
||
|
||
btdev->commands[22] |= 0x20; /* Read Local AMP Info */
|
||
}
|
||
|
||
static void set_bredrle_features(struct btdev *btdev)
|
||
{
|
||
btdev->features[0] |= 0x04; /* Encryption */
|
||
btdev->features[0] |= 0x20; /* Role switch */
|
||
btdev->features[0] |= 0x80; /* Sniff mode */
|
||
btdev->features[1] |= 0x08; /* SCO link */
|
||
btdev->features[2] |= 0x08; /* Transparent SCO */
|
||
btdev->features[3] |= 0x40; /* RSSI with inquiry results */
|
||
btdev->features[3] |= 0x80; /* Extended SCO link */
|
||
btdev->features[4] |= 0x08; /* AFH capable peripheral */
|
||
btdev->features[4] |= 0x10; /* AFH classification peripheral */
|
||
btdev->features[4] |= 0x40; /* LE Supported */
|
||
btdev->features[5] |= 0x02; /* Sniff subrating */
|
||
btdev->features[5] |= 0x04; /* Pause encryption */
|
||
btdev->features[5] |= 0x08; /* AFH capable central */
|
||
btdev->features[5] |= 0x10; /* AFH classification central */
|
||
btdev->features[6] |= 0x01; /* Extended Inquiry Response */
|
||
btdev->features[6] |= 0x02; /* Simultaneous LE and BR/EDR */
|
||
btdev->features[6] |= 0x08; /* Secure Simple Pairing */
|
||
btdev->features[6] |= 0x10; /* Encapsulated PDU */
|
||
btdev->features[6] |= 0x20; /* Erroneous Data Reporting */
|
||
btdev->features[6] |= 0x40; /* Non-flushable Packet Boundary Flag */
|
||
btdev->features[7] |= 0x01; /* Link Supervision Timeout Event */
|
||
btdev->features[7] |= 0x02; /* Inquiry TX Power Level */
|
||
btdev->features[7] |= 0x80; /* Extended features */
|
||
|
||
if (btdev->type >= BTDEV_TYPE_BREDRLE50) {
|
||
/* These BREDR features are added to test new configuration
|
||
* command. If this is added above it will break existing tests
|
||
*/
|
||
btdev->features[0] |= 0x01; /* 3 slot Packets */
|
||
btdev->features[0] |= 0x02; /* 5 slot Packets */
|
||
btdev->features[3] |= 0x02; /* EDR ACL 2M mode */
|
||
btdev->features[3] |= 0x04; /* EDR ACL 3M mode */
|
||
btdev->features[4] |= 0x80; /* 3 slot EDR ACL packets */
|
||
btdev->features[5] |= 0x01; /* 5 slot EDR ACL packets */
|
||
|
||
btdev->le_features[0] |= 0x40; /* LE PRIVACY */
|
||
btdev->le_features[1] |= 0x01; /* LE 2M PHY */
|
||
btdev->le_features[1] |= 0x08; /* LE Coded PHY */
|
||
btdev->le_features[1] |= 0x10; /* LE EXT ADV */
|
||
}
|
||
|
||
if (btdev->type >= BTDEV_TYPE_BREDRLE52) {
|
||
btdev->le_features[1] |= 0x20; /* LE PER ADV */
|
||
btdev->le_features[3] |= 0x10; /* LE CIS Central */
|
||
btdev->le_features[3] |= 0x20; /* LE CIS Peripheral */
|
||
btdev->le_features[3] |= 0x40; /* LE ISO Broadcaster */
|
||
btdev->le_features[3] |= 0x80; /* LE Synchronized Receiver */
|
||
btdev->le_features[4] |= 0x01; /* LE ISO channels */
|
||
}
|
||
|
||
btdev->feat_page_2[0] |= 0x01; /* CPB - Central Operation */
|
||
btdev->feat_page_2[0] |= 0x02; /* CPB - Peripheral Operation */
|
||
btdev->feat_page_2[0] |= 0x04; /* Synchronization Train */
|
||
btdev->feat_page_2[0] |= 0x08; /* Synchronization Scan */
|
||
btdev->feat_page_2[0] |= 0x10; /* Inquiry Response Notification */
|
||
btdev->feat_page_2[1] |= 0x01; /* Secure Connections */
|
||
btdev->feat_page_2[1] |= 0x02; /* Ping */
|
||
|
||
btdev->max_page = 2;
|
||
}
|
||
|
||
static void set_bredr_features(struct btdev *btdev)
|
||
{
|
||
btdev->features[0] |= 0x04; /* Encryption */
|
||
btdev->features[0] |= 0x20; /* Role switch */
|
||
btdev->features[0] |= 0x80; /* Sniff mode */
|
||
btdev->features[1] |= 0x08; /* SCO link */
|
||
btdev->features[3] |= 0x40; /* RSSI with inquiry results */
|
||
btdev->features[3] |= 0x80; /* Extended SCO link */
|
||
btdev->features[4] |= 0x08; /* AFH capable peripheral */
|
||
btdev->features[4] |= 0x10; /* AFH classification peripheral */
|
||
btdev->features[5] |= 0x02; /* Sniff subrating */
|
||
btdev->features[5] |= 0x04; /* Pause encryption */
|
||
btdev->features[5] |= 0x08; /* AFH capable central */
|
||
btdev->features[5] |= 0x10; /* AFH classification central */
|
||
btdev->features[6] |= 0x01; /* Extended Inquiry Response */
|
||
btdev->features[6] |= 0x08; /* Secure Simple Pairing */
|
||
btdev->features[6] |= 0x10; /* Encapsulated PDU */
|
||
btdev->features[6] |= 0x20; /* Erroneous Data Reporting */
|
||
btdev->features[6] |= 0x40; /* Non-flushable Packet Boundary Flag */
|
||
btdev->features[7] |= 0x01; /* Link Supervision Timeout Event */
|
||
btdev->features[7] |= 0x02; /* Inquiry TX Power Level */
|
||
btdev->features[7] |= 0x80; /* Extended features */
|
||
|
||
btdev->max_page = 1;
|
||
}
|
||
|
||
static void set_bredr20_features(struct btdev *btdev)
|
||
{
|
||
btdev->features[0] |= 0x04; /* Encryption */
|
||
btdev->features[0] |= 0x20; /* Role switch */
|
||
btdev->features[0] |= 0x80; /* Sniff mode */
|
||
btdev->features[1] |= 0x08; /* SCO link */
|
||
btdev->features[3] |= 0x40; /* RSSI with inquiry results */
|
||
btdev->features[3] |= 0x80; /* Extended SCO link */
|
||
btdev->features[4] |= 0x08; /* AFH capable peripheral */
|
||
btdev->features[4] |= 0x10; /* AFH classification peripheral */
|
||
btdev->features[5] |= 0x02; /* Sniff subrating */
|
||
btdev->features[5] |= 0x04; /* Pause encryption */
|
||
btdev->features[5] |= 0x08; /* AFH capable central */
|
||
btdev->features[5] |= 0x10; /* AFH classification central */
|
||
btdev->features[7] |= 0x80; /* Extended features */
|
||
|
||
btdev->max_page = 1;
|
||
}
|
||
|
||
static void set_le_features(struct btdev *btdev)
|
||
{
|
||
btdev->features[4] |= 0x20; /* BR/EDR Not Supported */
|
||
btdev->features[4] |= 0x40; /* LE Supported */
|
||
|
||
btdev->max_page = 1;
|
||
|
||
btdev->le_features[0] |= 0x01; /* LE Encryption */
|
||
btdev->le_features[0] |= 0x02; /* Connection Parameters Request */
|
||
btdev->le_features[0] |= 0x08; /* Peripheral-initd Features Exchange */
|
||
}
|
||
|
||
static void set_le_states(struct btdev *btdev)
|
||
{
|
||
/* Set all 41 bits as per Bluetooth 5.0 specification */
|
||
btdev->le_states[0] = 0xff;
|
||
btdev->le_states[1] = 0xff;
|
||
btdev->le_states[2] = 0xff;
|
||
btdev->le_states[3] = 0xff;
|
||
btdev->le_states[4] = 0xff;
|
||
btdev->le_states[5] = 0x03;
|
||
|
||
al_clear(btdev);
|
||
rl_clear(btdev);
|
||
btdev->le_rl_enable = 0x00;
|
||
btdev->le_rl_timeout = 0x0384; /* 900 secs or 15 minutes */
|
||
}
|
||
|
||
static void set_amp_features(struct btdev *btdev)
|
||
{
|
||
}
|
||
|
||
struct btdev *btdev_create(enum btdev_type type, uint16_t id)
|
||
{
|
||
struct btdev *btdev;
|
||
int index;
|
||
unsigned int i;
|
||
|
||
btdev = malloc(sizeof(*btdev));
|
||
if (!btdev)
|
||
return NULL;
|
||
|
||
memset(btdev, 0, sizeof(*btdev));
|
||
|
||
if (type == BTDEV_TYPE_BREDRLE || type == BTDEV_TYPE_LE ||
|
||
type == BTDEV_TYPE_BREDRLE50 ||
|
||
type == BTDEV_TYPE_BREDRLE52) {
|
||
btdev->crypto = bt_crypto_new();
|
||
if (!btdev->crypto) {
|
||
free(btdev);
|
||
return NULL;
|
||
}
|
||
}
|
||
|
||
btdev->type = type;
|
||
btdev->id = id;
|
||
btdev->manufacturer = 1521;
|
||
btdev->revision = 0x0000;
|
||
|
||
switch (btdev->type) {
|
||
case BTDEV_TYPE_BREDRLE:
|
||
case BTDEV_TYPE_BREDRLE50:
|
||
case BTDEV_TYPE_BREDRLE52:
|
||
btdev->version = 0x09;
|
||
set_bredrle_features(btdev);
|
||
set_bredrle_commands(btdev);
|
||
set_le_states(btdev);
|
||
break;
|
||
case BTDEV_TYPE_BREDR:
|
||
btdev->version = 0x05;
|
||
set_bredr_features(btdev);
|
||
set_bredr_commands(btdev);
|
||
break;
|
||
case BTDEV_TYPE_LE:
|
||
btdev->version = 0x09;
|
||
set_le_features(btdev);
|
||
set_le_commands(btdev);
|
||
set_le_states(btdev);
|
||
break;
|
||
case BTDEV_TYPE_AMP:
|
||
btdev->version = 0x01;
|
||
set_amp_features(btdev);
|
||
set_amp_commands(btdev);
|
||
break;
|
||
case BTDEV_TYPE_BREDR20:
|
||
btdev->version = 0x03;
|
||
set_bredr20_features(btdev);
|
||
set_bredr20_commands(btdev);
|
||
break;
|
||
}
|
||
|
||
btdev->page_scan_interval = 0x0800;
|
||
btdev->page_scan_window = 0x0012;
|
||
btdev->page_scan_type = 0x00;
|
||
|
||
btdev->sync_train_interval = 0x0080;
|
||
btdev->sync_train_timeout = 0x0002ee00;
|
||
btdev->sync_train_service_data = 0x00;
|
||
|
||
btdev->acl_mtu = 192;
|
||
btdev->acl_max_pkt = 1;
|
||
|
||
btdev->sco_mtu = 72;
|
||
btdev->sco_max_pkt = 1;
|
||
|
||
btdev->iso_mtu = 251;
|
||
btdev->iso_max_pkt = 1;
|
||
|
||
for (i = 0; i < ARRAY_SIZE(btdev->le_cig); ++i)
|
||
btdev->le_cig[i].params.cig_id = 0xff;
|
||
|
||
btdev->country_code = 0x00;
|
||
|
||
index = add_btdev(btdev);
|
||
if (index < 0) {
|
||
bt_crypto_unref(btdev->crypto);
|
||
free(btdev);
|
||
return NULL;
|
||
}
|
||
|
||
get_bdaddr(id, index, btdev->bdaddr);
|
||
|
||
btdev->conns = queue_new();
|
||
btdev->le_ext_adv = queue_new();
|
||
btdev->le_per_adv = queue_new();
|
||
btdev->le_big = queue_new();
|
||
|
||
btdev->le_al_len = AL_SIZE;
|
||
btdev->le_rl_len = RL_SIZE;
|
||
return btdev;
|
||
}
|
||
|
||
void btdev_destroy(struct btdev *btdev)
|
||
{
|
||
if (!btdev)
|
||
return;
|
||
|
||
if (btdev->inquiry_id > 0)
|
||
timeout_remove(btdev->inquiry_id);
|
||
|
||
bt_crypto_unref(btdev->crypto);
|
||
del_btdev(btdev);
|
||
|
||
queue_destroy(btdev->conns, conn_remove);
|
||
queue_destroy(btdev->le_ext_adv, le_ext_adv_free);
|
||
queue_destroy(btdev->le_per_adv, free);
|
||
queue_destroy(btdev->le_big, le_big_free);
|
||
|
||
free(btdev);
|
||
}
|
||
|
||
bool btdev_set_debug(struct btdev *btdev, btdev_debug_func_t callback,
|
||
void *user_data, btdev_destroy_func_t destroy)
|
||
{
|
||
if (!btdev)
|
||
return false;
|
||
|
||
if (btdev->debug_destroy)
|
||
btdev->debug_destroy(btdev->debug_data);
|
||
|
||
btdev->debug_callback = callback;
|
||
btdev->debug_destroy = destroy;
|
||
btdev->debug_data = user_data;
|
||
|
||
return true;
|
||
}
|
||
|
||
const uint8_t *btdev_get_bdaddr(struct btdev *btdev)
|
||
{
|
||
return btdev->bdaddr;
|
||
}
|
||
|
||
bool btdev_set_bdaddr(struct btdev *btdev, const uint8_t *bdaddr)
|
||
{
|
||
if (!btdev || !bdaddr)
|
||
return false;
|
||
|
||
memcpy(btdev->bdaddr, bdaddr, sizeof(btdev->bdaddr));
|
||
|
||
return true;
|
||
}
|
||
|
||
uint8_t *btdev_get_features(struct btdev *btdev)
|
||
{
|
||
return btdev->features;
|
||
}
|
||
|
||
uint8_t btdev_get_scan_enable(struct btdev *btdev)
|
||
{
|
||
return btdev->scan_enable;
|
||
}
|
||
|
||
uint8_t btdev_get_le_scan_enable(struct btdev *btdev)
|
||
{
|
||
return btdev->le_scan_enable;
|
||
}
|
||
|
||
const uint8_t *btdev_get_adv_addr(struct btdev *btdev, uint8_t handle)
|
||
{
|
||
struct le_ext_adv *ext_adv;
|
||
|
||
/* Check if Ext Adv is already existed */
|
||
ext_adv = queue_find(btdev->le_ext_adv, match_ext_adv_handle,
|
||
UINT_TO_PTR(handle));
|
||
if (!ext_adv)
|
||
return NULL;
|
||
|
||
return ext_adv_addr(btdev, ext_adv);
|
||
}
|
||
|
||
void btdev_set_le_states(struct btdev *btdev, const uint8_t *le_states)
|
||
{
|
||
memcpy(btdev->le_states, le_states, sizeof(btdev->le_states));
|
||
}
|
||
|
||
|
||
void btdev_set_command_handler(struct btdev *btdev, btdev_command_func handler,
|
||
void *user_data)
|
||
{
|
||
if (!btdev)
|
||
return;
|
||
|
||
btdev->command_handler = handler;
|
||
btdev->command_data = user_data;
|
||
}
|
||
|
||
void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
|
||
void *user_data)
|
||
{
|
||
if (!btdev)
|
||
return;
|
||
|
||
btdev->send_handler = handler;
|
||
btdev->send_data = user_data;
|
||
}
|
||
|
||
static void num_completed_packets(struct btdev *btdev, uint16_t handle)
|
||
{
|
||
struct btdev_conn *conn;
|
||
|
||
conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
|
||
if (conn) {
|
||
struct bt_hci_evt_num_completed_packets ncp;
|
||
|
||
ncp.num_handles = 1;
|
||
ncp.handle = cpu_to_le16(handle);
|
||
ncp.count = cpu_to_le16(1);
|
||
|
||
send_event(btdev, BT_HCI_EVT_NUM_COMPLETED_PACKETS,
|
||
&ncp, sizeof(ncp));
|
||
}
|
||
}
|
||
|
||
static const struct btdev_cmd *run_cmd(struct btdev *btdev,
|
||
const struct btdev_cmd *cmd,
|
||
const void *data, uint8_t len)
|
||
{
|
||
uint8_t status = BT_HCI_ERR_UNKNOWN_COMMAND;
|
||
int err;
|
||
|
||
err = cmd->func(btdev, data, len);
|
||
switch (err) {
|
||
case 0:
|
||
return cmd;
|
||
case -ENOTSUP:
|
||
status = BT_HCI_ERR_UNKNOWN_COMMAND;
|
||
break;
|
||
case -EINVAL:
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
break;
|
||
case -EPERM:
|
||
status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||
break;
|
||
case -EEXIST:
|
||
status = BT_HCI_ERR_CONN_ALREADY_EXISTS;
|
||
break;
|
||
case -ENOENT:
|
||
status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
break;
|
||
default:
|
||
status = BT_HCI_ERR_UNSPECIFIED_ERROR;
|
||
break;
|
||
}
|
||
|
||
cmd_status(btdev, status, cmd->opcode);
|
||
|
||
return NULL;
|
||
}
|
||
|
||
static const struct btdev_cmd *vnd_cmd(struct btdev *btdev, uint8_t op,
|
||
const struct btdev_cmd *cmd,
|
||
const void *data, uint8_t len)
|
||
{
|
||
uint8_t opcode = ((const uint8_t *)data)[0];
|
||
|
||
for (; cmd && cmd->func; cmd++) {
|
||
if (cmd->opcode != opcode)
|
||
continue;
|
||
|
||
return run_cmd(btdev, cmd, data, len);
|
||
}
|
||
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"Unsupported Vendor subcommand 0x%2.2x", opcode);
|
||
|
||
cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, op);
|
||
|
||
return NULL;
|
||
}
|
||
|
||
static const struct btdev_cmd *default_cmd(struct btdev *btdev, uint16_t opcode,
|
||
const void *data, uint8_t len)
|
||
{
|
||
const struct btdev_cmd *cmd;
|
||
|
||
if (btdev->emu_opcode == opcode)
|
||
return vnd_cmd(btdev, opcode, btdev->emu_cmds, data, len);
|
||
|
||
if (btdev->msft_opcode == opcode)
|
||
return vnd_cmd(btdev, opcode, btdev->msft_cmds, data, len);
|
||
|
||
for (cmd = btdev->cmds; cmd->func; cmd++) {
|
||
if (cmd->opcode != opcode)
|
||
continue;
|
||
|
||
return run_cmd(btdev, cmd, data, len);
|
||
}
|
||
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"Unsupported command 0x%4.4x", opcode);
|
||
|
||
cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
|
||
|
||
return NULL;
|
||
}
|
||
|
||
struct btdev_callback {
|
||
void (*function)(btdev_callback callback, uint8_t response,
|
||
uint8_t status, const void *data, uint8_t len);
|
||
void *user_data;
|
||
uint16_t opcode;
|
||
const void *data;
|
||
uint8_t len;
|
||
};
|
||
|
||
void btdev_command_response(btdev_callback callback, uint8_t response,
|
||
uint8_t status, const void *data, uint8_t len)
|
||
{
|
||
callback->function(callback, response, status, data, len);
|
||
}
|
||
|
||
static void handler_callback(btdev_callback callback, uint8_t response,
|
||
uint8_t status, const void *data, uint8_t len)
|
||
{
|
||
struct btdev *btdev = callback->user_data;
|
||
const struct btdev_cmd *cmd;
|
||
|
||
switch (response) {
|
||
case BTDEV_RESPONSE_DEFAULT:
|
||
if (!run_hooks(btdev, BTDEV_HOOK_PRE_CMD, callback->opcode,
|
||
callback->data, callback->len))
|
||
return;
|
||
|
||
cmd = default_cmd(btdev, callback->opcode,
|
||
callback->data, callback->len);
|
||
if (!cmd)
|
||
return;
|
||
|
||
if (!run_hooks(btdev, BTDEV_HOOK_PRE_EVT, callback->opcode,
|
||
callback->data, callback->len))
|
||
return;
|
||
|
||
if (cmd->complete)
|
||
cmd->complete(btdev, callback->data, callback->len);
|
||
|
||
break;
|
||
case BTDEV_RESPONSE_COMMAND_STATUS:
|
||
cmd_status(btdev, status, callback->opcode);
|
||
break;
|
||
case BTDEV_RESPONSE_COMMAND_COMPLETE:
|
||
cmd_complete(btdev, callback->opcode, data, len);
|
||
break;
|
||
default:
|
||
cmd_status(btdev, BT_HCI_ERR_UNKNOWN_COMMAND,
|
||
callback->opcode);
|
||
break;
|
||
}
|
||
}
|
||
|
||
static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
|
||
{
|
||
struct btdev_callback callback;
|
||
const struct bt_hci_cmd_hdr *hdr = data;
|
||
const struct btdev_cmd *cmd;
|
||
|
||
if (len < sizeof(*hdr))
|
||
return;
|
||
|
||
callback.function = handler_callback;
|
||
callback.user_data = btdev;
|
||
callback.opcode = le16_to_cpu(hdr->opcode);
|
||
callback.data = data + sizeof(*hdr);
|
||
callback.len = hdr->plen;
|
||
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"command 0x%04x", callback.opcode);
|
||
|
||
if (btdev->command_handler)
|
||
btdev->command_handler(callback.opcode,
|
||
callback.data, callback.len,
|
||
&callback, btdev->command_data);
|
||
else {
|
||
if (!run_hooks(btdev, BTDEV_HOOK_PRE_CMD, callback.opcode,
|
||
callback.data, callback.len))
|
||
return;
|
||
|
||
cmd = default_cmd(btdev, callback.opcode,
|
||
callback.data, callback.len);
|
||
if (!cmd)
|
||
return;
|
||
|
||
if (!run_hooks(btdev, BTDEV_HOOK_PRE_EVT, callback.opcode,
|
||
callback.data, callback.len))
|
||
return;
|
||
|
||
if (cmd->complete)
|
||
cmd->complete(btdev, callback.data, callback.len);
|
||
}
|
||
}
|
||
|
||
static void send_acl(struct btdev *dev, const void *data, uint16_t len)
|
||
{
|
||
struct bt_hci_acl_hdr hdr;
|
||
struct iovec iov[3];
|
||
struct btdev_conn *conn;
|
||
uint8_t pkt_type = BT_H4_ACL_PKT;
|
||
|
||
/* Packet type */
|
||
iov[0].iov_base = &pkt_type;
|
||
iov[0].iov_len = sizeof(pkt_type);
|
||
|
||
memcpy(&hdr, data, sizeof(hdr));
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(acl_handle(hdr.handle)));
|
||
if (!conn)
|
||
return;
|
||
|
||
num_completed_packets(dev, conn->handle);
|
||
|
||
/* ACL_START_NO_FLUSH is only allowed from host to controller.
|
||
* From controller to host this should be converted to ACL_START.
|
||
*/
|
||
if (acl_flags(hdr.handle) == ACL_START_NO_FLUSH)
|
||
hdr.handle = acl_handle_pack(conn->handle, ACL_START);
|
||
|
||
iov[1].iov_base = &hdr;
|
||
iov[1].iov_len = sizeof(hdr);
|
||
|
||
iov[2].iov_base = (void *) (data + sizeof(hdr));
|
||
iov[2].iov_len = len - sizeof(hdr);
|
||
|
||
send_packet(conn->link->dev, iov, 3);
|
||
}
|
||
|
||
static void send_iso(struct btdev *dev, const void *data, uint16_t len)
|
||
{
|
||
struct bt_hci_acl_hdr *hdr;
|
||
struct iovec iov[2];
|
||
struct btdev_conn *conn;
|
||
uint8_t pkt_type = BT_H4_ISO_PKT;
|
||
|
||
/* Packet type */
|
||
iov[0].iov_base = &pkt_type;
|
||
iov[0].iov_len = sizeof(pkt_type);
|
||
|
||
iov[1].iov_base = hdr = (void *) (data);
|
||
iov[1].iov_len = len;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(acl_handle(hdr->handle)));
|
||
if (!conn)
|
||
return;
|
||
|
||
num_completed_packets(dev, conn->handle);
|
||
|
||
if (conn->link)
|
||
send_packet(conn->link->dev, iov, 2);
|
||
}
|
||
|
||
void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
|
||
{
|
||
uint8_t pkt_type;
|
||
|
||
if (!btdev)
|
||
return;
|
||
|
||
if (len < 1)
|
||
return;
|
||
|
||
util_hexdump('>', data, len, btdev->debug_callback,
|
||
btdev->debug_data);
|
||
|
||
pkt_type = ((const uint8_t *) data)[0];
|
||
|
||
switch (pkt_type) {
|
||
case BT_H4_CMD_PKT:
|
||
process_cmd(btdev, data + 1, len - 1);
|
||
break;
|
||
case BT_H4_ACL_PKT:
|
||
send_acl(btdev, data + 1, len - 1);
|
||
break;
|
||
case BT_H4_ISO_PKT:
|
||
send_iso(btdev, data + 1, len - 1);
|
||
break;
|
||
default:
|
||
util_debug(btdev->debug_callback, btdev->debug_data,
|
||
"Unsupported packet 0x%2.2x", pkt_type);
|
||
break;
|
||
}
|
||
}
|
||
|
||
int btdev_add_hook(struct btdev *btdev, enum btdev_hook_type type,
|
||
uint16_t opcode, btdev_hook_func handler,
|
||
void *user_data)
|
||
{
|
||
int i;
|
||
|
||
if (!btdev)
|
||
return -1;
|
||
|
||
if (get_hook_index(btdev, type, opcode) > 0)
|
||
return -1;
|
||
|
||
for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
|
||
if (btdev->hook_list[i] == NULL) {
|
||
btdev->hook_list[i] = malloc(sizeof(struct hook));
|
||
if (btdev->hook_list[i] == NULL)
|
||
return -1;
|
||
|
||
btdev->hook_list[i]->handler = handler;
|
||
btdev->hook_list[i]->user_data = user_data;
|
||
btdev->hook_list[i]->opcode = opcode;
|
||
btdev->hook_list[i]->type = type;
|
||
return i;
|
||
}
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
bool btdev_del_hook(struct btdev *btdev, enum btdev_hook_type type,
|
||
uint16_t opcode)
|
||
{
|
||
int i;
|
||
|
||
if (!btdev)
|
||
return false;
|
||
|
||
for (i = 0; i < MAX_HOOK_ENTRIES; i++) {
|
||
if (btdev->hook_list[i] == NULL)
|
||
continue;
|
||
|
||
if (btdev->hook_list[i]->type != type ||
|
||
btdev->hook_list[i]->opcode != opcode)
|
||
continue;
|
||
|
||
free(btdev->hook_list[i]);
|
||
btdev->hook_list[i] = NULL;
|
||
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
static int cmd_msft_read_features(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct msft_rsp_read_supported_features rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_READ_SUPPORTED_FEATURES;
|
||
rsp.features[0] = MSFT_MONITOR_BREDR_RSSI | MSFT_MONITOR_LE_RSSI |
|
||
MSFT_MONITOR_LE_LEGACY_RSSI |
|
||
MSFT_MONITOR_LE_ADV |
|
||
MSFT_MONITOR_SSP_VALIDATION |
|
||
MSFT_MONITOR_LE_ADV_CONTINUOS;
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_msft_monitor_rssi(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct msft_cmd_monitor_rssi *cmd = data;
|
||
struct msft_rsp_monitor_rssi rsp;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_MONITOR_RSSI;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn)
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_msft_cancel_monitor_rssi(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct msft_cmd_cancel_monitor_rssi *cmd = data;
|
||
struct msft_rsp_cancel_monitor_rssi rsp;
|
||
struct btdev_conn *conn;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_CANCEL_MONITOR_RSSI;
|
||
|
||
conn = queue_find(dev->conns, match_handle,
|
||
UINT_TO_PTR(le16_to_cpu(cmd->handle)));
|
||
if (!conn)
|
||
rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_msft_le_monitor_adv(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
const struct msft_cmd_le_monitor_adv *cmd = data;
|
||
struct msft_rsp_le_monitor_adv rsp;
|
||
static uint8_t handle;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_LE_MONITOR_ADV;
|
||
|
||
switch (cmd->type) {
|
||
case MSFT_LE_MONITOR_ADV_PATTERN:
|
||
case MSFT_LE_MONITOR_ADV_UUID:
|
||
case MSFT_LE_MONITOR_ADV_IRK:
|
||
case MSFT_LE_MONITOR_ADV_ADDR:
|
||
rsp.handle = handle++;
|
||
break;
|
||
default:
|
||
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
break;
|
||
}
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_msft_le_cancel_monitor_adv(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct msft_rsp_le_cancel_monitor_adv rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_LE_CANCEL_MONITOR_ADV;
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_msft_le_monitor_adv_enable(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct msft_rsp_le_cancel_monitor_adv rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_LE_MONITOR_ADV_ENABLE;
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_msft_read_abs_rssi(struct btdev *dev, const void *data,
|
||
uint8_t len)
|
||
{
|
||
struct msft_rsp_read_abs_rssi rsp;
|
||
|
||
memset(&rsp, 0, sizeof(rsp));
|
||
rsp.status = BT_HCI_ERR_SUCCESS;
|
||
rsp.subcmd = MSFT_SUBCMD_READ_ABS_RSSI;
|
||
|
||
cmd_complete(dev, dev->msft_opcode, &rsp, sizeof(rsp));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_MSFT \
|
||
CMD(MSFT_SUBCMD_READ_SUPPORTED_FEATURES, cmd_msft_read_features, \
|
||
NULL), \
|
||
CMD(MSFT_SUBCMD_MONITOR_RSSI, cmd_msft_monitor_rssi, NULL), \
|
||
CMD(MSFT_SUBCMD_CANCEL_MONITOR_RSSI, cmd_msft_cancel_monitor_rssi, \
|
||
NULL), \
|
||
CMD(MSFT_SUBCMD_LE_MONITOR_ADV, cmd_msft_le_monitor_adv, NULL), \
|
||
CMD(MSFT_SUBCMD_LE_CANCEL_MONITOR_ADV, cmd_msft_le_cancel_monitor_adv, \
|
||
NULL), \
|
||
CMD(MSFT_SUBCMD_LE_MONITOR_ADV_ENABLE, cmd_msft_le_monitor_adv_enable, \
|
||
NULL), \
|
||
CMD(MSFT_SUBCMD_READ_ABS_RSSI, cmd_msft_read_abs_rssi, NULL)
|
||
|
||
static const struct btdev_cmd cmd_msft[] = {
|
||
CMD_MSFT,
|
||
{}
|
||
};
|
||
|
||
int btdev_set_msft_opcode(struct btdev *btdev, uint16_t opcode)
|
||
{
|
||
if (!btdev)
|
||
return -EINVAL;
|
||
|
||
switch (btdev->type) {
|
||
case BTDEV_TYPE_BREDRLE:
|
||
case BTDEV_TYPE_BREDRLE50:
|
||
case BTDEV_TYPE_BREDRLE52:
|
||
btdev->msft_opcode = opcode;
|
||
btdev->msft_cmds = cmd_msft;
|
||
return 0;
|
||
case BTDEV_TYPE_BREDR:
|
||
case BTDEV_TYPE_LE:
|
||
case BTDEV_TYPE_AMP:
|
||
case BTDEV_TYPE_BREDR20:
|
||
default:
|
||
return -ENOTSUP;
|
||
}
|
||
}
|
||
|
||
int btdev_set_aosp_capable(struct btdev *btdev, bool enable)
|
||
{
|
||
if (!btdev)
|
||
return -EINVAL;
|
||
|
||
btdev->aosp_capable = enable;
|
||
|
||
return 0;
|
||
}
|
||
|
||
static int cmd_emu_test_event(struct btdev *dev, const void *data, uint8_t len)
|
||
{
|
||
const struct emu_cmd_test_event *cmd = data;
|
||
uint8_t status = BT_HCI_ERR_SUCCESS;
|
||
|
||
if (len < sizeof(*cmd)) {
|
||
status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||
goto done;
|
||
}
|
||
|
||
send_event(dev, cmd->evt, cmd->data, len - sizeof(*cmd));
|
||
|
||
done:
|
||
cmd_complete(dev, dev->emu_opcode, &status, sizeof(status));
|
||
|
||
return 0;
|
||
}
|
||
|
||
#define CMD_EMU \
|
||
CMD(EMU_SUBCMD_TEST_EVENT, cmd_emu_test_event, NULL)
|
||
|
||
static const struct btdev_cmd cmd_emu[] = {
|
||
CMD_EMU,
|
||
{}
|
||
};
|
||
|
||
int btdev_set_emu_opcode(struct btdev *btdev, uint16_t opcode)
|
||
{
|
||
if (!btdev)
|
||
return -EINVAL;
|
||
|
||
switch (btdev->type) {
|
||
case BTDEV_TYPE_BREDRLE:
|
||
case BTDEV_TYPE_BREDRLE50:
|
||
case BTDEV_TYPE_BREDRLE52:
|
||
btdev->emu_opcode = opcode;
|
||
btdev->emu_cmds = cmd_emu;
|
||
return 0;
|
||
case BTDEV_TYPE_BREDR:
|
||
case BTDEV_TYPE_LE:
|
||
case BTDEV_TYPE_AMP:
|
||
case BTDEV_TYPE_BREDR20:
|
||
default:
|
||
return -ENOTSUP;
|
||
}
|
||
}
|