1 /* btp_cap.c - Bluetooth CAP Tester */
2
3 /*
4 * Copyright (c) 2023 Codecoup
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8 #include <zephyr/bluetooth/audio/cap.h>
9
10 #include "btp/btp.h"
11 #include "btp_bap_audio_stream.h"
12 #include "bap_endpoint.h"
13 #include "zephyr/sys/byteorder.h"
14 #include <stdint.h>
15
16 #include <zephyr/logging/log.h>
17 #define LOG_MODULE_NAME bttester_cap
18 LOG_MODULE_REGISTER(LOG_MODULE_NAME, CONFIG_BTTESTER_LOG_LEVEL);
19
20 #include "btp_bap_unicast.h"
21 #include "btp_bap_broadcast.h"
22
23 static struct btp_bap_unicast_group *u_group;
24
25 static K_SEM_DEFINE(source_stopped_sem, 0U, CONFIG_BT_BAP_BROADCAST_SRC_COUNT);
26
27 struct cap_initiator_broadcast_params {
28 struct bt_cap_initiator_broadcast_subgroup_param
29 cap_subgroup_params[CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT];
30 struct bt_cap_initiator_broadcast_stream_param
31 cap_stream_params[CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT]
32 [CONFIG_BT_BAP_BROADCAST_SRC_STREAM_COUNT];
33 };
34
35 static struct cap_initiator_broadcast_params
36 cap_broadcast_params[CONFIG_BT_BAP_BROADCAST_SRC_COUNT];
37
38
39 extern struct bt_csip_set_coordinator_set_member *btp_csip_set_members[CONFIG_BT_MAX_CONN];
40
stream_unicast_to_bap(struct btp_bap_unicast_stream * stream)41 static struct bt_bap_stream *stream_unicast_to_bap(struct btp_bap_unicast_stream *stream)
42 {
43 return &stream->audio_stream.cap_stream.bap_stream;
44 }
45
stream_unicast_to_cap(struct btp_bap_unicast_stream * stream)46 static struct bt_cap_stream *stream_unicast_to_cap(struct btp_bap_unicast_stream *stream)
47 {
48 return &stream->audio_stream.cap_stream;
49 }
50
stream_broadcast_to_cap(struct btp_bap_broadcast_stream * stream)51 static struct bt_cap_stream *stream_broadcast_to_cap(struct btp_bap_broadcast_stream *stream)
52 {
53 return &stream->audio_stream.cap_stream;
54 }
55
btp_send_discovery_completed_ev(struct bt_conn * conn,uint8_t status)56 static void btp_send_discovery_completed_ev(struct bt_conn *conn, uint8_t status)
57 {
58 struct btp_cap_discovery_completed_ev ev;
59
60 bt_addr_le_copy(&ev.address, bt_conn_get_dst(conn));
61 ev.status = status;
62
63 tester_event(BTP_SERVICE_ID_CAP, BTP_CAP_EV_DISCOVERY_COMPLETED, &ev, sizeof(ev));
64 }
65
cap_discovery_complete_cb(struct bt_conn * conn,int err,const struct bt_csip_set_coordinator_set_member * member,const struct bt_csip_set_coordinator_csis_inst * csis_inst)66 static void cap_discovery_complete_cb(struct bt_conn *conn, int err,
67 const struct bt_csip_set_coordinator_set_member *member,
68 const struct bt_csip_set_coordinator_csis_inst *csis_inst)
69 {
70 LOG_DBG("");
71
72 if (err != 0) {
73 LOG_DBG("Failed to discover CAS: %d", err);
74 btp_send_discovery_completed_ev(conn, BTP_CAP_DISCOVERY_STATUS_FAILED);
75
76 return;
77 }
78
79 if (IS_ENABLED(CONFIG_BT_CAP_ACCEPTOR_SET_MEMBER)) {
80 if (csis_inst == NULL) {
81 LOG_DBG("Failed to discover CAS CSIS");
82 btp_send_discovery_completed_ev(conn, BTP_CAP_DISCOVERY_STATUS_FAILED);
83
84 return;
85 }
86
87 LOG_DBG("Found CAS with CSIS %p", csis_inst);
88 } else {
89 LOG_DBG("Found CAS");
90 }
91
92 btp_send_discovery_completed_ev(conn, BTP_CAP_DISCOVERY_STATUS_SUCCESS);
93 }
94
btp_send_cap_unicast_start_completed_ev(uint8_t cig_id,uint8_t status)95 static void btp_send_cap_unicast_start_completed_ev(uint8_t cig_id, uint8_t status)
96 {
97 struct btp_cap_unicast_start_completed_ev ev;
98
99 ev.cig_id = cig_id;
100 ev.status = status;
101
102 tester_event(BTP_SERVICE_ID_CAP, BTP_CAP_EV_UNICAST_START_COMPLETED, &ev, sizeof(ev));
103 }
104
btp_send_cap_unicast_stop_completed_ev(uint8_t cig_id,uint8_t status)105 static void btp_send_cap_unicast_stop_completed_ev(uint8_t cig_id, uint8_t status)
106 {
107 struct btp_cap_unicast_stop_completed_ev ev;
108
109 ev.cig_id = cig_id;
110 ev.status = status;
111
112 tester_event(BTP_SERVICE_ID_CAP, BTP_CAP_EV_UNICAST_STOP_COMPLETED, &ev, sizeof(ev));
113 }
114
unicast_start_complete_cb(int err,struct bt_conn * conn)115 static void unicast_start_complete_cb(int err, struct bt_conn *conn)
116 {
117 LOG_DBG("");
118
119 if (err != 0) {
120 LOG_DBG("Failed to unicast-start, err %d", err);
121 btp_send_cap_unicast_start_completed_ev(u_group->cig_id,
122 BTP_CAP_UNICAST_START_STATUS_FAILED);
123
124 return;
125 }
126
127 btp_send_cap_unicast_start_completed_ev(u_group->cig_id,
128 BTP_CAP_UNICAST_START_STATUS_SUCCESS);
129 }
130
unicast_update_complete_cb(int err,struct bt_conn * conn)131 static void unicast_update_complete_cb(int err, struct bt_conn *conn)
132 {
133 LOG_DBG("");
134
135 if (err != 0) {
136 LOG_DBG("Failed to unicast-update, err %d", err);
137 }
138 }
139
unicast_stop_complete_cb(int err,struct bt_conn * conn)140 static void unicast_stop_complete_cb(int err, struct bt_conn *conn)
141 {
142 LOG_DBG("");
143
144 if (err != 0) {
145 LOG_DBG("Failed to unicast-stop, err %d", err);
146 btp_send_cap_unicast_stop_completed_ev(u_group->cig_id,
147 BTP_CAP_UNICAST_START_STATUS_FAILED);
148
149 return;
150 }
151
152 btp_send_cap_unicast_stop_completed_ev(u_group->cig_id,
153 BTP_CAP_UNICAST_START_STATUS_SUCCESS);
154 }
155
broadcast_stopped_cb(struct bt_cap_broadcast_source * source,uint8_t reason)156 static void broadcast_stopped_cb(struct bt_cap_broadcast_source *source, uint8_t reason)
157 {
158 LOG_DBG("");
159
160 k_sem_give(&source_stopped_sem);
161 }
162
163 static struct bt_cap_initiator_cb cap_cb = {
164 .unicast_discovery_complete = cap_discovery_complete_cb,
165 .unicast_start_complete = unicast_start_complete_cb,
166 .unicast_update_complete = unicast_update_complete_cb,
167 .unicast_stop_complete = unicast_stop_complete_cb,
168 .broadcast_stopped = broadcast_stopped_cb,
169 };
170
btp_cap_supported_commands(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)171 static uint8_t btp_cap_supported_commands(const void *cmd, uint16_t cmd_len,
172 void *rsp, uint16_t *rsp_len)
173 {
174 struct btp_cap_read_supported_commands_rp *rp = rsp;
175
176 /* octet 0 */
177 tester_set_bit(rp->data, BTP_CAP_READ_SUPPORTED_COMMANDS);
178 tester_set_bit(rp->data, BTP_CAP_DISCOVER);
179
180 *rsp_len = sizeof(*rp) + 1;
181
182 return BTP_STATUS_SUCCESS;
183 }
184
btp_cap_discover(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)185 static uint8_t btp_cap_discover(const void *cmd, uint16_t cmd_len,
186 void *rsp, uint16_t *rsp_len)
187 {
188 const struct btp_cap_discover_cmd *cp = cmd;
189 struct bt_conn *conn;
190 int err;
191
192 LOG_DBG("");
193
194 conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &cp->address);
195 if (!conn) {
196 LOG_ERR("Unknown connection");
197 return BTP_STATUS_FAILED;
198 }
199
200 err = bt_cap_initiator_unicast_discover(conn);
201 if (err != 0) {
202 LOG_DBG("Failed to discover remote ASEs: %d", err);
203 bt_conn_unref(conn);
204
205 return BTP_STATUS_FAILED;
206 }
207
208 bt_conn_unref(conn);
209
210 return BTP_STATUS_SUCCESS;
211 }
212
cap_unicast_setup_ase(struct bt_conn * conn,uint8_t ase_id,uint8_t cis_id,uint8_t cig_id,struct bt_audio_codec_cfg * codec_cfg,struct bt_bap_qos_cfg * qos)213 static int cap_unicast_setup_ase(struct bt_conn *conn, uint8_t ase_id, uint8_t cis_id,
214 uint8_t cig_id, struct bt_audio_codec_cfg *codec_cfg,
215 struct bt_bap_qos_cfg *qos)
216 {
217 struct btp_bap_unicast_group *group;
218 struct btp_bap_unicast_stream *u_stream;
219 struct bt_bap_stream *stream;
220 struct btp_bap_unicast_connection *u_conn = btp_bap_unicast_conn_get(bt_conn_index(conn));
221
222 u_stream = btp_bap_unicast_stream_find(u_conn, ase_id);
223 if (u_stream == NULL) {
224 /* Configure a new u_stream */
225 u_stream = btp_bap_unicast_stream_alloc(u_conn);
226 if (u_stream == NULL) {
227 LOG_DBG("No streams available");
228
229 return -ENOMEM;
230 }
231 }
232
233 stream = stream_unicast_to_bap(u_stream);
234 bt_cap_stream_ops_register(&u_stream->audio_stream.cap_stream, stream->ops);
235
236 u_stream->conn_id = bt_conn_index(conn);
237 u_stream->ase_id = ase_id;
238 u_stream->cig_id = cig_id;
239 u_stream->cis_id = cis_id;
240 memcpy(&u_stream->codec_cfg, codec_cfg, sizeof(*codec_cfg));
241
242 group = btp_bap_unicast_group_find(cig_id);
243 if (group == NULL) {
244 return -EINVAL;
245 }
246
247 memcpy(&group->qos[cis_id], qos, sizeof(*qos));
248
249 return 0;
250 }
251
btp_cap_unicast_setup_ase(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)252 static uint8_t btp_cap_unicast_setup_ase(const void *cmd, uint16_t cmd_len,
253 void *rsp, uint16_t *rsp_len)
254 {
255 const struct btp_cap_unicast_setup_ase_cmd *cp = cmd;
256 struct bt_audio_codec_cfg codec_cfg;
257 struct bt_bap_qos_cfg qos;
258 struct bt_conn *conn;
259 const uint8_t *ltv_ptr;
260 int err;
261
262 LOG_DBG("");
263
264 conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &cp->address);
265 if (!conn) {
266 LOG_ERR("Unknown connection");
267
268 return BTP_STATUS_FAILED;
269 }
270
271 memset(&qos, 0, sizeof(qos));
272 qos.phy = BT_BAP_QOS_CFG_2M;
273 qos.framing = cp->framing;
274 qos.rtn = cp->retransmission_num;
275 qos.sdu = sys_le16_to_cpu(cp->max_sdu);
276 qos.latency = sys_le16_to_cpu(cp->max_transport_latency);
277 qos.interval = sys_get_le24(cp->sdu_interval);
278 qos.pd = sys_get_le24(cp->presentation_delay);
279
280 memset(&codec_cfg, 0, sizeof(codec_cfg));
281 codec_cfg.id = cp->coding_format;
282 codec_cfg.vid = cp->vid;
283 codec_cfg.cid = cp->cid;
284
285 ltv_ptr = cp->ltvs;
286 if (cp->cc_ltvs_len != 0) {
287 codec_cfg.data_len = cp->cc_ltvs_len;
288 memcpy(codec_cfg.data, ltv_ptr, cp->cc_ltvs_len);
289 ltv_ptr += cp->cc_ltvs_len;
290 }
291
292 if (cp->metadata_ltvs_len != 0) {
293 codec_cfg.meta_len = cp->metadata_ltvs_len;
294 memcpy(codec_cfg.meta, ltv_ptr, cp->metadata_ltvs_len);
295 }
296
297 err = cap_unicast_setup_ase(conn, cp->ase_id, cp->cis_id, cp->cig_id, &codec_cfg, &qos);
298 bt_conn_unref(conn);
299
300 return BTP_STATUS_VAL(err);
301 }
302
btp_cap_unicast_audio_start(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)303 static uint8_t btp_cap_unicast_audio_start(const void *cmd, uint16_t cmd_len,
304 void *rsp, uint16_t *rsp_len)
305 {
306 int err;
307 size_t stream_count = 0;
308 const struct btp_cap_unicast_audio_start_cmd *cp = cmd;
309 struct bt_cap_unicast_audio_start_param start_param;
310 struct bt_cap_unicast_audio_start_stream_param stream_params[
311 ARRAY_SIZE(btp_csip_set_members) * BTP_BAP_UNICAST_MAX_STREAMS_COUNT];
312
313 LOG_DBG("");
314
315 err = btp_bap_unicast_group_create(cp->cig_id, &u_group);
316 if (err != 0) {
317 LOG_ERR("Failed to create unicast group");
318
319 return BTP_STATUS_FAILED;
320 }
321
322 for (size_t conn_index = 0; conn_index < ARRAY_SIZE(btp_csip_set_members); conn_index++) {
323 struct btp_bap_unicast_connection *u_conn = btp_bap_unicast_conn_get(conn_index);
324
325 if (u_conn->end_points_count == 0) {
326 /* Connection not initialized */
327 continue;
328 }
329
330 for (size_t i = 0; i < ARRAY_SIZE(u_conn->streams); i++) {
331 struct bt_cap_unicast_audio_start_stream_param *stream_param;
332 struct btp_bap_unicast_stream *u_stream = &u_conn->streams[i];
333
334 if (!u_stream->in_use || u_stream->cig_id != cp->cig_id) {
335 continue;
336 }
337
338 stream_param = &stream_params[stream_count++];
339 stream_param->stream = stream_unicast_to_cap(u_stream);
340 stream_param->codec_cfg = &u_stream->codec_cfg;
341 stream_param->member.member = bt_conn_lookup_addr_le(BT_ID_DEFAULT,
342 &u_conn->address);
343 stream_param->ep = btp_bap_unicast_end_point_find(u_conn, u_stream->ase_id);
344 }
345 }
346
347 start_param.type = cp->set_type;
348 start_param.count = stream_count;
349 start_param.stream_params = stream_params;
350
351 err = bt_cap_initiator_unicast_audio_start(&start_param);
352 if (err != 0) {
353 LOG_ERR("Failed to start unicast audio: %d", err);
354
355 return BTP_STATUS_FAILED;
356 }
357
358 return BTP_STATUS_SUCCESS;
359 }
360
btp_cap_unicast_audio_update(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)361 static uint8_t btp_cap_unicast_audio_update(const void *cmd, uint16_t cmd_len,
362 void *rsp, uint16_t *rsp_len)
363 {
364 int err;
365 const uint8_t *data_ptr;
366 const struct btp_cap_unicast_audio_update_cmd *cp = cmd;
367 struct bt_cap_unicast_audio_update_stream_param
368 stream_params[ARRAY_SIZE(btp_csip_set_members) * BTP_BAP_UNICAST_MAX_STREAMS_COUNT];
369 struct bt_cap_unicast_audio_update_param param = {0};
370
371 LOG_DBG("");
372
373 if (cp->stream_count == 0) {
374 return BTP_STATUS_FAILED;
375 }
376
377 data_ptr = cp->update_data;
378 for (size_t i = 0; i < cp->stream_count; i++) {
379 struct btp_bap_unicast_connection *u_conn;
380 struct btp_bap_unicast_stream *u_stream;
381 struct bt_conn *conn;
382 struct btp_cap_unicast_audio_update_data *update_data =
383 (struct btp_cap_unicast_audio_update_data *)data_ptr;
384 struct bt_cap_unicast_audio_update_stream_param *stream_param = &stream_params[i];
385
386 conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &update_data->address);
387 if (!conn) {
388 LOG_ERR("Unknown connection");
389
390 return BTP_STATUS_FAILED;
391 }
392
393 u_conn = btp_bap_unicast_conn_get(bt_conn_index(conn));
394 bt_conn_unref(conn);
395 if (u_conn->end_points_count == 0) {
396 /* Connection not initialized */
397
398 return BTP_STATUS_FAILED;
399 }
400
401 u_stream = btp_bap_unicast_stream_find(u_conn, update_data->ase_id);
402 if (u_stream == NULL) {
403 return BTP_STATUS_FAILED;
404 }
405
406 stream_param->stream = &u_stream->audio_stream.cap_stream;
407 stream_param->meta_len = update_data->metadata_ltvs_len;
408 stream_param->meta = update_data->metadata_ltvs;
409
410 data_ptr = ((uint8_t *)update_data) + stream_param->meta_len +
411 sizeof(struct btp_cap_unicast_audio_update_data);
412 }
413
414 param.count = cp->stream_count;
415 param.stream_params = stream_params;
416 param.type = BT_CAP_SET_TYPE_AD_HOC;
417
418 err = bt_cap_initiator_unicast_audio_update(¶m);
419 if (err != 0) {
420 LOG_ERR("Failed to start unicast audio: %d", err);
421
422 return BTP_STATUS_FAILED;
423 }
424
425 return BTP_STATUS_SUCCESS;
426 }
427
btp_cap_unicast_audio_stop(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)428 static uint8_t btp_cap_unicast_audio_stop(const void *cmd, uint16_t cmd_len,
429 void *rsp, uint16_t *rsp_len)
430 {
431 struct bt_cap_stream
432 *streams[ARRAY_SIZE(btp_csip_set_members) * BTP_BAP_UNICAST_MAX_STREAMS_COUNT];
433 struct bt_cap_unicast_audio_stop_param param = {0};
434 int err;
435 const struct btp_cap_unicast_audio_stop_cmd *cp = cmd;
436 size_t stream_cnt = 0U;
437
438 LOG_DBG("");
439
440 /* Get generate the same stream list as used by btp_cap_unicast_audio_start */
441 for (size_t conn_index = 0; conn_index < ARRAY_SIZE(btp_csip_set_members); conn_index++) {
442 struct btp_bap_unicast_connection *u_conn = btp_bap_unicast_conn_get(conn_index);
443
444 if (u_conn->end_points_count == 0) {
445 /* Connection not initialized */
446 continue;
447 }
448
449 for (size_t i = 0; i < ARRAY_SIZE(u_conn->streams); i++) {
450 struct btp_bap_unicast_stream *u_stream = &u_conn->streams[i];
451
452 if (!u_stream->in_use || u_stream->cig_id != cp->cig_id) {
453 continue;
454 }
455
456 streams[stream_cnt++] = stream_unicast_to_cap(u_stream);
457 }
458 }
459
460 param.streams = streams;
461 param.count = stream_cnt;
462 param.type = BT_CAP_SET_TYPE_AD_HOC;
463 param.release = (cp->flags & BTP_CAP_UNICAST_AUDIO_STOP_FLAG_RELEASE) != 0;
464
465 err = bt_cap_initiator_unicast_audio_stop(¶m);
466 if (err != 0) {
467 LOG_ERR("Failed to start unicast audio: %d", err);
468
469 return BTP_STATUS_FAILED;
470 }
471
472 return BTP_STATUS_SUCCESS;
473 }
474
btp_cap_broadcast_source_setup_stream(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)475 static uint8_t btp_cap_broadcast_source_setup_stream(const void *cmd, uint16_t cmd_len,
476 void *rsp, uint16_t *rsp_len)
477 {
478 const uint8_t *ltv_ptr;
479 struct btp_bap_broadcast_stream *stream;
480 const struct btp_cap_broadcast_source_setup_stream_cmd *cp = cmd;
481 struct btp_bap_broadcast_local_source *source =
482 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
483
484 if (source == NULL) {
485 return BTP_STATUS_FAILED;
486 }
487
488 struct bt_audio_codec_cfg *codec_cfg;
489
490 stream = btp_bap_broadcast_stream_alloc(source);
491 if (stream == NULL) {
492 return BTP_STATUS_FAILED;
493 }
494
495 stream->subgroup_id = cp->subgroup_id;
496 codec_cfg = &stream->codec_cfg;
497 memset(codec_cfg, 0, sizeof(*codec_cfg));
498 codec_cfg->id = cp->coding_format;
499 codec_cfg->vid = cp->vid;
500 codec_cfg->cid = cp->cid;
501
502 ltv_ptr = cp->ltvs;
503 if (cp->cc_ltvs_len != 0) {
504 codec_cfg->data_len = cp->cc_ltvs_len;
505 memcpy(codec_cfg->data, ltv_ptr, cp->cc_ltvs_len);
506 ltv_ptr += cp->cc_ltvs_len;
507 }
508
509 if (cp->metadata_ltvs_len != 0) {
510 codec_cfg->meta_len = cp->metadata_ltvs_len;
511 memcpy(codec_cfg->meta, ltv_ptr, cp->metadata_ltvs_len);
512 }
513
514 return BTP_STATUS_SUCCESS;
515 }
516
btp_cap_broadcast_source_setup_subgroup(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)517 static uint8_t btp_cap_broadcast_source_setup_subgroup(const void *cmd, uint16_t cmd_len,
518 void *rsp, uint16_t *rsp_len)
519 {
520 const uint8_t *ltv_ptr;
521 struct bt_audio_codec_cfg *codec_cfg;
522 const struct btp_cap_broadcast_source_setup_subgroup_cmd *cp = cmd;
523 struct btp_bap_broadcast_local_source *source =
524 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
525
526 if (source == NULL) {
527 return BTP_STATUS_FAILED;
528 }
529
530 if (cp->subgroup_id >= ARRAY_SIZE(cap_broadcast_params[0].cap_subgroup_params)) {
531 return BTP_STATUS_FAILED;
532 }
533
534 uint8_t idx = btp_bap_broadcast_local_source_idx_get(source);
535
536 if (idx >= ARRAY_SIZE(cap_broadcast_params)) {
537 return BTP_STATUS_FAILED;
538 }
539
540 struct bt_cap_initiator_broadcast_subgroup_param *subgroup_param =
541 &cap_broadcast_params[idx].cap_subgroup_params[cp->subgroup_id];
542
543 subgroup_param->codec_cfg = &source->subgroup_codec_cfg[cp->subgroup_id];
544
545 codec_cfg = subgroup_param->codec_cfg;
546
547 memset(codec_cfg, 0, sizeof(*codec_cfg));
548 codec_cfg->id = cp->coding_format;
549 codec_cfg->vid = cp->vid;
550 codec_cfg->cid = cp->cid;
551
552 ltv_ptr = cp->ltvs;
553 if (cp->cc_ltvs_len != 0) {
554 codec_cfg->data_len = cp->cc_ltvs_len;
555 memcpy(codec_cfg->data, ltv_ptr, cp->cc_ltvs_len);
556 ltv_ptr += cp->cc_ltvs_len;
557 }
558
559 if (cp->metadata_ltvs_len != 0) {
560 codec_cfg->meta_len = cp->metadata_ltvs_len;
561 memcpy(codec_cfg->meta, ltv_ptr, cp->metadata_ltvs_len);
562 }
563
564 return BTP_STATUS_SUCCESS;
565 }
566
cap_broadcast_source_adv_setup(struct btp_bap_broadcast_local_source * source,uint32_t * gap_settings)567 static int cap_broadcast_source_adv_setup(struct btp_bap_broadcast_local_source *source,
568 uint32_t *gap_settings)
569 {
570 int err;
571
572 NET_BUF_SIMPLE_DEFINE(base_buf, 128);
573
574 /* Broadcast Audio Streaming Endpoint advertising data */
575 struct bt_data per_ad;
576
577 /* A more specialized adv instance may already have been created by another btp module */
578 if (source->ext_adv == NULL) {
579 struct bt_le_adv_param param = *BT_LE_EXT_ADV_NCONN;
580 struct bt_data base_ad[2];
581
582 NET_BUF_SIMPLE_DEFINE(ad_buf, BT_UUID_SIZE_16 + BT_AUDIO_BROADCAST_ID_SIZE);
583 *gap_settings = BIT(BTP_GAP_SETTINGS_DISCOVERABLE) |
584 BIT(BTP_GAP_SETTINGS_EXTENDED_ADVERTISING);
585 /* Setup extended advertising data */
586 net_buf_simple_add_le16(&ad_buf, BT_UUID_BROADCAST_AUDIO_VAL);
587 net_buf_simple_add_le24(&ad_buf, source->broadcast_id);
588 base_ad[0].type = BT_DATA_SVC_DATA16;
589 base_ad[0].data_len = ad_buf.len;
590 base_ad[0].data = ad_buf.data;
591 base_ad[1].type = BT_DATA_NAME_COMPLETE;
592 base_ad[1].data_len = sizeof(CONFIG_BT_DEVICE_NAME) - 1;
593 base_ad[1].data = CONFIG_BT_DEVICE_NAME;
594
595 err = tester_gap_create_adv_instance(¶m, BTP_GAP_ADDR_TYPE_IDENTITY,
596 base_ad, 2, NULL, 0, gap_settings,
597 &source->ext_adv);
598 if (err != 0) {
599 LOG_DBG("Failed to create extended advertising instance: %d", err);
600 return -EINVAL;
601 }
602 }
603
604 err = tester_gap_padv_configure(source->ext_adv,
605 BT_LE_PER_ADV_PARAM(BT_GAP_PER_ADV_FAST_INT_MIN_2,
606 BT_GAP_PER_ADV_FAST_INT_MAX_2,
607 BT_LE_PER_ADV_OPT_NONE));
608 if (err != 0) {
609 LOG_DBG("Failed to configure periodic advertising: %d", err);
610
611 return -EINVAL;
612 }
613
614 err = bt_cap_initiator_broadcast_get_base(source->cap_broadcast, &base_buf);
615 if (err != 0) {
616 LOG_DBG("Failed to get encoded BASE: %d\n", err);
617
618 return -EINVAL;
619 }
620
621 per_ad.type = BT_DATA_SVC_DATA16;
622 per_ad.data_len = base_buf.len;
623 per_ad.data = base_buf.data;
624 err = tester_gap_padv_set_data(source->ext_adv, &per_ad, 1);
625 if (err != 0) {
626 return -EINVAL;
627 }
628
629 return 0;
630 }
631
btp_cap_broadcast_source_setup(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)632 static uint8_t btp_cap_broadcast_source_setup(const void *cmd, uint16_t cmd_len,
633 void *rsp, uint16_t *rsp_len)
634 {
635 int err;
636 uint32_t gap_settings;
637 static struct bt_cap_initiator_broadcast_create_param create_param;
638 const struct btp_cap_broadcast_source_setup_cmd *cp = cmd;
639 struct btp_cap_broadcast_source_setup_rp *rp = rsp;
640 struct btp_bap_broadcast_local_source *source =
641 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
642
643 if (source == NULL) {
644 return BTP_STATUS_FAILED;
645 }
646
647 struct cap_initiator_broadcast_params *cap_params =
648 &cap_broadcast_params[cp->source_id];
649
650 struct bt_bap_qos_cfg *qos = &source->qos;
651
652 LOG_DBG("");
653
654 memset(&create_param, 0, sizeof(create_param));
655
656 for (size_t i = 0; i < ARRAY_SIZE(source->streams); i++) {
657 struct btp_bap_broadcast_stream *stream = &source->streams[i];
658 struct bt_cap_initiator_broadcast_stream_param *stream_param;
659 uint8_t bis_id;
660
661 if (!stream->in_use) {
662 /* No more streams set up */
663 break;
664 }
665
666 bis_id = cap_params->cap_subgroup_params[stream->subgroup_id].stream_count++;
667 stream_param = &cap_params->cap_stream_params[stream->subgroup_id][bis_id];
668
669 stream_param->stream = stream_broadcast_to_cap(stream);
670
671 if (cp->flags & BTP_CAP_BROADCAST_SOURCE_SETUP_FLAG_SUBGROUP_CODEC) {
672 stream_param->data_len = 0;
673 stream_param->data = NULL;
674 } else {
675 stream_param->data_len = stream->codec_cfg.data_len;
676 stream_param->data = stream->codec_cfg.data;
677 }
678 }
679
680 for (size_t i = 0; i < ARRAY_SIZE(cap_params->cap_subgroup_params); i++) {
681 if (cap_params->cap_subgroup_params[i].stream_count == 0) {
682 /* No gaps allowed */
683 break;
684 }
685
686 cap_params->cap_subgroup_params[i].stream_params = cap_params->cap_stream_params[i];
687 create_param.subgroup_count++;
688 }
689
690 if (create_param.subgroup_count == 0) {
691 return BTP_STATUS_FAILED;
692 }
693
694 memset(qos, 0, sizeof(*qos));
695 qos->phy = BT_BAP_QOS_CFG_2M;
696 qos->framing = cp->framing;
697 qos->rtn = cp->retransmission_num;
698 qos->sdu = sys_le16_to_cpu(cp->max_sdu);
699 qos->latency = sys_le16_to_cpu(cp->max_transport_latency);
700 qos->interval = sys_get_le24(cp->sdu_interval);
701 qos->pd = sys_get_le24(cp->presentation_delay);
702
703 create_param.subgroup_params = cap_params->cap_subgroup_params;
704 create_param.qos = qos;
705 create_param.packing = BT_ISO_PACKING_SEQUENTIAL;
706 create_param.encryption = cp->flags & BTP_CAP_BROADCAST_SOURCE_SETUP_FLAG_ENCRYPTION;
707 memcpy(create_param.broadcast_code, cp->broadcast_code, sizeof(cp->broadcast_code));
708
709 err = bt_cap_initiator_broadcast_audio_create(&create_param, &source->cap_broadcast);
710 memset(&cap_params->cap_subgroup_params, 0, sizeof(cap_params->cap_subgroup_params));
711 memset(&create_param, 0, sizeof(create_param));
712 if (err != 0) {
713 LOG_ERR("Failed to create audio source: %d", err);
714
715 return BTP_STATUS_FAILED;
716 }
717
718 err = cap_broadcast_source_adv_setup(source, &gap_settings);
719 if (err != 0) {
720 return BTP_STATUS_FAILED;
721 }
722
723 rp->gap_settings = gap_settings;
724 sys_put_le24(source->broadcast_id, rp->broadcast_id);
725 *rsp_len = sizeof(*rp) + 1;
726
727 return BTP_STATUS_SUCCESS;
728 }
729
btp_cap_broadcast_source_release(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)730 static uint8_t btp_cap_broadcast_source_release(const void *cmd, uint16_t cmd_len,
731 void *rsp, uint16_t *rsp_len)
732 {
733 int err;
734 const struct btp_cap_broadcast_source_release_cmd *cp = cmd;
735 struct btp_bap_broadcast_local_source *source =
736 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
737
738 LOG_DBG("");
739
740 /* If no source has been created yet, there is nothing to release */
741 if (source == NULL || source->cap_broadcast == NULL) {
742 return BTP_STATUS_SUCCESS;
743 }
744
745 err = bt_cap_initiator_broadcast_audio_delete(source->cap_broadcast);
746 if (err != 0) {
747 LOG_DBG("Unable to delete broadcast source: %d", err);
748
749 return BTP_STATUS_FAILED;
750 }
751
752 memset(source, 0, sizeof(*source));
753
754 return BTP_STATUS_SUCCESS;
755 }
756
btp_cap_broadcast_adv_start(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)757 static uint8_t btp_cap_broadcast_adv_start(const void *cmd, uint16_t cmd_len,
758 void *rsp, uint16_t *rsp_len)
759 {
760 int err;
761 const struct btp_cap_broadcast_adv_start_cmd *cp = cmd;
762 struct btp_bap_broadcast_local_source *source =
763 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
764
765 if (source == NULL) {
766 return BTP_STATUS_FAILED;
767 }
768
769 if (source->ext_adv == NULL) {
770 return BTP_STATUS_FAILED;
771 }
772
773 err = tester_gap_start_ext_adv(source->ext_adv);
774 if (err != 0) {
775 return BTP_STATUS_FAILED;
776 }
777
778 err = tester_gap_padv_start(source->ext_adv);
779 if (err != 0) {
780 LOG_DBG("Unable to start periodic advertising: %d", err);
781
782 return BTP_STATUS_FAILED;
783 }
784
785 return BTP_STATUS_SUCCESS;
786 }
787
btp_cap_broadcast_adv_stop(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)788 static uint8_t btp_cap_broadcast_adv_stop(const void *cmd, uint16_t cmd_len,
789 void *rsp, uint16_t *rsp_len)
790 {
791 int err;
792 const struct btp_cap_broadcast_adv_stop_cmd *cp = cmd;
793
794 LOG_DBG("");
795
796 struct btp_bap_broadcast_local_source *source =
797 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
798
799 if (source == NULL) {
800 return BTP_STATUS_FAILED;
801 }
802
803 err = tester_gap_padv_stop(source->ext_adv);
804 if (err == -ESRCH) {
805 /* Ext adv hasn't been created yet */
806 return BTP_STATUS_SUCCESS;
807 } else if (err != 0) {
808 LOG_DBG("Failed to stop periodic adv, err: %d", err);
809 return BTP_STATUS_FAILED;
810 }
811
812 err = tester_gap_stop_ext_adv(source->ext_adv);
813
814 return BTP_STATUS_VAL(err);
815 }
816
btp_cap_broadcast_source_start(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)817 static uint8_t btp_cap_broadcast_source_start(const void *cmd, uint16_t cmd_len,
818 void *rsp, uint16_t *rsp_len)
819 {
820 int err;
821 const struct btp_cap_broadcast_source_start_cmd *cp = cmd;
822 struct btp_bap_broadcast_local_source *source =
823 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
824
825 if (source == NULL) {
826 return BTP_STATUS_FAILED;
827 }
828
829 LOG_DBG("");
830
831 if (source->ext_adv == NULL) {
832 return BTP_STATUS_FAILED;
833 }
834
835 err = bt_cap_initiator_broadcast_audio_start(source->cap_broadcast, source->ext_adv);
836 if (err != 0) {
837 LOG_ERR("Failed to start audio source: %d", err);
838
839 return BTP_STATUS_FAILED;
840 }
841
842 return BTP_STATUS_SUCCESS;
843 }
844
btp_cap_broadcast_source_stop(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)845 static uint8_t btp_cap_broadcast_source_stop(const void *cmd, uint16_t cmd_len,
846 void *rsp, uint16_t *rsp_len)
847 {
848 int err;
849 const struct btp_cap_broadcast_source_stop_cmd *cp = cmd;
850 struct btp_bap_broadcast_local_source *source =
851 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
852
853 /* If no source has been started yet, there is nothing to stop */
854 if (source == NULL || source->cap_broadcast == NULL) {
855 return BTP_STATUS_SUCCESS;
856 }
857
858 err = bt_cap_initiator_broadcast_audio_stop(source->cap_broadcast);
859 if (err != 0) {
860 LOG_ERR("Failed to stop audio source: %d", err);
861
862 return BTP_STATUS_FAILED;
863 }
864
865 /* Make sure source is stopped before proceeding */
866 err = k_sem_take(&source_stopped_sem, K_SECONDS(1));
867 if (err) {
868 LOG_ERR("Semaphore timed out: %d", err);
869
870 return BTP_STATUS_FAILED;
871 }
872
873 return BTP_STATUS_SUCCESS;
874 }
875
btp_cap_broadcast_source_update(const void * cmd,uint16_t cmd_len,void * rsp,uint16_t * rsp_len)876 static uint8_t btp_cap_broadcast_source_update(const void *cmd, uint16_t cmd_len,
877 void *rsp, uint16_t *rsp_len)
878 {
879 int err;
880 struct bt_data per_ad;
881 const struct btp_cap_broadcast_source_update_cmd *cp = cmd;
882 struct btp_bap_broadcast_local_source *source =
883 btp_bap_broadcast_local_source_from_src_id_get(cp->source_id);
884
885 if (source == NULL) {
886 return BTP_STATUS_FAILED;
887 }
888
889 NET_BUF_SIMPLE_DEFINE(base_buf, 128);
890
891 LOG_DBG("");
892
893 if (cp->metadata_ltvs_len == 0) {
894 return BTP_STATUS_FAILED;
895 }
896
897 err = bt_cap_initiator_broadcast_audio_update(source->cap_broadcast, cp->metadata_ltvs,
898 cp->metadata_ltvs_len);
899 if (err != 0) {
900 LOG_ERR("Failed to update audio source: %d", err);
901
902 return BTP_STATUS_FAILED;
903 }
904
905 err = bt_cap_initiator_broadcast_get_base(source->cap_broadcast, &base_buf);
906 if (err != 0) {
907 LOG_DBG("Failed to get encoded BASE: %d\n", err);
908
909 return -EINVAL;
910 }
911
912 per_ad.type = BT_DATA_SVC_DATA16;
913 per_ad.data_len = base_buf.len;
914 per_ad.data = base_buf.data;
915 err = tester_gap_padv_set_data(source->ext_adv, &per_ad, 1);
916 if (err != 0) {
917 return -EINVAL;
918 }
919
920 return BTP_STATUS_SUCCESS;
921 }
922
923 static const struct btp_handler cap_handlers[] = {
924 {
925 .opcode = BTP_CAP_READ_SUPPORTED_COMMANDS,
926 .index = BTP_INDEX_NONE,
927 .expect_len = 0,
928 .func = btp_cap_supported_commands
929 },
930 {
931 .opcode = BTP_CAP_DISCOVER,
932 .expect_len = sizeof(struct btp_cap_discover_cmd),
933 .func = btp_cap_discover
934 },
935 {
936 .opcode = BTP_CAP_UNICAST_SETUP_ASE,
937 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
938 .func = btp_cap_unicast_setup_ase
939 },
940 {
941 .opcode = BTP_CAP_UNICAST_AUDIO_START,
942 .expect_len = sizeof(struct btp_cap_unicast_audio_start_cmd),
943 .func = btp_cap_unicast_audio_start
944 },
945 {
946 .opcode = BTP_CAP_UNICAST_AUDIO_UPDATE,
947 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
948 .func = btp_cap_unicast_audio_update
949 },
950 {
951 .opcode = BTP_CAP_UNICAST_AUDIO_STOP,
952 .expect_len = sizeof(struct btp_cap_unicast_audio_stop_cmd),
953 .func = btp_cap_unicast_audio_stop
954 },
955 {
956 .opcode = BTP_CAP_BROADCAST_SOURCE_SETUP_STREAM,
957 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
958 .func = btp_cap_broadcast_source_setup_stream
959 },
960 {
961 .opcode = BTP_CAP_BROADCAST_SOURCE_SETUP_SUBGROUP,
962 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
963 .func = btp_cap_broadcast_source_setup_subgroup
964 },
965 {
966 .opcode = BTP_CAP_BROADCAST_SOURCE_SETUP,
967 .expect_len = sizeof(struct btp_cap_broadcast_source_setup_cmd),
968 .func = btp_cap_broadcast_source_setup
969 },
970 {
971 .opcode = BTP_CAP_BROADCAST_SOURCE_RELEASE,
972 .expect_len = sizeof(struct btp_cap_broadcast_source_release_cmd),
973 .func = btp_cap_broadcast_source_release
974 },
975 {
976 .opcode = BTP_CAP_BROADCAST_ADV_START,
977 .expect_len = sizeof(struct btp_cap_broadcast_adv_start_cmd),
978 .func = btp_cap_broadcast_adv_start
979 },
980 {
981 .opcode = BTP_CAP_BROADCAST_ADV_STOP,
982 .expect_len = sizeof(struct btp_cap_broadcast_adv_stop_cmd),
983 .func = btp_cap_broadcast_adv_stop
984 },
985 {
986 .opcode = BTP_CAP_BROADCAST_SOURCE_START,
987 .expect_len = sizeof(struct btp_cap_broadcast_source_start_cmd),
988 .func = btp_cap_broadcast_source_start
989 },
990 {
991 .opcode = BTP_CAP_BROADCAST_SOURCE_STOP,
992 .expect_len = sizeof(struct btp_cap_broadcast_source_stop_cmd),
993 .func = btp_cap_broadcast_source_stop
994 },
995 {
996 .opcode = BTP_CAP_BROADCAST_SOURCE_UPDATE,
997 .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
998 .func = btp_cap_broadcast_source_update
999 },
1000 };
1001
tester_init_cap(void)1002 uint8_t tester_init_cap(void)
1003 {
1004 int err;
1005
1006 err = bt_cap_initiator_register_cb(&cap_cb);
1007 if (err != 0) {
1008 LOG_DBG("Failed to register CAP callbacks (err %d)", err);
1009 return err;
1010 }
1011
1012 tester_register_command_handlers(BTP_SERVICE_ID_CAP, cap_handlers,
1013 ARRAY_SIZE(cap_handlers));
1014
1015 return BTP_STATUS_SUCCESS;
1016 }
1017
tester_unregister_cap(void)1018 uint8_t tester_unregister_cap(void)
1019 {
1020 return BTP_STATUS_SUCCESS;
1021 }
1022
