1 /** @file
2 * @brief Bluetooth Basic Audio Profile (BAP) Unicast Server role.
3 *
4 * Copyright (c) 2021-2023 Nordic Semiconductor ASA
5 * Copyright (c) 2022 Codecoup
6 *
7 * SPDX-License-Identifier: Apache-2.0
8 */
9 #include <errno.h>
10 #include <stddef.h>
11 #include <stdint.h>
12 #include <string.h>
13
14 #include <zephyr/bluetooth/addr.h>
15 #include <zephyr/bluetooth/audio/lc3.h>
16 #include <zephyr/bluetooth/bluetooth.h>
17 #include <zephyr/bluetooth/conn.h>
18 #include <zephyr/bluetooth/gap.h>
19 #include <zephyr/bluetooth/hci.h>
20 #include <zephyr/bluetooth/audio/audio.h>
21 #include <zephyr/bluetooth/audio/bap.h>
22 #include <zephyr/bluetooth/audio/pacs.h>
23 #include <zephyr/bluetooth/hci_types.h>
24 #include <zephyr/bluetooth/iso.h>
25 #include <zephyr/kernel.h>
26 #include <zephyr/net_buf.h>
27 #include <zephyr/sys/byteorder.h>
28 #include <zephyr/sys/printk.h>
29 #include <zephyr/sys/util.h>
30 #include <zephyr/sys/util_macro.h>
31
32 NET_BUF_POOL_FIXED_DEFINE(tx_pool, CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT,
33 BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU),
34 CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL);
35
36 static const struct bt_audio_codec_cap lc3_codec_cap = BT_AUDIO_CODEC_CAP_LC3(
37 BT_AUDIO_CODEC_CAP_FREQ_16KHZ | BT_AUDIO_CODEC_CAP_FREQ_24KHZ,
38 BT_AUDIO_CODEC_CAP_DURATION_10, BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), 40u, 60u, 1u,
39 (BT_AUDIO_CONTEXT_TYPE_CONVERSATIONAL | BT_AUDIO_CONTEXT_TYPE_MEDIA));
40
41 static struct bt_conn *default_conn;
42 static struct k_work_delayable audio_send_work;
43 static struct bt_bap_stream streams[CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT +
44 CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT];
45 static struct audio_source {
46 struct bt_bap_stream *stream;
47 uint16_t seq_num;
48 } source_streams[CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT];
49 static size_t configured_source_stream_count;
50
51 static const struct bt_bap_qos_cfg_pref qos_pref =
52 BT_BAP_QOS_CFG_PREF(true, BT_GAP_LE_PHY_2M, 0x02, 10, 20000, 40000, 20000, 40000);
53
get_and_incr_seq_num(const struct bt_bap_stream * stream)54 static uint16_t get_and_incr_seq_num(const struct bt_bap_stream *stream)
55 {
56 for (size_t i = 0U; i < configured_source_stream_count; i++) {
57 if (stream == source_streams[i].stream) {
58 return source_streams[i].seq_num++;
59 }
60 }
61
62 printk("Could not find endpoint from stream %p\n", stream);
63
64 return 0;
65 }
66
print_hex(const uint8_t * ptr,size_t len)67 static void print_hex(const uint8_t *ptr, size_t len)
68 {
69 while (len-- != 0) {
70 printk("%02x", *ptr++);
71 }
72 }
73
print_cb(struct bt_data * data,void * user_data)74 static bool print_cb(struct bt_data *data, void *user_data)
75 {
76 const char *str = (const char *)user_data;
77
78 printk("%s: type 0x%02x value_len %u\n", str, data->type, data->data_len);
79 print_hex(data->data, data->data_len);
80 printk("\n");
81
82 return true;
83 }
84
print_codec_cfg(const struct bt_audio_codec_cfg * codec_cfg)85 static void print_codec_cfg(const struct bt_audio_codec_cfg *codec_cfg)
86 {
87 printk("codec_cfg 0x%02x cid 0x%04x vid 0x%04x count %u\n", codec_cfg->id, codec_cfg->cid,
88 codec_cfg->vid, codec_cfg->data_len);
89
90 if (codec_cfg->id == BT_HCI_CODING_FORMAT_LC3) {
91 enum bt_audio_location chan_allocation;
92 int ret;
93
94 /* LC3 uses the generic LTV format - other codecs might do as well */
95
96 bt_audio_data_parse(codec_cfg->data, codec_cfg->data_len, print_cb, "data");
97
98 ret = bt_audio_codec_cfg_get_freq(codec_cfg);
99 if (ret > 0) {
100 printk(" Frequency: %d Hz\n", bt_audio_codec_cfg_freq_to_freq_hz(ret));
101 }
102
103 ret = bt_audio_codec_cfg_get_frame_dur(codec_cfg);
104 if (ret > 0) {
105 printk(" Frame Duration: %d us\n",
106 bt_audio_codec_cfg_frame_dur_to_frame_dur_us(ret));
107 }
108
109 ret = bt_audio_codec_cfg_get_chan_allocation(codec_cfg, &chan_allocation, false);
110 if (ret == 0) {
111 printk(" Channel allocation: 0x%x\n", chan_allocation);
112 }
113
114 printk(" Octets per frame: %d (negative means value not pressent)\n",
115 bt_audio_codec_cfg_get_octets_per_frame(codec_cfg));
116 printk(" Frames per SDU: %d\n",
117 bt_audio_codec_cfg_get_frame_blocks_per_sdu(codec_cfg, true));
118 } else {
119 print_hex(codec_cfg->data, codec_cfg->data_len);
120 }
121
122 bt_audio_data_parse(codec_cfg->meta, codec_cfg->meta_len, print_cb, "meta");
123 }
124
print_qos(const struct bt_bap_qos_cfg * qos)125 static void print_qos(const struct bt_bap_qos_cfg *qos)
126 {
127 printk("QoS: interval %u framing 0x%02x phy 0x%02x sdu %u "
128 "rtn %u latency %u pd %u\n",
129 qos->interval, qos->framing, qos->phy, qos->sdu,
130 qos->rtn, qos->latency, qos->pd);
131 }
132
133 /**
134 * @brief Send audio data on timeout
135 *
136 * This will send an increasing amount of audio data, starting from 1 octet.
137 * The data is just mock data, and does not actually represent any audio.
138 *
139 * First iteration : 0x00
140 * Second iteration: 0x00 0x01
141 * Third iteration : 0x00 0x01 0x02
142 *
143 * And so on, until it wraps around the configured MTU (CONFIG_BT_ISO_TX_MTU)
144 *
145 * @param work Pointer to the work structure
146 */
audio_timer_timeout(struct k_work * work)147 static void audio_timer_timeout(struct k_work *work)
148 {
149 int ret;
150 static uint8_t buf_data[CONFIG_BT_ISO_TX_MTU];
151 static bool data_initialized;
152 struct net_buf *buf;
153 static size_t len_to_send = 1;
154
155 if (!data_initialized) {
156 /* TODO: Actually encode some audio data */
157 for (size_t i = 0U; i < ARRAY_SIZE(buf_data); i++) {
158 buf_data[i] = (uint8_t)i;
159 }
160
161 data_initialized = true;
162 }
163
164 /* We configured the sink streams to be first in `streams`, so that
165 * we can use `stream[i]` to select sink streams (i.e. streams with
166 * data going to the server)
167 */
168 for (size_t i = 0; i < configured_source_stream_count; i++) {
169 struct bt_bap_stream *stream = source_streams[i].stream;
170
171 buf = net_buf_alloc(&tx_pool, K_NO_WAIT);
172 if (buf == NULL) {
173 printk("Failed to allocate TX buffer\n");
174 /* Break and retry later */
175 break;
176 }
177 net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
178
179 net_buf_add_mem(buf, buf_data, len_to_send);
180
181 ret = bt_bap_stream_send(stream, buf, get_and_incr_seq_num(stream));
182 if (ret < 0) {
183 printk("Failed to send audio data on streams[%zu] (%p): (%d)\n",
184 i, stream, ret);
185 net_buf_unref(buf);
186 } else {
187 printk("Sending mock data with len %zu on streams[%zu] (%p)\n",
188 len_to_send, i, stream);
189 }
190 }
191
192 k_work_schedule(&audio_send_work, K_MSEC(1000U));
193
194 len_to_send++;
195 if (len_to_send > ARRAY_SIZE(buf_data)) {
196 len_to_send = 1;
197 }
198 }
199
stream_alloc(void)200 static struct bt_bap_stream *stream_alloc(void)
201 {
202 for (size_t i = 0; i < ARRAY_SIZE(streams); i++) {
203 struct bt_bap_stream *stream = &streams[i];
204
205 if (!stream->conn) {
206 return stream;
207 }
208 }
209
210 return NULL;
211 }
212
lc3_config(struct bt_conn * conn,const struct bt_bap_ep * ep,enum bt_audio_dir dir,const struct bt_audio_codec_cfg * codec_cfg,struct bt_bap_stream ** stream,struct bt_bap_qos_cfg_pref * const pref,struct bt_bap_ascs_rsp * rsp)213 static int lc3_config(struct bt_conn *conn, const struct bt_bap_ep *ep, enum bt_audio_dir dir,
214 const struct bt_audio_codec_cfg *codec_cfg, struct bt_bap_stream **stream,
215 struct bt_bap_qos_cfg_pref *const pref, struct bt_bap_ascs_rsp *rsp)
216 {
217 printk("ASE Codec Config: conn %p ep %p dir %u\n", conn, ep, dir);
218
219 print_codec_cfg(codec_cfg);
220
221 *stream = stream_alloc();
222 if (*stream == NULL) {
223 printk("No streams available\n");
224 *rsp = BT_BAP_ASCS_RSP(BT_BAP_ASCS_RSP_CODE_NO_MEM, BT_BAP_ASCS_REASON_NONE);
225 return -ENOMEM;
226 }
227
228 printk("ASE Codec Config stream %p\n", *stream);
229
230 if (dir == BT_AUDIO_DIR_SOURCE) {
231 source_streams[configured_source_stream_count++].stream = *stream;
232 }
233
234 *pref = qos_pref;
235
236 return 0;
237 }
238
lc3_reconfig(struct bt_bap_stream * stream,enum bt_audio_dir dir,const struct bt_audio_codec_cfg * codec_cfg,struct bt_bap_qos_cfg_pref * const pref,struct bt_bap_ascs_rsp * rsp)239 static int lc3_reconfig(struct bt_bap_stream *stream, enum bt_audio_dir dir,
240 const struct bt_audio_codec_cfg *codec_cfg,
241 struct bt_bap_qos_cfg_pref *const pref, struct bt_bap_ascs_rsp *rsp)
242 {
243 printk("ASE Codec Reconfig: stream %p\n", stream);
244
245 print_codec_cfg(codec_cfg);
246
247 *rsp = BT_BAP_ASCS_RSP(BT_BAP_ASCS_RSP_CODE_CONF_UNSUPPORTED, BT_BAP_ASCS_REASON_NONE);
248
249 /* We only support one QoS at the moment, reject changes */
250 return -ENOEXEC;
251 }
252
lc3_qos(struct bt_bap_stream * stream,const struct bt_bap_qos_cfg * qos,struct bt_bap_ascs_rsp * rsp)253 static int lc3_qos(struct bt_bap_stream *stream, const struct bt_bap_qos_cfg *qos,
254 struct bt_bap_ascs_rsp *rsp)
255 {
256 printk("QoS: stream %p qos %p\n", stream, qos);
257
258 print_qos(qos);
259
260 return 0;
261 }
262
lc3_enable(struct bt_bap_stream * stream,const uint8_t meta[],size_t meta_len,struct bt_bap_ascs_rsp * rsp)263 static int lc3_enable(struct bt_bap_stream *stream, const uint8_t meta[], size_t meta_len,
264 struct bt_bap_ascs_rsp *rsp)
265 {
266 printk("Enable: stream %p meta_len %zu\n", stream, meta_len);
267
268 return 0;
269 }
270
lc3_start(struct bt_bap_stream * stream,struct bt_bap_ascs_rsp * rsp)271 static int lc3_start(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp)
272 {
273 printk("Start: stream %p\n", stream);
274
275 if (IS_ENABLED(CONFIG_BT_ASCS_ASE_SRC)) {
276 for (size_t i = 0U; i < configured_source_stream_count; i++) {
277 if (source_streams[i].stream == stream) {
278 source_streams[i].seq_num = 0U;
279 break;
280 }
281 }
282
283 if (configured_source_stream_count > 0 &&
284 !k_work_delayable_is_pending(&audio_send_work)) {
285
286 /* Start send timer */
287 k_work_schedule(&audio_send_work, K_MSEC(0));
288 }
289 }
290
291 return 0;
292 }
293
data_func_cb(struct bt_data * data,void * user_data)294 static bool data_func_cb(struct bt_data *data, void *user_data)
295 {
296 struct bt_bap_ascs_rsp *rsp = (struct bt_bap_ascs_rsp *)user_data;
297
298 if (!BT_AUDIO_METADATA_TYPE_IS_KNOWN(data->type)) {
299 printk("Invalid metadata type %u or length %u\n", data->type, data->data_len);
300 *rsp = BT_BAP_ASCS_RSP(BT_BAP_ASCS_RSP_CODE_METADATA_REJECTED, data->type);
301
302 return -EINVAL;
303 }
304
305 return true;
306 }
307
lc3_metadata(struct bt_bap_stream * stream,const uint8_t meta[],size_t meta_len,struct bt_bap_ascs_rsp * rsp)308 static int lc3_metadata(struct bt_bap_stream *stream, const uint8_t meta[], size_t meta_len,
309 struct bt_bap_ascs_rsp *rsp)
310 {
311 printk("Metadata: stream %p meta_len %zu\n", stream, meta_len);
312
313 return bt_audio_data_parse(meta, meta_len, data_func_cb, rsp);
314 }
315
lc3_disable(struct bt_bap_stream * stream,struct bt_bap_ascs_rsp * rsp)316 static int lc3_disable(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp)
317 {
318 printk("Disable: stream %p\n", stream);
319
320 return 0;
321 }
322
lc3_stop(struct bt_bap_stream * stream,struct bt_bap_ascs_rsp * rsp)323 static int lc3_stop(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp)
324 {
325 printk("Stop: stream %p\n", stream);
326
327 return 0;
328 }
329
lc3_release(struct bt_bap_stream * stream,struct bt_bap_ascs_rsp * rsp)330 static int lc3_release(struct bt_bap_stream *stream, struct bt_bap_ascs_rsp *rsp)
331 {
332 printk("Release: stream %p\n", stream);
333 return 0;
334 }
335
336 static struct bt_bap_unicast_server_register_param param = {
337 CONFIG_BT_ASCS_MAX_ASE_SNK_COUNT,
338 CONFIG_BT_ASCS_MAX_ASE_SRC_COUNT
339 };
340
341 static const struct bt_bap_unicast_server_cb unicast_server_cb = {
342 .config = lc3_config,
343 .reconfig = lc3_reconfig,
344 .qos = lc3_qos,
345 .enable = lc3_enable,
346 .start = lc3_start,
347 .metadata = lc3_metadata,
348 .disable = lc3_disable,
349 .stop = lc3_stop,
350 .release = lc3_release,
351 };
352
stream_recv(struct bt_bap_stream * stream,const struct bt_iso_recv_info * info,struct net_buf * buf)353 static void stream_recv(struct bt_bap_stream *stream, const struct bt_iso_recv_info *info,
354 struct net_buf *buf)
355 {
356 printk("Incoming audio on stream %p len %u\n", stream, buf->len);
357 }
358
359 static struct bt_bap_stream_ops stream_ops = {
360 .recv = stream_recv
361 };
362
connected(struct bt_conn * conn,uint8_t err)363 static void connected(struct bt_conn *conn, uint8_t err)
364 {
365 char addr[BT_ADDR_LE_STR_LEN];
366
367 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
368
369 if (err != 0) {
370 printk("Failed to connect to %s %u %s\n", addr, err, bt_hci_err_to_str(err));
371
372 default_conn = NULL;
373 return;
374 }
375
376 printk("Connected: %s\n", addr);
377 default_conn = bt_conn_ref(conn);
378 }
379
disconnected(struct bt_conn * conn,uint8_t reason)380 static void disconnected(struct bt_conn *conn, uint8_t reason)
381 {
382 char addr[BT_ADDR_LE_STR_LEN];
383 struct k_work_sync sync;
384
385 if (conn != default_conn) {
386 return;
387 }
388
389 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
390
391 printk("Disconnected: %s, reason 0x%02x %s\n", addr, reason, bt_hci_err_to_str(reason));
392
393 bt_conn_unref(default_conn);
394 default_conn = NULL;
395
396 if (IS_ENABLED(CONFIG_BT_ASCS_ASE_SRC)) {
397 /* reset data */
398 (void)memset(source_streams, 0, sizeof(source_streams));
399 configured_source_stream_count = 0U;
400 k_work_cancel_delayable_sync(&audio_send_work, &sync);
401 }
402 }
403
404 BT_CONN_CB_DEFINE(conn_callbacks) = {
405 .connected = connected,
406 .disconnected = disconnected,
407 };
408
409 static struct bt_pacs_cap cap_sink = {
410 .codec_cap = &lc3_codec_cap,
411 };
412
413 static struct bt_pacs_cap cap_source = {
414 .codec_cap = &lc3_codec_cap,
415 };
416
bap_unicast_sr_init(void)417 int bap_unicast_sr_init(void)
418 {
419 const struct bt_pacs_register_param pacs_param = {
420 .snk_pac = true,
421 .snk_loc = true,
422 .src_pac = true,
423 .src_loc = true,
424 };
425 int err;
426
427 err = bt_pacs_register(&pacs_param);
428 if (err) {
429 printk("Could not register PACS (err %d)\n", err);
430 return err;
431 }
432
433 bt_bap_unicast_server_register(¶m);
434 bt_bap_unicast_server_register_cb(&unicast_server_cb);
435
436 bt_pacs_cap_register(BT_AUDIO_DIR_SINK, &cap_sink);
437
438 if (IS_ENABLED(CONFIG_BT_PAC_SNK_LOC)) {
439 if (IS_ENABLED(CONFIG_HAP_HA_HEARING_AID_BANDED)) {
440 /* HAP_d1.0r00; 3.7 BAP Unicast Server role requirements
441 * A Banded Hearing Aid in the HA role shall set the
442 * Front Left and the Front Right bits to a value of 0b1
443 * in the Sink Audio Locations characteristic value.
444 */
445 bt_pacs_set_location(BT_AUDIO_DIR_SINK,
446 (BT_AUDIO_LOCATION_FRONT_LEFT |
447 BT_AUDIO_LOCATION_FRONT_RIGHT));
448 } else if (IS_ENABLED(CONFIG_HAP_HA_HEARING_AID_LEFT)) {
449 bt_pacs_set_location(BT_AUDIO_DIR_SINK,
450 BT_AUDIO_LOCATION_FRONT_LEFT);
451 } else {
452 bt_pacs_set_location(BT_AUDIO_DIR_SINK,
453 BT_AUDIO_LOCATION_FRONT_RIGHT);
454 }
455 }
456
457 if (IS_ENABLED(CONFIG_BT_ASCS_ASE_SRC)) {
458 bt_pacs_cap_register(BT_AUDIO_DIR_SOURCE, &cap_source);
459 }
460
461 for (size_t i = 0; i < ARRAY_SIZE(streams); i++) {
462 bt_bap_stream_cb_register(&streams[i], &stream_ops);
463 }
464
465 if (IS_ENABLED(CONFIG_BT_ASCS_ASE_SRC)) {
466 k_work_init_delayable(&audio_send_work, audio_timer_timeout);
467 }
468
469 return 0;
470 }
471