diff options
| -rw-r--r-- | emulator/btdev.c | 930 | ||||
| -rw-r--r-- | 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; @@ -2143,19 +2324,37 @@ 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, @@ -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 */ |