diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h index 1c426de1..74f9ec72 100644 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h +++ b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/common/log.h @@ -89,10 +89,10 @@ extern "C" { #if defined(CONFIG_BT_ASSERT) #if defined(BFLB_BLE) -extern void user_vAssertCalled(void); +extern void vAssertCalled(void); #define BT_ASSERT(cond) \ if ((cond) == 0) \ - user_vAssertCalled() + vAssertCalled() #else #define BT_ASSERT(cond) \ if (!(cond)) { \ diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp.c deleted file mode 100644 index f7b203e0..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/avdtp.c +++ /dev/null @@ -1,716 +0,0 @@ -/* - * Audio Video Distribution Protocol - * - * SPDX-License-Identifier: Apache-2.0 - * - */ - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_AVDTP) -#define LOG_MODULE_NAME bt_avdtp -#include "log.h" - -#include "hci_core.h" -#include "conn_internal.h" -#include "l2cap_internal.h" -#include "avdtp_internal.h" -#include "a2dp-codec.h" - -#define AVDTP_MSG_POISTION 0x00 -#define AVDTP_PKT_POSITION 0x02 -#define AVDTP_TID_POSITION 0x04 -#define AVDTP_SIGID_MASK 0x3f - -#define AVDTP_GET_TR_ID(hdr) ((hdr & 0xf0) >> AVDTP_TID_POSITION) -#define AVDTP_GET_MSG_TYPE(hdr) (hdr & 0x03) -#define AVDTP_GET_PKT_TYPE(hdr) ((hdr & 0x0c) >> AVDTP_PKT_POSITION) -#define AVDTP_GET_SIG_ID(s) (s & AVDTP_SIGID_MASK) - -static struct bt_avdtp_event_cb *event_cb; - -static struct bt_avdtp_seid_lsep *lseps; - -static uint8_t tid; - -extern struct bt_a2dp_codec_sbc_params sbc_info; - -#define AVDTP_CHAN(_ch) CONTAINER_OF(_ch, struct bt_avdtp, br_chan.chan) - -#define AVDTP_KWORK(_work) CONTAINER_OF(_work, struct bt_avdtp_req, \ - timeout_work) - -#define AVDTP_TIMEOUT K_SECONDS(6) - -static void handle_avdtp_discover_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_get_cap_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_set_conf_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_get_conf_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_reconf_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_open_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_start_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_close_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_suspend_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_abort_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_sec_ctrl_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_get_all_cap_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); -static void handle_avdtp_dly_rpt_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type); - -static const struct { - uint8_t sig_id; - void (*func)(struct bt_avdtp *session, struct net_buf *buf, - uint8_t msg_type); -} handler[] = { - { BT_AVDTP_DISCOVER, handle_avdtp_discover_cmd }, - { BT_AVDTP_GET_CAPABILITIES, handle_avdtp_get_cap_cmd }, - { BT_AVDTP_SET_CONFIGURATION, handle_avdtp_set_conf_cmd }, - { BT_AVDTP_GET_CONFIGURATION, handle_avdtp_get_conf_cmd }, - { BT_AVDTP_RECONFIGURE, handle_avdtp_reconf_cmd }, - { BT_AVDTP_OPEN, handle_avdtp_open_cmd }, - { BT_AVDTP_START, handle_avdtp_start_cmd }, - { BT_AVDTP_CLOSE, handle_avdtp_close_cmd }, - { BT_AVDTP_SUSPEND, handle_avdtp_suspend_cmd }, - { BT_AVDTP_ABORT, handle_avdtp_abort_cmd }, - { BT_AVDTP_SECURITY_CONTROL, handle_avdtp_sec_ctrl_cmd }, - { BT_AVDTP_GET_ALL_CAPABILITIES, handle_avdtp_get_all_cap_cmd }, - { BT_AVDTP_DELAYREPORT, handle_avdtp_dly_rpt_cmd }, -}; - -static int avdtp_send(struct bt_avdtp *session, - struct net_buf *buf, struct bt_avdtp_req *req) -{ - int result; - struct bt_avdtp_single_sig_hdr *hdr; - - hdr = (struct bt_avdtp_single_sig_hdr *)buf->data; - - result = bt_l2cap_chan_send(&session->br_chan.chan, buf); - if (result < 0) { - BT_ERR("Error:L2CAP send fail - result = %d", result); - return result; - } - - /*Save the sent request*/ - req->sig = AVDTP_GET_SIG_ID(hdr->signal_id); - req->tid = AVDTP_GET_TR_ID(hdr->hdr); - BT_DBG("sig 0x%02X, tid 0x%02X", req->sig, req->tid); - - session->req = req; - /* Start timeout work */ - k_delayed_work_submit(&session->req->timeout_work, AVDTP_TIMEOUT); - return result; -} - -static struct net_buf *avdtp_create_pdu(uint8_t msg_type, - uint8_t pkt_type, - uint8_t sig_id) -{ - struct net_buf *buf; - struct bt_avdtp_single_sig_hdr *hdr; - - BT_DBG("tid = %d", tid); - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - - hdr->hdr = (msg_type | pkt_type << AVDTP_PKT_POSITION | - tid++ << AVDTP_TID_POSITION); - tid %= 16; /* Loop for 16*/ - hdr->signal_id = sig_id & AVDTP_SIGID_MASK; - - BT_DBG("hdr = 0x%02X, Signal_ID = 0x%02X", hdr->hdr, hdr->signal_id); - return buf; -} - -/* Timeout handler */ -static void avdtp_timeout(struct k_work *work) -{ - BT_DBG("Failed Signal_id = %d", (AVDTP_KWORK(work))->sig); - - /* Gracefully Disconnect the Signalling and streaming L2cap chann*/ -} - -/** -* @brief avdtp_parsing_capability : parsing avdtp capability content -*/ -static int avdtp_parsing_capability(struct net_buf *buf) -{ - BT_DBG(" "); - while (buf->len) { - uint8_t svc_cat = net_buf_pull_u8(buf); - uint8_t cat_len = net_buf_pull_u8(buf); - ; - switch (svc_cat) { - case BT_AVDTP_SERVICE_CAT_MEDIA_TRANSPORT: - - break; - - case BT_AVDTP_SERVICE_CAT_REPORTING: - - break; - - case BT_AVDTP_SERVICE_CAT_RECOVERY: - - break; - - case BT_AVDTP_SERVICE_CAT_CONTENT_PROTECTION: - - break; - - case BT_AVDTP_SERVICE_CAT_HDR_COMPRESSION: - - break; - - case BT_AVDTP_SERVICE_CAT_MULTIPLEXING: - - break; - - case BT_AVDTP_SERVICE_CAT_MEDIA_CODEC: - - break; - - case BT_AVDTP_SERVICE_CAT_DELAYREPORTING: - - break; - - default: - break; - } - } - - return 0; -} - -static void handle_avdtp_discover_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_DISCOVER); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - struct bt_avdtp_seid_lsep *disc_sep = lseps; - uint8_t acp_endpoint[2]; - while (disc_sep) { - acp_endpoint[0] = disc_sep->sep.id << 2 | disc_sep->sep.inuse << 1 | disc_sep->sep.rfa0; - acp_endpoint[1] = disc_sep->sep.media_type << 4 | disc_sep->sep.tsep << 3 | disc_sep->sep.rfa1; - memcpy(rsp_buf->data + rsp_buf->len, acp_endpoint, 2); - rsp_buf->len += 2; - disc_sep = disc_sep->next; - } - -#if 0 - BT_DBG("rsp_buf len: %d \n", rsp_buf->len); - for(int i = 0; i < rsp_buf->len; i++) - { - BT_WARN("0x%02x, ", rsp_buf->data[i]); - } - BT_WARN("\n"); -#endif - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_get_cap_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_GET_CAPABILITIES); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - uint8_t svc_cat_1[2]; - svc_cat_1[0] = BT_AVDTP_SERVICE_CAT_MEDIA_TRANSPORT; - svc_cat_1[1] = 0; - memcpy(rsp_buf->data + rsp_buf->len, svc_cat_1, 2); - rsp_buf->len += 2; - - uint8_t svc_cat_2[8]; - svc_cat_2[0] = BT_AVDTP_SERVICE_CAT_MEDIA_CODEC; - svc_cat_2[1] = 6; - svc_cat_2[2] = BT_A2DP_AUDIO; - svc_cat_2[3] = BT_A2DP_CODEC_TYPE_SBC; - svc_cat_2[4] = sbc_info.config[0]; - svc_cat_2[5] = sbc_info.config[1]; - svc_cat_2[6] = sbc_info.min_bitpool; - svc_cat_2[7] = sbc_info.max_bitpool; - memcpy(rsp_buf->data + rsp_buf->len, svc_cat_2, 8); - rsp_buf->len += 8; - - uint8_t svc_cat_3[4]; - svc_cat_3[0] = BT_AVDTP_SERVICE_CAT_CONTENT_PROTECTION; - svc_cat_3[1] = 2; - svc_cat_3[2] = BT_AVDTP_CONTENT_PROTECTION_LSB_SCMS_T; - svc_cat_3[3] = BT_AVDTP_CONTENT_PROTECTION_MSB; - memcpy(rsp_buf->data + rsp_buf->len, svc_cat_3, 4); - rsp_buf->len += 4; - -#if 0 - BT_DBG("rsp_buf len: %d \n", rsp_buf->len); - for(int i = 0; i < rsp_buf->len; i++) - { - BT_WARN("0x%02x, ", rsp_buf->data[i]); - } - BT_WARN("\n"); -#endif - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_set_conf_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - uint8_t acp_seid = net_buf_pull_u8(buf) >> 2; - uint8_t int_seid = net_buf_pull_u8(buf) >> 2; - - int res_pars = avdtp_parsing_capability(buf); - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_SET_CONFIGURATION); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_get_conf_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); -} - -static void handle_avdtp_reconf_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); -} - -static void handle_avdtp_open_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_OPEN); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_start_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_START); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_close_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_CLOSE); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_suspend_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_SUSPEND); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_abort_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); - if (!session) { - BT_DBG("Error: Session not valid"); - return; - } - - struct net_buf *rsp_buf; - rsp_buf = avdtp_create_pdu(BT_AVDTP_ACCEPT, BT_AVDTP_PACKET_TYPE_SINGLE, BT_AVDTP_ABORT); - if (!rsp_buf) { - BT_ERR("Error: No Buff available"); - return; - } - - int result = bt_l2cap_chan_send(&session->br_chan.chan, rsp_buf); - if (result < 0) { - BT_ERR("Error: BT L2CAP send fail - result = %d", result); - return; - } -} - -static void handle_avdtp_sec_ctrl_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); -} - -static void handle_avdtp_get_all_cap_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); -} - -static void handle_avdtp_dly_rpt_cmd(struct bt_avdtp *session, struct net_buf *buf, uint8_t msg_type) -{ - BT_DBG(" "); -} - -/* L2CAP Interface callbacks */ -void bt_avdtp_l2cap_connected(struct bt_l2cap_chan *chan) -{ - struct bt_avdtp *session; - - if (!chan) { - BT_ERR("Invalid AVDTP chan"); - return; - } - - session = AVDTP_CHAN(chan); - BT_DBG("chan %p session %p", chan, session); - /* Init the timer */ - k_delayed_work_init(&session->req->timeout_work, avdtp_timeout); -} - -void bt_avdtp_l2cap_disconnected(struct bt_l2cap_chan *chan) -{ - struct bt_avdtp *session = AVDTP_CHAN(chan); - - BT_DBG("chan %p session %p", chan, session); - session->br_chan.chan.conn = NULL; - /* Clear the Pending req if set*/ -} - -void bt_avdtp_l2cap_encrypt_changed(struct bt_l2cap_chan *chan, uint8_t status) -{ - BT_DBG(""); -} - -int bt_avdtp_l2cap_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) -{ - struct bt_avdtp_single_sig_hdr *hdr; - struct bt_avdtp *session = AVDTP_CHAN(chan); - uint8_t i, msgtype, sigid, tid; - - if (buf->len < sizeof(*hdr)) { - BT_ERR("Recvd Wrong AVDTP Header"); - return 0; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - msgtype = AVDTP_GET_MSG_TYPE(hdr->hdr); - sigid = AVDTP_GET_SIG_ID(hdr->signal_id); - tid = AVDTP_GET_TR_ID(hdr->hdr); - - BT_DBG("msg_type[0x%02x] sig_id[0x%02x] tid[0x%02x]", - msgtype, sigid, tid); - -#if 0 - BT_DBG("avdtp payload len: %d \n", buf->len); - for(int i = 0; i < buf->len; i++) - { - BT_WARN("0x%02x, ", buf->data[i]); - } - BT_WARN("\n"); -#endif - - /* validate if there is an outstanding resp expected*/ - if (msgtype != BT_AVDTP_CMD) { - if (session->req == NULL) { - BT_DBG("Unexpected peer response"); - return 0; - } - - if (session->req->sig != sigid || - session->req->tid != tid) { - BT_DBG("Peer mismatch resp, expected sig[0x%02x]" - "tid[0x%02x]", - session->req->sig, - session->req->tid); - return 0; - } - } - - for (i = 0U; i < ARRAY_SIZE(handler); i++) { - if (sigid == handler[i].sig_id) { - handler[i].func(session, buf, msgtype); - return 0; - } - } - - return 0; -} - -int bt_avdtp_l2cap_media_stream_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) -{ -#if 0 - BT_DBG("avdtp payload len: %d \n", buf->len); - for(int i = 0; i < buf->len; i++) - { - BT_WARN("0x%02x, ", buf->data[i]); - } - BT_WARN("\n"); -#endif - - int res = a2dp_sbc_decode_process(buf->data, buf->len); - if (res) { - BT_DBG("decode fail, error: %d \n", res); - } - - return 0; -} - -/*A2DP Layer interface */ -int bt_avdtp_connect(struct bt_conn *conn, struct bt_avdtp *session) -{ - static const struct bt_l2cap_chan_ops ops = { - .connected = bt_avdtp_l2cap_connected, - .disconnected = bt_avdtp_l2cap_disconnected, - .encrypt_change = bt_avdtp_l2cap_encrypt_changed, - .recv = bt_avdtp_l2cap_recv - }; - - if (!session) { - return -EINVAL; - } - - session->br_chan.chan.ops = &ops; - session->br_chan.chan.required_sec_level = BT_SECURITY_L2; - - return bt_l2cap_chan_connect(conn, &session->br_chan.chan, - BT_L2CAP_PSM_AVDTP); -} - -int bt_avdtp_disconnect(struct bt_avdtp *session) -{ - if (!session) { - return -EINVAL; - } - - BT_DBG("session %p", session); - - return bt_l2cap_chan_disconnect(&session->br_chan.chan); -} - -int bt_avdtp_l2cap_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) -{ - struct bt_avdtp *session = NULL; - int result; - static const struct bt_l2cap_chan_ops ops = { - .connected = bt_avdtp_l2cap_connected, - .disconnected = bt_avdtp_l2cap_disconnected, - .recv = bt_avdtp_l2cap_recv, - }; - - static const struct bt_l2cap_chan_ops media_ops = { - .connected = bt_avdtp_l2cap_connected, - .disconnected = bt_avdtp_l2cap_disconnected, - .recv = bt_avdtp_l2cap_media_stream_recv, - }; - - BT_DBG("conn %p", conn); - /* Get the AVDTP session from upper layer */ - result = event_cb->accept(conn, &session); - if (result < 0) { - return result; - } - - if (!session->br_chan.chan.conn) { - BT_DBG("create l2cap_br signal stream, session %p", session); - session->br_chan.chan.ops = &ops; - session->br_chan.rx.mtu = BT_AVDTP_MAX_MTU; - *chan = &session->br_chan.chan; - tid = 0; - } else { - BT_DBG("create l2cap_br AV stream, session %p", session); - session->streams->chan.chan.ops = &media_ops; - session->streams->chan.rx.mtu = BT_AVDTP_MAX_MTU; - *chan = &session->streams->chan.chan; - } - - return 0; -} - -/* Application will register its callback */ -int bt_avdtp_register(struct bt_avdtp_event_cb *cb) -{ - BT_DBG(""); - - if (event_cb) { - return -EALREADY; - } - - event_cb = cb; - - return 0; -} - -int bt_avdtp_register_sep(uint8_t media_type, uint8_t role, - struct bt_avdtp_seid_lsep *lsep) -{ - BT_DBG(""); - - static uint8_t bt_avdtp_seid = BT_AVDTP_MIN_SEID; - - if (!lsep) { - return -EIO; - } - - if (bt_avdtp_seid == BT_AVDTP_MAX_SEID) { - return -EIO; - } - - lsep->sep.id = bt_avdtp_seid++; - lsep->sep.inuse = 0U; - lsep->sep.media_type = media_type; - lsep->sep.tsep = role; - - lsep->next = lseps; - lseps = lsep; - - return 0; -} - -/* init function */ -int bt_avdtp_init(void) -{ - int err; - static struct bt_l2cap_server avdtp_l2cap = { - .psm = BT_L2CAP_PSM_AVDTP, - .sec_level = BT_SECURITY_L2, - .accept = bt_avdtp_l2cap_accept, - }; - - BT_DBG(""); - - /* Register AVDTP PSM with L2CAP */ - err = bt_l2cap_br_server_register(&avdtp_l2cap); - if (err < 0) { - BT_ERR("AVDTP L2CAP Registration failed %d", err); - } - - return err; -} - -/* AVDTP Discover Request */ -int bt_avdtp_discover(struct bt_avdtp *session, - struct bt_avdtp_discover_params *param) -{ - struct net_buf *buf; - - BT_DBG(""); - if (!param || !session) { - BT_DBG("Error: Callback/Session not valid"); - return -EINVAL; - } - - buf = avdtp_create_pdu(BT_AVDTP_CMD, - BT_AVDTP_PACKET_TYPE_SINGLE, - BT_AVDTP_DISCOVER); - if (!buf) { - BT_ERR("Error: No Buff available"); - return -ENOMEM; - } - - /* Body of the message */ - - return avdtp_send(session, buf, ¶m->req); -} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap_br.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap_br.c deleted file mode 100644 index a282debc..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/l2cap_br.c +++ /dev/null @@ -1,1574 +0,0 @@ -/* l2cap_br.c - L2CAP BREDR oriented handling */ - -/* - * Copyright (c) 2016 Intel Corporation - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_L2CAP) -#define LOG_MODULE_NAME bt_l2cap_br -#include "log.h" - -#include "hci_core.h" -#include "conn_internal.h" -#include "l2cap_internal.h" -#include "sdp_internal.h" -#include "avdtp_internal.h" -#include "a2dp_internal.h" -#include "rfcomm_internal.h" -#include "hfp_hf.h" - -#define BR_CHAN(_ch) CONTAINER_OF(_ch, struct bt_l2cap_br_chan, chan) -#define BR_CHAN_RTX(_w) CONTAINER_OF(_w, struct bt_l2cap_br_chan, chan.rtx_work) - -#define L2CAP_BR_PSM_START 0x0001 -#define L2CAP_BR_PSM_END 0xffff - -#define L2CAP_BR_CID_DYN_START 0x0040 -#define L2CAP_BR_CID_DYN_END 0xffff -#define L2CAP_BR_CID_IS_DYN(_cid) \ - (_cid >= L2CAP_BR_CID_DYN_START && _cid <= L2CAP_BR_CID_DYN_END) - -#define L2CAP_BR_MIN_MTU 48 -#define L2CAP_BR_DEFAULT_MTU 672 - -#define L2CAP_BR_PSM_SDP 0x0001 - -#define L2CAP_BR_INFO_TIMEOUT K_SECONDS(4) -#define L2CAP_BR_CFG_TIMEOUT K_SECONDS(4) -#define L2CAP_BR_DISCONN_TIMEOUT K_SECONDS(1) -#define L2CAP_BR_CONN_TIMEOUT K_SECONDS(40) - -/* - * L2CAP extended feature mask: - * BR/EDR fixed channel support enabled - */ -#define L2CAP_FEAT_FIXED_CHAN_MASK 0x00000080 - -enum { - /* Connection oriented channels flags */ - L2CAP_FLAG_CONN_LCONF_DONE, /* local config accepted by remote */ - L2CAP_FLAG_CONN_RCONF_DONE, /* remote config accepted by local */ - L2CAP_FLAG_CONN_ACCEPTOR, /* getting incoming connection req */ - L2CAP_FLAG_CONN_PENDING, /* remote sent pending result in rsp */ - - /* Signaling channel flags */ - L2CAP_FLAG_SIG_INFO_PENDING, /* retrieving remote l2cap info */ - L2CAP_FLAG_SIG_INFO_DONE, /* remote l2cap info is done */ - - /* fixed channels flags */ - L2CAP_FLAG_FIXED_CONNECTED, /* fixed connected */ -}; - -static sys_slist_t br_servers; - -/* Pool for outgoing BR/EDR signaling packets, min MTU is 48 */ -#if !defined(BFLB_DYNAMIC_ALLOC_MEM) -NET_BUF_POOL_FIXED_DEFINE(br_sig_pool, CONFIG_BT_MAX_CONN, - BT_L2CAP_BUF_SIZE(L2CAP_BR_MIN_MTU), NULL); -#else -struct net_buf_pool br_sig_pool; -#endif - -/* BR/EDR L2CAP signalling channel specific context */ -struct bt_l2cap_br { - /* The channel this context is associated with */ - struct bt_l2cap_br_chan chan; - uint8_t info_ident; - uint8_t info_fixed_chan; - uint32_t info_feat_mask; -}; - -static struct bt_l2cap_br bt_l2cap_br_pool[CONFIG_BT_MAX_CONN]; - -struct bt_l2cap_chan *bt_l2cap_br_lookup_rx_cid(struct bt_conn *conn, - uint16_t cid) -{ - struct bt_l2cap_chan *chan; - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) - { - if (BR_CHAN(chan)->rx.cid == cid) { - return chan; - } - } - - return NULL; -} - -struct bt_l2cap_chan *bt_l2cap_br_lookup_tx_cid(struct bt_conn *conn, - uint16_t cid) -{ - struct bt_l2cap_chan *chan; - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) - { - if (BR_CHAN(chan)->tx.cid == cid) { - return chan; - } - } - - return NULL; -} - -static struct bt_l2cap_br_chan * -l2cap_br_chan_alloc_cid(struct bt_conn *conn, struct bt_l2cap_chan *chan) -{ - struct bt_l2cap_br_chan *ch = BR_CHAN(chan); - uint16_t cid; - - /* - * No action needed if there's already a CID allocated, e.g. in - * the case of a fixed channel. - */ - if (ch->rx.cid > 0) { - return ch; - } - - /* - * L2CAP_BR_CID_DYN_END is 0xffff so we don't check against it since - * cid is uint16_t, just check against uint16_t overflow - */ - for (cid = L2CAP_BR_CID_DYN_START; cid; cid++) { - if (!bt_l2cap_br_lookup_rx_cid(conn, cid)) { - ch->rx.cid = cid; - return ch; - } - } - - return NULL; -} - -static void l2cap_br_chan_cleanup(struct bt_l2cap_chan *chan) -{ - bt_l2cap_chan_remove(chan->conn, chan); - bt_l2cap_chan_del(chan); -} - -static void l2cap_br_chan_destroy(struct bt_l2cap_chan *chan) -{ - BT_DBG("chan %p cid 0x%04x", BR_CHAN(chan), BR_CHAN(chan)->rx.cid); - - /* Cancel ongoing work */ - k_delayed_work_cancel(&chan->rtx_work); - - atomic_clear(BR_CHAN(chan)->flags); -} - -static void l2cap_br_rtx_timeout(struct k_work *work) -{ - struct bt_l2cap_br_chan *chan = BR_CHAN_RTX(work); - - BT_WARN("chan %p timeout", chan); - - if (chan->rx.cid == BT_L2CAP_CID_BR_SIG) { - BT_DBG("Skip BR/EDR signalling channel "); - atomic_clear_bit(chan->flags, L2CAP_FLAG_SIG_INFO_PENDING); - return; - } - - BT_DBG("chan %p %s scid 0x%04x", chan, - bt_l2cap_chan_state_str(chan->chan.state), - chan->rx.cid); - - switch (chan->chan.state) { - case BT_L2CAP_CONFIG: - bt_l2cap_br_chan_disconnect(&chan->chan); - break; - case BT_L2CAP_DISCONNECT: - case BT_L2CAP_CONNECT: - l2cap_br_chan_cleanup(&chan->chan); - break; - default: - break; - } -} - -static bool l2cap_br_chan_add(struct bt_conn *conn, struct bt_l2cap_chan *chan, - bt_l2cap_chan_destroy_t destroy) -{ - struct bt_l2cap_br_chan *ch = l2cap_br_chan_alloc_cid(conn, chan); - - if (!ch) { - BT_DBG("Unable to allocate L2CAP CID"); - return false; - } - - k_delayed_work_init(&chan->rtx_work, l2cap_br_rtx_timeout); - bt_l2cap_chan_add(conn, chan, destroy); - - return true; -} - -static uint8_t l2cap_br_get_ident(void) -{ - static uint8_t ident; - - ident++; - /* handle integer overflow (0 is not valid) */ - if (!ident) { - ident++; - } - - return ident; -} - -static void l2cap_br_chan_send_req(struct bt_l2cap_br_chan *chan, - struct net_buf *buf, s32_t timeout) -{ - /* BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part A] page 126: - * - * The value of this timer is implementation-dependent but the minimum - * initial value is 1 second and the maximum initial value is 60 - * seconds. One RTX timer shall exist for each outstanding signaling - * request, including each Echo Request. The timer disappears on the - * final expiration, when the response is received, or the physical - * link is lost. - */ - k_delayed_work_submit(&chan->chan.rtx_work, timeout); - - bt_l2cap_send(chan->chan.conn, BT_L2CAP_CID_BR_SIG, buf); -} - -static void l2cap_br_get_info(struct bt_l2cap_br *l2cap, uint16_t info_type) -{ - struct bt_l2cap_info_req *info; - struct net_buf *buf; - struct bt_l2cap_sig_hdr *hdr; - - BT_DBG("info type %u", info_type); - - if (atomic_test_bit(l2cap->chan.flags, L2CAP_FLAG_SIG_INFO_PENDING)) { - return; - } - - switch (info_type) { - case BT_L2CAP_INFO_FEAT_MASK: - case BT_L2CAP_INFO_FIXED_CHAN: - break; - default: - BT_WARN("Unsupported info type %u", info_type); - return; - } - - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - atomic_set_bit(l2cap->chan.flags, L2CAP_FLAG_SIG_INFO_PENDING); - l2cap->info_ident = l2cap_br_get_ident(); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_INFO_REQ; - hdr->ident = l2cap->info_ident; - hdr->len = sys_cpu_to_le16(sizeof(*info)); - - info = net_buf_add(buf, sizeof(*info)); - info->type = sys_cpu_to_le16(info_type); - - l2cap_br_chan_send_req(&l2cap->chan, buf, L2CAP_BR_INFO_TIMEOUT); -} - -static void connect_fixed_channel(struct bt_l2cap_br_chan *chan) -{ - if (atomic_test_and_set_bit(chan->flags, L2CAP_FLAG_FIXED_CONNECTED)) { - return; - } - - if (chan->chan.ops && chan->chan.ops->connected) { - chan->chan.ops->connected(&chan->chan); - } -} - -static void connect_optional_fixed_channels(struct bt_l2cap_br *l2cap) -{ - /* can be change to loop if more BR/EDR fixed channels are added */ - if (l2cap->info_fixed_chan & BIT(BT_L2CAP_CID_BR_SMP)) { - struct bt_l2cap_chan *chan; - - chan = bt_l2cap_br_lookup_rx_cid(l2cap->chan.chan.conn, - BT_L2CAP_CID_BR_SMP); - if (chan) { - connect_fixed_channel(BR_CHAN(chan)); - } - } -} - -static int l2cap_br_info_rsp(struct bt_l2cap_br *l2cap, uint8_t ident, - struct net_buf *buf) -{ - struct bt_l2cap_info_rsp *rsp; - uint16_t type, result; - int err = 0; - - if (atomic_test_bit(l2cap->chan.flags, L2CAP_FLAG_SIG_INFO_DONE)) { - return 0; - } - - if (atomic_test_and_clear_bit(l2cap->chan.flags, - L2CAP_FLAG_SIG_INFO_PENDING)) { - /* - * Release RTX timer since got the response & there's pending - * command request. - */ - k_delayed_work_cancel(&l2cap->chan.chan.rtx_work); - } - - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small info rsp packet size"); - err = -EINVAL; - goto done; - } - - if (ident != l2cap->info_ident) { - BT_WARN("Idents mismatch"); - err = -EINVAL; - goto done; - } - - rsp = net_buf_pull_mem(buf, sizeof(*rsp)); - result = sys_le16_to_cpu(rsp->result); - if (result != BT_L2CAP_INFO_SUCCESS) { - BT_WARN("Result unsuccessful"); - err = -EINVAL; - goto done; - } - - type = sys_le16_to_cpu(rsp->type); - - switch (type) { - case BT_L2CAP_INFO_FEAT_MASK: - l2cap->info_feat_mask = net_buf_pull_le32(buf); - BT_DBG("remote info mask 0x%08x", l2cap->info_feat_mask); - - if (!(l2cap->info_feat_mask & L2CAP_FEAT_FIXED_CHAN_MASK)) { - break; - } - - l2cap_br_get_info(l2cap, BT_L2CAP_INFO_FIXED_CHAN); - return 0; - case BT_L2CAP_INFO_FIXED_CHAN: - l2cap->info_fixed_chan = net_buf_pull_u8(buf); - BT_DBG("remote fixed channel mask 0x%02x", - l2cap->info_fixed_chan); - - connect_optional_fixed_channels(l2cap); - - break; - default: - BT_WARN("type 0x%04x unsupported", type); - err = -EINVAL; - break; - } -done: - atomic_set_bit(l2cap->chan.flags, L2CAP_FLAG_SIG_INFO_DONE); - l2cap->info_ident = 0U; - return err; -} - -static uint8_t get_fixed_channels_mask(void) -{ - uint8_t mask = 0U; - - /* this needs to be enhanced if AMP Test Manager support is added */ - Z_STRUCT_SECTION_FOREACH(bt_l2cap_br_fixed_chan, fchan) - { - mask |= BIT(fchan->cid); - } - - return mask; -} - -static int l2cap_br_info_req(struct bt_l2cap_br *l2cap, uint8_t ident, - struct net_buf *buf) -{ - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_info_req *req = (void *)buf->data; - struct bt_l2cap_info_rsp *rsp; - struct net_buf *rsp_buf; - struct bt_l2cap_sig_hdr *hdr_info; - uint16_t type; - - if (buf->len < sizeof(*req)) { - BT_ERR("Too small info req packet size"); - return -EINVAL; - } - - rsp_buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - type = sys_le16_to_cpu(req->type); - BT_DBG("type 0x%04x", type); - - hdr_info = net_buf_add(rsp_buf, sizeof(*hdr_info)); - hdr_info->code = BT_L2CAP_INFO_RSP; - hdr_info->ident = ident; - - rsp = net_buf_add(rsp_buf, sizeof(*rsp)); - - switch (type) { - case BT_L2CAP_INFO_FEAT_MASK: - rsp->type = sys_cpu_to_le16(BT_L2CAP_INFO_FEAT_MASK); - rsp->result = sys_cpu_to_le16(BT_L2CAP_INFO_SUCCESS); - net_buf_add_le32(rsp_buf, L2CAP_FEAT_FIXED_CHAN_MASK); - hdr_info->len = sys_cpu_to_le16(sizeof(*rsp) + sizeof(uint32_t)); - break; - case BT_L2CAP_INFO_FIXED_CHAN: - rsp->type = sys_cpu_to_le16(BT_L2CAP_INFO_FIXED_CHAN); - rsp->result = sys_cpu_to_le16(BT_L2CAP_INFO_SUCCESS); - /* fixed channel mask protocol data is 8 octets wide */ - (void)memset(net_buf_add(rsp_buf, 8), 0, 8); - rsp->data[0] = get_fixed_channels_mask(); - - hdr_info->len = sys_cpu_to_le16(sizeof(*rsp) + 8); - break; - default: - rsp->type = req->type; - rsp->result = sys_cpu_to_le16(BT_L2CAP_INFO_NOTSUPP); - hdr_info->len = sys_cpu_to_le16(sizeof(*rsp)); - break; - } - - bt_l2cap_send(conn, BT_L2CAP_CID_BR_SIG, rsp_buf); - - return 0; -} - -void bt_l2cap_br_connected(struct bt_conn *conn) -{ - struct bt_l2cap_chan *chan; - - Z_STRUCT_SECTION_FOREACH(bt_l2cap_br_fixed_chan, fchan) - { - struct bt_l2cap_br_chan *ch; - - if (!fchan->accept) { - continue; - } - - if (fchan->accept(conn, &chan) < 0) { - continue; - } - - ch = BR_CHAN(chan); - - ch->rx.cid = fchan->cid; - ch->tx.cid = fchan->cid; - - if (!l2cap_br_chan_add(conn, chan, NULL)) { - return; - } - - /* - * other fixed channels will be connected after Information - * Response is received - */ - if (fchan->cid == BT_L2CAP_CID_BR_SIG) { - struct bt_l2cap_br *sig_ch; - - connect_fixed_channel(ch); - - sig_ch = CONTAINER_OF(ch, struct bt_l2cap_br, chan); - l2cap_br_get_info(sig_ch, BT_L2CAP_INFO_FEAT_MASK); - } - } -} - -static struct bt_l2cap_server *l2cap_br_server_lookup_psm(uint16_t psm) -{ - struct bt_l2cap_server *server; - - SYS_SLIST_FOR_EACH_CONTAINER(&br_servers, server, node) - { - if (server->psm == psm) { - return server; - } - } - - return NULL; -} - -static void l2cap_br_conf_add_mtu(struct net_buf *buf, const uint16_t mtu) -{ - net_buf_add_u8(buf, BT_L2CAP_CONF_OPT_MTU); - net_buf_add_u8(buf, sizeof(mtu)); - net_buf_add_le16(buf, mtu); -} - -static void l2cap_br_conf(struct bt_l2cap_chan *chan) -{ - struct bt_l2cap_sig_hdr *hdr; - struct bt_l2cap_conf_req *conf; - struct net_buf *buf; - - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_CONF_REQ; - hdr->ident = l2cap_br_get_ident(); - conf = net_buf_add(buf, sizeof(*conf)); - (void)memset(conf, 0, sizeof(*conf)); - - conf->dcid = sys_cpu_to_le16(BR_CHAN(chan)->tx.cid); - /* - * Add MTU option if app set non default BR/EDR L2CAP MTU, - * otherwise sent empty configuration data meaning default MTU - * to be used. - */ - if (BR_CHAN(chan)->rx.mtu != L2CAP_BR_DEFAULT_MTU) { - l2cap_br_conf_add_mtu(buf, BR_CHAN(chan)->rx.mtu); - } - - hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); - - /* - * TODO: - * might be needed to start tracking number of configuration iterations - * on both directions - */ - l2cap_br_chan_send_req(BR_CHAN(chan), buf, L2CAP_BR_CFG_TIMEOUT); -} - -enum l2cap_br_conn_security_result { - L2CAP_CONN_SECURITY_PASSED, - L2CAP_CONN_SECURITY_REJECT, - L2CAP_CONN_SECURITY_PENDING -}; - -/* - * Security helper against channel connection. - * Returns L2CAP_CONN_SECURITY_PASSED if: - * - existing security on link is applicable for requested PSM in connection, - * - legacy (non SSP) devices connecting with low security requirements, - * Returns L2CAP_CONN_SECURITY_PENDING if: - * - channel connection process is on hold since there were valid security - * conditions triggering authentication indirectly in subcall. - * Returns L2CAP_CONN_SECURITY_REJECT if: - * - bt_conn_set_security API returns < 0. - */ - -static enum l2cap_br_conn_security_result -l2cap_br_conn_security(struct bt_l2cap_chan *chan, const uint16_t psm) -{ - int check; - - /* For SDP PSM there's no need to change existing security on link */ - if (chan->required_sec_level == BT_SECURITY_L0) { - return L2CAP_CONN_SECURITY_PASSED; - } - - /* - * No link key needed for legacy devices (pre 2.1) and when low security - * level is required. - */ - if (chan->required_sec_level == BT_SECURITY_L1 && - !BT_FEAT_HOST_SSP(chan->conn->br.features)) { - return L2CAP_CONN_SECURITY_PASSED; - } - - switch (chan->required_sec_level) { - case BT_SECURITY_L4: - case BT_SECURITY_L3: - case BT_SECURITY_L2: - break; - default: - /* - * For non SDP PSM connections GAP's Security Mode 4 requires at - * least unauthenticated link key and enabled encryption if - * remote supports SSP before any L2CAP CoC traffic. So preset - * local to MEDIUM security to trigger it if needed. - */ - if (BT_FEAT_HOST_SSP(chan->conn->br.features)) { - chan->required_sec_level = BT_SECURITY_L2; - } - break; - } - - check = bt_conn_set_security(chan->conn, chan->required_sec_level); - - /* - * Check case when on existing connection security level already covers - * channel (service) security requirements against link security and - * bt_conn_set_security API returns 0 what implies also there was no - * need to trigger authentication. - */ - if (check == 0 && - chan->conn->sec_level >= chan->required_sec_level) { - return L2CAP_CONN_SECURITY_PASSED; - } - - /* - * If 'check' still holds 0, it means local host just sent HCI - * authentication command to start procedure to increase link security - * since service/profile requires that. - */ - if (check == 0) { - return L2CAP_CONN_SECURITY_PENDING; - }; - - /* - * For any other values in 'check' it means there was internal - * validation condition forbidding to start authentication at this - * moment. - */ - return L2CAP_CONN_SECURITY_REJECT; -} - -static void l2cap_br_send_conn_rsp(struct bt_conn *conn, uint16_t scid, - uint16_t dcid, uint8_t ident, uint16_t result) -{ - struct net_buf *buf; - struct bt_l2cap_conn_rsp *rsp; - struct bt_l2cap_sig_hdr *hdr; - - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_CONN_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(dcid); - rsp->scid = sys_cpu_to_le16(scid); - rsp->result = sys_cpu_to_le16(result); - - if (result == BT_L2CAP_BR_PENDING) { - rsp->status = sys_cpu_to_le16(BT_L2CAP_CS_AUTHEN_PEND); - } else { - rsp->status = sys_cpu_to_le16(BT_L2CAP_CS_NO_INFO); - } - - bt_l2cap_send(conn, BT_L2CAP_CID_BR_SIG, buf); -} - -static int l2cap_br_conn_req_reply(struct bt_l2cap_chan *chan, uint16_t result) -{ - /* Send response to connection request only when in acceptor role */ - if (!atomic_test_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_ACCEPTOR)) { - return -ESRCH; - } - - l2cap_br_send_conn_rsp(chan->conn, BR_CHAN(chan)->tx.cid, - BR_CHAN(chan)->rx.cid, chan->ident, result); - chan->ident = 0U; - - return 0; -} - -static void l2cap_br_conn_req(struct bt_l2cap_br *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_conn_req *req = (void *)buf->data; - uint16_t psm, scid, result; - - if (buf->len < sizeof(*req)) { - BT_ERR("Too small L2CAP conn req packet size"); - return; - } - - psm = sys_le16_to_cpu(req->psm); - scid = sys_le16_to_cpu(req->scid); - - BT_DBG("psm 0x%02x scid 0x%04x", psm, scid); - - /* Check if there is a server registered */ - server = l2cap_br_server_lookup_psm(psm); - if (!server) { - result = BT_L2CAP_BR_ERR_PSM_NOT_SUPP; - goto no_chan; - } - - /* - * Report security violation for non SDP channel without encryption when - * remote supports SSP. - */ - if (server->sec_level != BT_SECURITY_L0 && - BT_FEAT_HOST_SSP(conn->br.features) && !conn->encrypt) { - result = BT_L2CAP_BR_ERR_SEC_BLOCK; - goto no_chan; - } - - if (!L2CAP_BR_CID_IS_DYN(scid)) { - result = BT_L2CAP_BR_ERR_INVALID_SCID; - goto no_chan; - } - - chan = bt_l2cap_br_lookup_tx_cid(conn, scid); - if (chan) { - /* - * we have a chan here but this is due to SCID being already in - * use so it is not channel we are suppose to pass to - * l2cap_br_conn_req_reply as wrong DCID would be used - */ - result = BT_L2CAP_BR_ERR_SCID_IN_USE; - goto no_chan; - } - - /* - * Request server to accept the new connection and allocate the - * channel. If no free channels available for PSM server reply with - * proper result and quit since chan pointer is uninitialized then. - */ - if (server->accept(conn, &chan) < 0) { - result = BT_L2CAP_BR_ERR_NO_RESOURCES; - goto no_chan; - } - - chan->required_sec_level = server->sec_level; - - l2cap_br_chan_add(conn, chan, l2cap_br_chan_destroy); - BR_CHAN(chan)->tx.cid = scid; - chan->ident = ident; - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECT); - atomic_set_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_ACCEPTOR); - - /* Disable fragmentation of l2cap rx pdu */ - BR_CHAN(chan)->rx.mtu = MIN(BR_CHAN(chan)->rx.mtu, - CONFIG_BT_L2CAP_RX_MTU); - - switch (l2cap_br_conn_security(chan, psm)) { - case L2CAP_CONN_SECURITY_PENDING: - result = BT_L2CAP_BR_PENDING; - /* TODO: auth timeout */ - break; - case L2CAP_CONN_SECURITY_PASSED: - result = BT_L2CAP_BR_SUCCESS; - break; - case L2CAP_CONN_SECURITY_REJECT: - default: - result = BT_L2CAP_BR_ERR_SEC_BLOCK; - break; - } - /* Reply on connection request as acceptor */ - l2cap_br_conn_req_reply(chan, result); - - if (result != BT_L2CAP_BR_SUCCESS) { - /* Disconnect link when security rules were violated */ - if (result == BT_L2CAP_BR_ERR_SEC_BLOCK) { - bt_conn_disconnect(conn, BT_HCI_ERR_AUTH_FAIL); - } - - return; - } - - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONFIG); - l2cap_br_conf(chan); - return; - -no_chan: - l2cap_br_send_conn_rsp(conn, scid, 0, ident, result); -} - -static void l2cap_br_conf_rsp(struct bt_l2cap_br *l2cap, uint8_t ident, - uint16_t len, struct net_buf *buf) -{ - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_chan *chan; - struct bt_l2cap_conf_rsp *rsp = (void *)buf->data; - uint16_t flags, scid, result, opt_len; - - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small L2CAP conf rsp packet size"); - return; - } - - flags = sys_le16_to_cpu(rsp->flags); - scid = sys_le16_to_cpu(rsp->scid); - result = sys_le16_to_cpu(rsp->result); - opt_len = len - sizeof(*rsp); - - BT_DBG("scid 0x%04x flags 0x%02x result 0x%02x len %u", scid, flags, - result, opt_len); - - chan = bt_l2cap_br_lookup_rx_cid(conn, scid); - if (!chan) { - BT_ERR("channel mismatch!"); - return; - } - - /* Release RTX work since got the response */ - k_delayed_work_cancel(&chan->rtx_work); - - /* - * TODO: handle other results than success and parse response data if - * available - */ - switch (result) { - case BT_L2CAP_CONF_SUCCESS: - atomic_set_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_LCONF_DONE); - - if (chan->state == BT_L2CAP_CONFIG && - atomic_test_bit(BR_CHAN(chan)->flags, - L2CAP_FLAG_CONN_RCONF_DONE)) { - BT_DBG("scid 0x%04x rx MTU %u dcid 0x%04x tx MTU %u", - BR_CHAN(chan)->rx.cid, BR_CHAN(chan)->rx.mtu, - BR_CHAN(chan)->tx.cid, BR_CHAN(chan)->tx.mtu); - - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECTED); - if (chan->ops && chan->ops->connected) { - chan->ops->connected(chan); - } - } - break; - default: - /* currently disconnect channel on non success result */ - bt_l2cap_chan_disconnect(chan); - break; - } -} - -int bt_l2cap_br_server_register(struct bt_l2cap_server *server) -{ - if (server->psm < L2CAP_BR_PSM_START || !server->accept) { - return -EINVAL; - } - - /* PSM must be odd and lsb of upper byte must be 0 */ - if ((server->psm & 0x0101) != 0x0001) { - return -EINVAL; - } - - if (server->sec_level > BT_SECURITY_L4) { - return -EINVAL; - } else if (server->sec_level == BT_SECURITY_L0 && - server->psm != L2CAP_BR_PSM_SDP) { - server->sec_level = BT_SECURITY_L1; - } - - /* Check if given PSM is already in use */ - if (l2cap_br_server_lookup_psm(server->psm)) { - BT_DBG("PSM already registered"); - return -EADDRINUSE; - } - - BT_DBG("PSM 0x%04x", server->psm); - - sys_slist_append(&br_servers, &server->node); - - return 0; -} - -static void l2cap_br_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(&br_sig_pool, 0); - - 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); - - /* - * 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) { - net_buf_add_mem(buf, data, data_len); - } - - bt_l2cap_send(conn, BT_L2CAP_CID_BR_SIG, buf); -} - -static uint16_t l2cap_br_conf_opt_mtu(struct bt_l2cap_chan *chan, - struct net_buf *buf, size_t len) -{ - uint16_t mtu, result = BT_L2CAP_CONF_SUCCESS; - - /* Core 4.2 [Vol 3, Part A, 5.1] MTU payload length */ - if (len != 2) { - BT_ERR("tx MTU length %zu invalid", len); - result = BT_L2CAP_CONF_REJECT; - goto done; - } - - /* pulling MTU value moves buf data to next option item */ - mtu = net_buf_pull_le16(buf); - if (mtu < L2CAP_BR_MIN_MTU) { - result = BT_L2CAP_CONF_UNACCEPT; - BR_CHAN(chan)->tx.mtu = L2CAP_BR_MIN_MTU; - BT_DBG("tx MTU %u invalid", mtu); - goto done; - } - - BR_CHAN(chan)->tx.mtu = mtu; - BT_DBG("tx MTU %u", mtu); -done: - return result; -} - -static void l2cap_br_conf_req(struct bt_l2cap_br *l2cap, uint8_t ident, - uint16_t len, struct net_buf *buf) -{ - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_chan *chan; - struct bt_l2cap_conf_req *req; - struct bt_l2cap_sig_hdr *hdr; - struct bt_l2cap_conf_rsp *rsp; - struct bt_l2cap_conf_opt *opt; - uint16_t flags, dcid, opt_len, hint, result = BT_L2CAP_CONF_SUCCESS; - - if (buf->len < sizeof(*req)) { - BT_ERR("Too small L2CAP conf req packet size"); - return; - } - - req = net_buf_pull_mem(buf, sizeof(*req)); - flags = sys_le16_to_cpu(req->flags); - dcid = sys_le16_to_cpu(req->dcid); - opt_len = len - sizeof(*req); - - BT_DBG("dcid 0x%04x flags 0x%02x len %u", dcid, flags, opt_len); - - chan = bt_l2cap_br_lookup_rx_cid(conn, dcid); - if (!chan) { - BT_ERR("rx channel mismatch!"); - struct bt_l2cap_cmd_reject_cid_data data = { - .scid = req->dcid, - .dcid = 0, - }; - - l2cap_br_send_reject(conn, ident, BT_L2CAP_REJ_INVALID_CID, - &data, sizeof(data)); - return; - } - - if (!opt_len) { - BT_DBG("tx default MTU %u", L2CAP_BR_DEFAULT_MTU); - BR_CHAN(chan)->tx.mtu = L2CAP_BR_DEFAULT_MTU; - goto send_rsp; - } - - while (buf->len >= sizeof(*opt)) { - opt = net_buf_pull_mem(buf, sizeof(*opt)); - - /* make sure opt object can get safe dereference in iteration */ - if (buf->len < opt->len) { - BT_ERR("Received too short option data"); - result = BT_L2CAP_CONF_REJECT; - break; - } - - hint = opt->type & BT_L2CAP_CONF_HINT; - - switch (opt->type & BT_L2CAP_CONF_MASK) { - case BT_L2CAP_CONF_OPT_MTU: - /* getting MTU modifies buf internals */ - result = l2cap_br_conf_opt_mtu(chan, buf, opt->len); - /* - * MTU is done. For now bailout the loop but later on - * there can be a need to continue checking next options - * that are after MTU value and then goto is not proper - * way out here. - */ - goto send_rsp; - default: - if (!hint) { - BT_DBG("option %u not handled", opt->type); - goto send_rsp; - } - - /* Update buffer to point at next option */ - net_buf_pull(buf, opt->len); - break; - } - } - -send_rsp: - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_CONF_RSP; - hdr->ident = ident; - rsp = net_buf_add(buf, sizeof(*rsp)); - (void)memset(rsp, 0, sizeof(*rsp)); - - rsp->result = sys_cpu_to_le16(result); - rsp->scid = sys_cpu_to_le16(BR_CHAN(chan)->tx.cid); - - /* - * TODO: If options other than MTU bacame meaningful then processing - * the options chain need to be modified and taken into account when - * sending back to peer. - */ - if (result == BT_L2CAP_CONF_UNACCEPT) { - l2cap_br_conf_add_mtu(buf, BR_CHAN(chan)->tx.mtu); - } - - hdr->len = sys_cpu_to_le16(buf->len - sizeof(*hdr)); - - bt_l2cap_send(conn, BT_L2CAP_CID_BR_SIG, buf); - - if (result != BT_L2CAP_CONF_SUCCESS) { - return; - } - - atomic_set_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_RCONF_DONE); - - if (atomic_test_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_LCONF_DONE) && - chan->state == BT_L2CAP_CONFIG) { - BT_DBG("scid 0x%04x rx MTU %u dcid 0x%04x tx MTU %u", - BR_CHAN(chan)->rx.cid, BR_CHAN(chan)->rx.mtu, - BR_CHAN(chan)->tx.cid, BR_CHAN(chan)->tx.mtu); - - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECTED); - if (chan->ops && chan->ops->connected) { - chan->ops->connected(chan); - } - } -} - -static struct bt_l2cap_br_chan *l2cap_br_remove_tx_cid(struct bt_conn *conn, - uint16_t cid) -{ - struct bt_l2cap_chan *chan; - sys_snode_t *prev = NULL; - - /* Protect fixed channels against accidental removal */ - if (!L2CAP_BR_CID_IS_DYN(cid)) { - return NULL; - } - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) - { - if (BR_CHAN(chan)->tx.cid == cid) { - sys_slist_remove(&conn->channels, prev, &chan->node); - return BR_CHAN(chan); - } - - prev = &chan->node; - } - - return NULL; -} - -static void l2cap_br_disconn_req(struct bt_l2cap_br *l2cap, uint8_t ident, - struct net_buf *buf) -{ - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_br_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 disconn 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_br_remove_tx_cid(conn, scid); - if (!chan) { - 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; - } - - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - 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); - - bt_l2cap_chan_del(&chan->chan); - - bt_l2cap_send(conn, BT_L2CAP_CID_BR_SIG, buf); -} - -static void l2cap_br_connected(struct bt_l2cap_chan *chan) -{ - BT_DBG("ch %p cid 0x%04x", BR_CHAN(chan), BR_CHAN(chan)->rx.cid); -} - -static void l2cap_br_disconnected(struct bt_l2cap_chan *chan) -{ - BT_DBG("ch %p cid 0x%04x", BR_CHAN(chan), BR_CHAN(chan)->rx.cid); - - if (atomic_test_and_clear_bit(BR_CHAN(chan)->flags, - L2CAP_FLAG_SIG_INFO_PENDING)) { - /* Cancel RTX work on signal channel */ - k_delayed_work_cancel(&chan->rtx_work); - } -} - -int bt_l2cap_br_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_br_chan *ch; - - if (!conn) { - return -ENOTCONN; - } - - if (chan->state == BT_L2CAP_DISCONNECT) { - return -EALREADY; - } - - ch = BR_CHAN(chan); - - BT_DBG("chan %p scid 0x%04x dcid 0x%04x", chan, ch->rx.cid, - ch->tx.cid); - - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_DISCONN_REQ; - hdr->ident = l2cap_br_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); - - l2cap_br_chan_send_req(ch, buf, L2CAP_BR_DISCONN_TIMEOUT); - bt_l2cap_chan_set_state(chan, BT_L2CAP_DISCONNECT); - - return 0; -} - -static void l2cap_br_disconn_rsp(struct bt_l2cap_br *l2cap, uint8_t ident, - struct net_buf *buf) -{ - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_br_chan *chan; - struct bt_l2cap_disconn_rsp *rsp = (void *)buf->data; - uint16_t dcid, scid; - - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small disconn rsp packet size"); - return; - } - - dcid = sys_le16_to_cpu(rsp->dcid); - scid = sys_le16_to_cpu(rsp->scid); - - BT_DBG("dcid 0x%04x scid 0x%04x", dcid, scid); - - chan = l2cap_br_remove_tx_cid(conn, dcid); - if (!chan) { - BT_WARN("No dcid 0x%04x channel found", dcid); - return; - } - - bt_l2cap_chan_del(&chan->chan); -} - -int bt_l2cap_br_chan_connect(struct bt_conn *conn, struct bt_l2cap_chan *chan, - uint16_t psm) -{ - struct net_buf *buf; - struct bt_l2cap_sig_hdr *hdr; - struct bt_l2cap_conn_req *req; - - if (!psm) { - return -EINVAL; - } - - if (chan->psm) { - return -EEXIST; - } - - /* PSM must be odd and lsb of upper byte must be 0 */ - if ((psm & 0x0101) != 0x0001) { - return -EINVAL; - } - - if (chan->required_sec_level > BT_SECURITY_L4) { - return -EINVAL; - } else if (chan->required_sec_level == BT_SECURITY_L0 && - psm != L2CAP_BR_PSM_SDP) { - chan->required_sec_level = BT_SECURITY_L1; - } - - switch (chan->state) { - case BT_L2CAP_CONNECTED: - /* Already connected */ - return -EISCONN; - case BT_L2CAP_DISCONNECTED: - /* Can connect */ - break; - case BT_L2CAP_CONFIG: - case BT_L2CAP_DISCONNECT: - default: - /* Bad context */ - return -EBUSY; - } - - if (!l2cap_br_chan_add(conn, chan, l2cap_br_chan_destroy)) { - return -ENOMEM; - } - - chan->psm = psm; - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONNECT); - atomic_set_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_PENDING); - - switch (l2cap_br_conn_security(chan, psm)) { - case L2CAP_CONN_SECURITY_PENDING: - /* - * Authentication was triggered, wait with sending request on - * connection security changed callback context. - */ - return 0; - case L2CAP_CONN_SECURITY_PASSED: - break; - case L2CAP_CONN_SECURITY_REJECT: - default: - l2cap_br_chan_cleanup(chan); - return -EIO; - } - - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_CONN_REQ; - hdr->ident = l2cap_br_get_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(BR_CHAN(chan)->rx.cid); - - l2cap_br_chan_send_req(BR_CHAN(chan), buf, L2CAP_BR_CONN_TIMEOUT); - - return 0; -} - -static void l2cap_br_conn_rsp(struct bt_l2cap_br *l2cap, uint8_t ident, - struct net_buf *buf) -{ - struct bt_conn *conn = l2cap->chan.chan.conn; - struct bt_l2cap_chan *chan; - struct bt_l2cap_conn_rsp *rsp = (void *)buf->data; - uint16_t dcid, scid, result, status; - - if (buf->len < sizeof(*rsp)) { - BT_ERR("Too small L2CAP conn rsp packet size"); - return; - } - - dcid = sys_le16_to_cpu(rsp->dcid); - scid = sys_le16_to_cpu(rsp->scid); - result = sys_le16_to_cpu(rsp->result); - status = sys_le16_to_cpu(rsp->status); - - BT_DBG("dcid 0x%04x scid 0x%04x result %u status %u", dcid, scid, - result, status); - - chan = bt_l2cap_br_lookup_rx_cid(conn, scid); - if (!chan) { - BT_ERR("No scid 0x%04x channel found", scid); - return; - } - - /* Release RTX work since got the response */ - k_delayed_work_cancel(&chan->rtx_work); - - if (chan->state != BT_L2CAP_CONNECT) { - BT_DBG("Invalid channel %p state %s", chan, - bt_l2cap_chan_state_str(chan->state)); - return; - } - - switch (result) { - case BT_L2CAP_BR_SUCCESS: - chan->ident = 0U; - BR_CHAN(chan)->tx.cid = dcid; - l2cap_br_conf(chan); - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONFIG); - atomic_clear_bit(BR_CHAN(chan)->flags, L2CAP_FLAG_CONN_PENDING); - break; - case BT_L2CAP_BR_PENDING: - k_delayed_work_submit(&chan->rtx_work, L2CAP_BR_CONN_TIMEOUT); - break; - default: - l2cap_br_chan_cleanup(chan); - break; - } -} - -int bt_l2cap_br_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf) -{ - struct bt_l2cap_br_chan *ch = BR_CHAN(chan); - - if (buf->len > ch->tx.mtu) { - return -EMSGSIZE; - } - - bt_l2cap_send(ch->chan.conn, ch->tx.cid, buf); - - return buf->len; -} - -static int l2cap_br_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) -{ - struct bt_l2cap_br *l2cap = CONTAINER_OF(chan, struct bt_l2cap_br, chan); - struct bt_l2cap_sig_hdr *hdr; - uint16_t len; - - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small L2CAP signaling PDU"); - return 0; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - len = sys_le16_to_cpu(hdr->len); - - BT_DBG("Signaling code 0x%02x ident %u len %u", hdr->code, - hdr->ident, len); - - if (buf->len != len) { - BT_ERR("L2CAP length mismatch (%u != %u)", buf->len, len); - return 0; - } - - if (!hdr->ident) { - BT_ERR("Invalid ident value in L2CAP PDU"); - return 0; - } - - switch (hdr->code) { - case BT_L2CAP_INFO_RSP: - l2cap_br_info_rsp(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_INFO_REQ: - l2cap_br_info_req(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_DISCONN_REQ: - l2cap_br_disconn_req(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_CONN_REQ: - l2cap_br_conn_req(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_CONF_RSP: - l2cap_br_conf_rsp(l2cap, hdr->ident, len, buf); - break; - case BT_L2CAP_CONF_REQ: - l2cap_br_conf_req(l2cap, hdr->ident, len, buf); - break; - case BT_L2CAP_DISCONN_RSP: - l2cap_br_disconn_rsp(l2cap, hdr->ident, buf); - break; - case BT_L2CAP_CONN_RSP: - l2cap_br_conn_rsp(l2cap, hdr->ident, buf); - break; - 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, NULL, 0); - break; - } - - return 0; -} - -static void l2cap_br_conn_pend(struct bt_l2cap_chan *chan, uint8_t status) -{ - struct net_buf *buf; - struct bt_l2cap_sig_hdr *hdr; - struct bt_l2cap_conn_req *req; - - if (chan->state != BT_L2CAP_CONNECT) { - return; - } - - BT_DBG("chan %p status 0x%02x encr 0x%02x", chan, status, - chan->conn->encrypt); - - if (status) { - /* - * Security procedure status is non-zero so respond with - * security violation only as channel acceptor. - */ - l2cap_br_conn_req_reply(chan, BT_L2CAP_BR_ERR_SEC_BLOCK); - - /* Release channel allocated to outgoing connection request */ - if (atomic_test_bit(BR_CHAN(chan)->flags, - L2CAP_FLAG_CONN_PENDING)) { - l2cap_br_chan_cleanup(chan); - } - - return; - } - - if (!chan->conn->encrypt) { - return; - } - - /* - * For incoming connection state send confirming outstanding - * response and initiate configuration request. - */ - if (l2cap_br_conn_req_reply(chan, BT_L2CAP_BR_SUCCESS) == 0) { - bt_l2cap_chan_set_state(chan, BT_L2CAP_CONFIG); - /* - * Initialize config request since remote needs to know - * local MTU segmentation. - */ - l2cap_br_conf(chan); - } else if (atomic_test_and_clear_bit(BR_CHAN(chan)->flags, - L2CAP_FLAG_CONN_PENDING)) { - buf = bt_l2cap_create_pdu(&br_sig_pool, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr->code = BT_L2CAP_CONN_REQ; - hdr->ident = l2cap_br_get_ident(); - hdr->len = sys_cpu_to_le16(sizeof(*req)); - - req = net_buf_add(buf, sizeof(*req)); - req->psm = sys_cpu_to_le16(chan->psm); - req->scid = sys_cpu_to_le16(BR_CHAN(chan)->rx.cid); - - l2cap_br_chan_send_req(BR_CHAN(chan), buf, - L2CAP_BR_CONN_TIMEOUT); - } -} - -void l2cap_br_encrypt_change(struct bt_conn *conn, uint8_t hci_status) -{ - struct bt_l2cap_chan *chan; - - SYS_SLIST_FOR_EACH_CONTAINER(&conn->channels, chan, node) - { - l2cap_br_conn_pend(chan, hci_status); - - if (chan->ops && chan->ops->encrypt_change) { - chan->ops->encrypt_change(chan, hci_status); - } - } -} - -static void check_fixed_channel(struct bt_l2cap_chan *chan) -{ - struct bt_l2cap_br_chan *br_chan = BR_CHAN(chan); - - if (br_chan->rx.cid < L2CAP_BR_CID_DYN_START) { - connect_fixed_channel(br_chan); - } -} - -void bt_l2cap_br_recv(struct bt_conn *conn, struct net_buf *buf) -{ - struct bt_l2cap_hdr *hdr; - struct bt_l2cap_chan *chan; - uint16_t cid; - - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small L2CAP PDU received"); - net_buf_unref(buf); - return; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - cid = sys_le16_to_cpu(hdr->cid); - - chan = bt_l2cap_br_lookup_rx_cid(conn, cid); - if (!chan) { - BT_WARN("Ignoring data for unknown CID 0x%04x", cid); - net_buf_unref(buf); - return; - } - - /* - * if data was received for fixed channel before Information - * Response we connect channel here. - */ - check_fixed_channel(chan); - - chan->ops->recv(chan, buf); - net_buf_unref(buf); -} - -static int l2cap_br_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) -{ - int i; - static const struct bt_l2cap_chan_ops ops = { - .connected = l2cap_br_connected, - .disconnected = l2cap_br_disconnected, - .recv = l2cap_br_recv, - }; - - BT_DBG("conn %p handle %u", conn, conn->handle); - - for (i = 0; i < ARRAY_SIZE(bt_l2cap_br_pool); i++) { - struct bt_l2cap_br *l2cap = &bt_l2cap_br_pool[i]; - - if (l2cap->chan.chan.conn) { - continue; - } - - l2cap->chan.chan.ops = &ops; - *chan = &l2cap->chan.chan; - atomic_set(l2cap->chan.flags, 0); - return 0; - } - - BT_ERR("No available L2CAP context for conn %p", conn); - - return -ENOMEM; -} - -BT_L2CAP_BR_CHANNEL_DEFINE(br_fixed_chan, BT_L2CAP_CID_BR_SIG, l2cap_br_accept); - -void bt_l2cap_br_init(void) -{ -#if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&br_sig_pool, CONFIG_BT_MAX_CONN, BT_L2CAP_BUF_SIZE(L2CAP_BR_MIN_MTU), NULL); -#endif - sys_slist_init(&br_servers); - - bt_sdp_init(); - - if (IS_ENABLED(CONFIG_BT_RFCOMM)) { - bt_rfcomm_init(); - } - - if (IS_ENABLED(CONFIG_BT_HFP)) { - bt_hfp_hf_init(); - } - - if (IS_ENABLED(CONFIG_BT_AVDTP)) { - bt_avdtp_init(); - } - - if (IS_ENABLED(CONFIG_BT_A2DP)) { - bt_a2dp_init(); - } -} diff --git a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/rfcomm.c b/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/rfcomm.c deleted file mode 100644 index 1ce95ada..00000000 --- a/source/Core/BSP/Pinecilv2/bl_mcu_sdk/components/ble/ble_stack/host/rfcomm.c +++ /dev/null @@ -1,1746 +0,0 @@ -/* rfcomm.c - RFCOMM handling */ - -/* - * Copyright (c) 2016 Intel Corporation - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_RFCOMM) -#define LOG_MODULE_NAME bt_rfcomm -#include "log.h" - -#include -#include - -#include "hci_core.h" -#include "conn_internal.h" -#include "l2cap_internal.h" -#include "rfcomm_internal.h" - -#define RFCOMM_CHANNEL_START 0x01 -#define RFCOMM_CHANNEL_END 0x1e - -#define RFCOMM_MIN_MTU BT_RFCOMM_SIG_MIN_MTU -#define RFCOMM_DEFAULT_MTU 127 - -#if defined(CONFIG_BT_HCI_ACL_FLOW_CONTROL) -#define RFCOMM_MAX_CREDITS (CONFIG_BT_ACL_RX_COUNT - 1) -#else -#define RFCOMM_MAX_CREDITS (CONFIG_BT_RX_BUF_COUNT - 1) -#endif - -#define RFCOMM_CREDITS_THRESHOLD (RFCOMM_MAX_CREDITS / 2) -#define RFCOMM_DEFAULT_CREDIT RFCOMM_MAX_CREDITS - -#define RFCOMM_CONN_TIMEOUT K_SECONDS(60) -#define RFCOMM_DISC_TIMEOUT K_SECONDS(20) -#define RFCOMM_IDLE_TIMEOUT K_SECONDS(2) - -#define DLC_RTX(_w) CONTAINER_OF(_w, struct bt_rfcomm_dlc, rtx_work) - -#define SESSION_RTX(_w) CONTAINER_OF(_w, struct bt_rfcomm_session, rtx_work) - -static struct bt_rfcomm_server *servers; - -#if !defined(BFLB_DYNAMIC_ALLOC_MEM) -/* Pool for dummy buffers to wake up the tx threads */ -NET_BUF_POOL_DEFINE(dummy_pool, CONFIG_BT_MAX_CONN, 0, 0, NULL); -#else -struct net_buf_pool dummy_pool; -#endif - -#define RFCOMM_SESSION(_ch) CONTAINER_OF(_ch, \ - struct bt_rfcomm_session, br_chan.chan) - -static struct bt_rfcomm_session bt_rfcomm_pool[CONFIG_BT_MAX_CONN]; -static struct k_thread *rfcomm_tx_thread; -static struct bt_rfcomm_dlc *thread_dlc; - -/* reversed, 8-bit, poly=0x07 */ -static const uint8_t rfcomm_crc_table[256] = { - 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, - 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, - 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, - 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, - - 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, - 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, - 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, - 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, - - 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, - 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, - 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, - 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, - - 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, - 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, - 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, - 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, - - 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, - 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, - 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, - 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, - - 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, - 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, - 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, - 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, - - 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, - 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, - 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, - 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, - - 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, - 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, - 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, - 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf -}; - -static uint8_t rfcomm_calc_fcs(uint16_t len, const uint8_t *data) -{ - uint8_t fcs = 0xff; - - while (len--) { - fcs = rfcomm_crc_table[fcs ^ *data++]; - } - - /* Ones compliment */ - return (0xff - fcs); -} - -static bool rfcomm_check_fcs(uint16_t len, const uint8_t *data, - uint8_t recvd_fcs) -{ - uint8_t fcs = 0xff; - - while (len--) { - fcs = rfcomm_crc_table[fcs ^ *data++]; - } - - /* Ones compliment */ - fcs = rfcomm_crc_table[fcs ^ recvd_fcs]; - - /*0xCF is the reversed order of 11110011.*/ - return (fcs == 0xcf); -} - -static struct bt_rfcomm_dlc *rfcomm_dlcs_lookup_dlci(struct bt_rfcomm_dlc *dlcs, - uint8_t dlci) -{ - for (; dlcs; dlcs = dlcs->_next) { - if (dlcs->dlci == dlci) { - return dlcs; - } - } - - return NULL; -} - -static struct bt_rfcomm_dlc *rfcomm_dlcs_remove_dlci(struct bt_rfcomm_dlc *dlcs, - uint8_t dlci) -{ - struct bt_rfcomm_dlc *tmp; - - if (!dlcs) { - return NULL; - } - - /* If first node is the one to be removed */ - if (dlcs->dlci == dlci) { - dlcs->session->dlcs = dlcs->_next; - return dlcs; - } - - for (tmp = dlcs, dlcs = dlcs->_next; dlcs; dlcs = dlcs->_next) { - if (dlcs->dlci == dlci) { - tmp->_next = dlcs->_next; - return dlcs; - } - tmp = dlcs; - } - - return NULL; -} - -static struct bt_rfcomm_server *rfcomm_server_lookup_channel(uint8_t channel) -{ - struct bt_rfcomm_server *server; - - for (server = servers; server; server = server->_next) { - if (server->channel == channel) { - return server; - } - } - - return NULL; -} - -static struct bt_rfcomm_session * -rfcomm_sessions_lookup_bt_conn(struct bt_conn *conn) -{ - int i; - - for (i = 0; i < ARRAY_SIZE(bt_rfcomm_pool); i++) { - struct bt_rfcomm_session *session = &bt_rfcomm_pool[i]; - - if (session->br_chan.chan.conn == conn) { - return session; - } - } - - return NULL; -} - -int bt_rfcomm_server_register(struct bt_rfcomm_server *server) -{ - if (server->channel < RFCOMM_CHANNEL_START || - server->channel > RFCOMM_CHANNEL_END || !server->accept) { - return -EINVAL; - } - - /* Check if given channel is already in use */ - if (rfcomm_server_lookup_channel(server->channel)) { - BT_DBG("Channel already registered"); - return -EADDRINUSE; - } - - BT_DBG("Channel 0x%02x", server->channel); - - server->_next = servers; - servers = server; - - return 0; -} - -static void rfcomm_dlc_tx_give_credits(struct bt_rfcomm_dlc *dlc, - uint8_t credits) -{ - BT_DBG("dlc %p credits %u", dlc, credits); - - while (credits--) { - k_sem_give(&dlc->tx_credits); - } - - BT_DBG("dlc %p updated credits %u", dlc, - k_sem_count_get(&dlc->tx_credits)); -} - -static void rfcomm_dlc_destroy(struct bt_rfcomm_dlc *dlc) -{ - BT_DBG("dlc %p", dlc); - - k_delayed_work_cancel(&dlc->rtx_work); - dlc->state = BT_RFCOMM_STATE_IDLE; - dlc->session = NULL; - - if (dlc->ops && dlc->ops->disconnected) { - dlc->ops->disconnected(dlc); - } -} - -static void rfcomm_dlc_disconnect(struct bt_rfcomm_dlc *dlc) -{ - uint8_t old_state = dlc->state; - - BT_DBG("dlc %p", dlc); - - if (dlc->state == BT_RFCOMM_STATE_DISCONNECTED) { - return; - } - - dlc->state = BT_RFCOMM_STATE_DISCONNECTED; - - switch (old_state) { - case BT_RFCOMM_STATE_CONNECTED: - /* Queue a dummy buffer to wake up and stop the - * tx thread for states where it was running. - */ - net_buf_put(&dlc->tx_queue, - net_buf_alloc(&dummy_pool, K_NO_WAIT)); - - /* There could be a writer waiting for credits so return a - * dummy credit to wake it up. - */ - rfcomm_dlc_tx_give_credits(dlc, 1); - k_sem_give(&dlc->session->fc); - break; - default: - rfcomm_dlc_destroy(dlc); - break; - } -} - -static void rfcomm_session_disconnected(struct bt_rfcomm_session *session) -{ - struct bt_rfcomm_dlc *dlc; - - BT_DBG("Session %p", session); - - if (session->state == BT_RFCOMM_STATE_DISCONNECTED) { - return; - } - - for (dlc = session->dlcs; dlc;) { - struct bt_rfcomm_dlc *next; - - /* prefetch since disconnected callback may cleanup */ - next = dlc->_next; - dlc->_next = NULL; - - rfcomm_dlc_disconnect(dlc); - - dlc = next; - } - - session->state = BT_RFCOMM_STATE_DISCONNECTED; - session->dlcs = NULL; -} - -struct net_buf *bt_rfcomm_create_pdu(struct net_buf_pool *pool) -{ - /* Length in RFCOMM header can be 2 bytes depending on length of user - * data - */ - return bt_conn_create_pdu(pool, - sizeof(struct bt_l2cap_hdr) + - sizeof(struct bt_rfcomm_hdr) + 1); -} - -static int rfcomm_send_sabm(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_hdr *hdr; - struct net_buf *buf; - uint8_t cr, fcs; - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - cr = BT_RFCOMM_CMD_CR(session->role); - hdr->address = BT_RFCOMM_SET_ADDR(dlci, cr); - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_SABM, BT_RFCOMM_PF_NON_UIH); - hdr->length = BT_RFCOMM_SET_LEN_8(0); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_NON_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static int rfcomm_send_disc(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_hdr *hdr; - struct net_buf *buf; - uint8_t fcs, cr; - - BT_DBG("dlci %d", dlci); - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - cr = BT_RFCOMM_RESP_CR(session->role); - hdr->address = BT_RFCOMM_SET_ADDR(dlci, cr); - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_DISC, BT_RFCOMM_PF_NON_UIH); - hdr->length = BT_RFCOMM_SET_LEN_8(0); - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_NON_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static void rfcomm_session_disconnect(struct bt_rfcomm_session *session) -{ - if (session->dlcs) { - return; - } - - session->state = BT_RFCOMM_STATE_DISCONNECTING; - rfcomm_send_disc(session, 0); - k_delayed_work_submit(&session->rtx_work, RFCOMM_DISC_TIMEOUT); -} - -static struct net_buf *rfcomm_make_uih_msg(struct bt_rfcomm_session *session, - uint8_t cr, uint8_t type, - uint8_t len) -{ - struct bt_rfcomm_hdr *hdr; - struct bt_rfcomm_msg_hdr *msg_hdr; - struct net_buf *buf; - uint8_t hdr_cr; - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - hdr_cr = BT_RFCOMM_UIH_CR(session->role); - hdr->address = BT_RFCOMM_SET_ADDR(0, hdr_cr); - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_UIH, BT_RFCOMM_PF_UIH); - hdr->length = BT_RFCOMM_SET_LEN_8(sizeof(*msg_hdr) + len); - - msg_hdr = net_buf_add(buf, sizeof(*msg_hdr)); - msg_hdr->type = BT_RFCOMM_SET_MSG_TYPE(type, cr); - msg_hdr->len = BT_RFCOMM_SET_LEN_8(len); - - return buf; -} - -static void rfcomm_connected(struct bt_l2cap_chan *chan) -{ - struct bt_rfcomm_session *session = RFCOMM_SESSION(chan); - - BT_DBG("Session %p", session); - - /* Need to include UIH header and FCS*/ - session->mtu = MIN(session->br_chan.rx.mtu, - session->br_chan.tx.mtu) - - BT_RFCOMM_HDR_SIZE + BT_RFCOMM_FCS_SIZE; - - if (session->state == BT_RFCOMM_STATE_CONNECTING) { - rfcomm_send_sabm(session, 0); - } -} - -static void rfcomm_disconnected(struct bt_l2cap_chan *chan) -{ - struct bt_rfcomm_session *session = RFCOMM_SESSION(chan); - - BT_DBG("Session %p", session); - - k_delayed_work_cancel(&session->rtx_work); - rfcomm_session_disconnected(session); - session->state = BT_RFCOMM_STATE_IDLE; -} - -static void rfcomm_dlc_rtx_timeout(struct k_work *work) -{ - struct bt_rfcomm_dlc *dlc = DLC_RTX(work); - struct bt_rfcomm_session *session = dlc->session; - - BT_WARN("dlc %p state %d timeout", dlc, dlc->state); - - rfcomm_dlcs_remove_dlci(session->dlcs, dlc->dlci); - rfcomm_dlc_disconnect(dlc); - rfcomm_session_disconnect(session); -} - -static void rfcomm_dlc_init(struct bt_rfcomm_dlc *dlc, - struct bt_rfcomm_session *session, - uint8_t dlci, - bt_rfcomm_role_t role) -{ - BT_DBG("dlc %p", dlc); - - dlc->dlci = dlci; - dlc->session = session; - dlc->rx_credit = RFCOMM_DEFAULT_CREDIT; - dlc->state = BT_RFCOMM_STATE_INIT; - dlc->role = role; - k_delayed_work_init(&dlc->rtx_work, rfcomm_dlc_rtx_timeout); - - /* Start a conn timer which includes auth as well */ - k_delayed_work_submit(&dlc->rtx_work, RFCOMM_CONN_TIMEOUT); - - dlc->_next = session->dlcs; - session->dlcs = dlc; -} - -static struct bt_rfcomm_dlc *rfcomm_dlc_accept(struct bt_rfcomm_session *session, - uint8_t dlci) -{ - struct bt_rfcomm_server *server; - struct bt_rfcomm_dlc *dlc; - uint8_t channel; - - channel = BT_RFCOMM_GET_CHANNEL(dlci); - server = rfcomm_server_lookup_channel(channel); - if (!server) { - BT_ERR("Server Channel not registered"); - return NULL; - } - - if (server->accept(session->br_chan.chan.conn, &dlc) < 0) { - BT_DBG("Incoming connection rejected"); - return NULL; - } - - if (!BT_RFCOMM_CHECK_MTU(dlc->mtu)) { - rfcomm_dlc_destroy(dlc); - return NULL; - } - - rfcomm_dlc_init(dlc, session, dlci, BT_RFCOMM_ROLE_ACCEPTOR); - dlc->mtu = MIN(dlc->mtu, session->mtu); - - return dlc; -} - -static int rfcomm_send_dm(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_hdr *hdr; - struct net_buf *buf; - uint8_t fcs, cr; - - BT_DBG("dlci %d", dlci); - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - cr = BT_RFCOMM_RESP_CR(session->role); - hdr->address = BT_RFCOMM_SET_ADDR(dlci, cr); - /* For DM PF bit is not relevant, we set it 1 */ - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_DM, BT_RFCOMM_PF_NON_UIH); - hdr->length = BT_RFCOMM_SET_LEN_8(0); - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_NON_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static void rfcomm_check_fc(struct bt_rfcomm_dlc *dlc) -{ - BT_DBG("%p", dlc); - - BT_DBG("Wait for credits or MSC FC %p", dlc); - /* Wait for credits or MSC FC */ - k_sem_take(&dlc->tx_credits, K_FOREVER); - - if (dlc->session->cfc == BT_RFCOMM_CFC_SUPPORTED) { - return; - } - - k_sem_take(&dlc->session->fc, K_FOREVER); - - /* Give the sems immediately so that sem will be available for all - * the bufs in the queue. It will be blocked only once all the bufs - * are sent (which will preempt this thread) and FCOFF / FC bit - * with 1, is received. - */ - k_sem_give(&dlc->session->fc); - k_sem_give(&dlc->tx_credits); -} - -static void rfcomm_dlc_tx_thread(void *p1) -{ - struct bt_rfcomm_dlc *dlc = thread_dlc; - s32_t timeout = K_FOREVER; - struct net_buf *buf; - - BT_DBG("Started for dlc %p", dlc); - - while (dlc->state == BT_RFCOMM_STATE_CONNECTED || - dlc->state == BT_RFCOMM_STATE_USER_DISCONNECT) { - /* Get next packet for dlc */ - BT_DBG("Wait for buf %p", dlc); - buf = net_buf_get(&dlc->tx_queue, timeout); - /* If its dummy buffer or non user disconnect then break */ - if ((dlc->state != BT_RFCOMM_STATE_CONNECTED && - dlc->state != BT_RFCOMM_STATE_USER_DISCONNECT) || - !buf || !buf->len) { - if (buf) { - net_buf_unref(buf); - } - break; - } - - rfcomm_check_fc(dlc); - if (dlc->state != BT_RFCOMM_STATE_CONNECTED && - dlc->state != BT_RFCOMM_STATE_USER_DISCONNECT) { - net_buf_unref(buf); - break; - } - - if (bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf) < 0) { - /* This fails only if channel is disconnected */ - dlc->state = BT_RFCOMM_STATE_DISCONNECTED; - net_buf_unref(buf); - break; - } - - if (dlc->state == BT_RFCOMM_STATE_USER_DISCONNECT) { - timeout = K_NO_WAIT; - } - } - - BT_DBG("dlc %p disconnected - cleaning up", dlc); - - /* Give back any allocated buffers */ - while ((buf = net_buf_get(&dlc->tx_queue, K_NO_WAIT))) { - net_buf_unref(buf); - } - - if (dlc->state == BT_RFCOMM_STATE_USER_DISCONNECT) { - dlc->state = BT_RFCOMM_STATE_DISCONNECTING; - } - - if (dlc->state == BT_RFCOMM_STATE_DISCONNECTING) { - rfcomm_send_disc(dlc->session, dlc->dlci); - k_delayed_work_submit(&dlc->rtx_work, RFCOMM_DISC_TIMEOUT); - } else { - rfcomm_dlc_destroy(dlc); - } - - BT_DBG("dlc %p exiting", dlc); - k_thread_delete(rfcomm_tx_thread); -} - -static int rfcomm_send_ua(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_hdr *hdr; - struct net_buf *buf; - uint8_t cr, fcs; - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - cr = BT_RFCOMM_RESP_CR(session->role); - hdr->address = BT_RFCOMM_SET_ADDR(dlci, cr); - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_UA, BT_RFCOMM_PF_NON_UIH); - hdr->length = BT_RFCOMM_SET_LEN_8(0); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_NON_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static int rfcomm_send_msc(struct bt_rfcomm_dlc *dlc, uint8_t cr, - uint8_t v24_signal) -{ - struct bt_rfcomm_msc *msc; - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(dlc->session, cr, BT_RFCOMM_MSC, - sizeof(*msc)); - - msc = net_buf_add(buf, sizeof(*msc)); - /* cr bit should be always 1 in MSC */ - msc->dlci = BT_RFCOMM_SET_ADDR(dlc->dlci, 1); - msc->v24_signal = v24_signal; - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf); -} - -static int rfcomm_send_rls(struct bt_rfcomm_dlc *dlc, uint8_t cr, - uint8_t line_status) -{ - struct bt_rfcomm_rls *rls; - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(dlc->session, cr, BT_RFCOMM_RLS, - sizeof(*rls)); - - rls = net_buf_add(buf, sizeof(*rls)); - /* cr bit should be always 1 in RLS */ - rls->dlci = BT_RFCOMM_SET_ADDR(dlc->dlci, 1); - rls->line_status = line_status; - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf); -} - -static int rfcomm_send_rpn(struct bt_rfcomm_session *session, uint8_t cr, - struct bt_rfcomm_rpn *rpn) -{ - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(session, cr, BT_RFCOMM_RPN, sizeof(*rpn)); - - net_buf_add_mem(buf, rpn, sizeof(*rpn)); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static int rfcomm_send_test(struct bt_rfcomm_session *session, uint8_t cr, - uint8_t *pattern, uint8_t len) -{ - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(session, cr, BT_RFCOMM_TEST, len); - - net_buf_add_mem(buf, pattern, len); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static int rfcomm_send_nsc(struct bt_rfcomm_session *session, uint8_t cmd_type) -{ - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(session, BT_RFCOMM_MSG_RESP_CR, - BT_RFCOMM_NSC, sizeof(cmd_type)); - - net_buf_add_u8(buf, cmd_type); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static int rfcomm_send_fcon(struct bt_rfcomm_session *session, uint8_t cr) -{ - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(session, cr, BT_RFCOMM_FCON, 0); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static int rfcomm_send_fcoff(struct bt_rfcomm_session *session, uint8_t cr) -{ - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(session, cr, BT_RFCOMM_FCOFF, 0); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&session->br_chan.chan, buf); -} - -static void rfcomm_dlc_connected(struct bt_rfcomm_dlc *dlc) -{ - dlc->state = BT_RFCOMM_STATE_CONNECTED; - - rfcomm_send_msc(dlc, BT_RFCOMM_MSG_CMD_CR, BT_RFCOMM_DEFAULT_V24_SIG); - - if (dlc->session->cfc == BT_RFCOMM_CFC_UNKNOWN) { - /* This means PN negotiation is not done for this session and - * can happen only for 1.0b device. - */ - dlc->session->cfc = BT_RFCOMM_CFC_NOT_SUPPORTED; - } - - if (dlc->session->cfc == BT_RFCOMM_CFC_NOT_SUPPORTED) { - BT_DBG("CFC not supported %p", dlc); - rfcomm_send_fcon(dlc->session, BT_RFCOMM_MSG_CMD_CR); - /* Use tx_credits as binary sem for MSC FC */ - k_sem_init(&dlc->tx_credits, 0, 1); - } - - /* Cancel conn timer */ - k_delayed_work_cancel(&dlc->rtx_work); - - k_fifo_init(&dlc->tx_queue, 20); - rfcomm_tx_thread = &dlc->tx_thread; - thread_dlc = dlc; - k_thread_create(rfcomm_tx_thread, "rfcomm_dlc", - CONFIG_BT_RFCOMM_TX_STACK_SIZE, - rfcomm_dlc_tx_thread, - CONFIG_BT_RFCOMM_TX_PRIO); - - if (dlc->ops && dlc->ops->connected) { - dlc->ops->connected(dlc); - } -} - -enum security_result { - RFCOMM_SECURITY_PASSED, - RFCOMM_SECURITY_REJECT, - RFCOMM_SECURITY_PENDING -}; - -static enum security_result rfcomm_dlc_security(struct bt_rfcomm_dlc *dlc) -{ - struct bt_conn *conn = dlc->session->br_chan.chan.conn; - - BT_DBG("dlc %p", dlc); - - /* If current security level is greater than or equal to required - * security level then return SUCCESS. - * For SSP devices the current security will be atleast MEDIUM - * since L2CAP is enforcing it - */ - if (conn->sec_level >= dlc->required_sec_level) { - return RFCOMM_SECURITY_PASSED; - } - - if (!bt_conn_set_security(conn, dlc->required_sec_level)) { - /* If Security elevation is initiated or in progress */ - return RFCOMM_SECURITY_PENDING; - } - - /* Security request failed */ - return RFCOMM_SECURITY_REJECT; -} - -static void rfcomm_dlc_drop(struct bt_rfcomm_dlc *dlc) -{ - BT_DBG("dlc %p", dlc); - - rfcomm_dlcs_remove_dlci(dlc->session->dlcs, dlc->dlci); - rfcomm_dlc_destroy(dlc); -} - -static int rfcomm_dlc_close(struct bt_rfcomm_dlc *dlc) -{ - BT_DBG("dlc %p", dlc); - - switch (dlc->state) { - case BT_RFCOMM_STATE_SECURITY_PENDING: - if (dlc->role == BT_RFCOMM_ROLE_ACCEPTOR) { - rfcomm_send_dm(dlc->session, dlc->dlci); - } - //__fallthrough; - case BT_RFCOMM_STATE_INIT: - rfcomm_dlc_drop(dlc); - break; - case BT_RFCOMM_STATE_CONNECTING: - case BT_RFCOMM_STATE_CONFIG: - dlc->state = BT_RFCOMM_STATE_DISCONNECTING; - rfcomm_send_disc(dlc->session, dlc->dlci); - k_delayed_work_submit(&dlc->rtx_work, RFCOMM_DISC_TIMEOUT); - break; - case BT_RFCOMM_STATE_CONNECTED: - dlc->state = BT_RFCOMM_STATE_DISCONNECTING; - - /* Queue a dummy buffer to wake up and stop the - * tx thread. - */ - net_buf_put(&dlc->tx_queue, - net_buf_alloc(&dummy_pool, K_NO_WAIT)); - - /* There could be a writer waiting for credits so return a - * dummy credit to wake it up. - */ - rfcomm_dlc_tx_give_credits(dlc, 1); - break; - case BT_RFCOMM_STATE_DISCONNECTING: - case BT_RFCOMM_STATE_DISCONNECTED: - break; - case BT_RFCOMM_STATE_IDLE: - default: - return -EINVAL; - } - - return 0; -} - -static void rfcomm_handle_sabm(struct bt_rfcomm_session *session, uint8_t dlci) -{ - if (!dlci) { - if (rfcomm_send_ua(session, dlci) < 0) { - return; - } - - session->state = BT_RFCOMM_STATE_CONNECTED; - } else { - struct bt_rfcomm_dlc *dlc; - enum security_result result; - - dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, dlci); - if (!dlc) { - dlc = rfcomm_dlc_accept(session, dlci); - if (!dlc) { - rfcomm_send_dm(session, dlci); - return; - } - } - - result = rfcomm_dlc_security(dlc); - switch (result) { - case RFCOMM_SECURITY_PENDING: - dlc->state = BT_RFCOMM_STATE_SECURITY_PENDING; - return; - case RFCOMM_SECURITY_PASSED: - break; - case RFCOMM_SECURITY_REJECT: - default: - rfcomm_send_dm(session, dlci); - rfcomm_dlc_drop(dlc); - return; - } - - if (rfcomm_send_ua(session, dlci) < 0) { - return; - } - - /* Cancel idle timer if any */ - k_delayed_work_cancel(&session->rtx_work); - - rfcomm_dlc_connected(dlc); - } -} - -static int rfcomm_send_pn(struct bt_rfcomm_dlc *dlc, uint8_t cr) -{ - struct bt_rfcomm_pn *pn; - struct net_buf *buf; - uint8_t fcs; - - buf = rfcomm_make_uih_msg(dlc->session, cr, BT_RFCOMM_PN, sizeof(*pn)); - - BT_DBG("mtu %x", dlc->mtu); - - pn = net_buf_add(buf, sizeof(*pn)); - pn->dlci = dlc->dlci; - pn->mtu = sys_cpu_to_le16(dlc->mtu); - if (dlc->state == BT_RFCOMM_STATE_CONFIG && - (dlc->session->cfc == BT_RFCOMM_CFC_UNKNOWN || - dlc->session->cfc == BT_RFCOMM_CFC_SUPPORTED)) { - pn->credits = dlc->rx_credit; - if (cr) { - pn->flow_ctrl = BT_RFCOMM_PN_CFC_CMD; - } else { - pn->flow_ctrl = BT_RFCOMM_PN_CFC_RESP; - } - } else { - /* If PN comes in already opened dlc or cfc not supported - * these should be 0 - */ - pn->credits = 0U; - pn->flow_ctrl = 0U; - } - pn->max_retrans = 0U; - pn->ack_timer = 0U; - pn->priority = 0U; - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf); -} - -static int rfcomm_send_credit(struct bt_rfcomm_dlc *dlc, uint8_t credits) -{ - struct bt_rfcomm_hdr *hdr; - struct net_buf *buf; - uint8_t fcs, cr; - - BT_DBG("Dlc %p credits %d", dlc, credits); - - buf = bt_l2cap_create_pdu(NULL, 0); - - hdr = net_buf_add(buf, sizeof(*hdr)); - cr = BT_RFCOMM_UIH_CR(dlc->session->role); - hdr->address = BT_RFCOMM_SET_ADDR(dlc->dlci, cr); - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_UIH, - BT_RFCOMM_PF_UIH_CREDIT); - hdr->length = BT_RFCOMM_SET_LEN_8(0); - net_buf_add_u8(buf, credits); - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - return bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf); -} - -static int rfcomm_dlc_start(struct bt_rfcomm_dlc *dlc) -{ - enum security_result result; - - BT_DBG("dlc %p", dlc); - - result = rfcomm_dlc_security(dlc); - switch (result) { - case RFCOMM_SECURITY_PASSED: - dlc->mtu = MIN(dlc->mtu, dlc->session->mtu); - dlc->state = BT_RFCOMM_STATE_CONFIG; - rfcomm_send_pn(dlc, BT_RFCOMM_MSG_CMD_CR); - break; - case RFCOMM_SECURITY_PENDING: - dlc->state = BT_RFCOMM_STATE_SECURITY_PENDING; - break; - case RFCOMM_SECURITY_REJECT: - default: - return -EIO; - } - - return 0; -} - -static void rfcomm_handle_ua(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_dlc *dlc, *next; - int err; - - if (!dlci) { - switch (session->state) { - case BT_RFCOMM_STATE_CONNECTING: - session->state = BT_RFCOMM_STATE_CONNECTED; - for (dlc = session->dlcs; dlc; dlc = next) { - next = dlc->_next; - if (dlc->role == BT_RFCOMM_ROLE_INITIATOR && - dlc->state == BT_RFCOMM_STATE_INIT) { - if (rfcomm_dlc_start(dlc) < 0) { - rfcomm_dlc_drop(dlc); - } - } - } - /* Disconnect session if there is no dlcs left */ - rfcomm_session_disconnect(session); - break; - case BT_RFCOMM_STATE_DISCONNECTING: - session->state = BT_RFCOMM_STATE_DISCONNECTED; - /* Cancel disc timer */ - k_delayed_work_cancel(&session->rtx_work); - err = bt_l2cap_chan_disconnect(&session->br_chan.chan); - if (err < 0) { - session->state = BT_RFCOMM_STATE_IDLE; - } - break; - default: - break; - } - } else { - dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, dlci); - if (!dlc) { - return; - } - - switch (dlc->state) { - case BT_RFCOMM_STATE_CONNECTING: - rfcomm_dlc_connected(dlc); - break; - case BT_RFCOMM_STATE_DISCONNECTING: - rfcomm_dlc_drop(dlc); - rfcomm_session_disconnect(session); - break; - default: - break; - } - } -} - -static void rfcomm_handle_dm(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_dlc *dlc; - - BT_DBG("dlci %d", dlci); - - dlc = rfcomm_dlcs_remove_dlci(session->dlcs, dlci); - if (!dlc) { - return; - } - - rfcomm_dlc_disconnect(dlc); - rfcomm_session_disconnect(session); -} - -static void rfcomm_handle_msc(struct bt_rfcomm_session *session, - struct net_buf *buf, uint8_t cr) -{ - struct bt_rfcomm_msc *msc = (void *)buf->data; - struct bt_rfcomm_dlc *dlc; - uint8_t dlci = BT_RFCOMM_GET_DLCI(msc->dlci); - - BT_DBG("dlci %d", dlci); - - dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, dlci); - if (!dlc) { - return; - } - - if (cr == BT_RFCOMM_MSG_RESP_CR) { - return; - } - - if (dlc->session->cfc == BT_RFCOMM_CFC_NOT_SUPPORTED) { - /* Only FC bit affects the flow on RFCOMM level */ - if (BT_RFCOMM_GET_FC(msc->v24_signal)) { - /* If FC bit is 1 the device is unable to accept frames. - * Take the semaphore with timeout K_NO_WAIT so that - * dlc thread will be blocked when it tries sem_take - * before sending the data. K_NO_WAIT timeout will make - * sure that RX thread will not be blocked while taking - * the semaphore. - */ - k_sem_take(&dlc->tx_credits, K_NO_WAIT); - } else { - /* Give the sem so that it will unblock the waiting dlc - * thread in sem_take(). - */ - k_sem_give(&dlc->tx_credits); - } - } - - rfcomm_send_msc(dlc, BT_RFCOMM_MSG_RESP_CR, msc->v24_signal); -} - -static void rfcomm_handle_rls(struct bt_rfcomm_session *session, - struct net_buf *buf, uint8_t cr) -{ - struct bt_rfcomm_rls *rls = (void *)buf->data; - uint8_t dlci = BT_RFCOMM_GET_DLCI(rls->dlci); - struct bt_rfcomm_dlc *dlc; - - BT_DBG("dlci %d", dlci); - - if (!cr) { - /* Ignore if its a response */ - return; - } - - dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, dlci); - if (!dlc) { - return; - } - - /* As per the ETSI same line status has to returned in the response */ - rfcomm_send_rls(dlc, BT_RFCOMM_MSG_RESP_CR, rls->line_status); -} - -static void rfcomm_handle_rpn(struct bt_rfcomm_session *session, - struct net_buf *buf, uint8_t cr) -{ - struct bt_rfcomm_rpn default_rpn, *rpn = (void *)buf->data; - uint8_t dlci = BT_RFCOMM_GET_DLCI(rpn->dlci); - uint8_t data_bits, stop_bits, parity_bits; - /* Exclude fcs to get number of value bytes */ - uint8_t value_len = buf->len - 1; - - BT_DBG("dlci %d", dlci); - - if (!cr) { - /* Ignore if its a response */ - return; - } - - if (value_len == sizeof(*rpn)) { - /* Accept all the values proposed by the sender */ - rpn->param_mask = sys_cpu_to_le16(BT_RFCOMM_RPN_PARAM_MASK_ALL); - rfcomm_send_rpn(session, BT_RFCOMM_MSG_RESP_CR, rpn); - return; - } - - if (value_len != 1U) { - return; - } - - /* If only one value byte then current port settings has to be returned - * We will send default values - */ - default_rpn.dlci = BT_RFCOMM_SET_ADDR(dlci, 1); - default_rpn.baud_rate = BT_RFCOMM_RPN_BAUD_RATE_9600; - default_rpn.flow_control = BT_RFCOMM_RPN_FLOW_NONE; - default_rpn.xoff_char = BT_RFCOMM_RPN_XOFF_CHAR; - default_rpn.xon_char = BT_RFCOMM_RPN_XON_CHAR; - data_bits = BT_RFCOMM_RPN_DATA_BITS_8; - stop_bits = BT_RFCOMM_RPN_STOP_BITS_1; - parity_bits = BT_RFCOMM_RPN_PARITY_NONE; - default_rpn.line_settings = BT_RFCOMM_SET_LINE_SETTINGS(data_bits, - stop_bits, - parity_bits); - default_rpn.param_mask = sys_cpu_to_le16(BT_RFCOMM_RPN_PARAM_MASK_ALL); - - rfcomm_send_rpn(session, BT_RFCOMM_MSG_RESP_CR, &default_rpn); -} - -static void rfcomm_handle_pn(struct bt_rfcomm_session *session, - struct net_buf *buf, uint8_t cr) -{ - struct bt_rfcomm_pn *pn = (void *)buf->data; - struct bt_rfcomm_dlc *dlc; - - dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, pn->dlci); - if (!dlc) { - /* Ignore if it is a response */ - if (!cr) { - return; - } - - if (!BT_RFCOMM_CHECK_MTU(pn->mtu)) { - BT_ERR("Invalid mtu %d", pn->mtu); - rfcomm_send_dm(session, pn->dlci); - return; - } - - dlc = rfcomm_dlc_accept(session, pn->dlci); - if (!dlc) { - rfcomm_send_dm(session, pn->dlci); - return; - } - - BT_DBG("Incoming connection accepted dlc %p", dlc); - - dlc->mtu = MIN(dlc->mtu, sys_le16_to_cpu(pn->mtu)); - - if (pn->flow_ctrl == BT_RFCOMM_PN_CFC_CMD) { - if (session->cfc == BT_RFCOMM_CFC_UNKNOWN) { - session->cfc = BT_RFCOMM_CFC_SUPPORTED; - } - k_sem_init(&dlc->tx_credits, 0, UINT32_MAX); - rfcomm_dlc_tx_give_credits(dlc, pn->credits); - } else { - session->cfc = BT_RFCOMM_CFC_NOT_SUPPORTED; - } - - dlc->state = BT_RFCOMM_STATE_CONFIG; - rfcomm_send_pn(dlc, BT_RFCOMM_MSG_RESP_CR); - /* Cancel idle timer if any */ - k_delayed_work_cancel(&session->rtx_work); - } else { - /* If its a command */ - if (cr) { - if (!BT_RFCOMM_CHECK_MTU(pn->mtu)) { - BT_ERR("Invalid mtu %d", pn->mtu); - rfcomm_dlc_close(dlc); - return; - } - dlc->mtu = MIN(dlc->mtu, sys_le16_to_cpu(pn->mtu)); - rfcomm_send_pn(dlc, BT_RFCOMM_MSG_RESP_CR); - } else { - if (dlc->state != BT_RFCOMM_STATE_CONFIG) { - return; - } - - dlc->mtu = MIN(dlc->mtu, sys_le16_to_cpu(pn->mtu)); - if (pn->flow_ctrl == BT_RFCOMM_PN_CFC_RESP) { - if (session->cfc == BT_RFCOMM_CFC_UNKNOWN) { - session->cfc = BT_RFCOMM_CFC_SUPPORTED; - } - k_sem_init(&dlc->tx_credits, 0, UINT32_MAX); - rfcomm_dlc_tx_give_credits(dlc, pn->credits); - } else { - session->cfc = BT_RFCOMM_CFC_NOT_SUPPORTED; - } - - dlc->state = BT_RFCOMM_STATE_CONNECTING; - rfcomm_send_sabm(session, dlc->dlci); - } - } -} - -static void rfcomm_handle_disc(struct bt_rfcomm_session *session, uint8_t dlci) -{ - struct bt_rfcomm_dlc *dlc; - - BT_DBG("Dlci %d", dlci); - - if (dlci) { - dlc = rfcomm_dlcs_remove_dlci(session->dlcs, dlci); - if (!dlc) { - rfcomm_send_dm(session, dlci); - return; - } - - rfcomm_send_ua(session, dlci); - rfcomm_dlc_disconnect(dlc); - - if (!session->dlcs) { - /* Start a session idle timer */ - k_delayed_work_submit(&dlc->session->rtx_work, - RFCOMM_IDLE_TIMEOUT); - } - } else { - /* Cancel idle timer */ - k_delayed_work_cancel(&session->rtx_work); - rfcomm_send_ua(session, 0); - rfcomm_session_disconnected(session); - } -} - -static void rfcomm_handle_msg(struct bt_rfcomm_session *session, - struct net_buf *buf) -{ - struct bt_rfcomm_msg_hdr *hdr; - uint8_t msg_type, len, cr; - - if (buf->len < sizeof(*hdr)) { - BT_ERR("Too small RFCOMM message"); - return; - } - - hdr = net_buf_pull_mem(buf, sizeof(*hdr)); - msg_type = BT_RFCOMM_GET_MSG_TYPE(hdr->type); - cr = BT_RFCOMM_GET_MSG_CR(hdr->type); - len = BT_RFCOMM_GET_LEN(hdr->len); - - BT_DBG("msg type %x cr %x", msg_type, cr); - - switch (msg_type) { - case BT_RFCOMM_PN: - rfcomm_handle_pn(session, buf, cr); - break; - case BT_RFCOMM_MSC: - rfcomm_handle_msc(session, buf, cr); - break; - case BT_RFCOMM_RLS: - rfcomm_handle_rls(session, buf, cr); - break; - case BT_RFCOMM_RPN: - rfcomm_handle_rpn(session, buf, cr); - break; - case BT_RFCOMM_TEST: - if (!cr) { - break; - } - rfcomm_send_test(session, BT_RFCOMM_MSG_RESP_CR, buf->data, - buf->len - 1); - break; - case BT_RFCOMM_FCON: - if (session->cfc == BT_RFCOMM_CFC_SUPPORTED) { - BT_ERR("FCON received when CFC is supported "); - return; - } - - if (!cr) { - break; - } - - /* Give the sem so that it will unblock the waiting dlc threads - * of this session in sem_take(). - */ - k_sem_give(&session->fc); - rfcomm_send_fcon(session, BT_RFCOMM_MSG_RESP_CR); - break; - case BT_RFCOMM_FCOFF: - if (session->cfc == BT_RFCOMM_CFC_SUPPORTED) { - BT_ERR("FCOFF received when CFC is supported "); - return; - } - - if (!cr) { - break; - } - - /* Take the semaphore with timeout K_NO_WAIT so that all the - * dlc threads in this session will be blocked when it tries - * sem_take before sending the data. K_NO_WAIT timeout will - * make sure that RX thread will not be blocked while taking - * the semaphore. - */ - k_sem_take(&session->fc, K_NO_WAIT); - rfcomm_send_fcoff(session, BT_RFCOMM_MSG_RESP_CR); - break; - default: - BT_WARN("Unknown/Unsupported RFCOMM Msg type 0x%02x", msg_type); - rfcomm_send_nsc(session, hdr->type); - break; - } -} - -static void rfcomm_dlc_update_credits(struct bt_rfcomm_dlc *dlc) -{ - uint8_t credits; - - if (dlc->session->cfc == BT_RFCOMM_CFC_NOT_SUPPORTED) { - return; - } - - BT_DBG("dlc %p credits %u", dlc, dlc->rx_credit); - - /* Only give more credits if it went below the defined threshold */ - if (dlc->rx_credit > RFCOMM_CREDITS_THRESHOLD) { - return; - } - - /* Restore credits */ - credits = RFCOMM_MAX_CREDITS - dlc->rx_credit; - dlc->rx_credit += credits; - - rfcomm_send_credit(dlc, credits); -} - -static void rfcomm_handle_data(struct bt_rfcomm_session *session, - struct net_buf *buf, uint8_t dlci, uint8_t pf) - -{ - struct bt_rfcomm_dlc *dlc; - - BT_DBG("dlci %d, pf %d", dlci, pf); - - dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, dlci); - if (!dlc) { - BT_ERR("Data recvd in non existing DLC"); - rfcomm_send_dm(session, dlci); - return; - } - - BT_DBG("dlc %p rx credit %d", dlc, dlc->rx_credit); - - if (dlc->state != BT_RFCOMM_STATE_CONNECTED) { - return; - } - - if (pf == BT_RFCOMM_PF_UIH_CREDIT) { - rfcomm_dlc_tx_give_credits(dlc, net_buf_pull_u8(buf)); - } - - if (buf->len > BT_RFCOMM_FCS_SIZE) { - if (dlc->session->cfc == BT_RFCOMM_CFC_SUPPORTED && - !dlc->rx_credit) { - BT_ERR("Data recvd when rx credit is 0"); - rfcomm_dlc_close(dlc); - return; - } - - /* Remove FCS */ - buf->len -= BT_RFCOMM_FCS_SIZE; - if (dlc->ops && dlc->ops->recv) { - dlc->ops->recv(dlc, buf); - } - - dlc->rx_credit--; - rfcomm_dlc_update_credits(dlc); - } -} - -int bt_rfcomm_dlc_send(struct bt_rfcomm_dlc *dlc, struct net_buf *buf) -{ - struct bt_rfcomm_hdr *hdr; - uint8_t fcs, cr; - - if (!buf) { - return -EINVAL; - } - - BT_DBG("dlc %p tx credit %d", dlc, k_sem_count_get(&dlc->tx_credits)); - - if (dlc->state != BT_RFCOMM_STATE_CONNECTED) { - return -ENOTCONN; - } - - if (buf->len > dlc->mtu) { - return -EMSGSIZE; - } - - if (buf->len > BT_RFCOMM_MAX_LEN_8) { - uint16_t *len; - - /* Length is 2 byte */ - hdr = net_buf_push(buf, sizeof(*hdr) + 1); - len = (uint16_t *)&hdr->length; - *len = BT_RFCOMM_SET_LEN_16(sys_cpu_to_le16(buf->len - - sizeof(*hdr) - 1)); - } else { - hdr = net_buf_push(buf, sizeof(*hdr)); - hdr->length = BT_RFCOMM_SET_LEN_8(buf->len - sizeof(*hdr)); - } - - cr = BT_RFCOMM_UIH_CR(dlc->session->role); - hdr->address = BT_RFCOMM_SET_ADDR(dlc->dlci, cr); - hdr->control = BT_RFCOMM_SET_CTRL(BT_RFCOMM_UIH, - BT_RFCOMM_PF_UIH_NO_CREDIT); - - fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data); - net_buf_add_u8(buf, fcs); - - net_buf_put(&dlc->tx_queue, buf); - - return buf->len; -} - -static int rfcomm_recv(struct bt_l2cap_chan *chan, struct net_buf *buf) -{ - struct bt_rfcomm_session *session = RFCOMM_SESSION(chan); - struct bt_rfcomm_hdr *hdr = (void *)buf->data; - uint8_t dlci, frame_type, fcs, fcs_len; - - /* Need to consider FCS also*/ - if (buf->len < (sizeof(*hdr) + 1)) { - BT_ERR("Too small RFCOMM Frame"); - return 0; - } - - dlci = BT_RFCOMM_GET_DLCI(hdr->address); - frame_type = BT_RFCOMM_GET_FRAME_TYPE(hdr->control); - - BT_DBG("session %p dlci %x type %x", session, dlci, frame_type); - - fcs_len = (frame_type == BT_RFCOMM_UIH) ? BT_RFCOMM_FCS_LEN_UIH : - BT_RFCOMM_FCS_LEN_NON_UIH; - fcs = *(net_buf_tail(buf) - 1); - if (!rfcomm_check_fcs(fcs_len, buf->data, fcs)) { - BT_ERR("FCS check failed"); - return 0; - } - - if (BT_RFCOMM_LEN_EXTENDED(hdr->length)) { - net_buf_pull(buf, sizeof(*hdr) + 1); - } else { - net_buf_pull(buf, sizeof(*hdr)); - } - - switch (frame_type) { - case BT_RFCOMM_SABM: - rfcomm_handle_sabm(session, dlci); - break; - case BT_RFCOMM_UIH: - if (!dlci) { - rfcomm_handle_msg(session, buf); - } else { - rfcomm_handle_data(session, buf, dlci, - BT_RFCOMM_GET_PF(hdr->control)); - } - break; - case BT_RFCOMM_DISC: - rfcomm_handle_disc(session, dlci); - break; - case BT_RFCOMM_UA: - rfcomm_handle_ua(session, dlci); - break; - case BT_RFCOMM_DM: - rfcomm_handle_dm(session, dlci); - break; - default: - BT_WARN("Unknown/Unsupported RFCOMM Frame type 0x%02x", - frame_type); - break; - } - - return 0; -} - -static void rfcomm_encrypt_change(struct bt_l2cap_chan *chan, - uint8_t hci_status) -{ - struct bt_rfcomm_session *session = RFCOMM_SESSION(chan); - struct bt_conn *conn = chan->conn; - struct bt_rfcomm_dlc *dlc, *next; - - BT_DBG("session %p status 0x%02x encr 0x%02x", session, hci_status, - conn->encrypt); - - for (dlc = session->dlcs; dlc; dlc = next) { - next = dlc->_next; - - if (dlc->state != BT_RFCOMM_STATE_SECURITY_PENDING) { - continue; - } - - if (hci_status || !conn->encrypt || - conn->sec_level < dlc->required_sec_level) { - rfcomm_dlc_close(dlc); - continue; - } - - if (dlc->role == BT_RFCOMM_ROLE_ACCEPTOR) { - rfcomm_send_ua(session, dlc->dlci); - rfcomm_dlc_connected(dlc); - } else { - dlc->mtu = MIN(dlc->mtu, session->mtu); - dlc->state = BT_RFCOMM_STATE_CONFIG; - rfcomm_send_pn(dlc, BT_RFCOMM_MSG_CMD_CR); - } - } -} - -static void rfcomm_session_rtx_timeout(struct k_work *work) -{ - struct bt_rfcomm_session *session = SESSION_RTX(work); - - BT_WARN("session %p state %d timeout", session, session->state); - - switch (session->state) { - case BT_RFCOMM_STATE_CONNECTED: - rfcomm_session_disconnect(session); - break; - case BT_RFCOMM_STATE_DISCONNECTING: - session->state = BT_RFCOMM_STATE_DISCONNECTED; - if (bt_l2cap_chan_disconnect(&session->br_chan.chan) < 0) { - session->state = BT_RFCOMM_STATE_IDLE; - } - break; - } -} - -static struct bt_rfcomm_session *rfcomm_session_new(bt_rfcomm_role_t role) -{ - int i; - static const struct bt_l2cap_chan_ops ops = { - .connected = rfcomm_connected, - .disconnected = rfcomm_disconnected, - .recv = rfcomm_recv, - .encrypt_change = rfcomm_encrypt_change, - }; - - for (i = 0; i < ARRAY_SIZE(bt_rfcomm_pool); i++) { - struct bt_rfcomm_session *session = &bt_rfcomm_pool[i]; - - if (session->br_chan.chan.conn) { - continue; - } - - BT_DBG("session %p initialized", session); - - session->br_chan.chan.ops = &ops; - session->br_chan.rx.mtu = CONFIG_BT_L2CAP_RX_MTU; - session->state = BT_RFCOMM_STATE_INIT; - session->role = role; - session->cfc = BT_RFCOMM_CFC_UNKNOWN; - k_delayed_work_init(&session->rtx_work, - rfcomm_session_rtx_timeout); - k_sem_init(&session->fc, 0, 1); - - return session; - } - - return NULL; -} - -int bt_rfcomm_dlc_connect(struct bt_conn *conn, struct bt_rfcomm_dlc *dlc, - uint8_t channel) -{ - struct bt_rfcomm_session *session; - struct bt_l2cap_chan *chan; - uint8_t dlci; - int ret; - - BT_DBG("conn %p dlc %p channel %d", conn, dlc, channel); - - if (!dlc) { - return -EINVAL; - } - - if (!conn || conn->state != BT_CONN_CONNECTED) { - return -ENOTCONN; - } - - if (channel < RFCOMM_CHANNEL_START || channel > RFCOMM_CHANNEL_END) { - return -EINVAL; - } - - if (!BT_RFCOMM_CHECK_MTU(dlc->mtu)) { - return -EINVAL; - } - - session = rfcomm_sessions_lookup_bt_conn(conn); - if (!session) { - session = rfcomm_session_new(BT_RFCOMM_ROLE_INITIATOR); - if (!session) { - return -ENOMEM; - } - } - - dlci = BT_RFCOMM_DLCI(session->role, channel); - - if (rfcomm_dlcs_lookup_dlci(session->dlcs, dlci)) { - return -EBUSY; - } - - rfcomm_dlc_init(dlc, session, dlci, BT_RFCOMM_ROLE_INITIATOR); - - switch (session->state) { - case BT_RFCOMM_STATE_INIT: - if (session->role == BT_RFCOMM_ROLE_ACCEPTOR) { - /* There is an ongoing incoming conn */ - break; - } - chan = &session->br_chan.chan; - chan->required_sec_level = dlc->required_sec_level; - ret = bt_l2cap_chan_connect(conn, chan, BT_L2CAP_PSM_RFCOMM); - if (ret < 0) { - session->state = BT_RFCOMM_STATE_IDLE; - goto fail; - } - session->state = BT_RFCOMM_STATE_CONNECTING; - break; - case BT_RFCOMM_STATE_CONNECTING: - break; - case BT_RFCOMM_STATE_CONNECTED: - ret = rfcomm_dlc_start(dlc); - if (ret < 0) { - goto fail; - } - /* Cancel idle timer if any */ - k_delayed_work_cancel(&session->rtx_work); - break; - default: - BT_ERR("Invalid session state %d", session->state); - ret = -EINVAL; - goto fail; - } - - return 0; - -fail: - rfcomm_dlcs_remove_dlci(session->dlcs, dlc->dlci); - dlc->state = BT_RFCOMM_STATE_IDLE; - dlc->session = NULL; - return ret; -} - -int bt_rfcomm_dlc_disconnect(struct bt_rfcomm_dlc *dlc) -{ - BT_DBG("dlc %p", dlc); - - if (!dlc) { - return -EINVAL; - } - - if (dlc->state == BT_RFCOMM_STATE_CONNECTED) { - /* This is to handle user initiated disconnect to send pending - * bufs in the queue before disconnecting - * Queue a dummy buffer (in case if queue is empty) to wake up - * and stop the tx thread. - */ - dlc->state = BT_RFCOMM_STATE_USER_DISCONNECT; - net_buf_put(&dlc->tx_queue, - net_buf_alloc(&dummy_pool, K_NO_WAIT)); - - k_delayed_work_submit(&dlc->rtx_work, RFCOMM_DISC_TIMEOUT); - - return 0; - } - - return rfcomm_dlc_close(dlc); -} - -static int rfcomm_accept(struct bt_conn *conn, struct bt_l2cap_chan **chan) -{ - struct bt_rfcomm_session *session; - - BT_DBG("conn %p", conn); - - session = rfcomm_session_new(BT_RFCOMM_ROLE_ACCEPTOR); - if (session) { - *chan = &session->br_chan.chan; - return 0; - } - - BT_ERR("No available RFCOMM context for conn %p", conn); - - return -ENOMEM; -} - -void bt_rfcomm_init(void) -{ -#if defined(BFLB_DYNAMIC_ALLOC_MEM) - net_buf_init(&dummy_pool, CONFIG_BT_MAX_CONN, 1, NULL); -#endif - static struct bt_l2cap_server server = { - .psm = BT_L2CAP_PSM_RFCOMM, - .accept = rfcomm_accept, - .sec_level = BT_SECURITY_L1, - }; - - bt_l2cap_br_server_register(&server); -}