1 /*
2 * Copyright (c) 2022-2023 Nordic Semiconductor ASA
3 * Copyright 2023 NXP
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #if defined(CONFIG_BT_CAP_INITIATOR)
9
10 #include <zephyr/types.h>
11 #include <stddef.h>
12 #include <zephyr/kernel.h>
13 #include <zephyr/bluetooth/bluetooth.h>
14 #include <zephyr/bluetooth/conn.h>
15 #include <zephyr/bluetooth/audio/audio.h>
16 #include <zephyr/bluetooth/audio/bap_lc3_preset.h>
17 #include <zephyr/bluetooth/audio/cap.h>
18 #include <zephyr/bluetooth/audio/bap.h>
19
20 static struct k_work_delayable audio_send_work;
21
22 static struct bt_cap_stream unicast_streams[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT +
23 CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT];
24 static struct bt_bap_ep *unicast_sink_eps[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT];
25 static struct bt_bap_ep *unicast_source_eps[CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT];
26
27 NET_BUF_POOL_FIXED_DEFINE(tx_pool, CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SNK_COUNT,
28 CONFIG_BT_ISO_TX_MTU + BT_ISO_CHAN_SEND_RESERVE, 8, NULL);
29
30 static K_SEM_DEFINE(sem_cas_discovery, 0, 1);
31 static K_SEM_DEFINE(sem_discover_sink, 0, 1);
32 static K_SEM_DEFINE(sem_discover_source, 0, 1);
33 static K_SEM_DEFINE(sem_audio_start, 0, 1);
34
unicast_stream_configured(struct bt_bap_stream * stream,const struct bt_audio_codec_qos_pref * pref)35 static void unicast_stream_configured(struct bt_bap_stream *stream,
36 const struct bt_audio_codec_qos_pref *pref)
37 {
38 printk("Configured stream %p\n", stream);
39
40 /* TODO: The preference should be used/taken into account when
41 * setting the QoS
42 */
43 }
44
unicast_stream_qos_set(struct bt_bap_stream * stream)45 static void unicast_stream_qos_set(struct bt_bap_stream *stream)
46 {
47 printk("QoS set stream %p\n", stream);
48 }
49
unicast_stream_enabled(struct bt_bap_stream * stream)50 static void unicast_stream_enabled(struct bt_bap_stream *stream)
51 {
52 printk("Enabled stream %p\n", stream);
53 }
54
unicast_stream_started(struct bt_bap_stream * stream)55 static void unicast_stream_started(struct bt_bap_stream *stream)
56 {
57 printk("Started stream %p\n", stream);
58
59 k_sem_give(&sem_audio_start);
60 }
61
unicast_stream_metadata_updated(struct bt_bap_stream * stream)62 static void unicast_stream_metadata_updated(struct bt_bap_stream *stream)
63 {
64 printk("Metadata updated stream %p\n", stream);
65 }
66
unicast_stream_disabled(struct bt_bap_stream * stream)67 static void unicast_stream_disabled(struct bt_bap_stream *stream)
68 {
69 printk("Disabled stream %p\n", stream);
70 }
71
unicast_stream_stopped(struct bt_bap_stream * stream,uint8_t reason)72 static void unicast_stream_stopped(struct bt_bap_stream *stream, uint8_t reason)
73 {
74 printk("Stopped stream %p with reason 0x%02X\n", stream, reason);
75
76 /* Stop send timer */
77 k_work_cancel_delayable(&audio_send_work);
78 }
79
unicast_stream_released(struct bt_bap_stream * stream)80 static void unicast_stream_released(struct bt_bap_stream *stream)
81 {
82 printk("Released stream %p\n", stream);
83 }
84
85 static struct bt_bap_stream_ops unicast_stream_ops = {
86 .configured = unicast_stream_configured,
87 .qos_set = unicast_stream_qos_set,
88 .enabled = unicast_stream_enabled,
89 .started = unicast_stream_started,
90 .metadata_updated = unicast_stream_metadata_updated,
91 .disabled = unicast_stream_disabled,
92 .stopped = unicast_stream_stopped,
93 .released = unicast_stream_released,
94 };
95
96 static struct bt_bap_lc3_preset unicast_preset_48_2_1 =
97 BT_BAP_LC3_UNICAST_PRESET_48_2_1(BT_AUDIO_LOCATION_FRONT_LEFT,
98 BT_AUDIO_CONTEXT_TYPE_MEDIA);
99
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)100 static void cap_discovery_complete_cb(struct bt_conn *conn, int err,
101 const struct bt_csip_set_coordinator_set_member *member,
102 const struct bt_csip_set_coordinator_csis_inst *csis_inst)
103 {
104 if (err != 0) {
105 printk("Failed to discover CAS: %d", err);
106 return;
107 }
108
109 if (IS_ENABLED(CONFIG_BT_CAP_ACCEPTOR_SET_MEMBER)) {
110 if (csis_inst == NULL) {
111 printk("Failed to discover CAS CSIS");
112 return;
113 }
114
115 printk("Found CAS with CSIS %p\n", csis_inst);
116 } else {
117 printk("Found CAS\n");
118 }
119
120 k_sem_give(&sem_cas_discovery);
121 }
122
unicast_start_complete_cb(int err,struct bt_conn * conn)123 static void unicast_start_complete_cb(int err, struct bt_conn *conn)
124 {
125 if (err != 0) {
126 printk("Failed to start (failing conn %p): %d", conn, err);
127 return;
128 }
129
130 k_sem_give(&sem_audio_start);
131 }
132
unicast_update_complete_cb(int err,struct bt_conn * conn)133 static void unicast_update_complete_cb(int err, struct bt_conn *conn)
134 {
135 if (err != 0) {
136 printk("Failed to update (failing conn %p): %d", conn, err);
137 return;
138 }
139 }
140
unicast_stop_complete_cb(int err,struct bt_conn * conn)141 static void unicast_stop_complete_cb(int err, struct bt_conn *conn)
142 {
143 if (err != 0) {
144 printk("Failed to stop (failing conn %p): %d", conn, err);
145 return;
146 }
147 }
148
149 static struct bt_cap_initiator_cb cap_cb = {
150 .unicast_discovery_complete = cap_discovery_complete_cb,
151 .unicast_start_complete = unicast_start_complete_cb,
152 .unicast_update_complete = unicast_update_complete_cb,
153 .unicast_stop_complete = unicast_stop_complete_cb,
154 };
155
discover_cas(struct bt_conn * conn)156 static int discover_cas(struct bt_conn *conn)
157 {
158 int err;
159
160 err = bt_cap_initiator_unicast_discover(conn);
161 if (err != 0) {
162 printk("Failed to discover CAS: %d\n", err);
163 return err;
164 }
165
166 err = k_sem_take(&sem_cas_discovery, K_FOREVER);
167 if (err != 0) {
168 printk("failed to take sem_cas_discovery (err %d)\n", err);
169 return err;
170 }
171
172 return err;
173 }
174
print_hex(const uint8_t * ptr,size_t len)175 static void print_hex(const uint8_t *ptr, size_t len)
176 {
177 while (len-- != 0) {
178 printk("%02x", *ptr++);
179 }
180 }
181
print_cb(struct bt_data * data,void * user_data)182 static bool print_cb(struct bt_data *data, void *user_data)
183 {
184 const char *str = (const char *)user_data;
185
186 printk("%s: type 0x%02x value_len %u\n", str, data->type, data->data_len);
187 print_hex(data->data, data->data_len);
188 printk("\n");
189
190 return true;
191 }
192
print_remote_codec(const struct bt_audio_codec_cap * codec_cap,enum bt_audio_dir dir)193 static void print_remote_codec(const struct bt_audio_codec_cap *codec_cap, enum bt_audio_dir dir)
194 {
195 printk("codec id 0x%02x cid 0x%04x vid 0x%04x count %u\n", codec_cap->id, codec_cap->cid,
196 codec_cap->vid, codec_cap->data_len);
197
198 if (codec_cap->id == BT_HCI_CODING_FORMAT_LC3) {
199 bt_audio_data_parse(codec_cap->data, codec_cap->data_len, print_cb, "data");
200 } else { /* If not LC3, we cannot assume it's LTV */
201 printk("data: ");
202 print_hex(codec_cap->data, codec_cap->data_len);
203 printk("\n");
204 }
205
206 bt_audio_data_parse(codec_cap->meta, codec_cap->meta_len, print_cb, "meta");
207 }
208
add_remote_sink(struct bt_bap_ep * ep)209 static void add_remote_sink(struct bt_bap_ep *ep)
210 {
211 for (size_t i = 0U; i < ARRAY_SIZE(unicast_sink_eps); i++) {
212 if (unicast_sink_eps[i] == NULL) {
213 printk("Sink #%zu: ep %p\n", i, ep);
214 unicast_sink_eps[i] = ep;
215 return;
216 }
217 }
218 }
219
add_remote_source(struct bt_bap_ep * ep)220 static void add_remote_source(struct bt_bap_ep *ep)
221 {
222 for (size_t i = 0U; i < ARRAY_SIZE(unicast_source_eps); i++) {
223 if (unicast_source_eps[i] == NULL) {
224 printk("Source #%zu: ep %p\n", i, ep);
225 unicast_source_eps[i] = ep;
226 return;
227 }
228 }
229 }
230
discover_cb(struct bt_conn * conn,int err,enum bt_audio_dir dir)231 static void discover_cb(struct bt_conn *conn, int err, enum bt_audio_dir dir)
232 {
233 if (err != 0) {
234 printk("Discovery failed: %d\n", err);
235 return;
236 }
237
238 if (dir == BT_AUDIO_DIR_SINK) {
239 printk("Sink discover complete\n");
240 k_sem_give(&sem_discover_sink);
241 } else if (dir == BT_AUDIO_DIR_SOURCE) {
242 printk("Discover sources complete: err %d\n", err);
243 k_sem_give(&sem_discover_source);
244 }
245 }
246
pac_record_cb(struct bt_conn * conn,enum bt_audio_dir dir,const struct bt_audio_codec_cap * codec_cap)247 static void pac_record_cb(struct bt_conn *conn, enum bt_audio_dir dir,
248 const struct bt_audio_codec_cap *codec_cap)
249 {
250 print_remote_codec(codec_cap, dir);
251 }
252
endpoint_cb(struct bt_conn * conn,enum bt_audio_dir dir,struct bt_bap_ep * ep)253 static void endpoint_cb(struct bt_conn *conn, enum bt_audio_dir dir, struct bt_bap_ep *ep)
254 {
255 if (dir == BT_AUDIO_DIR_SOURCE) {
256 add_remote_source(ep);
257 } else if (dir == BT_AUDIO_DIR_SINK) {
258 add_remote_sink(ep);
259 }
260 }
261
discover_sinks(struct bt_conn * conn)262 static int discover_sinks(struct bt_conn *conn)
263 {
264 int err;
265
266 err = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SINK);
267 if (err != 0) {
268 printk("Failed to discover sink: %d\n", err);
269 return err;
270 }
271
272 err = k_sem_take(&sem_discover_sink, K_FOREVER);
273 if (err != 0) {
274 printk("failed to take sem_discover_sink (err %d)\n", err);
275 return err;
276 }
277
278 return err;
279 }
280
discover_sources(struct bt_conn * conn)281 static int discover_sources(struct bt_conn *conn)
282 {
283 int err;
284
285 err = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SOURCE);
286 if (err != 0) {
287 printk("Failed to discover sources: %d\n", err);
288 return err;
289 }
290
291 err = k_sem_take(&sem_discover_source, K_FOREVER);
292 if (err != 0) {
293 printk("failed to take sem_discover_source (err %d)\n", err);
294 return err;
295 }
296
297 return 0;
298 }
299
300 static struct bt_bap_unicast_client_cb unicast_client_cbs = {
301 .discover = discover_cb,
302 .pac_record = pac_record_cb,
303 .endpoint = endpoint_cb,
304 };
305
unicast_group_create(struct bt_bap_unicast_group ** out_unicast_group)306 static int unicast_group_create(struct bt_bap_unicast_group **out_unicast_group)
307 {
308 int err = 0;
309 struct bt_bap_unicast_group_stream_param group_stream_params;
310 struct bt_bap_unicast_group_stream_pair_param pair_params;
311 struct bt_bap_unicast_group_param group_param;
312
313 group_stream_params.qos = &unicast_preset_48_2_1.qos;
314 group_stream_params.stream = &unicast_streams[0].bap_stream;
315 pair_params.tx_param = &group_stream_params;
316 pair_params.rx_param = NULL;
317
318 group_param.packing = BT_ISO_PACKING_SEQUENTIAL;
319 group_param.params_count = 1;
320 group_param.params = &pair_params;
321
322 err = bt_bap_unicast_group_create(&group_param, out_unicast_group);
323 if (err != 0) {
324 printk("Failed to create group: %d\n", err);
325 return err;
326 }
327 printk("Created group\n");
328
329 return err;
330 }
331
unicast_audio_start(struct bt_conn * conn)332 static int unicast_audio_start(struct bt_conn *conn)
333 {
334 int err = 0;
335 struct bt_cap_unicast_audio_start_stream_param stream_param;
336 struct bt_cap_unicast_audio_start_param param;
337
338 param.type = BT_CAP_SET_TYPE_AD_HOC;
339 param.count = 1u;
340 param.stream_params = &stream_param;
341
342 stream_param.member.member = conn;
343 stream_param.stream = &unicast_streams[0];
344 stream_param.ep = unicast_sink_eps[0];
345 stream_param.codec_cfg = &unicast_preset_48_2_1.codec_cfg;
346
347 err = bt_cap_initiator_unicast_audio_start(¶m);
348 if (err != 0) {
349 printk("Failed to start unicast audio: %d\n", err);
350 return err;
351 }
352
353 return err;
354 }
355
356 /**
357 * @brief Send audio data on timeout
358 *
359 * This will send an amount of data equal to the configured QoS SDU.
360 * The data is just mock data, and does not actually represent any audio.
361
362 *
363 * @param work Pointer to the work structure
364 */
audio_timer_timeout(struct k_work * work)365 static void audio_timer_timeout(struct k_work *work)
366 {
367 static uint8_t buf_data[CONFIG_BT_ISO_TX_MTU];
368 static bool data_initialized;
369 struct net_buf *buf;
370 struct net_buf *buf_to_send;
371 int ret;
372 static size_t len_to_send;
373 struct bt_bap_stream *stream = &unicast_streams[0].bap_stream;
374
375 len_to_send = unicast_preset_48_2_1.qos.sdu;
376
377 if (!data_initialized) {
378 /* TODO: Actually encode some audio data */
379 for (size_t i = 0; i < ARRAY_SIZE(buf_data); i++) {
380 buf_data[i] = (uint8_t)i;
381 }
382
383 data_initialized = true;
384 }
385
386 buf = net_buf_alloc(&tx_pool, K_FOREVER);
387 net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
388 net_buf_add_mem(buf, buf_data, len_to_send);
389 buf_to_send = buf;
390
391 ret = bt_bap_stream_send(stream, buf_to_send, 0);
392 if (ret < 0) {
393 printk("Failed to send audio data on streams: (%d)\n", ret);
394 net_buf_unref(buf_to_send);
395 } else {
396 printk("Sending mock data with len %zu\n", len_to_send);
397 }
398
399 k_work_schedule(&audio_send_work, K_MSEC(1000));
400 }
401
cap_initiator_init(void)402 int cap_initiator_init(void)
403 {
404 int err = 0;
405
406 if (IS_ENABLED(CONFIG_BT_BAP_UNICAST_CLIENT)) {
407 err = bt_cap_initiator_register_cb(&cap_cb);
408 if (err != 0) {
409 printk("Failed to register CAP callbacks (err %d)\n", err);
410 return err;
411 }
412
413 err = bt_bap_unicast_client_register_cb(&unicast_client_cbs);
414 if (err != 0) {
415 printk("Failed to register BAP unicast client callbacks (err %d)\n", err);
416 return err;
417 }
418
419 for (size_t i = 0U; i < ARRAY_SIZE(unicast_streams); i++) {
420 bt_cap_stream_ops_register(&unicast_streams[i],
421 &unicast_stream_ops);
422 }
423 k_work_init_delayable(&audio_send_work, audio_timer_timeout);
424 }
425
426 return 0;
427 }
428
cap_initiator_setup(struct bt_conn * conn)429 int cap_initiator_setup(struct bt_conn *conn)
430 {
431 int err = 0;
432 struct bt_bap_unicast_group *unicast_group;
433
434 k_sem_reset(&sem_cas_discovery);
435 k_sem_reset(&sem_discover_sink);
436 k_sem_reset(&sem_discover_source);
437 k_sem_reset(&sem_audio_start);
438
439 err = discover_cas(conn);
440 if (err != 0) {
441 return err;
442 }
443
444 err = discover_sinks(conn);
445 if (err != 0) {
446 return err;
447 }
448
449 err = discover_sources(conn);
450 if (err != 0) {
451 return err;
452 }
453
454 err = unicast_group_create(&unicast_group);
455 if (err != 0) {
456 return err;
457 }
458
459 err = unicast_audio_start(conn);
460 if (err != 0) {
461 return err;
462 }
463
464 k_sem_take(&sem_audio_start, K_FOREVER);
465
466 k_work_schedule(&audio_send_work, K_MSEC(0));
467
468 return err;
469 }
470
471 #endif /* CONFIG_BT_CAP_INITIATOR */
472
