diff --git a/net/bluetooth/l2cap_br.c b/net/bluetooth/l2cap_br.c index 512eed1b279..55621cee59b 100644 --- a/net/bluetooth/l2cap_br.c +++ b/net/bluetooth/l2cap_br.c @@ -386,7 +386,7 @@ int bt_l2cap_br_server_register(struct bt_l2cap_server *server) } static void l2cap_br_send_reject(struct bt_conn *conn, uint8_t ident, - uint16_t reason) + uint16_t reason, void *data, uint8_t data_len) { struct bt_l2cap_cmd_reject *rej; struct bt_l2cap_sig_hdr *hdr; @@ -400,11 +400,20 @@ static void l2cap_br_send_reject(struct bt_conn *conn, uint8_t ident, hdr = net_buf_add(buf, sizeof(*hdr)); hdr->code = BT_L2CAP_CMD_REJECT; hdr->ident = ident; - hdr->len = sys_cpu_to_le16(sizeof(*rej)); + hdr->len = sys_cpu_to_le16(sizeof(*rej) + data_len); rej = net_buf_add(buf, sizeof(*rej)); rej->reason = sys_cpu_to_le16(reason); + /* + * optional data if available must be already in little-endian format + * made by caller.and be compliant with Core 4.2 [Vol 3, Part A, 4.1, + * table 4.4] + */ + if (data) { + memcpy(net_buf_add(buf, data_len), data, data_len); + } + bt_l2cap_send(conn, BT_L2CAP_CID_BR_SIG, buf); } @@ -472,7 +481,12 @@ static void l2cap_br_disconn_req(struct bt_l2cap_br *l2cap, uint8_t ident, chan = l2cap_br_remove_tx_cid(conn, scid); if (!chan) { - l2cap_br_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID); + struct bt_l2cap_cmd_reject_cid_data data; + + data.scid = req->scid; + data.dcid = req->dcid; + l2cap_br_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID, + &data, sizeof(data)); return; } @@ -545,7 +559,7 @@ static void l2cap_br_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) default: BT_WARN("Unknown/Unsupported L2CAP PDU code 0x%02x", hdr->code); l2cap_br_send_reject(chan->conn, hdr->ident, - BT_L2CAP_REJ_NOT_UNDERSTOOD); + BT_L2CAP_REJ_NOT_UNDERSTOOD, NULL, 0); break; } }