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_csis_inst * csis_inst)100 static void cap_discovery_complete_cb(struct bt_conn *conn, int err,
101 const struct bt_csip_set_coordinator_csis_inst *csis_inst)
102 {
103 if (err != 0) {
104 printk("Failed to discover CAS: %d", err);
105 return;
106 }
107
108 if (IS_ENABLED(CONFIG_BT_CAP_ACCEPTOR_SET_MEMBER)) {
109 if (csis_inst == NULL) {
110 printk("Failed to discover CAS CSIS");
111 return;
112 }
113
114 printk("Found CAS with CSIS %p\n", csis_inst);
115 } else {
116 printk("Found CAS\n");
117 }
118
119 k_sem_give(&sem_cas_discovery);
120 }
121
unicast_start_complete_cb(struct bt_bap_unicast_group * unicast_group,int err,struct bt_conn * conn)122 static void unicast_start_complete_cb(struct bt_bap_unicast_group *unicast_group,
123 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(struct bt_bap_unicast_group * unicast_group,int err,struct bt_conn * conn)141 static void unicast_stop_complete_cb(struct bt_bap_unicast_group *unicast_group, int err,
142 struct bt_conn *conn)
143 {
144 if (err != 0) {
145 printk("Failed to stop (failing conn %p): %d", conn, err);
146 return;
147 }
148 }
149
150 static struct bt_cap_initiator_cb cap_cb = {
151 .unicast_discovery_complete = cap_discovery_complete_cb,
152 .unicast_start_complete = unicast_start_complete_cb,
153 .unicast_update_complete = unicast_update_complete_cb,
154 .unicast_stop_complete = unicast_stop_complete_cb,
155 };
156
discover_cas(struct bt_conn * conn)157 static int discover_cas(struct bt_conn *conn)
158 {
159 int err;
160
161 err = bt_cap_initiator_unicast_discover(conn);
162 if (err != 0) {
163 printk("Failed to discover CAS: %d\n", err);
164 return err;
165 }
166
167 err = k_sem_take(&sem_cas_discovery, K_FOREVER);
168 if (err != 0) {
169 printk("failed to take sem_cas_discovery (err %d)\n", err);
170 return err;
171 }
172
173 return err;
174 }
175
print_hex(const uint8_t * ptr,size_t len)176 static void print_hex(const uint8_t *ptr, size_t len)
177 {
178 while (len-- != 0) {
179 printk("%02x", *ptr++);
180 }
181 }
182
print_cb(struct bt_data * data,void * user_data)183 static bool print_cb(struct bt_data *data, void *user_data)
184 {
185 const char *str = (const char *)user_data;
186
187 printk("%s: type 0x%02x value_len %u\n", str, data->type, data->data_len);
188 print_hex(data->data, data->data_len);
189 printk("\n");
190
191 return true;
192 }
193
print_remote_codec(const struct bt_audio_codec_cap * codec_cap,enum bt_audio_dir dir)194 static void print_remote_codec(const struct bt_audio_codec_cap *codec_cap, enum bt_audio_dir dir)
195 {
196 printk("codec id 0x%02x cid 0x%04x vid 0x%04x count %u\n", codec_cap->id, codec_cap->cid,
197 codec_cap->vid, codec_cap->data_len);
198
199 if (codec_cap->id == BT_HCI_CODING_FORMAT_LC3) {
200 bt_audio_data_parse(codec_cap->data, codec_cap->data_len, print_cb, "data");
201 } else { /* If not LC3, we cannot assume it's LTV */
202 printk("data: ");
203 print_hex(codec_cap->data, codec_cap->data_len);
204 printk("\n");
205 }
206
207 bt_audio_data_parse(codec_cap->meta, codec_cap->meta_len, print_cb, "meta");
208 }
209
add_remote_sink(struct bt_bap_ep * ep)210 static void add_remote_sink(struct bt_bap_ep *ep)
211 {
212 for (size_t i = 0U; i < ARRAY_SIZE(unicast_sink_eps); i++) {
213 if (unicast_sink_eps[i] == NULL) {
214 printk("Sink #%zu: ep %p\n", i, ep);
215 unicast_sink_eps[i] = ep;
216 return;
217 }
218 }
219 }
220
add_remote_source(struct bt_bap_ep * ep)221 static void add_remote_source(struct bt_bap_ep *ep)
222 {
223 for (size_t i = 0U; i < ARRAY_SIZE(unicast_source_eps); i++) {
224 if (unicast_source_eps[i] == NULL) {
225 printk("Source #%zu: ep %p\n", i, ep);
226 unicast_source_eps[i] = ep;
227 return;
228 }
229 }
230 }
231
discover_cb(struct bt_conn * conn,int err,enum bt_audio_dir dir)232 static void discover_cb(struct bt_conn *conn, int err, enum bt_audio_dir dir)
233 {
234 if (err != 0) {
235 printk("Discovery failed: %d\n", err);
236 return;
237 }
238
239 if (dir == BT_AUDIO_DIR_SINK) {
240 printk("Sink discover complete\n");
241 k_sem_give(&sem_discover_sink);
242 } else if (dir == BT_AUDIO_DIR_SOURCE) {
243 printk("Discover sources complete: err %d\n", err);
244 k_sem_give(&sem_discover_source);
245 }
246 }
247
pac_record_cb(struct bt_conn * conn,enum bt_audio_dir dir,const struct bt_audio_codec_cap * codec_cap)248 static void pac_record_cb(struct bt_conn *conn, enum bt_audio_dir dir,
249 const struct bt_audio_codec_cap *codec_cap)
250 {
251 print_remote_codec(codec_cap, dir);
252 }
253
endpoint_cb(struct bt_conn * conn,enum bt_audio_dir dir,struct bt_bap_ep * ep)254 static void endpoint_cb(struct bt_conn *conn, enum bt_audio_dir dir, struct bt_bap_ep *ep)
255 {
256 if (dir == BT_AUDIO_DIR_SOURCE) {
257 add_remote_source(ep);
258 } else if (dir == BT_AUDIO_DIR_SINK) {
259 add_remote_sink(ep);
260 }
261 }
262
discover_sinks(struct bt_conn * conn)263 static int discover_sinks(struct bt_conn *conn)
264 {
265 int err;
266
267 err = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SINK);
268 if (err != 0) {
269 printk("Failed to discover sink: %d\n", err);
270 return err;
271 }
272
273 err = k_sem_take(&sem_discover_sink, K_FOREVER);
274 if (err != 0) {
275 printk("failed to take sem_discover_sink (err %d)\n", err);
276 return err;
277 }
278
279 return err;
280 }
281
discover_sources(struct bt_conn * conn)282 static int discover_sources(struct bt_conn *conn)
283 {
284 int err;
285
286 err = bt_bap_unicast_client_discover(conn, BT_AUDIO_DIR_SOURCE);
287 if (err != 0) {
288 printk("Failed to discover sources: %d\n", err);
289 return err;
290 }
291
292 err = k_sem_take(&sem_discover_source, K_FOREVER);
293 if (err != 0) {
294 printk("failed to take sem_discover_source (err %d)\n", err);
295 return err;
296 }
297
298 return 0;
299 }
300
301 static struct bt_bap_unicast_client_cb unicast_client_cbs = {
302 .discover = discover_cb,
303 .pac_record = pac_record_cb,
304 .endpoint = endpoint_cb,
305 };
306
unicast_group_create(struct bt_bap_unicast_group ** out_unicast_group)307 static int unicast_group_create(struct bt_bap_unicast_group **out_unicast_group)
308 {
309 int err = 0;
310 struct bt_bap_unicast_group_stream_param group_stream_params;
311 struct bt_bap_unicast_group_stream_pair_param pair_params;
312 struct bt_bap_unicast_group_param group_param;
313
314 group_stream_params.qos = &unicast_preset_48_2_1.qos;
315 group_stream_params.stream = &unicast_streams[0].bap_stream;
316 pair_params.tx_param = &group_stream_params;
317 pair_params.rx_param = NULL;
318
319 group_param.packing = BT_ISO_PACKING_SEQUENTIAL;
320 group_param.params_count = 1;
321 group_param.params = &pair_params;
322
323 err = bt_bap_unicast_group_create(&group_param, out_unicast_group);
324 if (err != 0) {
325 printk("Failed to create group: %d\n", err);
326 return err;
327 }
328 printk("Created group\n");
329
330 return err;
331 }
332
unicast_audio_start(struct bt_conn * conn,struct bt_bap_unicast_group * unicast_group)333 static int unicast_audio_start(struct bt_conn *conn, struct bt_bap_unicast_group *unicast_group)
334 {
335 int err = 0;
336 struct bt_cap_unicast_audio_start_stream_param stream_param;
337 struct bt_cap_unicast_audio_start_param param;
338
339 param.type = BT_CAP_SET_TYPE_AD_HOC;
340 param.count = 1u;
341 param.stream_params = &stream_param;
342
343 stream_param.member.member = conn;
344 stream_param.stream = &unicast_streams[0];
345 stream_param.ep = unicast_sink_eps[0];
346 stream_param.codec_cfg = &unicast_preset_48_2_1.codec_cfg;
347
348 err = bt_cap_initiator_unicast_audio_start(¶m, unicast_group);
349 if (err != 0) {
350 printk("Failed to start unicast audio: %d\n", err);
351 return err;
352 }
353
354 return err;
355 }
356
357 /**
358 * @brief Send audio data on timeout
359 *
360 * This will send an amount of data equal to the configured QoS SDU.
361 * The data is just mock data, and does not actually represent any audio.
362
363 *
364 * @param work Pointer to the work structure
365 */
audio_timer_timeout(struct k_work * work)366 static void audio_timer_timeout(struct k_work *work)
367 {
368 static uint8_t buf_data[CONFIG_BT_ISO_TX_MTU];
369 static bool data_initialized;
370 struct net_buf *buf;
371 struct net_buf *buf_to_send;
372 int ret;
373 static size_t len_to_send;
374 struct bt_bap_stream *stream = &unicast_streams[0].bap_stream;
375
376 len_to_send = unicast_preset_48_2_1.qos.sdu;
377
378 if (!data_initialized) {
379 /* TODO: Actually encode some audio data */
380 for (size_t i = 0; i < ARRAY_SIZE(buf_data); i++) {
381 buf_data[i] = (uint8_t)i;
382 }
383
384 data_initialized = true;
385 }
386
387 buf = net_buf_alloc(&tx_pool, K_FOREVER);
388 net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
389 net_buf_add_mem(buf, buf_data, len_to_send);
390 buf_to_send = buf;
391
392 ret = bt_bap_stream_send(stream, buf_to_send, 0, BT_ISO_TIMESTAMP_NONE);
393 if (ret < 0) {
394 printk("Failed to send audio data on streams: (%d)\n", ret);
395 net_buf_unref(buf_to_send);
396 } else {
397 printk("Sending mock data with len %zu\n", len_to_send);
398 }
399
400 k_work_schedule(&audio_send_work, K_MSEC(1000));
401 }
402
cap_initiator_init(void)403 int cap_initiator_init(void)
404 {
405 int err = 0;
406
407 if (IS_ENABLED(CONFIG_BT_BAP_UNICAST_CLIENT)) {
408 err = bt_cap_initiator_register_cb(&cap_cb);
409 if (err != 0) {
410 printk("Failed to register CAP callbacks (err %d)\n", err);
411 return err;
412 }
413
414 err = bt_bap_unicast_client_register_cb(&unicast_client_cbs);
415 if (err != 0) {
416 printk("Failed to register BAP unicast client callbacks (err %d)\n", err);
417 return err;
418 }
419
420 for (size_t i = 0U; i < ARRAY_SIZE(unicast_streams); i++) {
421 bt_cap_stream_ops_register(&unicast_streams[i],
422 &unicast_stream_ops);
423 }
424 k_work_init_delayable(&audio_send_work, audio_timer_timeout);
425 }
426
427 return 0;
428 }
429
cap_initiator_setup(struct bt_conn * conn)430 int cap_initiator_setup(struct bt_conn *conn)
431 {
432 int err = 0;
433 struct bt_bap_unicast_group *unicast_group;
434
435 k_sem_reset(&sem_cas_discovery);
436 k_sem_reset(&sem_discover_sink);
437 k_sem_reset(&sem_discover_source);
438 k_sem_reset(&sem_audio_start);
439
440 err = discover_cas(conn);
441 if (err != 0) {
442 return err;
443 }
444
445 err = discover_sinks(conn);
446 if (err != 0) {
447 return err;
448 }
449
450 err = discover_sources(conn);
451 if (err != 0) {
452 return err;
453 }
454
455 err = unicast_group_create(&unicast_group);
456 if (err != 0) {
457 return err;
458 }
459
460 err = unicast_audio_start(conn, unicast_group);
461 if (err != 0) {
462 return err;
463 }
464
465 k_sem_take(&sem_audio_start, K_FOREVER);
466
467 k_work_schedule(&audio_send_work, K_MSEC(0));
468
469 return err;
470 }
471
472 #endif /* CONFIG_BT_CAP_INITIATOR */
473
