From 37585d8ce1c4c969300026e6a6f72400c8016c62 Mon Sep 17 00:00:00 2001 From: Johan Hedberg Date: Fri, 20 Sep 2013 01:12:27 +0300 Subject: [PATCH] emulator/bthost: Add LE L2CAP Connect support --- emulator/bthost.c | 52 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/emulator/bthost.c b/emulator/bthost.c index 384dd7cd6..10e7a0572 100644 --- a/emulator/bthost.c +++ b/emulator/bthost.c @@ -975,6 +975,48 @@ static bool l2cap_conn_param_rsp(struct bthost *bthost, struct btconn *conn, return true; } +static bool l2cap_le_conn_req(struct bthost *bthost, struct btconn *conn, + uint8_t ident, const void *data, uint16_t len) +{ + const struct bt_l2cap_pdu_le_conn_req *req = data; + struct bt_l2cap_pdu_le_conn_rsp rsp; + uint16_t psm; + + if (len < sizeof(*req)) + return false; + + psm = le16_to_cpu(req->psm); + + memset(&rsp, 0, sizeof(rsp)); + + rsp.mtu = 23; + rsp.mps = 23; + rsp.credits = 1; + + if (bthost->server_psm && bthost->server_psm == psm) + rsp.dcid = cpu_to_le16(conn->next_cid++); + else + rsp.result = cpu_to_le16(0x0002); /* PSM Not Supported */ + + l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_LE_CONN_RSP, ident, &rsp, + sizeof(rsp)); + + return true; +} + +static bool l2cap_le_conn_rsp(struct bthost *bthost, struct btconn *conn, + uint8_t ident, const void *data, uint16_t len) +{ + const struct bt_l2cap_pdu_le_conn_rsp *rsp = data; + + if (len < sizeof(*rsp)) + return false; + + bthost_add_l2cap_conn(bthost, conn, 0, le16_to_cpu(rsp->dcid)); + + return true; +} + static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn, const void *data, uint16_t len) { @@ -1012,6 +1054,16 @@ static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn, data + sizeof(*hdr), hdr_len); break; + case BT_L2CAP_PDU_LE_CONN_REQ: + ret = l2cap_le_conn_req(bthost, conn, hdr->ident, + data + sizeof(*hdr), hdr_len); + break; + + case BT_L2CAP_PDU_LE_CONN_RSP: + ret = l2cap_le_conn_rsp(bthost, conn, hdr->ident, + data + sizeof(*hdr), hdr_len); + break; + default: printf("Unknown L2CAP code 0x%02x\n", hdr->code); ret = false; -- 2.47.3