static void l2cap_chan_update_credits(struct bt_l2cap_le_chan *chan)
{
struct net_buf *buf;
struct bt_l2cap_le_credits *ev;
uint16_t credits;
/* Only give more credits if it went bellow the defined threshold */
if (k_sem_count_get(&chan->rx.credits) >
L2CAP_LE_CREDITS_THRESHOLD(chan->rx.init_credits)) {
goto done;
}
/* Restore credits */
credits = chan->rx.init_credits - k_sem_count_get(&chan->rx.credits);
l2cap_chan_rx_give_credits(chan, credits);
buf = l2cap_create_le_sig_pdu(BT_L2CAP_LE_CREDITS, get_ident(),
sizeof(*ev));
ev = net_buf_add(buf, sizeof(*ev));
ev->cid = sys_cpu_to_le16(chan->rx.cid);
ev->credits = sys_cpu_to_le16(credits);
bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf);
done:
BT_DBG("chan %p credits %u", chan, k_sem_count_get(&chan->rx.credits));
}
static void le_conn_rsp(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_le_chan *chan;
struct bt_l2cap_le_conn_rsp *rsp = (void *)buf->data;
uint16_t dcid, mtu, mps, credits, result;
if (buf->len < sizeof(*rsp)) {
BT_ERR("Too small LE conn rsp packet size");
return;
}
dcid = sys_le16_to_cpu(rsp->dcid);
mtu = sys_le16_to_cpu(rsp->mtu);
mps = sys_le16_to_cpu(rsp->mps);
credits = sys_le16_to_cpu(rsp->credits);
result = sys_le16_to_cpu(rsp->result);
BT_DBG("dcid 0x%04x mtu %u mps %u credits %u result 0x%04x", dcid,
mtu, mps, credits, result);
if (result == BT_L2CAP_SUCCESS) {
chan = l2cap_lookup_ident(conn, ident);
} else {
chan = l2cap_remove_ident(conn, ident);
}
if (!chan) {
BT_ERR("Cannot find channel for ident %u", ident);
return;
}
switch (result) {
case BT_L2CAP_SUCCESS:
/* Reset ident since it is no longer pending */
chan->ident = 0;
chan->tx.cid = dcid;
chan->tx.mtu = mtu;
chan->tx.mps = mps;
if (chan->chan.ops && chan->chan.ops->connected) {
chan->chan.ops->connected(&chan->chan);
}
/* Give credits */
l2cap_chan_tx_give_credits(chan, credits);
l2cap_chan_rx_give_credits(chan, L2CAP_LE_MAX_CREDITS);
break;
/* TODO: Retry on Authentication and Encryption errors */
default:
l2cap_chan_del(&chan->chan);
}
}
static void l2cap_chan_update_credits(struct bt_l2cap_le_chan *chan)
{
struct net_buf *buf;
struct bt_l2cap_sig_hdr *hdr;
struct bt_l2cap_le_credits *ev;
uint16_t credits;
/* Only give more credits if it went bellow the defined threshold */
if (chan->rx.credits.nsig > L2CAP_LE_CREDITS_THRESHOLD) {
goto done;
}
/* Restore credits */
credits = L2CAP_LE_MAX_CREDITS - chan->rx.credits.nsig;
l2cap_chan_rx_give_credits(chan, credits);
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
BT_ERR("Unable to send credits");
return;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_LE_CREDITS;
hdr->ident = get_ident();
hdr->len = sys_cpu_to_le16(sizeof(*ev));
ev = net_buf_add(buf, sizeof(*ev));
ev->cid = sys_cpu_to_le16(chan->rx.cid);
ev->credits = sys_cpu_to_le16(credits);
bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_LE_SIG, buf);
done:
BT_DBG("chan %p credits %u", chan, chan->rx.credits.nsig);
}
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_rsp(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
struct bt_l2cap_le_chan *chan;
struct bt_l2cap_le_conn_rsp *rsp = (void *)buf->data;
uint16_t dcid, mtu, mps, credits, result;
if (buf->len < sizeof(*rsp)) {
BT_ERR("Too small LE conn rsp packet size");
return;
}
dcid = sys_le16_to_cpu(rsp->dcid);
mtu = sys_le16_to_cpu(rsp->mtu);
mps = sys_le16_to_cpu(rsp->mps);
credits = sys_le16_to_cpu(rsp->credits);
result = sys_le16_to_cpu(rsp->result);
BT_DBG("dcid 0x%04x mtu %u mps %u credits %u result 0x%04x", dcid,
mtu, mps, credits, result);
/* Keep the channel in case of security errors */
if (result == BT_L2CAP_SUCCESS ||
result == BT_L2CAP_ERR_AUTHENTICATION ||
result == BT_L2CAP_ERR_ENCRYPTION) {
chan = l2cap_lookup_ident(conn, ident);
} else {
chan = l2cap_remove_ident(conn, ident);
}
if (!chan) {
BT_ERR("Cannot find channel for ident %u", ident);
return;
}
/* Cancel RTX work */
k_delayed_work_cancel(&chan->chan.rtx_work);
/* Reset ident since it got a response */
chan->chan.ident = 0;
switch (result) {
case BT_L2CAP_SUCCESS:
chan->tx.cid = dcid;
chan->tx.mtu = mtu;
chan->tx.mps = mps;
/* Update state */
bt_l2cap_chan_set_state(&chan->chan, BT_L2CAP_CONNECTED);
if (chan->chan.ops && chan->chan.ops->connected) {
chan->chan.ops->connected(&chan->chan);
}
/* Give credits */
l2cap_chan_tx_give_credits(chan, credits);
l2cap_chan_rx_give_credits(chan, chan->rx.init_credits);
break;
case BT_L2CAP_ERR_AUTHENTICATION:
case BT_L2CAP_ERR_ENCRYPTION:
/* If security needs changing wait it to be completed */
if (l2cap_change_security(chan, result) == 0) {
return;
}
l2cap_detach_chan(conn, &chan->chan);
default:
bt_l2cap_chan_del(&chan->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;
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);
}