mirror of
https://git.kernel.org/pub/scm/bluetooth/bluez.git
synced 2024-11-15 00:04:29 +08:00
btdev: support multiple CIG
Add support for more than one CIG simultaneously.
This commit is contained in:
Pauli Virtanen
Luiz Augusto von Dentz
parent
195b9abbae
commit
678265f37c
144
emulator/btdev.c
144
emulator/btdev.c
@ -42,6 +42,7 @@
|
||||
#define RL_SIZE 16
|
||||
#define CIS_SIZE 3
|
||||
#define BIS_SIZE 3
|
||||
#define CIG_SIZE 3
|
||||
|
||||
#define has_bredr(btdev) (!((btdev)->features[4] & 0x20))
|
||||
#define has_le(btdev) (!!((btdev)->features[4] & 0x40))
|
||||
@ -101,6 +102,11 @@ struct le_ext_adv {
|
||||
unsigned int id;
|
||||
};
|
||||
|
||||
struct le_cig {
|
||||
struct bt_hci_cmd_le_set_cig_params params;
|
||||
struct bt_hci_cis_params cis[CIS_SIZE];
|
||||
} __attribute__ ((packed));
|
||||
|
||||
struct btdev {
|
||||
enum btdev_type type;
|
||||
uint16_t id;
|
||||
@ -204,10 +210,7 @@ struct btdev {
|
||||
uint16_t le_pa_sync_handle;
|
||||
uint8_t big_handle;
|
||||
uint8_t le_ltk[16];
|
||||
struct {
|
||||
struct bt_hci_cmd_le_set_cig_params params;
|
||||
struct bt_hci_cis_params cis[CIS_SIZE];
|
||||
} __attribute__ ((packed)) le_cig;
|
||||
struct le_cig le_cig[CIG_SIZE];
|
||||
uint8_t le_iso_path[2];
|
||||
|
||||
/* Real time length of AL array */
|
||||
@ -5757,6 +5760,36 @@ static int cmd_read_iso_tx_sync(struct btdev *dev, const void *data,
|
||||
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)
|
||||
{
|
||||
@ -5766,12 +5799,13 @@ static int cmd_set_cig_params(struct btdev *dev, const void *data,
|
||||
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));
|
||||
|
||||
if (cmd->num_cis > ARRAY_SIZE(dev->le_cig.cis)) {
|
||||
if (cmd->num_cis > ARRAY_SIZE(dev->le_cig[0].cis)) {
|
||||
rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
|
||||
goto done;
|
||||
}
|
||||
@ -5820,14 +5854,14 @@ static int cmd_set_cig_params(struct btdev *dev, const void *data,
|
||||
goto done;
|
||||
}
|
||||
|
||||
if (dev->le_cig.params.cig_id != 0xff &&
|
||||
dev->le_cig.params.cig_id != cmd->cig_id) {
|
||||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||||
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;
|
||||
}
|
||||
|
||||
memcpy(&dev->le_cig, data, len);
|
||||
|
||||
rsp.params.status = BT_HCI_ERR_SUCCESS;
|
||||
rsp.params.cig_id = cmd->cig_id;
|
||||
|
||||
@ -5835,7 +5869,7 @@ static int cmd_set_cig_params(struct btdev *dev, const void *data,
|
||||
struct btdev_conn *iso;
|
||||
|
||||
rsp.params.num_handles++;
|
||||
rsp.handle[i] = cpu_to_le16(ISO_HANDLE + i);
|
||||
rsp.handle[i] = cpu_to_le16(make_cis_handle(cig_idx, i));
|
||||
|
||||
/* BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 4, Part E
|
||||
* page 2553
|
||||
@ -5845,14 +5879,16 @@ static int cmd_set_cig_params(struct btdev *dev, const void *data,
|
||||
* code Command Disallowed (0x0C).
|
||||
*/
|
||||
iso = queue_find(dev->conns, match_handle,
|
||||
UINT_TO_PTR(cpu_to_le16(rsp.handle[i])));
|
||||
UINT_TO_PTR(le16_to_cpu(rsp.handle[i])));
|
||||
if (iso) {
|
||||
rsp.params.status = BT_HCI_ERR_INVALID_PARAMETERS;
|
||||
rsp.params.status = BT_HCI_ERR_COMMAND_DISALLOWED;
|
||||
i = 0;
|
||||
goto done;
|
||||
}
|
||||
}
|
||||
|
||||
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)));
|
||||
@ -5878,35 +5914,40 @@ 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;
|
||||
|
||||
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;
|
||||
int i = conn->handle - ISO_HANDLE;
|
||||
struct le_cig *le_cig = &remote->le_cig[cig_idx];
|
||||
|
||||
/* TODO: Figure out if these values makes sense */
|
||||
memcpy(evt.cig_sync_delay, remote->le_cig.params.c_interval,
|
||||
sizeof(remote->le_cig.params.c_interval));
|
||||
memcpy(evt.cis_sync_delay, remote->le_cig.params.p_interval,
|
||||
sizeof(remote->le_cig.params.p_interval));
|
||||
memcpy(evt.c_latency, &remote->le_cig.params.c_interval,
|
||||
sizeof(remote->le_cig.params.c_interval));
|
||||
memcpy(evt.p_latency, &remote->le_cig.params.p_interval,
|
||||
sizeof(remote->le_cig.params.p_interval));
|
||||
evt.c_phy = remote->le_cig.cis[i].c_phy;
|
||||
evt.p_phy = remote->le_cig.cis[i].p_phy;
|
||||
memcpy(evt.cig_sync_delay, le_cig->params.c_interval,
|
||||
sizeof(le_cig->params.c_interval));
|
||||
memcpy(evt.cis_sync_delay, le_cig->params.p_interval,
|
||||
sizeof(le_cig->params.p_interval));
|
||||
memcpy(evt.c_latency, &le_cig->params.c_interval,
|
||||
sizeof(le_cig->params.c_interval));
|
||||
memcpy(evt.p_latency, &le_cig->params.p_interval,
|
||||
sizeof(le_cig->params.p_interval));
|
||||
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 = 0x01;
|
||||
evt.p_ft = 0x01;
|
||||
evt.c_mtu = remote->le_cig.cis[i].c_sdu;
|
||||
evt.p_mtu = remote->le_cig.cis[i].p_sdu;
|
||||
evt.interval = remote->le_cig.params.c_latency;
|
||||
evt.c_mtu = le_cig->cis[cis_idx].c_sdu;
|
||||
evt.p_mtu = le_cig->cis[cis_idx].p_sdu;
|
||||
evt.interval = le_cig->params.c_latency;
|
||||
}
|
||||
|
||||
le_meta_event(dev, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt, sizeof(evt));
|
||||
@ -5927,9 +5968,20 @@ static int cmd_create_cis_complete(struct btdev *dev, const void *data,
|
||||
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(cpu_to_le16(cis->acl_handle)));
|
||||
UINT_TO_PTR(le16_to_cpu(cis->acl_handle)));
|
||||
if (!acl) {
|
||||
le_cis_estabilished(dev, NULL,
|
||||
BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||||
@ -5937,9 +5989,9 @@ static int cmd_create_cis_complete(struct btdev *dev, const void *data,
|
||||
}
|
||||
|
||||
iso = queue_find(dev->conns, match_handle,
|
||||
UINT_TO_PTR(cpu_to_le16(cis->cis_handle)));
|
||||
UINT_TO_PTR(le16_to_cpu(cis->cis_handle)));
|
||||
if (!iso) {
|
||||
iso = conn_add_cis(acl, cpu_to_le16(cis->cis_handle));
|
||||
iso = conn_add_cis(acl, le16_to_cpu(cis->cis_handle));
|
||||
if (!iso) {
|
||||
le_cis_estabilished(dev, NULL,
|
||||
BT_HCI_ERR_UNKNOWN_CONN_ID);
|
||||
@ -5949,8 +6001,8 @@ static int cmd_create_cis_complete(struct btdev *dev, const void *data,
|
||||
|
||||
evt.acl_handle = cpu_to_le16(acl->handle);
|
||||
evt.cis_handle = cpu_to_le16(iso->handle);
|
||||
evt.cig_id = iso->dev->le_cig.params.cig_id;
|
||||
evt.cis_id = iso->dev->le_cig.cis[i].cis_id;
|
||||
evt.cig_id = le_cig->params.cig_id;
|
||||
evt.cis_id = le_cig->cis[cis_idx].cis_id;
|
||||
|
||||
le_meta_event(iso->link->dev, BT_HCI_EVT_LE_CIS_REQ, &evt,
|
||||
sizeof(evt));
|
||||
@ -5959,20 +6011,37 @@ static int cmd_create_cis_complete(struct btdev *dev, const void *data,
|
||||
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(&dev->le_cig, 0, sizeof(dev->le_cig));
|
||||
memset(&rsp, 0, sizeof(rsp));
|
||||
|
||||
rsp.cig_id = cmd->cig_id;
|
||||
|
||||
if (dev->le_cig.params.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
|
||||
} 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));
|
||||
|
||||
@ -6842,6 +6911,7 @@ 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)
|
||||
@ -6909,9 +6979,11 @@ struct btdev *btdev_create(enum btdev_type type, uint16_t id)
|
||||
|
||||
btdev->iso_mtu = 251;
|
||||
btdev->iso_max_pkt = 1;
|
||||
btdev->le_cig.params.cig_id = 0xff;
|
||||
btdev->big_handle = 0xff;
|
||||
|
||||
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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user