From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
This adds support for assigning different handles for connection
instead of always using fixed handles.
---
emulator/btdev.c | 930 +++++++++++++++++++++++++++++++----------------
lib/hci.h | 1 +
2 files changed, 610 insertions(+), 321 deletions(-)
diff --git a/emulator/btdev.c b/emulator/btdev.c
index a26d73865..ae382d9b9 100644
--- a/emulator/btdev.c
+++ b/emulator/btdev.c
@@ -29,6 +29,7 @@
#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 "btdev.h"
@@ -36,7 +37,7 @@
#define has_le(btdev) (!!((btdev)->features[4] & 0x40))
#define ACL_HANDLE 42
-#define ISO_HANDLE 44
+#define ISO_HANDLE 257
#define SCO_HANDLE 257
struct hook {
@@ -48,10 +49,17 @@ struct hook {
#define MAX_HOOK_ENTRIES 16
+struct btdev_conn {
+ uint16_t handle;
+ uint8_t type;
+ struct btdev *dev;
+ struct btdev_conn *link;
+};
+
struct btdev {
enum btdev_type type;
- struct btdev *conn;
+ struct queue *conns;
bool auth_init;
uint8_t link_key[16];
@@ -520,36 +528,70 @@ static void send_event(struct btdev *btdev, uint8_t event,
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 conn_unlink(struct btdev_conn *conn1, struct btdev_conn *conn2)
+{
+ conn1->link = NULL;
+ conn2->link = NULL;
+}
+
+static void conn_remove(struct btdev_conn *conn)
+{
+ 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);
+}
+
+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 *remote = dev->conn;
+ struct btdev_conn *conn;
memset(&rsp, 0, sizeof(rsp));
- if (!remote) {
- rsp.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
- rsp.handle = cpu_to_le16(cmd->handle);
- rsp.reason = 0x00;
-
- send_event(dev, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp,
- 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;
}
- rsp.status = BT_HCI_ERR_SUCCESS;
- rsp.handle = cpu_to_le16(cmd->handle);
- rsp.reason = cmd->reason;
+ disconnect_complete(dev, conn->handle, BT_HCI_ERR_SUCCESS, cmd->reason);
+ disconnect_complete(conn->link->dev, conn->link->handle,
+ BT_HCI_ERR_SUCCESS, cmd->reason);
- if (rsp.handle == ACL_HANDLE) {
- dev->conn = NULL;
- remote->conn = NULL;
- }
-
- send_event(dev, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp, sizeof(rsp));
- send_event(remote, BT_HCI_EVT_DISCONNECT_COMPLETE, &rsp, sizeof(rsp));
+ conn_remove(conn);
return 0;
}
@@ -566,15 +608,18 @@ static int cmd_remote_version_complete(struct btdev *dev, const void *data,
{
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));
- if (dev->conn) {
+ 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 = dev->conn->version;
- ev.manufacturer = cpu_to_le16(dev->conn->manufacturer);
- ev.lmp_subver = cpu_to_le16(dev->conn->revision);
+ 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);
@@ -860,27 +905,105 @@ static int cmd_create_conn(struct btdev *dev, const void *data, uint8_t len)
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 conn2 %p handle 0x%04x",
+ conn1, conn1->handle, 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_iso(struct btdev_conn *acl, uint16_t handle)
+{
+ return conn_link(acl->dev, acl->link->dev, handle, HCI_ISODATA_PKT);
+}
+
static void conn_complete(struct btdev *btdev,
const uint8_t *bdaddr, uint8_t status)
{
struct bt_hci_evt_conn_complete cc;
if (!status) {
- struct btdev *remote = find_btdev_by_bdaddr(bdaddr);
+ struct btdev_conn *conn;
- btdev->conn = remote;
- remote->conn = btdev;
+ conn = conn_add_acl(btdev, bdaddr, BDADDR_BREDR);
+ if (!conn)
+ return;
cc.status = status;
memcpy(cc.bdaddr, btdev->bdaddr, 6);
cc.encr_mode = 0x00;
- cc.handle = cpu_to_le16(ACL_HANDLE);
+ cc.handle = cpu_to_le16(conn->link->handle);
cc.link_type = 0x01;
- send_event(remote, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
+ send_event(conn->link->dev, BT_HCI_EVT_CONN_COMPLETE, &cc,
+ sizeof(cc));
- cc.handle = cpu_to_le16(ACL_HANDLE);
+ cc.handle = cpu_to_le16(conn->handle);
cc.link_type = 0x01;
} else {
cc.handle = cpu_to_le16(0x0000);
@@ -917,17 +1040,32 @@ static int cmd_create_conn_complete(struct btdev *dev, const void *data,
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;
- if (!dev->conn)
- return 0;
+ 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, dev->conn->bdaddr, 6);
- cc.handle = cpu_to_le16(SCO_HANDLE);
+ memcpy(cc.bdaddr, conn->link->dev->bdaddr, 6);
+ cc.handle = cpu_to_le16(conn->handle);
cc.link_type = 0x00;
cc.encr_mode = 0x00;
+done:
send_event(dev, BT_HCI_EVT_CONN_COMPLETE, &cc, sizeof(cc));
return 0;
@@ -1004,35 +1142,57 @@ static int cmd_link_key_reply(struct btdev *dev, const void *data, uint8_t len)
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 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));
+}
+
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 *remote = dev->conn;
- struct bt_hci_evt_auth_complete ev;
-
- memcpy(dev->link_key, cmd->link_key, 16);
+ struct btdev_conn *conn;
+ uint8_t status;
- if (!remote) {
- remote = find_btdev_by_bdaddr(cmd->bdaddr);
- if (!remote)
- return 0;
+ conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
+ if (!conn) {
+ status = BT_HCI_ERR_INVALID_PARAMETERS;
+ goto done;
}
- if (!memcmp(remote->link_key, LINK_KEY_NONE, 16)) {
- send_event(remote, BT_HCI_EVT_LINK_KEY_REQUEST, dev->bdaddr, 6);
+ 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;
}
- ev.handle = cpu_to_le16(ACL_HANDLE);
-
- if (!memcmp(dev->link_key, remote->link_key, 16))
- ev.status = BT_HCI_ERR_SUCCESS;
+ if (!memcmp(dev->link_key, conn->link->dev->link_key, 16))
+ status = BT_HCI_ERR_SUCCESS;
else
- ev.status = BT_HCI_ERR_AUTH_FAILURE;
+ status = BT_HCI_ERR_AUTH_FAILURE;
+
+done:
+ auth_complete(conn, status);
- send_event(dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
- send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
+ if (conn)
+ auth_complete(conn->link, status);
return 0;
}
@@ -1063,13 +1223,11 @@ 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 = dev->conn;
+ struct btdev *remote;
- if (!remote) {
- remote = find_btdev_by_bdaddr(cmd->bdaddr);
- if (!remote)
- return 0;
- }
+ 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;
@@ -1100,21 +1258,23 @@ static int cmd_pin_code_reply(struct btdev *dev, const void *data, uint8_t len)
return 0;
}
-static uint8_t get_link_key_type(struct btdev *btdev)
+static uint8_t get_link_key_type(struct btdev *btdev, const uint8_t *bdaddr)
{
- struct btdev *remote = btdev->conn;
+ struct btdev_conn *conn;
uint8_t auth, unauth;
- if (!remote)
+ conn = queue_find(btdev->conns, match_bdaddr, bdaddr);
+ if (!conn)
return 0x00;
if (!btdev->simple_pairing_mode)
return 0x00;
- if (btdev->ssp_debug_mode || remote->ssp_debug_mode)
+ if (btdev->ssp_debug_mode || conn->link->dev->ssp_debug_mode)
return 0x03;
- if (btdev->secure_conn_support && remote->secure_conn_support) {
+ if (btdev->secure_conn_support &&
+ conn->link->dev->secure_conn_support) {
unauth = 0x07;
auth = 0x08;
} else {
@@ -1122,18 +1282,18 @@ static uint8_t get_link_key_type(struct btdev *btdev)
auth = 0x05;
}
- if (btdev->io_cap == 0x03 || remote->io_cap == 0x03)
+ if (btdev->io_cap == 0x03 || conn->link->dev->io_cap == 0x03)
return unauth;
- if (!(btdev->auth_req & 0x01) && !(remote->auth_req & 0x01))
+ if (!(btdev->auth_req & 0x01) && !(conn->link->dev->auth_req & 0x01))
return unauth;
/* DisplayOnly only produces authenticated with KeyboardOnly */
- if (btdev->io_cap == 0x00 && remote->io_cap != 0x02)
+ if (btdev->io_cap == 0x00 && conn->link->dev->io_cap != 0x02)
return unauth;
/* DisplayOnly only produces authenticated with KeyboardOnly */
- if (remote->io_cap == 0x00 && btdev->io_cap != 0x02)
+ if (conn->link->dev->io_cap == 0x00 && btdev->io_cap != 0x02)
return unauth;
return auth;
@@ -1148,7 +1308,7 @@ static void link_key_notify(struct btdev *btdev, const uint8_t *bdaddr,
memcpy(ev.bdaddr, bdaddr, 6);
memcpy(ev.link_key, key, 16);
- ev.key_type = get_link_key_type(btdev);
+ ev.key_type = get_link_key_type(btdev, bdaddr);
send_event(btdev, BT_HCI_EVT_LINK_KEY_NOTIFY, &ev, sizeof(ev));
}
@@ -1157,14 +1317,17 @@ 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 bt_hci_evt_auth_complete ev;
- struct btdev *remote = dev->conn;
+ struct btdev *remote;
+ struct btdev_conn *conn;
+ uint8_t status;
- if (!remote) {
+ 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;
@@ -1173,8 +1336,8 @@ static int cmd_pin_code_reply_complete(struct btdev *dev, const void *data,
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));
+ send_event(remote, BT_HCI_EVT_PIN_CODE_REQUEST,
+ &pin_req, sizeof(pin_req));
return 0;
}
@@ -1182,17 +1345,15 @@ static int cmd_pin_code_reply_complete(struct btdev *dev, const void *data,
!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);
- ev.status = BT_HCI_ERR_SUCCESS;
+ status = BT_HCI_ERR_SUCCESS;
} else {
- ev.status = BT_HCI_ERR_AUTH_FAILURE;
+ status = BT_HCI_ERR_AUTH_FAILURE;
}
- if (remote->conn) {
- ev.handle = cpu_to_le16(ACL_HANDLE);
- send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
- } else {
- conn_complete(remote, dev->bdaddr, ev.status);
- }
+ if (conn)
+ auth_complete(conn->link, status);
+ else
+ conn_complete(remote, dev->bdaddr, status);
dev->pin_len = 0;
remote->pin_len = 0;
@@ -1218,27 +1379,25 @@ 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 bt_hci_evt_auth_complete ev;
- struct btdev *remote = dev->conn;
+ struct btdev *remote;
+ struct btdev_conn *conn;
+ uint8_t status;
- if (!remote) {
- remote = find_btdev_by_bdaddr(cmd->bdaddr);
- if (!remote)
- return 0;
- }
+ remote = find_btdev_by_bdaddr(cmd->bdaddr);
+ if (!remote)
+ return 0;
- ev.status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
- ev.handle = cpu_to_le16(ACL_HANDLE);
+ status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
- if (dev->conn)
- send_event(dev, BT_HCI_EVT_AUTH_COMPLETE, &ev, sizeof(ev));
+ 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 (remote->conn) {
+ if (conn) {
if (remote->pin_len)
- send_event(remote, BT_HCI_EVT_AUTH_COMPLETE, &ev,
- sizeof(ev));
+ auth_complete(conn->link, status);
} else {
conn_complete(remote, dev->bdaddr,
BT_HCI_ERR_PIN_OR_KEY_MISSING);
@@ -1258,9 +1417,11 @@ 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 *remote = dev->conn;
+ struct btdev_conn *conn;
- if (!remote) {
+ 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;
@@ -1273,7 +1434,8 @@ static int cmd_auth_requested_complete(struct btdev *dev, const void *data,
dev->auth_init = true;
- send_event(dev, BT_HCI_EVT_LINK_KEY_REQUEST, remote->bdaddr, 6);
+ send_event(dev, BT_HCI_EVT_LINK_KEY_REQUEST, conn->link->dev->bdaddr,
+ sizeof(conn->link->dev->bdaddr));
return 0;
}
@@ -1286,35 +1448,45 @@ static int cmd_set_conn_encrypt(struct btdev *dev, const void *data,
return 0;
}
-static void encrypt_change(struct btdev *btdev, uint8_t mode, uint8_t status)
+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(ACL_HANDLE);
+ ev.handle = cpu_to_le16(conn->handle);
ev.encr_mode = mode;
- send_event(btdev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
+ 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;
- if (!dev->conn)
+ 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 && dev->conn->secure_conn_support)
+ else if (dev->secure_conn_support &&
+ conn->link->dev->secure_conn_support)
mode = 0x02;
else
mode = 0x01;
- encrypt_change(dev, mode, BT_HCI_ERR_SUCCESS);
- encrypt_change(dev->conn, mode, BT_HCI_ERR_SUCCESS);
+ encrypt_change(conn, mode, BT_HCI_ERR_SUCCESS);
+ encrypt_change(conn->link, mode, BT_HCI_ERR_SUCCESS);
return 0;
}
@@ -1396,11 +1568,14 @@ static int cmd_read_remote_features_complete(struct btdev *dev,
{
const struct bt_hci_cmd_read_remote_features *cmd = data;
struct bt_hci_evt_remote_features_complete rfc;
+ struct btdev_conn *conn;
- if (dev->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, dev->conn->features, 8);
+ memcpy(rfc.features, conn->link->dev->features, 8);
} else {
rfc.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
rfc.handle = cpu_to_le16(cmd->handle);
@@ -1439,10 +1614,13 @@ static int cmd_read_remote_ext_features_compl(struct btdev *dev,
{
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));
- if (dev->conn && cmd->page < 0x02) {
+ 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;
@@ -1450,11 +1628,11 @@ static int cmd_read_remote_ext_features_compl(struct btdev *dev,
switch (cmd->page) {
case 0x00:
ev.status = BT_HCI_ERR_SUCCESS;
- memcpy(ev.features, dev->conn->features, 8);
+ memcpy(ev.features, conn->link->dev->features, 8);
break;
case 0x01:
ev.status = BT_HCI_ERR_SUCCESS;
- btdev_get_host_features(dev->conn, ev.features);
+ btdev_get_host_features(conn->link->dev, ev.features);
break;
default:
ev.status = BT_HCI_ERR_INVALID_PARAMETERS;
@@ -1488,10 +1666,13 @@ static int cmd_read_clock_offset_complete(struct btdev *dev, const void *data,
{
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));
- if (dev->conn) {
+ 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;
@@ -2142,20 +2323,38 @@ static void set_common_commands_bredr20(struct btdev *btdev)
}
static int cmd_setup_sync_conn(struct btdev *dev, const void *data, uint8_t len)
+{
+ cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_EVT_SYNC_CONN_COMPLETE);
+
+ 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;
- uint8_t status = BT_HCI_ERR_SUCCESS;
+ struct btdev_conn *conn;
- cmd_status(dev, status, BT_HCI_EVT_SYNC_CONN_COMPLETE);
+ memset(&cc, 0, sizeof(cc));
- if (!dev->conn)
- return 0;
+ 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;
+ }
- cc.status = status;
- memcpy(cc.bdaddr, dev->conn->bdaddr, 6);
+ conn = conn_add_sco(conn);
+ if (!conn) {
+ cc.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
+ goto done;
+ }
- cc.handle = cpu_to_le16(status == BT_HCI_ERR_SUCCESS ? SCO_HANDLE : 0);
+ 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;
@@ -2163,6 +2362,7 @@ static int cmd_setup_sync_conn(struct btdev *dev, const void *data, uint8_t len)
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;
@@ -2255,13 +2455,14 @@ static int cmd_write_inquiry_tx_power(struct btdev *dev, const void *data,
static int cmd_io_cap_reply(struct btdev *dev, const void *data, uint8_t len)
{
- struct btdev *remote = dev->conn;
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;
- if (!remote) {
+ conn = queue_find(dev->conns, match_bdaddr, cmd->bdaddr);
+ if (!conn) {
status = BT_HCI_ERR_UNKNOWN_CONN_ID;
goto done;
}
@@ -2276,22 +2477,23 @@ static int cmd_io_cap_reply(struct btdev *dev, const void *data, uint8_t len)
ev.oob_data = cmd->oob_data;
ev.authentication = cmd->authentication;
- send_event(remote, BT_HCI_EVT_IO_CAPABILITY_RESPONSE, &ev, sizeof(ev));
+ send_event(conn->link->dev, BT_HCI_EVT_IO_CAPABILITY_RESPONSE, &ev,
+ sizeof(ev));
- if (remote->io_cap) {
+ 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(remote, BT_HCI_EVT_USER_CONFIRM_REQUEST,
+ 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(remote, BT_HCI_EVT_IO_CAPABILITY_REQUEST,
+ send_event(conn->link->dev, BT_HCI_EVT_IO_CAPABILITY_REQUEST,
dev->bdaddr, 6);
}
@@ -2308,51 +2510,49 @@ 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 bt_hci_evt_auth_complete auth;
- struct btdev *remote = btdev->conn;
- struct btdev *init, *accp;
+ struct btdev_conn *conn;
+ struct btdev_conn *init, *accp;
- if (!remote)
+ conn = queue_find(btdev->conns, match_bdaddr, bdaddr);
+ if (!conn)
return;
btdev->ssp_status = status;
btdev->ssp_auth_complete = true;
- if (!remote->ssp_auth_complete && wait)
+ if (!conn->link->dev->ssp_auth_complete && wait)
return;
if (status == BT_HCI_ERR_SUCCESS &&
- remote->ssp_status != BT_HCI_ERR_SUCCESS)
- status = remote->ssp_status;
+ 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 = btdev;
- accp = remote;
+ init = conn;
+ accp = conn->link;
memcpy(iev.bdaddr, bdaddr, 6);
memcpy(aev.bdaddr, btdev->bdaddr, 6);
} else {
- init = remote;
- accp = btdev;
+ init = conn->link;
+ accp = conn;
memcpy(iev.bdaddr, btdev->bdaddr, 6);
memcpy(aev.bdaddr, bdaddr, 6);
}
- send_event(init, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &iev,
- sizeof(iev));
- send_event(accp, BT_HCI_EVT_SIMPLE_PAIRING_COMPLETE, &aev,
- sizeof(aev));
+ 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, iev.bdaddr, LINK_KEY_DUMMY);
- link_key_notify(accp, aev.bdaddr, LINK_KEY_DUMMY);
+ link_key_notify(init->dev, iev.bdaddr, LINK_KEY_DUMMY);
+ link_key_notify(accp->dev, aev.bdaddr, LINK_KEY_DUMMY);
}
- auth.status = status;
- auth.handle = cpu_to_le16(ACL_HANDLE);
- send_event(init, BT_HCI_EVT_AUTH_COMPLETE, &auth, sizeof(auth));
+ auth_complete(init, status);
}
static int cmd_user_confirm_reply(struct btdev *dev, const void *data,
@@ -2419,10 +2619,15 @@ static int cmd_read_encrypt_key_size(struct btdev *dev, const void *data,
{
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;
- if (dev->conn) {
+ 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 {
@@ -2485,7 +2690,8 @@ static int cmd_get_mws_transport_config(struct btdev *dev, const void *data,
}
#define CMD_BREDR \
- CMD(BT_HCI_CMD_SETUP_SYNC_CONN, cmd_setup_sync_conn, NULL), \
+ 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), \
@@ -2986,50 +3192,67 @@ static bool adv_connectable(struct btdev *btdev)
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)
{
- char buf[1 + sizeof(struct bt_hci_evt_le_conn_complete)];
- struct bt_hci_evt_le_conn_complete *cc = (void *) &buf[1];
+ struct bt_hci_evt_le_conn_complete cc;
- memset(buf, 0, sizeof(buf));
-
- buf[0] = BT_HCI_EVT_LE_CONN_COMPLETE;
+ memset(&cc, 0, sizeof(cc));
if (!status) {
- struct btdev *remote;
+ struct btdev_conn *conn;
- remote = find_btdev_by_bdaddr_type(lecc->peer_addr,
- lecc->peer_addr_type);
+ conn = conn_add_acl(btdev, lecc->peer_addr,
+ lecc->peer_addr_type);
+ if (!conn)
+ return;
- btdev->conn = remote;
btdev->le_adv_enable = 0;
- remote->conn = btdev;
- remote->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);
+ 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(ACL_HANDLE);
- cc->interval = lecc->max_interval;
- cc->latency = lecc->latency;
- cc->supv_timeout = lecc->supv_timeout;
-
- send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+ 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;
+ cc.status = status;
+ cc.peer_addr_type = lecc->peer_addr_type;
+ memcpy(cc.peer_addr, lecc->peer_addr, 6);
+ cc.role = 0x00;
- send_event(btdev, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+ 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)
@@ -3092,27 +3315,29 @@ static void le_conn_update(struct btdev *btdev, uint16_t handle,
uint16_t latency, uint16_t supv_timeout,
uint16_t min_length, uint16_t max_length)
{
- struct btdev *remote = btdev->conn;
- struct __packed {
- uint8_t subevent;
- struct bt_hci_evt_le_conn_update_complete ev;
- } ev;
+ struct bt_hci_evt_le_conn_update_complete ev;
+ struct btdev_conn *conn;
+
+ memset(&ev, 0, sizeof(ev));
- ev.subevent = BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE;
- ev.ev.handle = cpu_to_le16(handle);
- ev.ev.interval = cpu_to_le16(min_interval);
- ev.ev.latency = cpu_to_le16(latency);
- ev.ev.supv_timeout = cpu_to_le16(supv_timeout);
+ 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);
- if (remote)
- ev.ev.status = BT_HCI_ERR_SUCCESS;
+ conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
+ if (conn)
+ ev.status = BT_HCI_ERR_SUCCESS;
else
- ev.ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
+ ev.status = BT_HCI_ERR_UNKNOWN_CONN_ID;
- send_event(btdev, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
+ le_meta_event(btdev, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
+ sizeof(ev));
- if (remote)
- send_event(remote, BT_HCI_EVT_LE_META_EVENT, &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,
@@ -3120,23 +3345,23 @@ static void le_conn_param_req(struct btdev *btdev, uint16_t handle,
uint16_t latency, uint16_t supv_timeout,
uint16_t min_length, uint16_t max_length)
{
- struct btdev *remote = btdev->conn;
- struct __packed {
- uint8_t subevent;
- struct bt_hci_evt_le_conn_param_request ev;
- } ev;
+ struct bt_hci_evt_le_conn_param_request ev;
+ struct btdev_conn *conn;
- if (!remote)
+ conn = queue_find(btdev->conns, match_handle, UINT_TO_PTR(handle));
+ if (!conn)
return;
- ev.subevent = BT_HCI_EVT_LE_CONN_PARAM_REQUEST;
- ev.ev.handle = cpu_to_le16(handle);
- ev.ev.min_interval = cpu_to_le16(min_interval);
- ev.ev.max_interval = cpu_to_le16(max_interval);
- ev.ev.latency = cpu_to_le16(latency);
- ev.ev.supv_timeout = cpu_to_le16(supv_timeout);
+ 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);
- send_event(remote, BT_HCI_EVT_LE_META_EVENT, &ev, sizeof(ev));
+ 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,
@@ -3164,44 +3389,28 @@ static int cmd_conn_update_complete(struct btdev *dev, const void *data,
return 0;
}
-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 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 *remote = dev->conn;
+ struct btdev_conn *conn;
+ uint8_t status = BT_HCI_ERR_SUCCESS;
- if (!remote) {
- cmd_status(dev, BT_HCI_ERR_UNKNOWN_CONN_ID,
- BT_HCI_CMD_LE_READ_REMOTE_FEATURES);
- return 0;
- }
+ 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, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_READ_REMOTE_FEATURES);
+ 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(ACL_HANDLE);
- memcpy(ev.features, remote->le_features, 8);
+ 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));
@@ -3246,27 +3455,27 @@ static int cmd_rand(struct btdev *dev, const void *data, uint8_t len)
static int cmd_start_encrypt(struct btdev *dev, const void *data, uint8_t len)
{
const struct bt_hci_cmd_le_start_encrypt *cmd = data;
- char buf[1 + sizeof(struct bt_hci_evt_le_long_term_key_request)];
- struct bt_hci_evt_le_long_term_key_request *ev = (void *) &buf[1];
- struct btdev *remote = dev->conn;
+ struct bt_hci_evt_le_long_term_key_request ev;
+ struct btdev_conn *conn;
- if (!remote) {
+ 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);
+ BT_HCI_CMD_LE_START_ENCRYPT);
return 0;
}
- memcpy(dev->le_ltk, cmd->ltk, 16);
-
cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_START_ENCRYPT);
- memset(buf, 0, sizeof(buf));
- buf[0] = BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST;
- ev->handle = cpu_to_le16(ACL_HANDLE);
- ev->ediv = cmd->ediv;
- ev->rand = cmd->rand;
+ memcpy(dev->le_ltk, cmd->ltk, 16);
+
+ ev.handle = cpu_to_le16(conn->handle);
+ ev.ediv = cmd->ediv;
+ ev.rand = cmd->rand;
- send_event(remote, BT_HCI_EVT_LE_META_EVENT, buf, sizeof(buf));
+ le_meta_event(conn->link->dev, BT_HCI_EVT_LE_LONG_TERM_KEY_REQUEST, &ev,
+ sizeof(ev));
return 0;
}
@@ -3274,11 +3483,13 @@ static int cmd_start_encrypt(struct btdev *dev, const void *data, uint8_t len)
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_evt_encrypt_change ev;
struct bt_hci_rsp_le_ltk_req_reply rp;
- struct btdev *remote = dev->conn;
+ struct btdev_conn *conn;
+ uint8_t mode, status;
- if (!remote) {
+ 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;
@@ -3287,36 +3498,34 @@ static int cmd_ltk_reply(struct btdev *dev, const void *data, uint8_t len)
memcpy(dev->le_ltk, cmd->ltk, 16);
memset(&rp, 0, sizeof(rp));
- rp.handle = cpu_to_le16(ACL_HANDLE);
+ 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));
- memset(&ev, 0, sizeof(ev));
-
- if (memcmp(dev->le_ltk, remote->le_ltk, 16)) {
- ev.status = BT_HCI_ERR_AUTH_FAILURE;
- ev.encr_mode = 0x00;
+ if (memcmp(dev->le_ltk, conn->link->dev->le_ltk, 16)) {
+ status = BT_HCI_ERR_AUTH_FAILURE;
+ mode = 0x00;
} else {
- ev.status = BT_HCI_ERR_SUCCESS;
- ev.encr_mode = 0x01;
+ status = BT_HCI_ERR_SUCCESS;
+ mode = 0x01;
}
- ev.handle = cpu_to_le16(ACL_HANDLE);
-
- send_event(dev, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
- send_event(remote, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
+ 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 bt_hci_evt_encrypt_change ev;
- struct btdev *remote = dev->conn;
+ struct btdev_conn *conn;
- if (!remote) {
+ 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));
@@ -3324,15 +3533,11 @@ static int cmd_ltk_neg_reply(struct btdev *dev, const void *data, uint8_t len)
}
memset(&rp, 0, sizeof(rp));
- rp.handle = cpu_to_le16(ACL_HANDLE);
+ 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));
- memset(&ev, 0, sizeof(ev));
- ev.status = BT_HCI_ERR_PIN_OR_KEY_MISSING;
- ev.handle = cpu_to_le16(ACL_HANDLE);
-
- send_event(remote, BT_HCI_EVT_ENCRYPT_CHANGE, &ev, sizeof(ev));
+ encrypt_change(conn->link, 0x00, BT_HCI_ERR_PIN_OR_KEY_MISSING);
return 0;
}
@@ -3413,10 +3618,12 @@ 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 *remote = dev->conn;
+ struct btdev_conn *conn;
struct bt_hci_evt_le_conn_update_complete ev;
- if (!remote)
+ conn = queue_find(dev->conns, match_handle,
+ UINT_TO_PTR(le16_to_cpu(cmd->handle)));
+ if (!conn)
return 0;
memset(&ev, 0, sizeof(ev));
@@ -3424,7 +3631,7 @@ static int cmd_conn_param_neg_reply_complete(struct btdev *dev,
ev.handle = cpu_to_le16(cmd->handle);
ev.status = cpu_to_le16(cmd->reason);
- le_meta_event(remote, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
+ le_meta_event(conn->link->dev, BT_HCI_EVT_LE_CONN_UPDATE_COMPLETE, &ev,
sizeof(ev));
return 0;
@@ -3861,15 +4068,14 @@ static void le_ext_conn_complete(struct btdev *btdev,
memset(&ev, 0, sizeof(ev));
if (!status) {
- struct btdev *remote;
+ struct btdev_conn *conn;
- remote = find_btdev_by_bdaddr_type(cmd->peer_addr,
- cmd->peer_addr_type);
+ conn = conn_add_acl(btdev, cmd->peer_addr, cmd->peer_addr_type);
+ if (!conn)
+ return;
- btdev->conn = remote;
btdev->le_adv_enable = 0;
- remote->conn = btdev;
- remote->le_adv_enable = 0;
+ conn->link->dev->le_adv_enable = 0;
ev.status = status;
ev.peer_addr_type = btdev->le_scan_own_addr_type;
@@ -3879,13 +4085,14 @@ static void le_ext_conn_complete(struct btdev *btdev,
memcpy(ev.peer_addr, btdev->bdaddr, 6);
ev.role = 0x01;
- ev.handle = cpu_to_le16(ACL_HANDLE);
+ ev.handle = cpu_to_le16(conn->handle);
ev.interval = lecc->max_interval;
ev.latency = lecc->latency;
ev.supv_timeout = lecc->supv_timeout;
- le_meta_event(remote, BT_HCI_EVT_LE_ENHANCED_CONN_COMPLETE, &ev,
- sizeof(ev));
+ le_meta_event(conn->link->dev,
+ BT_HCI_EVT_LE_ENHANCED_CONN_COMPLETE, &ev,
+ sizeof(ev));
}
ev.status = status;
@@ -4066,17 +4273,30 @@ static int cmd_set_cig_params(struct btdev *dev, const void *data,
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;
+ uint16_t handle[3];
} __attribute__ ((packed)) rsp;
+ int i = 0;
memset(&rsp, 0, sizeof(rsp));
memcpy(&dev->le_cig, data, len);
+
+ if (cmd->num_cis > ARRAY_SIZE(rsp.handle)) {
+ rsp.params.status = BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
+ goto done;
+ }
+
rsp.params.status = BT_HCI_ERR_SUCCESS;
rsp.params.cig_id = cmd->cig_id;
- rsp.params.num_handles = 1;
- rsp.handle = cpu_to_le16(ISO_HANDLE);
- cmd_complete(dev, BT_HCI_CMD_LE_SET_CIG_PARAMS, &rsp, sizeof(rsp));
+
+ for (i = 0; i < cmd->num_cis; i++) {
+ rsp.params.num_handles++;
+ rsp.handle[i] = cpu_to_le16(ISO_HANDLE + i);
+ }
+
+done:
+ cmd_complete(dev, BT_HCI_CMD_LE_SET_CIG_PARAMS, &rsp,
+ sizeof(rsp.params) + (i * sizeof(uint16_t)));
return 0;
}
@@ -4095,7 +4315,8 @@ static int cmd_create_cis(struct btdev *dev, const void *data, uint8_t len)
return 0;
}
-static void le_cis_estabilished(struct btdev *dev, uint8_t status)
+static void le_cis_estabilished(struct btdev *dev, struct btdev_conn *conn,
+ uint8_t status)
{
struct bt_hci_evt_le_cis_established evt;
@@ -4103,11 +4324,8 @@ static void le_cis_estabilished(struct btdev *dev, uint8_t status)
evt.status = status;
- if (dev->conn)
- dev = dev->conn;
-
if (!evt.status) {
- evt.conn_handle = cpu_to_le16(ISO_HANDLE);
+ evt.conn_handle = cpu_to_le16(conn->handle);
/* TODO: Figure out if these values makes sense */
memcpy(evt.cig_sync_delay, dev->le_cig.params.m_interval,
sizeof(dev->le_cig.params.m_interval));
@@ -4131,27 +4349,49 @@ static void le_cis_estabilished(struct btdev *dev, uint8_t status)
le_meta_event(dev, BT_HCI_EVT_LE_CIS_ESTABLISHED, &evt, sizeof(evt));
- if (dev->conn)
- le_meta_event(dev->conn, 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)
{
- struct btdev *remote = dev->conn;
+ const struct bt_hci_cmd_le_create_cis *cmd = data;
+ int i;
- if (remote) {
+ 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;
- evt.acl_handle = cpu_to_le16(ACL_HANDLE);
- evt.cis_handle = cpu_to_le16(ISO_HANDLE);
+ acl = queue_find(dev->conns, match_handle,
+ UINT_TO_PTR(cpu_to_le16(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(cpu_to_le16(cis->cis_handle)));
+ if (!iso) {
+ iso = conn_add_iso(acl, cpu_to_le16(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 = 0x00;
evt.cis_id = 0x00;
- le_meta_event(remote, BT_HCI_EVT_LE_CIS_REQ, &evt, sizeof(evt));
- } else {
- le_cis_estabilished(dev, BT_HCI_ERR_UNKNOWN_CONN_ID);
+ le_meta_event(iso->dev, BT_HCI_EVT_LE_CIS_REQ, &evt,
+ sizeof(evt));
}
return 0;
@@ -4173,8 +4413,19 @@ static int cmd_remove_cig(struct btdev *dev, const void *data, uint8_t len)
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, BT_HCI_ERR_SUCCESS);
+ le_cis_estabilished(dev, conn, BT_HCI_ERR_SUCCESS);
return 0;
}
@@ -4182,9 +4433,18 @@ static int cmd_accept_cis(struct btdev *dev, const void *data, uint8_t len)
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;
- cmd_status(dev, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_LE_ACCEPT_CIS);
- le_cis_estabilished(dev, cmd->reason);
+ 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;
}
@@ -4229,10 +4489,13 @@ 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));
- if (le16_to_cpu(cmd->handle) != ISO_HANDLE) {
+ 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;
}
@@ -4246,11 +4509,11 @@ static int cmd_setup_iso_path(struct btdev *dev, const void *data, uint8_t len)
switch (cmd->direction) {
case 0x00:
dev->le_iso_path[0] = cmd->path;
- rsp.handle = cpu_to_le16(ISO_HANDLE);
+ rsp.handle = cpu_to_le16(conn->handle);
break;
case 0x01:
dev->le_iso_path[1] = cmd->path;
- rsp.handle = cpu_to_le16(ISO_HANDLE);
+ rsp.handle = cpu_to_le16(conn->handle);
break;
default:
rsp.status = BT_HCI_ERR_INVALID_PARAMETERS;
@@ -4266,9 +4529,14 @@ 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;
- if (!dev->conn || le16_to_cpu(cmd->handle) != ISO_HANDLE)
+ 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:
@@ -4281,6 +4549,7 @@ static int cmd_remove_iso_path(struct btdev *dev, const void *data, uint8_t len)
status = BT_HCI_ERR_INVALID_PARAMETERS;
}
+done:
cmd_complete(dev, BT_HCI_CMD_LE_REMOVE_ISO_PATH, &status,
sizeof(status));
@@ -4797,6 +5066,8 @@ struct btdev *btdev_create(enum btdev_type type, uint16_t id)
get_bdaddr(id, index, btdev->bdaddr);
+ btdev->conns = queue_new();
+
return btdev;
}
@@ -4878,7 +5149,10 @@ void btdev_set_send_handler(struct btdev *btdev, btdev_send_func handler,
static void num_completed_packets(struct btdev *btdev, uint16_t handle)
{
- if (btdev->conn) {
+ 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;
@@ -5019,10 +5293,11 @@ static void process_cmd(struct btdev *btdev, const void *data, uint16_t len)
}
}
-static void send_acl(struct btdev *conn, const void *data, uint16_t 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;
/* Packet type */
iov[0].iov_base = (void *) data;
@@ -5032,8 +5307,16 @@ static void send_acl(struct btdev *conn, const void *data, uint16_t len)
* From controller to host this should be converted to ACL_START.
*/
memcpy(&hdr, data + 1, 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);
+
if (acl_flags(hdr.handle) == ACL_START_NO_FLUSH)
- hdr.handle = acl_handle_pack(acl_handle(hdr.handle), ACL_START);
+ hdr.handle = acl_handle_pack(conn->handle, ACL_START);
iov[1].iov_base = &hdr;
iov[1].iov_len = sizeof(hdr);
@@ -5041,17 +5324,26 @@ static void send_acl(struct btdev *conn, const void *data, uint16_t len)
iov[2].iov_base = (void *) (data + 1 + sizeof(hdr));
iov[2].iov_len = len - 1 - sizeof(hdr);
- send_packet(conn, iov, 3);
+ send_packet(conn->link->dev, iov, 3);
}
-static void send_iso(struct btdev *conn, const void *data, uint16_t len)
+static void send_iso(struct btdev *dev, const void *data, uint16_t len)
{
+ struct bt_hci_acl_hdr *hdr;
struct iovec iov;
+ struct btdev_conn *conn;
- iov.iov_base = (void *) (data);
+ iov.iov_base = hdr = (void *) (data);
iov.iov_len = len;
- send_packet(conn, &iov, 1);
+ conn = queue_find(dev->conns, match_handle,
+ UINT_TO_PTR(acl_handle(hdr->handle)));
+ if (!conn)
+ return;
+
+ num_completed_packets(dev, conn->handle);
+
+ send_packet(conn->link->dev, &iov, 1);
}
void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
@@ -5074,14 +5366,10 @@ void btdev_receive_h4(struct btdev *btdev, const void *data, uint16_t len)
process_cmd(btdev, data + 1, len - 1);
break;
case BT_H4_ACL_PKT:
- if (btdev->conn)
- send_acl(btdev->conn, data, len);
- num_completed_packets(btdev, ACL_HANDLE);
+ send_acl(btdev, data, len);
break;
case BT_H4_ISO_PKT:
- num_completed_packets(btdev, ISO_HANDLE);
- if (btdev->conn)
- send_iso(btdev->conn, data, len);
+ send_iso(btdev, data, len);
break;
default:
util_debug(btdev->debug_callback, btdev->debug_data,
diff --git a/lib/hci.h b/lib/hci.h
index 0ca20421a..3382b87bf 100644
--- a/lib/hci.h
+++ b/lib/hci.h
@@ -107,6 +107,7 @@ enum {
#define HCI_ACLDATA_PKT 0x02
#define HCI_SCODATA_PKT 0x03
#define HCI_EVENT_PKT 0x04
+#define HCI_ISODATA_PKT 0x05
#define HCI_VENDOR_PKT 0xff
/* HCI Packet types */
--
2.26.2
