bluetooth: a2dp: use avdtp struct to fill data to net buf
avdtp defines the data format by struct, use it to fill net buf.
Signed-off-by: Mark Wang <yichang.wang@nxp.com>
(cherry picked from commit a89f4eddac6f13f981f2ca0b94f07edaad104afa)
diff --git a/subsys/bluetooth/host/classic/a2dp.c b/subsys/bluetooth/host/classic/a2dp.c
index df69417..7a4e8a6 100644
--- a/subsys/bluetooth/host/classic/a2dp.c
+++ b/subsys/bluetooth/host/classic/a2dp.c
@@ -133,30 +133,56 @@
* are not supported.
*/
struct bt_a2dp_ep *ep;
+ struct bt_avdtp_generic_service_cap *cap;
+ struct bt_avdtp_media_codec_capabilities *media_cap;
__ASSERT(sep, "Invalid sep");
*errcode = 0;
+
+ if (net_buf_tailroom(rsp_buf) < sizeof(*cap)) {
+ goto set_err_and_return;
+ }
+
/* Service Category: Media Transport */
- net_buf_add_u8(rsp_buf, BT_AVDTP_SERVICE_MEDIA_TRANSPORT);
- net_buf_add_u8(rsp_buf, 0);
- /* Service Category: Media Codec */
- net_buf_add_u8(rsp_buf, BT_AVDTP_SERVICE_MEDIA_CODEC);
+ cap = net_buf_add(rsp_buf, sizeof(*cap));
+
+ cap->service_category = BT_AVDTP_SERVICE_MEDIA_TRANSPORT;
+ cap->losc = 0;
+
ep = CONTAINER_OF(sep, struct bt_a2dp_ep, sep);
- /* Length Of Service Capability */
- net_buf_add_u8(rsp_buf, ep->codec_cap->len + 2U);
- /* Media Type */
- net_buf_add_u8(rsp_buf, sep->sep_info.media_type << 4U);
- /* Media Codec Type */
- net_buf_add_u8(rsp_buf, ep->codec_type);
+
+ if (net_buf_tailroom(rsp_buf) < ep->codec_cap->len + sizeof(*cap) + sizeof(*media_cap)) {
+ goto set_err_and_return;
+ }
+
+ /* Service Category: Media Codec */
+ cap = net_buf_add(rsp_buf, sizeof(*cap));
+ cap->service_category = BT_AVDTP_SERVICE_MEDIA_CODEC;
+ cap->losc = ep->codec_cap->len + 2U;
+
+ media_cap = net_buf_add(rsp_buf, sizeof(*media_cap));
+ media_cap->media_type = sep->sep_info.media_type << 4U;
+ media_cap->media_code_type = ep->codec_type;
/* Codec Info Element */
net_buf_add_mem(rsp_buf, &ep->codec_cap->codec_ie[0], ep->codec_cap->len);
if (get_all_caps && ep->delay_report) {
- net_buf_add_u8(rsp_buf, BT_AVDTP_SERVICE_DELAY_REPORTING);
- net_buf_add_u8(rsp_buf, 0);
+ if (net_buf_tailroom(rsp_buf) < sizeof(*cap)) {
+ goto set_err_and_return;
+ }
+
+ /* Service Category: Delay Report */
+ cap = net_buf_add(rsp_buf, sizeof(*cap));
+
+ cap->service_category = BT_AVDTP_SERVICE_DELAY_REPORTING;
+ cap->losc = 0;
}
return 0;
+
+set_err_and_return:
+ *errcode = BT_AVDTP_INVALID_CAPABILITIES;
+ return -ENOMEM;
}
#define IS_BIT_DUPLICATED(val) (((val) & ((val)-1)) != 0)
@@ -511,6 +537,8 @@
struct net_buf *rsp_buf, uint8_t *errcode)
{
struct bt_a2dp_ep *ep;
+ struct bt_avdtp_generic_service_cap *cap;
+ struct bt_avdtp_media_codec_capabilities *media_cap;
__ASSERT(sep, "Invalid sep");
@@ -534,27 +562,50 @@
}
}
+ if (net_buf_tailroom(rsp_buf) < sizeof(*cap)) {
+ goto set_err_and_return;
+ }
+
/* Service Category: Media Transport */
- net_buf_add_u8(rsp_buf, BT_AVDTP_SERVICE_MEDIA_TRANSPORT);
- net_buf_add_u8(rsp_buf, 0);
+ cap = net_buf_add(rsp_buf, sizeof(*cap));
+
+ cap->service_category = BT_AVDTP_SERVICE_MEDIA_TRANSPORT;
+ cap->losc = 0;
+
+ if (net_buf_tailroom(rsp_buf) < ep->stream->codec_config.len + sizeof(*cap) +
+ sizeof(*media_cap)) {
+ goto set_err_and_return;
+ }
+
/* Service Category: Media Codec */
- net_buf_add_u8(rsp_buf, BT_AVDTP_SERVICE_MEDIA_CODEC);
- /* Length Of Service Capability */
- net_buf_add_u8(rsp_buf, ep->stream->codec_config.len + 2U);
- /* Media Type */
- net_buf_add_u8(rsp_buf, sep->sep_info.media_type << 4U);
- /* Media Codec Type */
- net_buf_add_u8(rsp_buf, ep->codec_type);
+ cap = net_buf_add(rsp_buf, sizeof(*cap));
+ cap->service_category = BT_AVDTP_SERVICE_MEDIA_CODEC;
+ cap->losc = ep->stream->codec_config.len + 2U;
+
+ media_cap = net_buf_add(rsp_buf, sizeof(*media_cap));
+ media_cap->media_type = sep->sep_info.media_type << 4U;
+ media_cap->media_code_type = ep->codec_type;
/* Codec Info Element */
net_buf_add_mem(rsp_buf, &ep->stream->codec_config.codec_ie[0],
ep->stream->codec_config.len);
if (ep->stream->delay_report) {
- net_buf_add_u8(rsp_buf, BT_AVDTP_SERVICE_DELAY_REPORTING);
- net_buf_add_u8(rsp_buf, 0);
+ if (net_buf_tailroom(rsp_buf) < sizeof(*cap)) {
+ goto set_err_and_return;
+ }
+
+ /* Service Category: Delay Report */
+ cap = net_buf_add(rsp_buf, sizeof(*cap));
+
+ cap->service_category = BT_AVDTP_SERVICE_DELAY_REPORTING;
+ cap->losc = 0;
}
return 0;
+
+set_err_and_return:
+ *errcode = BT_AVDTP_INVALID_CAPABILITIES;
+ return -ENOMEM;
}
#ifdef CONFIG_BT_A2DP_SOURCE
diff --git a/subsys/bluetooth/host/classic/avdtp.c b/subsys/bluetooth/host/classic/avdtp.c
index f1daf76..8ac753f 100644
--- a/subsys/bluetooth/host/classic/avdtp.c
+++ b/subsys/bluetooth/host/classic/avdtp.c
@@ -2396,6 +2396,8 @@
struct bt_avdtp_set_configuration_params *param)
{
struct net_buf *buf;
+ struct bt_avdtp_generic_service_cap *cap;
+ struct bt_avdtp_media_codec_capabilities *media_cap;
LOG_DBG("");
if (!param || !session) {
@@ -2411,33 +2413,55 @@
/* Body of the message */
/* ACP Stream Endpoint ID */
+ __ASSERT_NO_MSG(net_buf_tailroom(buf) >= 1);
net_buf_add_u8(buf, (param->acp_stream_ep_id << 2U));
+
if (cmd == BT_AVDTP_SET_CONFIGURATION) {
+ if (net_buf_tailroom(buf) < sizeof(*cap) + 1) {
+ goto unref_and_return;
+ }
+
/* INT Stream Endpoint ID */
net_buf_add_u8(buf, (param->int_stream_endpoint_id << 2U));
+
/* Service Category: Media Transport */
- net_buf_add_u8(buf, BT_AVDTP_SERVICE_MEDIA_TRANSPORT);
- /* LOSC */
- net_buf_add_u8(buf, 0);
+ cap = net_buf_add(buf, sizeof(*cap));
+ cap->service_category = BT_AVDTP_SERVICE_MEDIA_TRANSPORT;
+ cap->losc = 0;
}
+
+ if (net_buf_tailroom(buf) < param->codec_specific_ie_len + sizeof(*cap) +
+ sizeof(*media_cap)) {
+ goto unref_and_return;
+ }
+
/* Service Category: Media Codec */
- net_buf_add_u8(buf, BT_AVDTP_SERVICE_MEDIA_CODEC);
- /* LOSC */
- net_buf_add_u8(buf, param->codec_specific_ie_len + 2);
- /* Media Type */
- net_buf_add_u8(buf, param->media_type << 4U);
- /* Media Codec Type */
- net_buf_add_u8(buf, param->media_codec_type);
+ cap = net_buf_add(buf, sizeof(*cap));
+ cap->service_category = BT_AVDTP_SERVICE_MEDIA_CODEC;
+ cap->losc = param->codec_specific_ie_len + 2;
+
+ media_cap = net_buf_add(buf, sizeof(*media_cap));
+ media_cap->media_type = param->media_type << 4U;
+ media_cap->media_code_type = param->media_codec_type;
/* Codec Info Element */
net_buf_add_mem(buf, param->codec_specific_ie, param->codec_specific_ie_len);
if (param->delay_report) {
- net_buf_add_u8(buf, BT_AVDTP_SERVICE_DELAY_REPORTING);
- /* LOSC */
- net_buf_add_u8(buf, 0);
+ if (net_buf_tailroom(buf) < sizeof(*cap)) {
+ goto unref_and_return;
+ }
+
+ /* Service Category: Delay Report */
+ cap = net_buf_add(buf, sizeof(*cap));
+ cap->service_category = BT_AVDTP_SERVICE_DELAY_REPORTING;
+ cap->losc = 0;
}
return avdtp_send_cmd(session, buf, ¶m->req);
+
+unref_and_return:
+ net_buf_unref(buf);
+ return -ENOMEM;
}
int bt_avdtp_set_configuration(struct bt_avdtp *session,
