1 /* 2 * Copyright (C) 2022 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define BTSTACK_FILE__ "le_audio_broadcast_sink.c" 39 40 /* 41 * LE Audio Broadcast Sink 42 */ 43 44 45 #include "btstack_config.h" 46 47 #include <stdint.h> 48 #include <stdio.h> 49 #include <stdlib.h> 50 #include <string.h> 51 #include <inttypes.h> 52 53 #include "ad_parser.h" 54 #include "ble/gatt-service/broadcast_audio_scan_service_server.h" 55 #include "ble/att_server.h" 56 #include "ble/sm.h" 57 #include "bluetooth_data_types.h" 58 #include "bluetooth_gatt.h" 59 #include "btstack_debug.h" 60 #include "btstack_audio.h" 61 #include "btstack_event.h" 62 #include "btstack_run_loop.h" 63 #include "btstack_ring_buffer.h" 64 #include "btstack_stdin.h" 65 #include "btstack_util.h" 66 #include "gap.h" 67 #include "hci.h" 68 #include "hci_cmd.h" 69 #include "btstack_lc3.h" 70 #include "btstack_lc3_google.h" 71 #include "btstack_lc3plus_fraunhofer.h" 72 #include "l2cap.h" 73 74 #include "le_audio_broadcast_sink.h" 75 76 #ifdef HAVE_POSIX_FILE_IO 77 #include "wav_util.h" 78 #endif 79 80 // max config 81 #define MAX_NUM_BIS 2 82 #define MAX_SAMPLES_PER_FRAME 480 83 84 // playback 85 #define MAX_NUM_LC3_FRAMES 5 86 #define MAX_BYTES_PER_SAMPLE 4 87 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_BYTES_PER_SAMPLE) 88 89 // analysis 90 #define PACKET_PREFIX_LEN 10 91 92 #define ANSI_COLOR_RED "\x1b[31m" 93 #define ANSI_COLOR_GREEN "\x1b[32m" 94 #define ANSI_COLOR_YELLOW "\x1b[33m" 95 #define ANSI_COLOR_BLUE "\x1b[34m" 96 #define ANSI_COLOR_MAGENTA "\x1b[35m" 97 #define ANSI_COLOR_CYAN "\x1b[36m" 98 #define ANSI_COLOR_RESET "\x1b[0m" 99 100 static void show_usage(void); 101 102 static const char * filename_wav = "le_audio_broadcast_sink.wav"; 103 104 static enum { 105 APP_W4_WORKING, 106 APP_W4_BROADCAST_ADV, 107 APP_W4_PA_AND_BIG_INFO, 108 APP_W4_BIG_SYNC_ESTABLISHED, 109 APP_STREAMING, 110 APP_IDLE 111 } app_state = APP_W4_WORKING; 112 113 static const uint8_t adv_sid = 0; 114 static le_advertising_set_t le_advertising_set; 115 static uint8_t adv_handle = 0; 116 117 static const le_extended_advertising_parameters_t extended_params = { 118 .advertising_event_properties = 1, // connectable 119 .primary_advertising_interval_min = 0x4b0, // 750 ms 120 .primary_advertising_interval_max = 0x4b0, // 750 ms 121 .primary_advertising_channel_map = 7, 122 .own_address_type = 0, 123 .peer_address_type = 0, 124 .peer_address = { 0 }, 125 .advertising_filter_policy = 0, 126 .advertising_tx_power = 10, // 10 dBm 127 .primary_advertising_phy = 1, // LE 1M PHY 128 .secondary_advertising_max_skip = 0, 129 .secondary_advertising_phy = 1, // LE 1M PHY 130 .advertising_sid = adv_sid, 131 .scan_request_notification_enable = 0, 132 }; 133 134 static const uint8_t extended_adv_data[] = { 135 // 16 bit service data, ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_SCAN_SERVICE, Broadcast ID 136 6, BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID, 137 ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_SCAN_SERVICE & 0xff, ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_SCAN_SERVICE >> 8, 138 0x30, 0x5d, 0x9b, 139 // name 140 5, BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME, 'S', 'i', 'n', 'k' 141 }; 142 143 #define BASS_NUM_CLIENTS 1 144 #define BASS_NUM_SOURCES 1 145 static bass_server_source_t bass_sources[BASS_NUM_SOURCES]; 146 static bass_remote_client_t bass_clients[BASS_NUM_CLIENTS]; 147 148 // 149 static btstack_packet_callback_registration_t hci_event_callback_registration; 150 static btstack_packet_callback_registration_t sm_event_callback_registration; 151 152 static bool have_base; 153 static bool have_big_info; 154 static bool have_past; 155 static bool standalone_mode; 156 157 uint32_t last_samples_report_ms; 158 uint16_t samples_received; 159 uint16_t samples_dropped; 160 uint16_t frames_per_second[MAX_NUM_BIS]; 161 162 // remote info 163 static char remote_name[20]; 164 static bd_addr_t remote; 165 static bd_addr_type_t remote_type; 166 static uint8_t remote_sid; 167 static bool count_mode; 168 static bool pts_mode; 169 static bool nrf5340_audio_demo; 170 171 172 // broadcast info 173 static const uint8_t big_handle = 1; 174 static hci_con_handle_t sync_handle; 175 static hci_con_handle_t bis_con_handles[MAX_NUM_BIS]; 176 static unsigned int next_bis_index; 177 static uint8_t encryption; 178 static uint8_t broadcast_code [] = {0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01}; 179 180 // analysis 181 static bool last_packet_received_big; 182 static uint16_t last_packet_sequence_big; 183 static bool last_packet_received[MAX_NUM_BIS]; 184 static uint16_t last_packet_sequence[MAX_NUM_BIS]; 185 static uint32_t last_packet_time_ms[MAX_NUM_BIS]; 186 static uint8_t last_packet_prefix[MAX_NUM_BIS * PACKET_PREFIX_LEN]; 187 188 // BIG Sync 189 static le_audio_big_sync_t big_sync_storage; 190 static le_audio_big_sync_params_t big_sync_params; 191 192 // lc3 writer 193 static uint32_t lc3_frames; 194 195 // lc3 codec config 196 static uint16_t sampling_frequency_hz; 197 static btstack_lc3_frame_duration_t frame_duration; 198 static uint16_t number_samples_per_frame; 199 static uint16_t octets_per_frame; 200 static uint8_t num_bis; 201 202 // lc3 decoder 203 static bool request_lc3plus_decoder = false; 204 static bool use_lc3plus_decoder = false; 205 static const btstack_lc3_decoder_t * lc3_decoder; 206 static int16_t pcm[MAX_NUM_BIS * MAX_SAMPLES_PER_FRAME]; 207 208 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_NUM_BIS]; 209 #ifdef HAVE_LC3PLUS 210 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_NUM_BIS]; 211 #endif 212 static void * decoder_contexts[MAX_NR_BIS]; 213 214 // playback 215 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; 216 static btstack_ring_buffer_t playback_buffer; 217 218 static btstack_timer_source_t next_packet_timer; 219 static uint16_t cached_iso_sdu_len; 220 static bool have_pcm[MAX_NUM_BIS]; 221 222 static void le_audio_broadcast_sink_playback(int16_t * buffer, uint16_t num_samples){ 223 // called from lower-layer but guaranteed to be on main thread 224 uint32_t bytes_needed = num_samples * num_bis * 2; 225 226 static bool underrun = true; 227 228 log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / ( num_bis * 2)); 229 230 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)){ 231 memset(buffer, 0, bytes_needed); 232 if (underrun == false){ 233 log_info("Playback underrun"); 234 underrun = true; 235 } 236 return; 237 } 238 239 if (underrun){ 240 underrun = false; 241 log_info("Playback started"); 242 } 243 uint32_t bytes_read; 244 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 245 btstack_assert(bytes_read == bytes_needed); 246 } 247 248 static void setup_lc3_decoder(void){ 249 uint8_t channel; 250 for (channel = 0 ; channel < num_bis ; channel++){ 251 // pick decoder 252 void * decoder_context = NULL; 253 #ifdef HAVE_LC3PLUS 254 if (use_lc3plus_decoder){ 255 decoder_context = &fraunhofer_decoder_contexts[channel]; 256 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); 257 } 258 else 259 #endif 260 { 261 decoder_context = &google_decoder_contexts[channel]; 262 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); 263 } 264 decoder_contexts[channel] = decoder_context; 265 lc3_decoder->configure(decoder_context, sampling_frequency_hz, frame_duration, octets_per_frame); 266 } 267 number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); 268 btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 269 } 270 271 static void close_files(void){ 272 #ifdef HAVE_POSIX_FILE_IO 273 printf("Close files\n"); 274 wav_writer_close(); 275 #endif 276 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 277 if (sink != NULL){ 278 sink->stop_stream(); 279 sink->close(); 280 } 281 } 282 283 static void handle_periodic_advertisement(const uint8_t * packet, uint16_t size){ 284 // nRF534_audio quirk - no BASE in periodic advertisement 285 if (nrf5340_audio_demo){ 286 // hard coded config LC3 287 // default: mono bitrate 96000, 10 ms with USB audio source, 120 octets per frame 288 count_mode = 0; 289 pts_mode = 0; 290 num_bis = 1; 291 sampling_frequency_hz = 48000; 292 frame_duration = BTSTACK_LC3_FRAME_DURATION_10000US; 293 octets_per_frame = 120; 294 have_base = true; 295 return; 296 } 297 298 // periodic advertisement contains the BASE 299 // TODO: BASE might be split across multiple advertisements 300 const uint8_t * adv_data = hci_subevent_le_periodic_advertising_report_get_data(packet); 301 uint16_t adv_size = hci_subevent_le_periodic_advertising_report_get_data_length(packet); 302 uint8_t adv_status = hci_subevent_le_periodic_advertising_report_get_data_status(packet); 303 304 if (adv_status != 0) { 305 printf("Periodic Advertisement (status %u): ", adv_status); 306 printf_hexdump(adv_data, adv_size); 307 return; 308 } 309 310 ad_context_t context; 311 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 312 uint8_t data_type = ad_iterator_get_data_type(&context); 313 // TODO: avoid out-of-bounds read 314 // uint8_t data_size = ad_iterator_get_data_len(&context); 315 const uint8_t * data = ad_iterator_get_data(&context); 316 uint16_t uuid; 317 switch (data_type){ 318 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 319 uuid = little_endian_read_16(data, 0); 320 if (uuid == ORG_BLUETOOTH_SERVICE_BASIC_AUDIO_ANNOUNCEMENT_SERVICE){ 321 have_base = true; 322 // Level 1: Group Level 323 const uint8_t * base_data = &data[2]; 324 // TODO: avoid out-of-bounds read 325 // uint16_t base_len = data_size - 2; 326 printf("BASE:\n"); 327 uint32_t presentation_delay = little_endian_read_24(base_data, 0); 328 printf("- presentation delay: %"PRIu32" us\n", presentation_delay); 329 uint8_t num_subgroups = base_data[3]; 330 printf("- num subgroups: %u\n", num_subgroups); 331 uint8_t i; 332 uint16_t offset = 4; 333 for (i=0;i<num_subgroups;i++){ 334 // Level 2: Subgroup Level 335 num_bis = base_data[offset++]; 336 printf(" - num bis[%u]: %u\n", i, num_bis); 337 // codec_id: coding format = 0x06, vendor and coded id = 0 338 offset += 5; 339 uint8_t codec_specific_configuration_length = base_data[offset++]; 340 const uint8_t * codec_specific_configuration = &base_data[offset]; 341 printf(" - codec specific config[%u]: ", i); 342 printf_hexdump(codec_specific_configuration, codec_specific_configuration_length); 343 // parse config to get sampling frequency and frame duration 344 uint8_t codec_offset = 0; 345 while ((codec_offset + 1) < codec_specific_configuration_length){ 346 uint8_t ltv_len = codec_specific_configuration[codec_offset++]; 347 uint8_t ltv_type = codec_specific_configuration[codec_offset]; 348 const uint32_t sampling_frequency_map[] = { 8000, 11025, 16000, 22050, 24000, 32000, 44100, 48000, 88200, 96000, 176400, 192000, 384000 }; 349 uint8_t sampling_frequency_index; 350 uint8_t frame_duration_index; 351 switch (ltv_type){ 352 case 0x01: // sampling frequency 353 sampling_frequency_index = codec_specific_configuration[codec_offset+1]; 354 // TODO: check range 355 sampling_frequency_hz = sampling_frequency_map[sampling_frequency_index - 1]; 356 printf(" - sampling frequency[%u]: %u\n", i, sampling_frequency_hz); 357 break; 358 case 0x02: // 0 = 7.5, 1 = 10 ms 359 frame_duration_index = codec_specific_configuration[codec_offset+1]; 360 frame_duration = (frame_duration_index == 0) ? BTSTACK_LC3_FRAME_DURATION_7500US : BTSTACK_LC3_FRAME_DURATION_10000US; 361 printf(" - frame duration[%u]: %s ms\n", i, (frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US) ? "7.5" : "10"); 362 break; 363 case 0x04: // octets per coding frame 364 octets_per_frame = little_endian_read_16(codec_specific_configuration, codec_offset+1); 365 printf(" - octets per codec frame[%u]: %u\n", i, octets_per_frame); 366 break; 367 default: 368 break; 369 } 370 codec_offset += ltv_len; 371 } 372 // 373 offset += codec_specific_configuration_length; 374 uint8_t metadata_length = base_data[offset++]; 375 const uint8_t * meta_data = &base_data[offset]; 376 offset += metadata_length; 377 printf(" - meta data[%u]: ", i); 378 printf_hexdump(meta_data, metadata_length); 379 uint8_t k; 380 for (k=0;k<num_bis;k++){ 381 // Level 3: BIS Level 382 uint8_t bis_index = base_data[offset++]; 383 printf(" - bis index[%u][%u]: %u\n", i, k, bis_index); 384 uint8_t codec_specific_configuration_length2 = base_data[offset++]; 385 const uint8_t * codec_specific_configuration2 = &base_data[offset]; 386 printf(" - codec specific config[%u][%u]: ", i, k); 387 printf_hexdump(codec_specific_configuration2, codec_specific_configuration_length2); 388 offset += codec_specific_configuration_length2; 389 } 390 } 391 } 392 break; 393 default: 394 break; 395 } 396 } 397 } 398 399 static void handle_big_info(const uint8_t * packet, uint16_t size){ 400 printf("BIG Info advertising report\n"); 401 sync_handle = hci_subevent_le_biginfo_advertising_report_get_sync_handle(packet); 402 encryption = hci_subevent_le_biginfo_advertising_report_get_encryption(packet); 403 if (encryption) { 404 printf("Stream is encrypted\n"); 405 } 406 have_big_info = true; 407 } 408 409 static void enter_create_big_sync(void){ 410 // stop scanning 411 gap_stop_scan(); 412 413 // switch to lc3plus if requested and possible 414 use_lc3plus_decoder = request_lc3plus_decoder && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); 415 416 // init decoder 417 setup_lc3_decoder(); 418 419 printf("Configure: %u channels, sampling rate %u, samples per frame %u, lc3plus %u\n", num_bis, sampling_frequency_hz, number_samples_per_frame, use_lc3plus_decoder); 420 421 #ifdef HAVE_POSIX_FILE_IO 422 // create wav file 423 printf("WAV file: %s\n", filename_wav); 424 wav_writer_open(filename_wav, num_bis, sampling_frequency_hz); 425 #endif 426 427 // init playback buffer 428 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 429 430 // start playback 431 // PTS 8.2 sends stereo at half speed for stereo, for now playback at half speed 432 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 433 if (sink != NULL){ 434 uint16_t playback_speed; 435 if ((num_bis > 1) && pts_mode){ 436 playback_speed = sampling_frequency_hz / num_bis; 437 printf("PTS workaround: playback at %u hz\n", playback_speed); 438 } else { 439 playback_speed = sampling_frequency_hz; 440 }; 441 sink->init(num_bis, sampling_frequency_hz, le_audio_broadcast_sink_playback); 442 sink->start_stream(); 443 } 444 445 big_sync_params.big_handle = big_handle; 446 big_sync_params.sync_handle = sync_handle; 447 big_sync_params.encryption = encryption; 448 if (encryption) { 449 memcpy(big_sync_params.broadcast_code, &broadcast_code[0], 16); 450 } else { 451 memset(big_sync_params.broadcast_code, 0, 16); 452 } 453 big_sync_params.mse = 0; 454 big_sync_params.big_sync_timeout_10ms = 100; 455 big_sync_params.num_bis = num_bis; 456 uint8_t i; 457 printf("BIG Create Sync for BIS: "); 458 for (i=0;i<num_bis;i++){ 459 big_sync_params.bis_indices[i] = i + 1; 460 printf("%u ", big_sync_params.bis_indices[i]); 461 } 462 printf("\n"); 463 app_state = APP_W4_BIG_SYNC_ESTABLISHED; 464 gap_big_sync_create(&big_sync_storage, &big_sync_params); 465 } 466 467 static void start_scanning() { 468 app_state = APP_W4_BROADCAST_ADV; 469 have_base = false; 470 have_big_info = false; 471 gap_set_scan_params(1, 0x30, 0x30, 0); 472 gap_start_scan(); 473 printf("Start scan..\n"); 474 } 475 476 static void setup_advertising() { 477 gap_extended_advertising_setup(&le_advertising_set, &extended_params, &adv_handle); 478 gap_extended_advertising_set_adv_data(adv_handle, sizeof(extended_adv_data), extended_adv_data); 479 gap_extended_advertising_start(adv_handle, 0, 0); 480 } 481 482 static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 483 UNUSED(channel); 484 if (packet_type != HCI_EVENT_PACKET) return; 485 switch (packet[0]) { 486 case BTSTACK_EVENT_STATE: 487 switch(btstack_event_state_get_state(packet)) { 488 case HCI_STATE_WORKING: 489 app_state = APP_IDLE; 490 #ifdef ENABLE_DEMO_MODE 491 start_scanning(); 492 #else 493 show_usage(); 494 #endif 495 break; 496 case HCI_STATE_OFF: 497 printf("Goodbye\n"); 498 exit(0); 499 break; 500 default: 501 break; 502 } 503 break; 504 case GAP_EVENT_EXTENDED_ADVERTISING_REPORT: 505 { 506 if (app_state != APP_W4_BROADCAST_ADV) break; 507 508 gap_event_extended_advertising_report_get_address(packet, remote); 509 uint8_t adv_size = gap_event_extended_advertising_report_get_data_length(packet); 510 const uint8_t * adv_data = gap_event_extended_advertising_report_get_data(packet); 511 512 ad_context_t context; 513 bool found = false; 514 remote_name[0] = '\0'; 515 uint16_t uuid; 516 uint32_t broadcast_id; 517 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 518 uint8_t data_type = ad_iterator_get_data_type(&context); 519 uint8_t size = ad_iterator_get_data_len(&context); 520 const uint8_t *data = ad_iterator_get_data(&context); 521 switch (data_type){ 522 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 523 uuid = little_endian_read_16(data, 0); 524 if (uuid == ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_ANNOUNCEMENT_SERVICE){ 525 broadcast_id = little_endian_read_24(data, 2); 526 found = true; 527 } 528 break; 529 case BLUETOOTH_DATA_TYPE_SHORTENED_LOCAL_NAME: 530 case BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME: 531 size = btstack_min(sizeof(remote_name) - 1, size); 532 memcpy(remote_name, data, size); 533 remote_name[size] = 0; 534 // support for nRF5340 Audio DK 535 if (strncmp("NRF5340", remote_name, 7) == 0){ 536 nrf5340_audio_demo = true; 537 found = true; 538 } 539 break; 540 default: 541 break; 542 } 543 } 544 if (!found) break; 545 remote_type = gap_event_extended_advertising_report_get_address_type(packet); 546 remote_sid = gap_event_extended_advertising_report_get_advertising_sid(packet); 547 pts_mode = strncmp("PTS-", remote_name, 4) == 0; 548 count_mode = strncmp("COUNT", remote_name, 5) == 0; 549 printf("Broadcast sink found, addr %s, name: '%s' (pts-mode: %u, count: %u), Broadcast_ID 0%06x\n", bd_addr_to_str(remote), remote_name, pts_mode, count_mode, broadcast_id); 550 // ignore other advertisements 551 gap_whitelist_add(remote_type, remote); 552 gap_set_scan_params(1, 0x30, 0x30, 1); 553 // sync to PA 554 gap_periodic_advertiser_list_clear(); 555 gap_periodic_advertiser_list_add(remote_type, remote, remote_sid); 556 app_state = APP_W4_PA_AND_BIG_INFO; 557 printf("Start Periodic Advertising Sync\n"); 558 gap_periodic_advertising_create_sync(0x01, remote_sid, remote_type, remote, 0, 1000, 0); 559 break; 560 } 561 562 case HCI_EVENT_LE_META: 563 switch(hci_event_le_meta_get_subevent_code(packet)) { 564 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_SYNC_TRANSFER_RECEIVED: 565 printf("Periodic advertising sync transfer received\n"); 566 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 567 app_state = APP_W4_PA_AND_BIG_INFO; 568 break; 569 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_SYNC_ESTABLISHMENT: 570 sync_handle = hci_subevent_le_periodic_advertising_sync_establishment_get_sync_handle(packet); 571 printf("Periodic advertising sync with handle 0x%04x established\n", sync_handle); 572 break; 573 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_REPORT: 574 if (app_state != APP_W4_PA_AND_BIG_INFO) break; 575 if (have_base) break; 576 handle_periodic_advertisement(packet, size); 577 if (have_base & have_big_info){ 578 enter_create_big_sync(); 579 } 580 break; 581 case HCI_SUBEVENT_LE_BIGINFO_ADVERTISING_REPORT: 582 if (app_state != APP_W4_PA_AND_BIG_INFO) break; 583 if (have_big_info) break; 584 handle_big_info(packet, size); 585 if (have_base & have_big_info){ 586 enter_create_big_sync(); 587 } 588 break; 589 case HCI_SUBEVENT_LE_BIG_SYNC_LOST: 590 printf("BIG Sync Lost\n"); 591 { 592 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 593 if (sink != NULL) { 594 sink->stop_stream(); 595 sink->close(); 596 } 597 } 598 // start over 599 if (!standalone_mode) break; 600 start_scanning(); 601 break; 602 default: 603 break; 604 } 605 break; 606 case HCI_EVENT_META_GAP: 607 switch (hci_event_gap_meta_get_subevent_code(packet)){ 608 case GAP_SUBEVENT_BIG_SYNC_CREATED: { 609 printf("BIG Sync created with BIS Connection handles: "); 610 uint8_t i; 611 for (i=0;i<num_bis;i++){ 612 bis_con_handles[i] = gap_subevent_big_sync_created_get_bis_con_handles(packet, i); 613 printf("0x%04x ", bis_con_handles[i]); 614 } 615 app_state = APP_STREAMING; 616 last_packet_received_big = false; 617 last_samples_report_ms = btstack_run_loop_get_time_ms(); 618 memset(last_packet_sequence, 0, sizeof(last_packet_sequence)); 619 memset(last_packet_received, 0, sizeof(last_packet_received)); 620 memset(pcm, 0, sizeof(pcm)); 621 printf("Start receiving\n"); 622 623 // update BIS Sync state 624 bass_sources[0].data.subgroups[0].bis_sync_state = 1; 625 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 626 break; 627 } 628 case GAP_SUBEVENT_BIG_SYNC_STOPPED: 629 printf("BIG Sync stopped, big_handle 0x%02x\n", gap_subevent_big_sync_stopped_get_big_handle(packet)); 630 break; 631 default: 632 break; 633 } 634 break; 635 case SM_EVENT_JUST_WORKS_REQUEST: 636 printf("Just Works requested\n"); 637 sm_just_works_confirm(sm_event_just_works_request_get_handle(packet)); 638 break; 639 default: 640 break; 641 } 642 } 643 644 static void store_samples_in_ringbuffer(void){ 645 // check if we have all channels 646 uint8_t bis_channel; 647 for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ 648 if (have_pcm[bis_channel] == false) return; 649 } 650 #ifdef HAVE_POSIX_FILE_IO 651 // write wav samples 652 wav_writer_write_int16(num_bis * number_samples_per_frame, pcm); 653 #endif 654 // store samples in playback buffer 655 uint32_t bytes_to_store = num_bis * number_samples_per_frame * 2; 656 samples_received += number_samples_per_frame; 657 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 658 btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm, bytes_to_store); 659 } else { 660 printf("Samples dropped\n"); 661 samples_dropped += number_samples_per_frame; 662 } 663 // reset 664 for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ 665 have_pcm[bis_channel] = false; 666 } 667 } 668 669 static void plc_do(uint8_t bis_channel) {// inject packet 670 uint8_t tmp_BEC_detect; 671 uint8_t BFI = 1; 672 (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], NULL, BFI, 673 &pcm[bis_channel], num_bis, 674 &tmp_BEC_detect); 675 676 printf("PLC channel %u - packet sequence %u\n", bis_channel, last_packet_sequence[bis_channel]); 677 678 have_pcm[bis_channel] = true; 679 store_samples_in_ringbuffer(); 680 } 681 682 static void plc_missing(uint16_t packet_sequence_number) { 683 while (btstack_time16_delta(packet_sequence_number, last_packet_sequence_big) > 1){ 684 uint8_t i; 685 for (i=0;i<num_bis;i++){ 686 // deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start 687 if (last_packet_received[i] == false){ 688 printf("PLC Missing: very first packet BIS %u missing\n", i); 689 have_pcm[i] = true; 690 store_samples_in_ringbuffer(); 691 692 last_packet_received[i] = true; 693 last_packet_sequence[i] = last_packet_sequence_big; 694 continue; 695 } 696 697 // missing packet if big sequence counter is higher than bis sequence counter 698 if (btstack_time16_delta(last_packet_sequence_big, last_packet_sequence[i]) > 0) { 699 plc_do(i); 700 last_packet_sequence[i] = last_packet_sequence_big; 701 } 702 } 703 last_packet_sequence_big++; 704 } 705 } 706 707 static void plc_timeout(btstack_timer_source_t * timer) { 708 if (app_state != APP_STREAMING) return; 709 710 // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets 711 uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 712 btstack_run_loop_set_timer(timer, frame_duration_ms); 713 btstack_run_loop_set_timer_handler(timer, plc_timeout); 714 btstack_run_loop_add_timer(timer); 715 716 // assume no packet received in iso interval 717 plc_missing(last_packet_sequence_big + 1); 718 } 719 720 static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 721 722 if (app_state != APP_STREAMING) return; 723 724 uint16_t header = little_endian_read_16(packet, 0); 725 hci_con_handle_t con_handle = header & 0x0fff; 726 uint8_t pb_flag = (header >> 12) & 3; 727 uint8_t ts_flag = (header >> 14) & 1; 728 uint16_t iso_load_len = little_endian_read_16(packet, 2); 729 730 uint16_t offset = 4; 731 uint32_t time_stamp = 0; 732 if (ts_flag){ 733 uint32_t time_stamp = little_endian_read_32(packet, offset); 734 offset += 4; 735 } 736 737 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 738 739 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 740 offset += 2; 741 742 uint16_t header_2 = little_endian_read_16(packet, offset); 743 uint16_t iso_sdu_length = header_2 & 0x3fff; 744 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 745 offset += 2; 746 747 if (iso_sdu_length == 0) return; 748 749 // infer channel from con handle - only works for up to 2 channels 750 uint8_t bis_channel = (con_handle == bis_con_handles[0]) ? 0 : 1; 751 752 if (count_mode){ 753 // check for missing packet 754 uint16_t last_seq_no = last_packet_sequence[bis_channel]; 755 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); 756 if (packet_missed){ 757 // print last packet 758 printf("\n"); 759 printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[bis_channel], bis_channel); 760 printf_hexdump(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); 761 last_seq_no++; 762 763 printf(ANSI_COLOR_RED); 764 while (last_seq_no < packet_sequence_number){ 765 printf("%04x %u MISSING\n", last_seq_no, bis_channel); 766 last_seq_no++; 767 } 768 printf(ANSI_COLOR_RESET); 769 770 // print current packet 771 printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, bis_channel); 772 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); 773 } 774 775 // cache current packet 776 memcpy(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); 777 778 } else { 779 780 if (last_packet_received[bis_channel]) { 781 int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, 782 last_packet_sequence[bis_channel]); 783 if (packet_sequence_delta < 1) { 784 // drop delayed packet that had already been generated by PLC 785 printf("Dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n", 786 packet_sequence_number, last_packet_sequence[bis_channel]); 787 return; 788 } 789 } else { 790 if (!last_packet_received_big) { 791 // track sequence number of very first received packet 792 last_packet_received_big = true; 793 last_packet_sequence_big = packet_sequence_number; 794 } 795 printf("BIS %u, first packet seq number %u\n", bis_channel, packet_sequence_number); 796 last_packet_received[bis_channel] = true; 797 } 798 799 // assert no channel is more than one packet behind 800 plc_missing(packet_sequence_number); 801 802 // decode codec frame 803 uint8_t tmp_BEC_detect; 804 uint8_t BFI = 0; 805 (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], &packet[offset], BFI, 806 &pcm[bis_channel], num_bis, 807 &tmp_BEC_detect); 808 have_pcm[bis_channel] = true; 809 store_samples_in_ringbuffer(); 810 811 lc3_frames++; 812 frames_per_second[bis_channel]++; 813 814 // PLC 815 cached_iso_sdu_len = iso_sdu_length; 816 uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 817 uint32_t timeout_ms = frame_duration_ms * 5 / 2; 818 btstack_run_loop_remove_timer(&next_packet_timer); 819 btstack_run_loop_set_timer(&next_packet_timer, timeout_ms); 820 btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); 821 btstack_run_loop_add_timer(&next_packet_timer); 822 823 uint32_t time_ms = btstack_run_loop_get_time_ms(); 824 if (btstack_time_delta(time_ms, last_samples_report_ms) >= 1000){ 825 last_samples_report_ms = time_ms; 826 printf("LC3 Frames: %4u - ", (int) (lc3_frames / num_bis)); 827 uint8_t i; 828 for (i=0;i<num_bis;i++){ 829 printf("%u ", frames_per_second[i]); 830 frames_per_second[i] = 0; 831 } 832 printf(" frames per second, dropped %u of %u\n", samples_dropped, samples_received); 833 samples_received = 0; 834 samples_dropped = 0; 835 } 836 } 837 838 last_packet_time_ms[bis_channel] = receive_time_ms; 839 last_packet_sequence[bis_channel] = packet_sequence_number; 840 } 841 842 static void show_usage(void){ 843 printf("\n--- LE Audio Broadcast Sink Test Console ---\n"); 844 printf("s - start scanning\n"); 845 #ifdef HAVE_LC3PLUS 846 printf("q - use LC3plus decoder if 10 ms ISO interval is used\n"); 847 #endif 848 printf("t - terminate BIS streams\n"); 849 printf("x - close files and exit\n"); 850 printf("---\n"); 851 } 852 853 static void stdin_process(char c){ 854 switch (c){ 855 case 's': 856 if (app_state != APP_IDLE) break; 857 standalone_mode = true; 858 start_scanning(); 859 break; 860 #ifdef HAVE_LC3PLUS 861 case 'q': 862 printf("Use LC3plus decoder for 10 ms ISO interval...\n"); 863 request_lc3plus_decoder = true; 864 break; 865 #endif 866 case 't': 867 switch (app_state){ 868 case APP_STREAMING: 869 case APP_W4_BIG_SYNC_ESTABLISHED: 870 app_state = APP_IDLE; 871 close_files(); 872 printf("Terminate BIG SYNC\n"); 873 gap_big_sync_terminate(big_handle); 874 break; 875 default: 876 break; 877 } 878 break; 879 case 'x': 880 close_files(); 881 printf("Shutdown...\n"); 882 hci_power_control(HCI_POWER_OFF); 883 break; 884 case '\n': 885 case '\r': 886 break; 887 default: 888 show_usage(); 889 break; 890 891 } 892 } 893 static void bass_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 894 UNUSED(packet_type); 895 UNUSED(channel); 896 btstack_assert (packet_type == HCI_EVENT_PACKET); 897 btstack_assert(hci_event_packet_get_type(packet) == HCI_EVENT_GATTSERVICE_META); 898 printf("BASS Event 0x%02x: ", hci_event_gattservice_meta_get_subevent_code(packet)); 899 printf_hexdump(packet, size); 900 switch (hci_event_gattservice_meta_get_subevent_code(packet)){ 901 case GATTSERVICE_SUBEVENT_BASS_SOURCE_ADDED: 902 printf("GATTSERVICE_SUBEVENT_BASS_SOURCE_ADDED, source_id 0x%04x, pa_sync %u\n", 903 gattservice_subevent_bass_source_added_get_source_id(packet), 904 gattservice_subevent_bass_source_added_get_pa_sync(packet)); 905 printf("Request SyncInfo\n"); 906 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCINFO_REQUEST); 907 break; 908 case GATTSERVICE_SUBEVENT_BASS_SOURCE_MODIFIED: 909 printf("GATTSERVICE_SUBEVENT_BASS_SOURCE_MODIFIED, source_id 0x%04x, pa_sync %u\n", 910 gattservice_subevent_bass_source_added_get_source_id(packet), 911 gattservice_subevent_bass_source_added_get_pa_sync(packet)); 912 // handle 'bis sync == 0' 913 printf("PA Sync %u, bis_sync[0] = %u\n", bass_sources[0].data.pa_sync, bass_sources[0].data.subgroups[0].bis_sync); 914 if (bass_sources[0].data.subgroups[0].bis_sync == 0){ 915 printf("Simulate BIS Sync has stopped\n"); 916 bass_sources[0].data.subgroups[0].bis_sync_state = 0; 917 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 918 } 919 default: 920 break; 921 } 922 } 923 924 int btstack_main(int argc, const char * argv[]); 925 int btstack_main(int argc, const char * argv[]){ 926 (void) argv; 927 (void) argc; 928 929 l2cap_init(); 930 sm_init(); 931 932 // setup ATT server 933 att_server_init(profile_data, NULL, NULL); 934 935 // register for HCI events 936 hci_event_callback_registration.callback = &packet_handler; 937 hci_add_event_handler(&hci_event_callback_registration); 938 939 // register for ISO Packet 940 hci_register_iso_packet_handler(&iso_packet_handler); 941 942 // register for SM events 943 sm_event_callback_registration.callback = &packet_handler; 944 sm_add_event_handler(&sm_event_callback_registration); 945 946 // setup BASS Server 947 broadcast_audio_scan_service_server_init(BASS_NUM_SOURCES, bass_sources, BASS_NUM_CLIENTS, bass_clients); 948 broadcast_audio_scan_service_server_register_packet_handler(&bass_packet_handler); 949 950 // setup advertising and allow to receive periodic advertising sync trasnfers 951 setup_advertising(); 952 gap_periodic_advertising_sync_transfer_set_default_parameters(2, 0, 0x2000, 0); 953 954 // turn on! 955 hci_power_control(HCI_POWER_ON); 956 957 btstack_stdin_setup(stdin_process); 958 return 0; 959 } 960