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;
/* TODO add L2CAP connection before with proper PSM */
bthost_add_l2cap_conn(bthost, conn, 0, le16_to_cpu(rsp->dcid), 0);
return true;
}
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 l2cap_conn_cb_data *cb_data;
struct bt_l2cap_pdu_conn_rsp rsp;
uint16_t psm;
if (len < sizeof(*req))
return false;
psm = le16_to_cpu(req->psm);
memset(&rsp, 0, sizeof(rsp));
rsp.scid = req->scid;
cb_data = bthost_find_l2cap_cb_by_psm(bthost, psm);
if (cb_data)
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_CONN_RSP, ident, &rsp,
sizeof(rsp));
if (!rsp.result) {
struct bt_l2cap_pdu_config_req conf_req;
struct l2conn *l2conn;
l2conn = bthost_add_l2cap_conn(bthost, conn,
le16_to_cpu(rsp.dcid),
le16_to_cpu(rsp.scid),
le16_to_cpu(psm));
memset(&conf_req, 0, sizeof(conf_req));
conf_req.dcid = rsp.scid;
l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
&conf_req, sizeof(conf_req));
if (cb_data && l2conn->psm == cb_data->psm && cb_data->func)
cb_data->func(conn->handle, l2conn->dcid,
cb_data->user_data);
}
return true;
}
bool bthost_l2cap_req(struct bthost *bthost, uint16_t handle, uint8_t code,
const void *data, uint16_t len,
bthost_l2cap_rsp_cb cb, void *user_data)
{
struct l2cap_pending_req *req;
struct btconn *conn;
uint8_t ident;
conn = bthost_find_conn(bthost, handle);
if (!conn)
return false;
if (code == BT_L2CAP_PDU_CONN_REQ &&
len == sizeof(struct bt_l2cap_pdu_conn_req)) {
const struct bt_l2cap_pdu_conn_req *req = data;
bthost_add_l2cap_conn(bthost, conn, le16_to_cpu(req->scid),
le16_to_cpu(req->scid),
le16_to_cpu(req->psm));
}
ident = l2cap_sig_send(bthost, conn, code, 0, data, len);
if (!ident)
return false;
if (!cb)
return true;
req = malloc(sizeof(*req));
if (!req)
return false;
memset(req, 0, sizeof(*req));
req->ident = ident;
req->cb = cb;
req->user_data = user_data;
req->next = bthost->l2reqs;
bthost->l2reqs = req;
return true;
}
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;
uint16_t psm;
if (len < sizeof(*req))
return false;
psm = le16_to_cpu(req->psm);
memset(&rsp, 0, sizeof(rsp));
rsp.scid = req->scid;
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_CONN_RSP, ident, &rsp,
sizeof(rsp));
if (!rsp.result) {
struct bt_l2cap_pdu_config_req conf_req;
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, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
&conf_req, sizeof(conf_req));
}
return true;
}
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, conn, le16_to_cpu(rsp->scid),
le16_to_cpu(rsp->dcid));
if (le16_to_cpu(rsp->result) == 0x0001) {
struct bt_l2cap_pdu_config_req req;
memset(&req, 0, sizeof(req));
req.dcid = rsp->dcid;
l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_CONFIG_REQ, 0,
&req, sizeof(req));
}
return true;
}