static void l2cap_send_reject(struct bt_conn *conn, uint8_t ident,
uint16_t reason, void *data, uint8_t data_len)
{
struct bt_l2cap_cmd_reject *rej;
struct bt_l2cap_sig_hdr *hdr;
struct net_buf *buf;
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_CMD_REJECT;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(sizeof(*rej) + data_len);
rej = net_buf_add(buf, sizeof(*rej));
rej->reason = sys_cpu_to_le16(reason);
if (data) {
memcpy(net_buf_add(buf, data_len), data, data_len);
}
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
}
struct net_buf *bt_att_create_pdu(struct bt_conn *conn, uint8_t op, size_t len)
{
struct bt_att_hdr *hdr;
struct net_buf *buf;
struct bt_att *att;
att = att_chan_get(conn);
if (!att) {
return NULL;
}
if (len + sizeof(op) > att->chan.tx.mtu) {
BT_WARN("ATT MTU exceeded, max %u, wanted %zu",
att->chan.tx.mtu, len + sizeof(op));
return NULL;
}
buf = bt_l2cap_create_pdu(&att_buf);
if (!buf) {
return NULL;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = op;
return buf;
}
int bt_l2cap_update_conn_param(struct bt_conn *conn,
const struct bt_le_conn_param *param)
{
struct bt_l2cap_sig_hdr *hdr;
struct bt_l2cap_conn_param_req *req;
struct net_buf *buf;
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return -ENOBUFS;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_CONN_PARAM_REQ;
hdr->ident = get_ident();
hdr->len = sys_cpu_to_le16(sizeof(*req));
req = net_buf_add(buf, sizeof(*req));
req->min_interval = sys_cpu_to_le16(param->interval_min);
req->max_interval = sys_cpu_to_le16(param->interval_max);
req->latency = sys_cpu_to_le16(param->latency);
req->timeout = sys_cpu_to_le16(param->timeout);
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
return 0;
}
static void bt_smp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
{
struct bt_conn *conn = chan->conn;
struct bt_smp_pairing_fail *rsp;
struct bt_smp_hdr *hdr;
/* If a device does not support pairing then it shall respond with
* a Pairing Failed command with the reason set to “Pairing Not
* Supported” when any command is received.
* Core Specification Vol. 3, Part H, 3.3
*/
buf = bt_l2cap_create_pdu(&smp_buf);
if (!buf) {
return;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_SMP_CMD_PAIRING_FAIL;
rsp = net_buf_add(buf, sizeof(*rsp));
rsp->reason = BT_SMP_ERR_PAIRING_NOTSUPP;
bt_l2cap_send(conn, BT_L2CAP_CID_SMP, buf);
}
static void le_conn_param_update_req(struct bt_l2cap *l2cap, uint8_t ident,
struct net_buf *buf)
{
struct bt_conn *conn = l2cap->chan.chan.conn;
const struct bt_le_conn_param *param;
uint16_t min, max, latency, timeout;
bool params_valid;
struct bt_l2cap_sig_hdr *hdr;
struct bt_l2cap_conn_param_rsp *rsp;
struct bt_l2cap_conn_param_req *req = (void *)buf->data;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn update param req");
return;
}
if (conn->role != BT_HCI_ROLE_MASTER) {
l2cap_send_reject(conn, ident, BT_L2CAP_REJ_NOT_UNDERSTOOD,
NULL, 0);
return;
}
min = sys_le16_to_cpu(req->min_interval);
max = sys_le16_to_cpu(req->max_interval);
latency = sys_le16_to_cpu(req->latency);
timeout = sys_le16_to_cpu(req->timeout);
param = BT_LE_CONN_PARAM(min, max, latency, timeout);
BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x timeout: 0x%4.4x",
min, max, latency, timeout);
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return;
}
params_valid = bt_le_conn_params_valid(min, max, latency, timeout);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_CONN_PARAM_RSP;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(sizeof(*rsp));
rsp = net_buf_add(buf, sizeof(*rsp));
if (params_valid) {
rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_ACCEPTED);
} else {
rsp->result = sys_cpu_to_le16(BT_L2CAP_CONN_PARAM_REJECTED);
}
bt_l2cap_send(l2cap->chan.chan.conn, BT_L2CAP_CID_LE_SIG, buf);
if (params_valid) {
bt_conn_le_conn_update(conn, param);
}
}
static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch,
struct net_buf *buf,
size_t sdu_hdr_len)
{
struct net_buf *seg;
uint16_t headroom;
uint16_t len;
/* Segment if data (+ data headroom) is bigger than MPS */
if (buf->len + sdu_hdr_len > ch->tx.mps) {
goto segment;
}
/* Segment if there is no space in the user_data */
if (buf->pool->user_data_size < BT_BUF_USER_DATA_MIN) {
BT_WARN("Too small buffer user_data_size %u",
buf->pool->user_data_size);
goto segment;
}
headroom = sizeof(struct bt_hci_acl_hdr) +
sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
/* Check if original buffer has enough headroom and don't have any
* fragments.
*/
if (net_buf_headroom(buf) >= headroom && !buf->frags) {
if (sdu_hdr_len) {
/* Push SDU length if set */
net_buf_push_le16(buf, net_buf_frags_len(buf));
}
return net_buf_ref(buf);
}
segment:
seg = bt_l2cap_create_pdu(&le_data_pool, 0);
if (sdu_hdr_len) {
net_buf_add_le16(seg, net_buf_frags_len(buf));
}
/* Don't send more that TX MPS including SDU length */
len = min(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len);
/* Limit if original buffer is smaller than the segment */
len = min(buf->len, len);
net_buf_add_mem(seg, buf->data, len);
net_buf_pull(buf, len);
BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
return seg;
}
static void le_disconn_req(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_disconn_req *req = (void *)buf->data;
struct bt_l2cap_disconn_rsp *rsp;
struct bt_l2cap_sig_hdr *hdr;
uint16_t scid, dcid;
if (buf->len < sizeof(*req)) {
BT_ERR("Too small LE conn req packet size");
return;
}
dcid = sys_le16_to_cpu(req->dcid);
scid = sys_le16_to_cpu(req->scid);
BT_DBG("scid 0x%04x dcid 0x%04x", dcid, scid);
chan = l2cap_remove_tx_cid(conn, scid);
if (!chan) {
struct bt_l2cap_cmd_reject_cid_data data;
data.scid = req->scid;
data.dcid = req->dcid;
l2cap_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID, &data,
sizeof(data));
return;
}
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
return;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_DISCONN_RSP;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(sizeof(*rsp));
rsp = net_buf_add(buf, sizeof(*rsp));
rsp->dcid = sys_cpu_to_le16(chan->rx.cid);
rsp->scid = sys_cpu_to_le16(chan->tx.cid);
l2cap_chan_del(&chan->chan);
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
}
static struct net_buf *l2cap_create_le_sig_pdu(uint8_t code, uint8_t ident,
uint16_t len)
{
struct net_buf *buf;
struct bt_l2cap_sig_hdr *hdr;
buf = bt_l2cap_create_pdu(NULL, 0);
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = code;
hdr->ident = ident;
hdr->len = sys_cpu_to_le16(len);
return buf;
}
static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch,
struct net_buf *buf,
size_t sdu_hdr_len)
{
struct net_buf *seg;
uint16_t headroom;
uint16_t len;
/* Segment if data (+ data headroom) is bigger than MPS */
if (buf->len + sdu_hdr_len > ch->tx.mps) {
goto segment;
}
headroom = sizeof(struct bt_hci_acl_hdr) +
sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
/* Check if original buffer has enough headroom */
if (net_buf_headroom(buf) >= headroom) {
if (sdu_hdr_len) {
/* Push SDU length if set */
net_buf_push_le16(buf, buf->len);
}
return net_buf_ref(buf);
}
segment:
seg = bt_l2cap_create_pdu(&le_data);
if (!seg) {
return NULL;
}
if (sdu_hdr_len) {
net_buf_add_le16(seg, buf->len);
}
len = min(min(buf->len, L2CAP_LE_MIN_MTU - sdu_hdr_len), ch->tx.mps);
memcpy(net_buf_add(seg, len), buf->data, len);
net_buf_pull(buf, len);
BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
return seg;
}
int bt_l2cap_chan_disconnect(struct bt_l2cap_chan *chan)
{
struct bt_conn *conn = chan->conn;
struct net_buf *buf;
struct bt_l2cap_disconn_req *req;
struct bt_l2cap_sig_hdr *hdr;
struct bt_l2cap_le_chan *ch;
if (!conn) {
return -ENOTCONN;
}
#if defined(CONFIG_BLUETOOTH_BREDR)
if (conn->type == BT_CONN_TYPE_BR) {
return bt_l2cap_br_chan_disconnect(chan);
}
#endif /* CONFIG_BLUETOOTH_BREDR */
ch = LE_CHAN(chan);
BT_DBG("chan %p scid 0x%04x dcid 0x%04x", chan, ch->rx.cid,
ch->tx.cid);
buf = bt_l2cap_create_pdu(&le_sig);
if (!buf) {
BT_ERR("Unable to send L2CP disconnect request");
return -ENOMEM;
}
hdr = net_buf_add(buf, sizeof(*hdr));
hdr->code = BT_L2CAP_DISCONN_REQ;
hdr->ident = get_ident();
hdr->len = sys_cpu_to_le16(sizeof(*req));
req = net_buf_add(buf, sizeof(*req));
req->dcid = sys_cpu_to_le16(ch->tx.cid);
req->scid = sys_cpu_to_le16(ch->rx.cid);
bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
return 0;
}
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;
}
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);
}