diff --git a/emulator/bthost.c b/emulator/bthost.c
index c6e58e5..5e3a186 100644
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
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;
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;
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)
{
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);
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;
}
}
-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;
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));
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;
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) {
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;
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;
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;
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;
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;
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)
{
}
}
-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;
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;
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)
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;
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;
&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;
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;
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;
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)
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));
}
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))
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)
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);