emulator/bthost: Pass full ACL connection context around

diff --git a/emulator/bthost.c b/emulator/bthost.c
index c6e58e5..5e3a186 100644 (file)
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -130,16 +130,11 @@ static struct btconn *bthost_find_conn(struct bthost *bthost, uint16_t handle)
        return NULL;
 }
 
-static void bthost_add_l2cap_conn(struct bthost *bthost, uint16_t handle,
+static void bthost_add_l2cap_conn(struct bthost *bthost, struct btconn *conn,
                                                uint16_t scid, uint16_t dcid)
 {
-       struct btconn *conn;
        struct l2conn *l2conn;
 
-       conn = bthost_find_conn(bthost, handle);
-       if (!conn)
-               return;
-
        l2conn = malloc(sizeof(*l2conn));
        if (!l2conn)
                return;
@@ -153,17 +148,11 @@ static void bthost_add_l2cap_conn(struct bthost *bthost, uint16_t handle,
        conn->l2conns = l2conn;
 }
 
-static struct l2conn *bthost_find_l2cap_conn_by_scid(struct bthost *bthost,
-                                                       uint16_t handle,
-                                                       uint16_t scid)
+static struct l2conn *btconn_find_l2cap_conn_by_scid(struct btconn *conn,
+                                                               uint16_t scid)
 {
-       struct btconn *conn;
        struct l2conn *l2conn;
 
-       conn = bthost_find_conn(bthost, handle);
-       if (!conn)
-               return NULL;
-
        for (l2conn = conn->l2conns; l2conn != NULL; l2conn = l2conn->next) {
                if (l2conn->scid == scid)
                        return l2conn;
@@ -273,7 +262,7 @@ static void send_acl(struct bthost *bthost, uint16_t handle, uint16_t cid,
        free(pkt_data);
 }
 
-static uint8_t l2cap_sig_send(struct bthost *bthost, uint16_t handle,
+static uint8_t l2cap_sig_send(struct bthost *bthost, struct btconn *conn,
                                        uint8_t code, uint8_t ident,
                                        const void *data, uint16_t len)
 {
@@ -302,7 +291,7 @@ static uint8_t l2cap_sig_send(struct bthost *bthost, uint16_t handle,
        if (len > 0)
                memcpy(pkt_data + sizeof(*hdr), data, len);
 
-       send_acl(bthost, handle, 0x0001, pkt_data, pkt_len);
+       send_acl(bthost, conn->handle, 0x0001, pkt_data, pkt_len);
 
        free(pkt_data);
 
@@ -314,9 +303,14 @@ bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
                                bthost_l2cap_rsp_cb cb, void *user_data)
 {
        struct l2cap_pending_req *req;
+       struct btconn *conn;
        uint8_t ident;
 
-       ident = l2cap_sig_send(bthost, handle, code, 0, data, len);
+       conn = bthost_find_conn(bthost, handle);
+       if (!conn)
+               return false;
+
+       ident = l2cap_sig_send(bthost, conn, code, 0, data, len);
        if (!ident)
                return false;
 
@@ -632,7 +626,7 @@ static void process_evt(struct bthost *bthost, const void *data, uint16_t len)
        }
 }
 
-static bool l2cap_cmd_rej(struct bthost *bthost, uint16_t handle,
+static bool l2cap_cmd_rej(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_cmd_reject *rsp = data;
@@ -643,21 +637,16 @@ static bool l2cap_cmd_rej(struct bthost *bthost, uint16_t handle,
        return true;
 }
 
-static bool l2cap_conn_req(struct bthost *bthost, uint16_t handle,
+static bool l2cap_conn_req(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_conn_req *req = data;
        struct bt_l2cap_pdu_conn_rsp rsp;
-       struct btconn *conn;
        uint16_t psm;
 
        if (len < sizeof(*req))
                return false;
 
-       conn = bthost_find_conn(bthost, handle);
-       if (!conn)
-               return false;
-
        psm = le16_to_cpu(req->psm);
 
        memset(&rsp, 0, sizeof(rsp));
@@ -668,26 +657,26 @@ static bool l2cap_conn_req(struct bthost *bthost, uint16_t handle,
        else
                rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */
 
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CONN_RSP, ident, &rsp,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_RSP, ident, &rsp,
                                                                sizeof(rsp));
 
        if (!rsp.result) {
                struct bt_l2cap_pdu_config_req conf_req;
 
-               bthost_add_l2cap_conn(bthost, handle, le16_to_cpu(rsp.dcid),
+               bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(rsp.dcid),
                                                        le16_to_cpu(rsp.scid));
 
                memset(&conf_req, 0, sizeof(conf_req));
                conf_req.dcid = rsp.dcid;
 
-               l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CONFIG_REQ, 0,
+               l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
                                                &conf_req, sizeof(conf_req));
        }
 
        return true;
 }
 
-static bool l2cap_conn_rsp(struct bthost *bthost, uint16_t handle,
+static bool l2cap_conn_rsp(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_conn_rsp *rsp = data;
@@ -695,7 +684,7 @@ static bool l2cap_conn_rsp(struct bthost *bthost, uint16_t handle,
        if (len < sizeof(*rsp))
                return false;
 
-       bthost_add_l2cap_conn(bthost, handle, le16_to_cpu(rsp->scid),
+       bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(rsp->scid),
                                                le16_to_cpu(rsp->dcid));
 
        if (le16_to_cpu(rsp->result) == 0x0001) {
@@ -704,14 +693,14 @@ static bool l2cap_conn_rsp(struct bthost *bthost, uint16_t handle,
                memset(&req, 0, sizeof(req));
                req.dcid = rsp->dcid;
 
-               l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CONFIG_REQ, 0,
+               l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
                                                        &req, sizeof(req));
        }
 
        return true;
 }
 
-static bool l2cap_config_req(struct bthost *bthost, uint16_t handle,
+static bool l2cap_config_req(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_config_req *req = data;
@@ -724,7 +713,7 @@ static bool l2cap_config_req(struct bthost *bthost, uint16_t handle,
 
        dcid = le16_to_cpu(req->dcid);
 
-       l2conn = bthost_find_l2cap_conn_by_scid(bthost, handle, dcid);
+       l2conn = btconn_find_l2cap_conn_by_scid(conn, dcid);
        if (!l2conn)
                return false;
 
@@ -732,13 +721,13 @@ static bool l2cap_config_req(struct bthost *bthost, uint16_t handle,
        rsp.scid  = cpu_to_le16(l2conn->dcid);
        rsp.flags = req->flags;
 
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CONFIG_RSP, ident, &rsp,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_RSP, ident, &rsp,
                                                                sizeof(rsp));
 
        return true;
 }
 
-static bool l2cap_config_rsp(struct bthost *bthost, uint16_t handle,
+static bool l2cap_config_rsp(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_config_rsp *rsp = data;
@@ -749,7 +738,7 @@ static bool l2cap_config_rsp(struct bthost *bthost, uint16_t handle,
        return true;
 }
 
-static bool l2cap_disconn_req(struct bthost *bthost, uint16_t handle,
+static bool l2cap_disconn_req(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_disconn_req *req = data;
@@ -762,13 +751,13 @@ static bool l2cap_disconn_req(struct bthost *bthost, uint16_t handle,
        rsp.dcid = req->dcid;
        rsp.scid = req->scid;
 
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_DISCONN_RSP, ident, &rsp,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_DISCONN_RSP, ident, &rsp,
                                                                sizeof(rsp));
 
        return true;
 }
 
-static bool l2cap_info_req(struct bthost *bthost, uint16_t handle,
+static bool l2cap_info_req(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_info_req *req = data;
@@ -780,13 +769,13 @@ static bool l2cap_info_req(struct bthost *bthost, uint16_t handle,
        rsp.type = req->type;
        rsp.result = cpu_to_le16(0x0001); /* Not Supported */
 
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_INFO_RSP, ident, &rsp,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_INFO_RSP, ident, &rsp,
                                                                sizeof(rsp));
 
        return true;
 }
 
-static void handle_pending_l2reqs(struct bthost *bthost, uint16_t handle,
+static void handle_pending_l2reqs(struct bthost *bthost, struct btconn *conn,
                                                uint8_t ident, uint8_t code,
                                                const void *data, uint16_t len)
 {
@@ -806,8 +795,8 @@ static void handle_pending_l2reqs(struct bthost *bthost, uint16_t handle,
        }
 }
 
-static void l2cap_sig(struct bthost *bthost, uint16_t handle, const void *data,
-                                                               uint16_t len)
+static void l2cap_sig(struct bthost *bthost, struct btconn *conn,
+                                               const void *data, uint16_t len)
 {
        const struct bt_l2cap_hdr_sig *hdr = data;
        struct bt_l2cap_pdu_cmd_reject rej;
@@ -824,37 +813,37 @@ static void l2cap_sig(struct bthost *bthost, uint16_t handle, const void *data,
 
        switch (hdr->code) {
        case BT_L2CAP_PDU_CMD_REJECT:
-               ret = l2cap_cmd_rej(bthost, handle, hdr->ident,
+               ret = l2cap_cmd_rej(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_CONN_REQ:
-               ret = l2cap_conn_req(bthost, handle, hdr->ident,
+               ret = l2cap_conn_req(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_CONN_RSP:
-               ret = l2cap_conn_rsp(bthost, handle, hdr->ident,
+               ret = l2cap_conn_rsp(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_CONFIG_REQ:
-               ret = l2cap_config_req(bthost, handle, hdr->ident,
+               ret = l2cap_config_req(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_CONFIG_RSP:
-               ret = l2cap_config_rsp(bthost, handle, hdr->ident,
+               ret = l2cap_config_rsp(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_DISCONN_REQ:
-               ret = l2cap_disconn_req(bthost, handle, hdr->ident,
+               ret = l2cap_disconn_req(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_INFO_REQ:
-               ret = l2cap_info_req(bthost, handle, hdr->ident,
+               ret = l2cap_info_req(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
@@ -863,7 +852,7 @@ static void l2cap_sig(struct bthost *bthost, uint16_t handle, const void *data,
                ret = false;
        }
 
-       handle_pending_l2reqs(bthost, handle, hdr->ident, hdr->code,
+       handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
                                                data + sizeof(*hdr), hdr_len);
 
        if (ret)
@@ -871,11 +860,11 @@ static void l2cap_sig(struct bthost *bthost, uint16_t handle, const void *data,
 
 reject:
        memset(&rej, 0, sizeof(rej));
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CMD_REJECT, 0,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
                                                        &rej, sizeof(rej));
 }
 
-static bool l2cap_conn_param_req(struct bthost *bthost, uint16_t handle,
+static bool l2cap_conn_param_req(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_conn_param_req *req = data;
@@ -886,7 +875,7 @@ static bool l2cap_conn_param_req(struct bthost *bthost, uint16_t handle,
                return false;
 
        memset(&hci_cmd, 0, sizeof(hci_cmd));
-       hci_cmd.handle = cpu_to_le16(handle);
+       hci_cmd.handle = cpu_to_le16(conn->handle);
        hci_cmd.min_interval = req->min_interval;
        hci_cmd.max_interval = req->max_interval;
        hci_cmd.latency = req->latency;
@@ -898,13 +887,13 @@ static bool l2cap_conn_param_req(struct bthost *bthost, uint16_t handle,
                                                &hci_cmd, sizeof(hci_cmd));
 
        memset(&rsp, 0, sizeof(rsp));
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CONN_PARAM_RSP, ident,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONN_PARAM_RSP, ident,
                                                        &rsp, sizeof(rsp));
 
        return true;
 }
 
-static bool l2cap_conn_param_rsp(struct bthost *bthost, uint16_t handle,
+static bool l2cap_conn_param_rsp(struct bthost *bthost, struct btconn *conn,
                                uint8_t ident, const void *data, uint16_t len)
 {
        const struct bt_l2cap_pdu_conn_param_req *rsp = data;
@@ -915,7 +904,7 @@ static bool l2cap_conn_param_rsp(struct bthost *bthost, uint16_t handle,
        return true;
 }
 
-static void l2cap_le_sig(struct bthost *bthost, uint16_t handle,
+static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn,
                                                const void *data, uint16_t len)
 {
        const struct bt_l2cap_hdr_sig *hdr = data;
@@ -933,17 +922,17 @@ static void l2cap_le_sig(struct bthost *bthost, uint16_t handle,
 
        switch (hdr->code) {
        case BT_L2CAP_PDU_CMD_REJECT:
-               ret = l2cap_cmd_rej(bthost, handle, hdr->ident,
+               ret = l2cap_cmd_rej(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_CONN_PARAM_REQ:
-               ret = l2cap_conn_param_req(bthost, handle, hdr->ident,
+               ret = l2cap_conn_param_req(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
        case BT_L2CAP_PDU_CONN_PARAM_RSP:
-               ret = l2cap_conn_param_rsp(bthost, handle, hdr->ident,
+               ret = l2cap_conn_param_rsp(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
 
@@ -952,7 +941,7 @@ static void l2cap_le_sig(struct bthost *bthost, uint16_t handle,
                ret = false;
        }
 
-       handle_pending_l2reqs(bthost, handle, hdr->ident, hdr->code,
+       handle_pending_l2reqs(bthost, conn, hdr->ident, hdr->code,
                                                data + sizeof(*hdr), hdr_len);
 
        if (ret)
@@ -960,7 +949,7 @@ static void l2cap_le_sig(struct bthost *bthost, uint16_t handle,
 
 reject:
        memset(&rej, 0, sizeof(rej));
-       l2cap_sig_send(bthost, handle, BT_L2CAP_PDU_CMD_REJECT, 0,
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CMD_REJECT, 0,
                                                        &rej, sizeof(rej));
 }
 
@@ -969,6 +958,7 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
        const struct bt_hci_acl_hdr *acl_hdr = data;
        const struct bt_l2cap_hdr *l2_hdr = data + sizeof(*acl_hdr);
        uint16_t handle, cid, acl_len, l2_len;
+       struct btconn *conn;
        const void *l2_data;
 
        if (len < sizeof(*acl_hdr) + sizeof(*l2_hdr))
@@ -979,6 +969,11 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
                return;
 
        handle = le16_to_cpu(acl_hdr->handle);
+       conn = bthost_find_conn(bthost, handle);
+       if (!conn) {
+               printf("ACL data for unknown handle 0x%04x\n", handle);
+               return;
+       }
 
        l2_len = le16_to_cpu(l2_hdr->len);
        if (len - sizeof(*acl_hdr) != sizeof(*l2_hdr) + l2_len)
@@ -990,10 +985,10 @@ static void process_acl(struct bthost *bthost, const void *data, uint16_t len)
 
        switch (cid) {
        case 0x0001:
-               l2cap_sig(bthost, handle, l2_data, l2_len);
+               l2cap_sig(bthost, conn, l2_data, l2_len);
                break;
        case 0x0005:
-               l2cap_le_sig(bthost, handle, l2_data, l2_len);
+               l2cap_le_sig(bthost, conn, l2_data, l2_len);
                break;
        default:
                printf("Packet for unknown CID 0x%04x (%u)\n", cid, cid);