static int l2cap_do_connect(struct sock *sk)
{
bdaddr_t *src = &bluez_pi(sk)->src;
bdaddr_t *dst = &bluez_pi(sk)->dst;
struct l2cap_conn *conn;
struct hci_conn *hcon;
struct hci_dev *hdev;
int err = 0;
BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), l2cap_pi(sk)->psm);
if (!(hdev = hci_get_route(dst, src)))
return -EHOSTUNREACH;
hci_dev_lock_bh(hdev);
err = -ENOMEM;
hcon = hci_connect(hdev, ACL_LINK, dst);
if (!hcon)
goto done;
conn = l2cap_conn_add(hcon, 0);
if (!conn) {
hci_conn_put(hcon);
goto done;
}
err = 0;
/* Update source addr of the socket */
bacpy(src, conn->src);
l2cap_chan_add(conn, sk, NULL);
sk->state = BT_CONNECT;
l2cap_sock_set_timer(sk, sk->sndtimeo);
if (hcon->state == BT_CONNECTED) {
if (sk->type == SOCK_SEQPACKET) {
l2cap_conn_req req;
req.scid = __cpu_to_le16(l2cap_pi(sk)->scid);
req.psm = l2cap_pi(sk)->psm;
l2cap_send_req(conn, L2CAP_CONN_REQ, L2CAP_CONN_REQ_SIZE, &req);
} else {
l2cap_sock_clear_timer(sk);
sk->state = BT_CONNECTED;
}
}
done:
hci_dev_unlock_bh(hdev);
hci_dev_put(hdev);
return err;
}
static int l2cap_le_connect(struct bt_conn *conn, struct bt_l2cap_le_chan *ch,
uint16_t psm)
{
if (psm < L2CAP_LE_PSM_START || psm > L2CAP_LE_PSM_END) {
return -EINVAL;
}
l2cap_chan_tx_init(ch);
l2cap_chan_rx_init(ch);
if (!l2cap_chan_add(conn, &ch->chan, l2cap_chan_destroy)) {
return -ENOMEM;
}
ch->chan.psm = psm;
return l2cap_le_conn_req(ch);
}
static int l2cap_le_connect(struct bt_conn *conn, struct bt_l2cap_le_chan *ch,
uint16_t psm)
{
struct net_buf *buf;
struct bt_l2cap_sig_hdr *hdr;
struct bt_l2cap_le_conn_req *req;
if (psm < L2CAP_LE_PSM_START || psm > L2CAP_LE_PSM_END) {
return -EINVAL;
}
l2cap_chan_tx_init(ch);
l2cap_chan_rx_init(ch);
if (!l2cap_chan_add(conn, &ch->chan)) {
return -ENOMEM;
}
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
BT_ERR("Unable to send L2CAP connection request");
return -ENOMEM;
}
ch->ident = get_ident();
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_LE_CONN_REQ;
hdr->ident = ch->ident;
hdr->len = sys_cpu_to_le16(sizeof(*req));
req = net_buf_add(buf, sizeof(*req));
req->psm = sys_cpu_to_le16(psm);
req->scid = sys_cpu_to_le16(ch->rx.cid);
req->mtu = sys_cpu_to_le16(ch->rx.mtu);
req->mps = sys_cpu_to_le16(ch->rx.mps);
req->credits = sys_cpu_to_le16(L2CAP_LE_MAX_CREDITS);
bt_l2cap_send(ch->chan.conn, BT_L2CAP_CID_LE_SIG, buf);
return 0;
}
void bt_l2cap_connected(struct bt_conn *conn)
{
struct bt_l2cap_fixed_chan *fchan;
struct bt_l2cap_chan *chan;
#if defined(CONFIG_BLUETOOTH_BREDR)
if (conn->type == BT_CONN_TYPE_BR) {
bt_l2cap_br_connected(conn);
return;
}
#endif /* CONFIG_BLUETOOTH_BREDR */
fchan = le_channels;
for (; fchan; fchan = fchan->_next) {
struct bt_l2cap_le_chan *ch;
if (fchan->accept(conn, &chan) < 0) {
continue;
}
ch = LE_CHAN(chan);
/* Fill up remaining fixed channel context attached in
* fchan->accept()
*/
ch->rx.cid = fchan->cid;
ch->tx.cid = fchan->cid;
if (!l2cap_chan_add(conn, chan)) {
return;
}
if (chan->ops->connected) {
chan->ops->connected(chan);
}
}
}
static void le_conn_req(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_chan *chan;
struct bt_l2cap_server *server;
struct bt_l2cap_le_conn_req *req = (void *)buf->data;
struct bt_l2cap_le_conn_rsp *rsp;
struct bt_l2cap_sig_hdr *hdr;
uint16_t psm, scid, mtu, mps, credits;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn req packet size");
return;
}
psm = sys_le16_to_cpu(req->psm);
scid = sys_le16_to_cpu(req->scid);
mtu = sys_le16_to_cpu(req->mtu);
mps = sys_le16_to_cpu(req->mps);
credits = sys_le16_to_cpu(req->credits);
BT_DBG("psm 0x%02x scid 0x%04x mtu %u mps %u credits %u", psm, scid,
mtu, mps, credits);
if (mtu < L2CAP_LE_MIN_MTU || mps < L2CAP_LE_MIN_MTU) {
BT_ERR("Invalid LE-Conn Req params");
return;
}
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_LE_CONN_RSP;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(sizeof(*rsp));
rsp = net_buf_add(buf, sizeof(*rsp));
memset(rsp, 0, sizeof(*rsp));
/* Check if there is a server registered */
server = l2cap_server_lookup_psm(psm);
if (!server) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_PSM_NOT_SUPP);
goto rsp;
}
/* TODO: Add security check */
if (scid < L2CAP_LE_DYN_CID_START || scid > L2CAP_LE_DYN_CID_END) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_INVALID_SCID);
goto rsp;
}
chan = bt_l2cap_le_lookup_tx_cid(conn, scid);
if (chan) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_SCID_IN_USE);
goto rsp;
}
/* Request server to accept the new connection and allocate the
* channel.
*
* TODO: Handle different errors, it may be required to respond async.
*/
if (server->accept(conn, &chan) < 0) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_NO_RESOURCES);
goto rsp;
}
if (l2cap_chan_add(conn, chan)) {
struct bt_l2cap_le_chan *ch = LE_CHAN(chan);
/* Init TX parameters */
l2cap_chan_tx_init(ch);
ch->tx.cid = scid;
ch->tx.mps = mps;
ch->tx.mtu = mtu;
l2cap_chan_tx_give_credits(ch, credits);
/* Init RX parameters */
l2cap_chan_rx_init(ch);
l2cap_chan_rx_give_credits(ch, L2CAP_LE_MAX_CREDITS);
if (chan->ops && chan->ops->connected) {
chan->ops->connected(chan);
}
/* Prepare response protocol data */
rsp->dcid = sys_cpu_to_le16(ch->rx.cid);
rsp->mps = sys_cpu_to_le16(ch->rx.mps);
rsp->mtu = sys_cpu_to_le16(ch->rx.mtu);
rsp->credits = sys_cpu_to_le16(L2CAP_LE_MAX_CREDITS);
rsp->result = BT_L2CAP_SUCCESS;
} else {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_NO_RESOURCES);
}
rsp:
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
}
static void le_conn_req(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_chan *chan;
struct bt_l2cap_server *server;
struct bt_l2cap_le_conn_req *req = (void *)buf->data;
struct bt_l2cap_le_conn_rsp *rsp;
uint16_t psm, scid, mtu, mps, credits;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn req packet size");
return;
}
psm = sys_le16_to_cpu(req->psm);
scid = sys_le16_to_cpu(req->scid);
mtu = sys_le16_to_cpu(req->mtu);
mps = sys_le16_to_cpu(req->mps);
credits = sys_le16_to_cpu(req->credits);
BT_DBG("psm 0x%02x scid 0x%04x mtu %u mps %u credits %u", psm, scid,
mtu, mps, credits);
if (mtu < L2CAP_LE_MIN_MTU || mps < L2CAP_LE_MIN_MTU) {
BT_ERR("Invalid LE-Conn Req params");
return;
}
buf = l2cap_create_le_sig_pdu(BT_L2CAP_LE_CONN_RSP, ident,
sizeof(*rsp));
rsp = net_buf_add(buf, sizeof(*rsp));
memset(rsp, 0, sizeof(*rsp));
/* Check if there is a server registered */
server = l2cap_server_lookup_psm(psm);
if (!server) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_PSM_NOT_SUPP);
goto rsp;
}
/* Check if connection has minimum required security level */
if (conn->sec_level < server->sec_level) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_AUTHENTICATION);
goto rsp;
}
if (!L2CAP_LE_CID_IS_DYN(scid)) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_INVALID_SCID);
goto rsp;
}
chan = bt_l2cap_le_lookup_tx_cid(conn, scid);
if (chan) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_SCID_IN_USE);
goto rsp;
}
/* Request server to accept the new connection and allocate the
* channel.
*
* TODO: Handle different errors, it may be required to respond async.
*/
if (server->accept(conn, &chan) < 0) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_NO_RESOURCES);
goto rsp;
}
chan->required_sec_level = server->sec_level;
if (l2cap_chan_add(conn, chan, l2cap_chan_destroy)) {
struct bt_l2cap_le_chan *ch = BT_L2CAP_LE_CHAN(chan);
/* Init TX parameters */
l2cap_chan_tx_init(ch);
ch->tx.cid = scid;
ch->tx.mps = mps;
ch->tx.mtu = mtu;
ch->tx.init_credits = credits;
l2cap_chan_tx_give_credits(ch, credits);
/* Init RX parameters */
l2cap_chan_rx_init(ch);
l2cap_chan_rx_give_credits(ch, ch->rx.init_credits);
/* Set channel PSM */
chan->psm = server->psm;
/* Update state */
bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECTED);
if (chan->ops && chan->ops->connected) {
chan->ops->connected(chan);
}
/* Prepare response protocol data */
rsp->dcid = sys_cpu_to_le16(ch->rx.cid);
rsp->mps = sys_cpu_to_le16(ch->rx.mps);
rsp->mtu = sys_cpu_to_le16(ch->rx.mtu);
rsp->credits = sys_cpu_to_le16(ch->rx.init_credits);
rsp->result = BT_L2CAP_SUCCESS;
} else {
rsp->result = sys_cpu_to_le16(BT_L2CAP_ERR_NO_RESOURCES);
}
rsp:
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
}
