diff --git a/subsys/bluetooth/host/rfcomm.c b/subsys/bluetooth/host/rfcomm.c index 24d693f260f..d2a4412a19a 100644 --- a/subsys/bluetooth/host/rfcomm.c +++ b/subsys/bluetooth/host/rfcomm.c @@ -618,6 +618,22 @@ static int rfcomm_send_rpn(struct bt_rfcomm_session *session, uint8_t cr, 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 void rfcomm_dlc_connected(struct bt_rfcomm_dlc *dlc) { dlc->state = BT_RFCOMM_STATE_CONNECTED; @@ -1102,6 +1118,13 @@ static void rfcomm_handle_msg(struct bt_rfcomm_session *session, 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; default: BT_WARN("Unknown/Unsupported RFCOMM Msg type 0x%02x", msg_type); break; diff --git a/subsys/bluetooth/host/rfcomm_internal.h b/subsys/bluetooth/host/rfcomm_internal.h index 8c4a247b7c1..f58e365425f 100644 --- a/subsys/bluetooth/host/rfcomm_internal.h +++ b/subsys/bluetooth/host/rfcomm_internal.h @@ -94,6 +94,8 @@ struct bt_rfcomm_rpn { uint16_t param_mask; } __packed; +#define BT_RFCOMM_TEST 0x08 + /* Default RPN Settings */ #define BT_RFCOMM_RPN_BAUD_RATE_9600 0x03 #define BT_RFCOMM_RPN_DATA_BITS_8 0x03