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/att_server.h" 55 #include "ble/sm.h" 56 #include "bluetooth_data_types.h" 57 #include "bluetooth_gatt.h" 58 #include "btstack_debug.h" 59 #include "btstack_audio.h" 60 #include "btstack_event.h" 61 #include "btstack_run_loop.h" 62 #include "btstack_ring_buffer.h" 63 #include "btstack_stdin.h" 64 #include "btstack_util.h" 65 #include "gap.h" 66 #include "hci.h" 67 #include "hci_cmd.h" 68 #include "btstack_lc3.h" 69 #include "btstack_lc3_google.h" 70 #include "btstack_lc3plus_fraunhofer.h" 71 #include "l2cap.h" 72 #include "le-audio/le_audio_base_parser.h" 73 #include "le-audio/gatt-service/broadcast_audio_scan_service_server.h" 74 75 #include "le_audio_broadcast_sink.h" 76 77 #ifdef HAVE_POSIX_FILE_IO 78 #include "wav_util.h" 79 #endif 80 81 // #define DEBUG_PLC 82 #ifdef DEBUG_PLC 83 #define printf_plc(...) printf(__VA_ARGS__) 84 #else 85 #define printf_plc(...) (void)(0); 86 #endif 87 88 // max config 89 #define MAX_NUM_BIS 2 90 #define MAX_SAMPLES_PER_FRAME 480 91 92 // playback 93 #define MAX_NUM_LC3_FRAMES 5 94 #define MAX_BYTES_PER_SAMPLE 4 95 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_BYTES_PER_SAMPLE) 96 97 // analysis 98 #define PACKET_PREFIX_LEN 10 99 100 #define ANSI_COLOR_RED "\x1b[31m" 101 #define ANSI_COLOR_GREEN "\x1b[32m" 102 #define ANSI_COLOR_YELLOW "\x1b[33m" 103 #define ANSI_COLOR_BLUE "\x1b[34m" 104 #define ANSI_COLOR_MAGENTA "\x1b[35m" 105 #define ANSI_COLOR_CYAN "\x1b[36m" 106 #define ANSI_COLOR_RESET "\x1b[0m" 107 108 static void show_usage(void); 109 110 static const char * filename_wav = "le_audio_broadcast_sink.wav"; 111 112 static enum { 113 APP_W4_WORKING, 114 APP_W4_BROADCAST_ADV, 115 APP_W4_PA_AND_BIG_INFO, 116 APP_W4_BROADCAST_CODE, 117 APP_W4_BIG_SYNC_ESTABLISHED, 118 APP_STREAMING, 119 APP_IDLE 120 } app_state = APP_W4_WORKING; 121 122 static const uint8_t adv_sid = 0; 123 static le_advertising_set_t le_advertising_set; 124 static uint8_t adv_handle = 0; 125 126 static le_extended_advertising_parameters_t extended_params = { 127 .advertising_event_properties = 1, // connectable 128 .primary_advertising_interval_min = 0x4b0, // 750 ms 129 .primary_advertising_interval_max = 0x4b0, // 750 ms 130 .primary_advertising_channel_map = 7, 131 .own_address_type = BD_ADDR_TYPE_LE_PUBLIC, 132 .peer_address_type = 0, 133 .peer_address = { 0 }, 134 .advertising_filter_policy = 0, 135 .advertising_tx_power = 10, // 10 dBm 136 .primary_advertising_phy = 1, // LE 1M PHY 137 .secondary_advertising_max_skip = 0, 138 .secondary_advertising_phy = 1, // LE 1M PHY 139 .advertising_sid = adv_sid, 140 .scan_request_notification_enable = 0, 141 }; 142 143 static const uint8_t extended_adv_data[] = { 144 // 16 bit service data, ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_SCAN_SERVICE, 145 3, BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID, 146 ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_SCAN_SERVICE & 0xff, ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_SCAN_SERVICE >> 8, 147 // name 148 5, BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME, 'S', 'i', 'n', 'k' 149 }; 150 151 #define BASS_NUM_CLIENTS 1 152 #define BASS_NUM_SOURCES 1 153 static bass_source_data_t bass_source_new; 154 static bass_server_source_t bass_sources[BASS_NUM_SOURCES]; 155 static bass_server_connection_t bass_clients[BASS_NUM_CLIENTS]; 156 157 // 158 static btstack_packet_callback_registration_t hci_event_callback_registration; 159 static btstack_packet_callback_registration_t sm_event_callback_registration; 160 161 static bool have_base; 162 static bool have_big_info; 163 static bool have_past; 164 static bool have_broadcast_code; 165 static bool standalone_mode; 166 static uint32_t bis_sync_mask; 167 168 uint32_t last_samples_report_ms; 169 uint16_t samples_received; 170 uint16_t samples_dropped; 171 uint16_t frames_per_second[MAX_NUM_BIS]; 172 173 // remote info 174 static char remote_name[20]; 175 static bd_addr_t remote; 176 static bd_addr_type_t remote_type; 177 static uint8_t remote_sid; 178 static bool count_mode; 179 static bool pts_mode; 180 static bool nrf5340_audio_demo; 181 182 183 // broadcast info 184 static const uint8_t big_handle = 1; 185 static hci_con_handle_t sync_handle; 186 static hci_con_handle_t bis_con_handles[MAX_NUM_BIS]; 187 static unsigned int next_bis_index; 188 static uint8_t encryption; 189 static uint8_t broadcast_code[16]; 190 191 static btstack_timer_source_t broadcast_sink_pa_sync_timer; 192 193 // analysis 194 static bool last_packet_received_big; 195 static uint16_t last_packet_sequence_big; 196 static bool last_packet_received[MAX_NUM_BIS]; 197 static uint16_t last_packet_sequence[MAX_NUM_BIS]; 198 static uint32_t last_packet_time_ms[MAX_NUM_BIS]; 199 static uint8_t last_packet_prefix[MAX_NUM_BIS * PACKET_PREFIX_LEN]; 200 201 // BIG Sync 202 static le_audio_big_sync_t big_sync_storage; 203 static le_audio_big_sync_params_t big_sync_params; 204 205 // lc3 writer 206 static uint32_t lc3_frames; 207 208 // lc3 codec config 209 static uint16_t sampling_frequency_hz; 210 static btstack_lc3_frame_duration_t frame_duration; 211 static uint16_t number_samples_per_frame; 212 static uint16_t octets_per_frame; 213 static uint8_t num_bis; 214 215 // lc3 decoder 216 static bool request_lc3plus_decoder = false; 217 static bool use_lc3plus_decoder = false; 218 static const btstack_lc3_decoder_t * lc3_decoder; 219 static int16_t pcm[MAX_NUM_BIS * MAX_SAMPLES_PER_FRAME]; 220 221 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_NUM_BIS]; 222 #ifdef HAVE_LC3PLUS 223 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_NUM_BIS]; 224 #endif 225 static void * decoder_contexts[MAX_NR_BIS]; 226 227 // playback 228 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; 229 static btstack_ring_buffer_t playback_buffer; 230 231 static btstack_timer_source_t next_packet_timer; 232 static uint16_t cached_iso_sdu_len; 233 static bool have_pcm[MAX_NUM_BIS]; 234 235 static void le_audio_broadcast_sink_playback(int16_t * buffer, uint16_t num_samples){ 236 // called from lower-layer but guaranteed to be on main thread 237 uint32_t bytes_needed = num_samples * num_bis * 2; 238 239 static bool underrun = true; 240 241 log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / ( num_bis * 2)); 242 243 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)){ 244 memset(buffer, 0, bytes_needed); 245 if (underrun == false){ 246 log_info("Playback underrun"); 247 underrun = true; 248 } 249 return; 250 } 251 252 if (underrun){ 253 underrun = false; 254 log_info("Playback started"); 255 } 256 uint32_t bytes_read; 257 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 258 btstack_assert(bytes_read == bytes_needed); 259 } 260 261 static void setup_lc3_decoder(void){ 262 uint8_t channel; 263 for (channel = 0 ; channel < num_bis ; channel++){ 264 // pick decoder 265 void * decoder_context = NULL; 266 #ifdef HAVE_LC3PLUS 267 if (use_lc3plus_decoder){ 268 decoder_context = &fraunhofer_decoder_contexts[channel]; 269 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); 270 } 271 else 272 #endif 273 { 274 decoder_context = &google_decoder_contexts[channel]; 275 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); 276 } 277 decoder_contexts[channel] = decoder_context; 278 lc3_decoder->configure(decoder_context, sampling_frequency_hz, frame_duration, octets_per_frame); 279 } 280 number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); 281 btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 282 } 283 284 static void close_files(void){ 285 #ifdef HAVE_POSIX_FILE_IO 286 printf("Close files\n"); 287 wav_writer_close(); 288 #endif 289 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 290 if (sink != NULL){ 291 sink->stop_stream(); 292 sink->close(); 293 } 294 } 295 296 static void handle_periodic_advertisement(const uint8_t * packet, uint16_t size){ 297 // nRF534_audio quirk - no BASE in periodic advertisement 298 if (nrf5340_audio_demo){ 299 // hard coded config LC3 300 // default: mono bitrate 96000, 10 ms with USB audio source, 120 octets per frame 301 count_mode = 0; 302 pts_mode = 0; 303 num_bis = 1; 304 sampling_frequency_hz = 48000; 305 frame_duration = BTSTACK_LC3_FRAME_DURATION_10000US; 306 octets_per_frame = 120; 307 have_base = true; 308 return; 309 } 310 311 // periodic advertisement contains the BASE 312 // TODO: BASE might be split across multiple advertisements 313 const uint8_t * adv_data = hci_subevent_le_periodic_advertising_report_get_data(packet); 314 uint16_t adv_size = hci_subevent_le_periodic_advertising_report_get_data_length(packet); 315 uint8_t adv_status = hci_subevent_le_periodic_advertising_report_get_data_status(packet); 316 317 if (adv_status != 0) { 318 printf("Periodic Advertisement (status %u): ", adv_status); 319 printf_hexdump(adv_data, adv_size); 320 return; 321 } 322 323 le_audio_base_parser_t parser; 324 bool ok = le_audio_base_parser_init(&parser, adv_data, adv_size); 325 if (ok == false){ 326 return; 327 } 328 have_base = true; 329 330 printf("BASE:\n"); 331 uint32_t presentation_delay = le_audio_base_parser_get_presentation_delay(&parser); 332 printf("- presentation delay: %"PRIu32" us\n", presentation_delay); 333 uint8_t num_subgroups = le_audio_base_parser_get_num_subgroups(&parser); 334 // Cache in new source struct 335 bass_source_new.subgroups_num = num_subgroups; 336 printf("- num subgroups: %u\n", num_subgroups); 337 uint8_t i; 338 for (i=0;i<num_subgroups;i++){ 339 // Level 2: Subgroup Level 340 printf(" - Subgroup %u\n", i); 341 num_bis = le_audio_base_parser_subgroup_get_num_bis(&parser); 342 printf(" - num bis[%u]: %u\n", i, num_bis); 343 uint8_t codec_specific_configuration_length = le_audio_base_parser_subgroup_get_codec_specific_configuration_length(&parser); 344 const uint8_t * codec_specific_configuration = le_audio_base_parser_subgroup_get_codec_specific_configuration(&parser); 345 printf(" - codec specific config[%u]: ", i); 346 printf_hexdump(codec_specific_configuration, codec_specific_configuration_length); 347 // parse config to get sampling frequency and frame duration 348 uint8_t codec_offset = 0; 349 while ((codec_offset + 1) < codec_specific_configuration_length){ 350 uint8_t ltv_len = codec_specific_configuration[codec_offset++]; 351 uint8_t ltv_type = codec_specific_configuration[codec_offset]; 352 const uint32_t sampling_frequency_map[] = { 8000, 11025, 16000, 22050, 24000, 32000, 44100, 48000, 88200, 96000, 176400, 192000, 384000 }; 353 uint8_t sampling_frequency_index; 354 uint8_t frame_duration_index; 355 switch (ltv_type){ 356 case 0x01: // sampling frequency 357 sampling_frequency_index = codec_specific_configuration[codec_offset+1]; 358 // TODO: check range 359 sampling_frequency_hz = sampling_frequency_map[sampling_frequency_index - 1]; 360 printf(" - sampling frequency[%u]: %u\n", i, sampling_frequency_hz); 361 break; 362 case 0x02: // 0 = 7.5, 1 = 10 ms 363 frame_duration_index = codec_specific_configuration[codec_offset+1]; 364 frame_duration = (frame_duration_index == 0) ? BTSTACK_LC3_FRAME_DURATION_7500US : BTSTACK_LC3_FRAME_DURATION_10000US; 365 printf(" - frame duration[%u]: %s ms\n", i, (frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US) ? "7.5" : "10"); 366 break; 367 case 0x04: // octets per coding frame 368 octets_per_frame = little_endian_read_16(codec_specific_configuration, codec_offset+1); 369 printf(" - octets per codec frame[%u]: %u\n", i, octets_per_frame); 370 break; 371 default: 372 break; 373 } 374 codec_offset += ltv_len; 375 } 376 uint8_t metadata_length = le_audio_base_parser_subgroup_get_metadata_length(&parser); 377 const uint8_t * meta_data = le_audio_base_subgroup_parser_get_metadata(&parser); 378 printf(" - meta data[%u]: ", i); 379 printf_hexdump(meta_data, metadata_length); 380 uint8_t k; 381 for (k=0;k<num_bis;k++){ 382 printf(" - BIS %u\n", k); 383 // Level 3: BIS Level 384 uint8_t bis_index = le_audio_base_parser_bis_get_index(&parser); 385 if ((bis_index == 0) || (bis_index > 30)){ 386 continue; 387 } 388 389 // collect bis sync mask 390 bis_sync_mask |= 1 << (bis_index - 1); 391 392 bass_source_new.subgroups[i].metadata_length = 0; 393 memset(&bass_source_new.subgroups[i].metadata, 0, sizeof(le_audio_metadata_t)); 394 395 printf(" - bis index[%u][%u]: %u\n", i, k, bis_index); 396 codec_specific_configuration_length = le_audio_base_parser_bis_get_codec_specific_configuration_length(&parser); 397 codec_specific_configuration = le_audio_base_bis_parser_get_codec_specific_configuration(&parser); 398 printf(" - codec specific config[%u][%u]: ", i, k); 399 printf_hexdump(codec_specific_configuration, codec_specific_configuration_length); 400 // parse next BIS 401 le_audio_base_parser_bis_next(&parser); 402 } 403 404 // parse next subgroup 405 le_audio_base_parser_subgroup_next(&parser); 406 } 407 } 408 409 static void handle_big_info(const uint8_t * packet, uint16_t size){ 410 printf("BIG Info advertising report\n"); 411 sync_handle = hci_subevent_le_biginfo_advertising_report_get_sync_handle(packet); 412 encryption = hci_subevent_le_biginfo_advertising_report_get_encryption(packet); 413 if (encryption) { 414 printf("Stream is encrypted\n"); 415 } 416 have_big_info = true; 417 } 418 419 static void enter_create_big_sync(void){ 420 // stop scanning 421 gap_stop_scan(); 422 423 // switch to lc3plus if requested and possible 424 use_lc3plus_decoder = request_lc3plus_decoder && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); 425 426 // init decoder 427 setup_lc3_decoder(); 428 429 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); 430 431 #ifdef HAVE_POSIX_FILE_IO 432 // create wav file 433 printf("WAV file: %s\n", filename_wav); 434 wav_writer_open(filename_wav, num_bis, sampling_frequency_hz); 435 #endif 436 437 // init playback buffer 438 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 439 440 // start playback 441 // PTS 8.2 sends stereo at half speed for stereo, for now playback at half speed 442 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 443 if (sink != NULL){ 444 uint16_t playback_speed; 445 if ((num_bis > 1) && pts_mode){ 446 playback_speed = sampling_frequency_hz / num_bis; 447 printf("PTS workaround: playback at %u hz\n", playback_speed); 448 } else { 449 playback_speed = sampling_frequency_hz; 450 }; 451 sink->init(num_bis, sampling_frequency_hz, le_audio_broadcast_sink_playback); 452 sink->start_stream(); 453 } 454 455 big_sync_params.big_handle = big_handle; 456 big_sync_params.sync_handle = sync_handle; 457 big_sync_params.encryption = encryption; 458 if (encryption) { 459 memcpy(big_sync_params.broadcast_code, &broadcast_code[0], 16); 460 } else { 461 memset(big_sync_params.broadcast_code, 0, 16); 462 } 463 big_sync_params.mse = 0; 464 big_sync_params.big_sync_timeout_10ms = 100; 465 big_sync_params.num_bis = num_bis; 466 uint8_t i; 467 printf("BIG Create Sync for BIS: "); 468 for (i=0;i<num_bis;i++){ 469 big_sync_params.bis_indices[i] = i + 1; 470 printf("%u ", big_sync_params.bis_indices[i]); 471 } 472 printf("\n"); 473 app_state = APP_W4_BIG_SYNC_ESTABLISHED; 474 gap_big_sync_create(&big_sync_storage, &big_sync_params); 475 } 476 477 static void start_scanning() { 478 app_state = APP_W4_BROADCAST_ADV; 479 have_base = false; 480 have_big_info = false; 481 gap_set_scan_params(1, 0x30, 0x30, 0); 482 gap_start_scan(); 483 printf("Start scan..\n"); 484 } 485 486 static bool is_zero_bd_addr( bd_addr_t address ) { 487 int sum = 0; 488 for( int i=0; i<6; ++i ) { 489 sum += address[i]; 490 } 491 return (sum>0)?false:true; 492 } 493 494 static void setup_advertising() { 495 bd_addr_t local_addr; 496 gap_local_bd_addr(local_addr); 497 bool local_address_invalid = is_zero_bd_addr( local_addr ); 498 if( local_address_invalid ) { 499 extended_params.own_address_type = BD_ADDR_TYPE_LE_RANDOM; 500 } 501 gap_extended_advertising_setup(&le_advertising_set, &extended_params, &adv_handle); 502 if( local_address_invalid ) { 503 bd_addr_t random_address = { 0xC1, 0x01, 0x01, 0x01, 0x01, 0x01 }; 504 gap_extended_advertising_set_random_address( adv_handle, random_address ); 505 } 506 gap_extended_advertising_set_adv_data(adv_handle, sizeof(extended_adv_data), extended_adv_data); 507 gap_extended_advertising_start(adv_handle, 0, 0); 508 } 509 510 static void got_base_and_big_info() { 511 // add source 512 if (standalone_mode) { 513 printf("BASS: add Broadcast Source\n"); 514 // add source 515 uint8_t source_index = 0; 516 broadcast_audio_scan_service_server_add_source(&bass_source_new, &source_index); 517 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 518 } 519 520 if ((encryption == false) || have_broadcast_code){ 521 enter_create_big_sync(); 522 } else { 523 printf("BASS: request Broadcast Code\n"); 524 bass_sources[0].big_encryption = LE_AUDIO_BIG_ENCRYPTION_BROADCAST_CODE_REQUIRED; 525 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 526 app_state = APP_W4_BROADCAST_CODE; 527 } 528 } 529 530 static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 531 UNUSED(channel); 532 if (packet_type != HCI_EVENT_PACKET) return; 533 switch (packet[0]) { 534 case BTSTACK_EVENT_STATE: 535 switch(btstack_event_state_get_state(packet)) { 536 case HCI_STATE_WORKING: 537 app_state = APP_IDLE; 538 #ifdef ENABLE_DEMO_MODE 539 start_scanning(); 540 #else 541 show_usage(); 542 #endif 543 break; 544 case HCI_STATE_OFF: 545 printf("Goodbye\n"); 546 exit(0); 547 break; 548 default: 549 break; 550 } 551 break; 552 case GAP_EVENT_EXTENDED_ADVERTISING_REPORT: 553 { 554 if (app_state != APP_W4_BROADCAST_ADV) break; 555 556 gap_event_extended_advertising_report_get_address(packet, remote); 557 uint8_t adv_size = gap_event_extended_advertising_report_get_data_length(packet); 558 const uint8_t * adv_data = gap_event_extended_advertising_report_get_data(packet); 559 560 ad_context_t context; 561 bool found = false; 562 remote_name[0] = '\0'; 563 uint16_t uuid; 564 uint32_t broadcast_id; 565 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 566 uint8_t data_type = ad_iterator_get_data_type(&context); 567 uint8_t size = ad_iterator_get_data_len(&context); 568 const uint8_t *data = ad_iterator_get_data(&context); 569 switch (data_type){ 570 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 571 uuid = little_endian_read_16(data, 0); 572 if (uuid == ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_ANNOUNCEMENT_SERVICE){ 573 broadcast_id = little_endian_read_24(data, 2); 574 found = true; 575 } 576 break; 577 case BLUETOOTH_DATA_TYPE_SHORTENED_LOCAL_NAME: 578 case BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME: 579 size = btstack_min(sizeof(remote_name) - 1, size); 580 memcpy(remote_name, data, size); 581 remote_name[size] = 0; 582 // support for nRF5340 Audio DK 583 if (strncmp("NRF5340", remote_name, 7) == 0){ 584 nrf5340_audio_demo = true; 585 found = true; 586 } 587 break; 588 default: 589 break; 590 } 591 } 592 if (!found) break; 593 remote_type = gap_event_extended_advertising_report_get_address_type(packet); 594 remote_sid = gap_event_extended_advertising_report_get_advertising_sid(packet); 595 pts_mode = strncmp("PTS-", remote_name, 4) == 0; 596 count_mode = strncmp("COUNT", remote_name, 5) == 0; 597 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); 598 // ignore other advertisements 599 gap_whitelist_add(remote_type, remote); 600 gap_set_scan_params(1, 0x30, 0x30, 1); 601 // sync to PA 602 gap_periodic_advertiser_list_clear(); 603 gap_periodic_advertiser_list_add(remote_type, remote, remote_sid); 604 app_state = APP_W4_PA_AND_BIG_INFO; 605 printf("Start Periodic Advertising Sync\n"); 606 gap_periodic_advertising_create_sync(0x01, remote_sid, remote_type, remote, 0, 1000, 0); 607 608 // setup bass source info 609 bass_source_new.address_type = remote_type; 610 memcpy(bass_source_new.address, remote, 6); 611 bass_source_new.adv_sid = remote_sid; 612 bass_source_new.broadcast_id = broadcast_id; 613 bass_source_new.pa_sync = LE_AUDIO_PA_SYNC_SYNCHRONIZE_TO_PA_PAST_AVAILABLE; 614 bass_source_new.pa_interval = gap_event_extended_advertising_report_get_periodic_advertising_interval(packet); 615 break; 616 } 617 618 case HCI_EVENT_LE_META: 619 switch(hci_event_le_meta_get_subevent_code(packet)) { 620 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_SYNC_TRANSFER_RECEIVED: 621 // PAST implies broadcast source has been added by client 622 printf("Periodic advertising sync transfer received\n"); 623 btstack_run_loop_remove_timer(&broadcast_sink_pa_sync_timer); 624 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 625 app_state = APP_W4_PA_AND_BIG_INFO; 626 break; 627 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_SYNC_ESTABLISHMENT: 628 sync_handle = hci_subevent_le_periodic_advertising_sync_establishment_get_sync_handle(packet); 629 printf("Periodic advertising sync with handle 0x%04x established\n", sync_handle); 630 app_state = APP_W4_PA_AND_BIG_INFO; 631 break; 632 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_REPORT: 633 if (app_state != APP_W4_PA_AND_BIG_INFO) break; 634 if (have_base) break; 635 handle_periodic_advertisement(packet, size); 636 if (have_base && have_big_info){ 637 got_base_and_big_info(); 638 } 639 break; 640 case HCI_SUBEVENT_LE_BIGINFO_ADVERTISING_REPORT: 641 if (app_state != APP_W4_PA_AND_BIG_INFO) break; 642 if (have_big_info) break; 643 handle_big_info(packet, size); 644 if (have_base && have_big_info){ 645 got_base_and_big_info(); 646 } 647 break; 648 case HCI_SUBEVENT_LE_BIG_SYNC_LOST: 649 printf("BIG Sync Lost\n"); 650 { 651 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 652 if (sink != NULL) { 653 sink->stop_stream(); 654 sink->close(); 655 } 656 } 657 // start over 658 if (!standalone_mode) break; 659 start_scanning(); 660 break; 661 default: 662 break; 663 } 664 break; 665 case HCI_EVENT_META_GAP: 666 switch (hci_event_gap_meta_get_subevent_code(packet)){ 667 case GAP_SUBEVENT_BIG_SYNC_CREATED: { 668 printf("BIG Sync created with BIS Connection handles: "); 669 uint8_t i; 670 for (i=0;i<num_bis;i++){ 671 bis_con_handles[i] = gap_subevent_big_sync_created_get_bis_con_handles(packet, i); 672 printf("0x%04x ", bis_con_handles[i]); 673 } 674 printf("\n"); 675 app_state = APP_STREAMING; 676 last_packet_received_big = false; 677 last_samples_report_ms = btstack_run_loop_get_time_ms(); 678 memset(last_packet_sequence, 0, sizeof(last_packet_sequence)); 679 memset(last_packet_received, 0, sizeof(last_packet_received)); 680 memset(pcm, 0, sizeof(pcm)); 681 printf("Start receiving\n"); 682 683 // update BIS Sync state 684 bass_sources[0].data.subgroups[0].bis_sync_state = bis_sync_mask; 685 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 686 break; 687 } 688 case GAP_SUBEVENT_BIG_SYNC_STOPPED: 689 printf("BIG Sync stopped, big_handle 0x%02x\n", gap_subevent_big_sync_stopped_get_big_handle(packet)); 690 btstack_run_loop_remove_timer(&next_packet_timer); 691 break; 692 default: 693 break; 694 } 695 break; 696 case SM_EVENT_JUST_WORKS_REQUEST: 697 printf("Just Works requested\n"); 698 sm_just_works_confirm(sm_event_just_works_request_get_handle(packet)); 699 break; 700 default: 701 break; 702 } 703 } 704 705 static void store_samples_in_ringbuffer(void){ 706 // check if we have all channels 707 uint8_t bis_channel; 708 for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ 709 if (have_pcm[bis_channel] == false) return; 710 } 711 #ifdef HAVE_POSIX_FILE_IO 712 // write wav samples 713 wav_writer_write_int16(num_bis * number_samples_per_frame, pcm); 714 #endif 715 // store samples in playback buffer 716 uint32_t bytes_to_store = num_bis * number_samples_per_frame * 2; 717 samples_received += number_samples_per_frame; 718 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 719 btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm, bytes_to_store); 720 } else { 721 //printf("Samples dropped\n"); 722 samples_dropped += number_samples_per_frame; 723 } 724 // reset 725 for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ 726 have_pcm[bis_channel] = false; 727 } 728 } 729 730 static void plc_do(uint8_t bis_channel) {// inject packet 731 uint8_t tmp_BEC_detect; 732 uint8_t BFI = 1; 733 (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], NULL, BFI, 734 &pcm[bis_channel], num_bis, 735 &tmp_BEC_detect); 736 737 have_pcm[bis_channel] = true; 738 store_samples_in_ringbuffer(); 739 } 740 741 // 742 // Perform PLC for packets missing in previous intervals 743 // 744 // assumptions: 745 // - packet sequence number is monotonic increasing 746 // - if packet with seq nr x is received, all packets with smaller seq number are either received or missed 747 static void plc_check(uint16_t packet_sequence_number) { 748 while (last_packet_sequence_big != packet_sequence_number){ 749 uint8_t i; 750 for (i=0;i<num_bis;i++){ 751 // deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start 752 if (last_packet_received[i] == false){ 753 printf_plc("- BIS #%u, very first packet missing\n", i); 754 have_pcm[i] = true; 755 store_samples_in_ringbuffer(); 756 757 last_packet_received[i] = true; 758 last_packet_sequence[i] = last_packet_sequence_big; 759 continue; 760 } 761 762 // missing packet if big sequence counter is higher than bis sequence counter 763 if (btstack_time16_delta(last_packet_sequence_big, last_packet_sequence[i]) > 0) { 764 printf_plc("- BIS #%u, PLC for %u\n", i, last_packet_sequence_big); 765 plc_do(i); 766 btstack_assert((last_packet_sequence[i] + 1) == last_packet_sequence_big); 767 last_packet_sequence[i] = last_packet_sequence_big; 768 } 769 } 770 last_packet_sequence_big++; 771 } 772 } 773 774 static void plc_timeout(btstack_timer_source_t * timer) { 775 if (app_state != APP_STREAMING) return; 776 777 // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets 778 uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 779 btstack_run_loop_set_timer(timer, frame_duration_ms); 780 btstack_run_loop_set_timer_handler(timer, plc_timeout); 781 btstack_run_loop_add_timer(timer); 782 783 // assume no packet received in iso interval 784 plc_check(last_packet_sequence_big + 1); 785 } 786 787 static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 788 789 if (app_state != APP_STREAMING) return; 790 791 uint16_t header = little_endian_read_16(packet, 0); 792 hci_con_handle_t con_handle = header & 0x0fff; 793 uint8_t pb_flag = (header >> 12) & 3; 794 uint8_t ts_flag = (header >> 14) & 1; 795 uint16_t iso_load_len = little_endian_read_16(packet, 2); 796 797 uint16_t offset = 4; 798 uint32_t time_stamp = 0; 799 if (ts_flag){ 800 uint32_t time_stamp = little_endian_read_32(packet, offset); 801 offset += 4; 802 } 803 804 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 805 806 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 807 offset += 2; 808 809 uint16_t header_2 = little_endian_read_16(packet, offset); 810 uint16_t iso_sdu_length = header_2 & 0x3fff; 811 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 812 offset += 2; 813 814 if (iso_sdu_length == 0) return; 815 816 // infer channel from con handle - only works for up to 2 channels 817 uint8_t bis_channel = (con_handle == bis_con_handles[0]) ? 0 : 1; 818 819 if (count_mode){ 820 // check for missing packet 821 uint16_t last_seq_no = last_packet_sequence[bis_channel]; 822 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); 823 if (packet_missed){ 824 // print last packet 825 printf("\n"); 826 printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[bis_channel], bis_channel); 827 printf_hexdump(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); 828 last_seq_no++; 829 830 printf(ANSI_COLOR_RED); 831 while (last_seq_no < packet_sequence_number){ 832 printf("%04x %u MISSING\n", last_seq_no, bis_channel); 833 last_seq_no++; 834 } 835 printf(ANSI_COLOR_RESET); 836 837 // print current packet 838 printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, bis_channel); 839 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); 840 } 841 842 // cache current packet 843 memcpy(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); 844 845 } else { 846 847 848 if (last_packet_received[bis_channel]) { 849 850 printf_plc("BIS #%u, receive %u\n", bis_channel, packet_sequence_number); 851 852 int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, 853 last_packet_sequence[bis_channel]); 854 if (packet_sequence_delta < 1) { 855 // drop delayed packet that had already been generated by PLC 856 printf_plc("- dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n", 857 packet_sequence_number, last_packet_sequence[bis_channel]); 858 return; 859 } 860 // simple check 861 if (packet_sequence_number != last_packet_sequence[bis_channel] + 1) { 862 printf_plc("- BIS #%u, missing %u\n", bis_channel, last_packet_sequence[bis_channel] + 1); 863 } 864 } else { 865 866 printf_plc("BIS %u, first packet seq number %u\n", bis_channel, packet_sequence_number); 867 868 if (!last_packet_received_big) { 869 // track sequence number of very first received packet 870 last_packet_received_big = true; 871 last_packet_sequence_big = packet_sequence_number; 872 } 873 last_packet_received[bis_channel] = true; 874 } 875 876 plc_check(packet_sequence_number); 877 878 // decode codec frame 879 uint8_t tmp_BEC_detect; 880 uint8_t BFI = 0; 881 (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], &packet[offset], BFI, 882 &pcm[bis_channel], num_bis, 883 &tmp_BEC_detect); 884 have_pcm[bis_channel] = true; 885 store_samples_in_ringbuffer(); 886 887 lc3_frames++; 888 frames_per_second[bis_channel]++; 889 890 // PLC 891 cached_iso_sdu_len = iso_sdu_length; 892 uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 893 uint32_t timeout_ms = frame_duration_ms * 5 / 2; 894 btstack_run_loop_remove_timer(&next_packet_timer); 895 btstack_run_loop_set_timer(&next_packet_timer, timeout_ms); 896 btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); 897 btstack_run_loop_add_timer(&next_packet_timer); 898 899 uint32_t time_ms = btstack_run_loop_get_time_ms(); 900 if (btstack_time_delta(time_ms, last_samples_report_ms) >= 1000){ 901 last_samples_report_ms = time_ms; 902 printf("LC3 Frames: %4u - ", (int) (lc3_frames / num_bis)); 903 uint8_t i; 904 for (i=0;i<num_bis;i++){ 905 printf("%u ", frames_per_second[i]); 906 frames_per_second[i] = 0; 907 } 908 printf(" frames per second, dropped %u of %u\n", samples_dropped, samples_received); 909 samples_received = 0; 910 samples_dropped = 0; 911 } 912 } 913 914 last_packet_time_ms[bis_channel] = receive_time_ms; 915 last_packet_sequence[bis_channel] = packet_sequence_number; 916 } 917 918 static void show_usage(void){ 919 printf("\n--- LE Audio Broadcast Sink Test Console ---\n"); 920 printf("s - start scanning\n"); 921 #ifdef HAVE_LC3PLUS 922 printf("q - use LC3plus decoder if 10 ms ISO interval is used\n"); 923 #endif 924 printf("e - set broadcast code to 0x11111111111111111111111111111111\n"); 925 printf("t - terminate BIS streams\n"); 926 printf("---\n"); 927 } 928 929 static void stdin_process(char c){ 930 switch (c){ 931 case 's': 932 if (app_state != APP_IDLE) break; 933 standalone_mode = true; 934 start_scanning(); 935 break; 936 case 'e': 937 printf("Set broadcast code to 0x11111111111111111111111111111111\n"); 938 memset(broadcast_code, 0x11, 16); 939 have_broadcast_code = true; 940 break; 941 #ifdef HAVE_LC3PLUS 942 case 'q': 943 printf("Use LC3plus decoder for 10 ms ISO interval...\n"); 944 request_lc3plus_decoder = true; 945 break; 946 #endif 947 case 't': 948 switch (app_state){ 949 case APP_STREAMING: 950 case APP_W4_BIG_SYNC_ESTABLISHED: 951 app_state = APP_IDLE; 952 btstack_run_loop_remove_timer(&next_packet_timer); 953 close_files(); 954 printf("Terminate BIG SYNC\n"); 955 gap_big_sync_terminate(big_handle); 956 break; 957 default: 958 break; 959 } 960 break; 961 case '\n': 962 case '\r': 963 break; 964 default: 965 show_usage(); 966 break; 967 968 } 969 } 970 971 static void broadcast_sync_pa_sync_timeout_handler(btstack_timer_source_t * ts){ 972 UNUSED(ts); 973 printf("PAST Timeout -> LE_AUDIO_PA_SYNC_STATE_NO_PAST\n"); 974 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_NO_PAST); 975 } 976 977 static void bass_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 978 UNUSED(packet_type); 979 UNUSED(channel); 980 btstack_assert (packet_type == HCI_EVENT_PACKET); 981 btstack_assert(hci_event_packet_get_type(packet) == HCI_EVENT_GATTSERVICE_META); 982 printf("BASS Event 0x%02x: ", hci_event_gattservice_meta_get_subevent_code(packet)); 983 printf_hexdump(packet, size); 984 switch (hci_event_gattservice_meta_get_subevent_code(packet)){ 985 case GATTSERVICE_SUBEVENT_BASS_SERVER_SOURCE_ADDED: 986 printf("GATTSERVICE_SUBEVENT_BASS_SOURCE_ADDED, source_id 0x%04x, pa_sync %u\n", 987 gattservice_subevent_bass_server_source_added_get_source_id(packet), 988 gattservice_subevent_bass_server_source_added_get_pa_sync(packet)); 989 switch (gattservice_subevent_bass_server_source_added_get_pa_sync(packet)){ 990 case LE_AUDIO_PA_SYNC_SYNCHRONIZE_TO_PA_PAST_AVAILABLE: 991 printf("LE_AUDIO_PA_SYNC_SYNCHRONIZE_TO_PA_PAST_AVAILABLE -> Request SyncInfo\n"); 992 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCINFO_REQUEST); 993 // start timeout 994 btstack_run_loop_set_timer_handler(&broadcast_sink_pa_sync_timer, broadcast_sync_pa_sync_timeout_handler); 995 btstack_run_loop_set_timer(&broadcast_sink_pa_sync_timer, 10000); // 10 seconds 996 btstack_run_loop_add_timer(&broadcast_sink_pa_sync_timer); 997 break; 998 } 999 break; 1000 case GATTSERVICE_SUBEVENT_BASS_SERVER_SOURCE_MODIFIED: 1001 printf("GATTSERVICE_SUBEVENT_BASS_SOURCE_MODIFIED, source_id 0x%04x, pa_sync %u\n", 1002 gattservice_subevent_bass_server_source_added_get_source_id(packet), 1003 gattservice_subevent_bass_server_source_added_get_pa_sync(packet)); 1004 // handle 'bis sync == 0' 1005 printf("PA Sync %u, bis_sync[0] = %u\n", bass_sources[0].data.pa_sync, bass_sources[0].data.subgroups[0].bis_sync); 1006 if (bass_sources[0].data.subgroups[0].bis_sync == 0){ 1007 printf("Simulate BIS Sync has stopped\n"); 1008 bass_sources[0].data.subgroups[0].bis_sync_state = 0; 1009 broadcast_audio_scan_service_server_set_pa_sync_state(0, LE_AUDIO_PA_SYNC_STATE_SYNCHRONIZED_TO_PA); 1010 } 1011 break; 1012 case GATTSERVICE_SUBEVENT_BASS_SERVER_BROADCAST_CODE: 1013 gattservice_subevent_bass_server_broadcast_code_get_broadcast_code(packet, broadcast_code); 1014 printf("GATTSERVICE_SUBEVENT_BASS_BROADCAST_CODE received: "); 1015 printf_hexdump(broadcast_code, 16); 1016 bass_sources[0].big_encryption = LE_AUDIO_BIG_ENCRYPTION_DECRYPTING; 1017 if (have_base && have_big_info){ 1018 enter_create_big_sync(); 1019 } 1020 break; 1021 default: 1022 break; 1023 } 1024 } 1025 1026 int btstack_main(int argc, const char * argv[]); 1027 int btstack_main(int argc, const char * argv[]){ 1028 (void) argv; 1029 (void) argc; 1030 1031 l2cap_init(); 1032 sm_init(); 1033 1034 // setup ATT server 1035 att_server_init(profile_data, NULL, NULL); 1036 1037 // register for HCI events 1038 hci_event_callback_registration.callback = &packet_handler; 1039 hci_add_event_handler(&hci_event_callback_registration); 1040 1041 // register for ISO Packet 1042 hci_register_iso_packet_handler(&iso_packet_handler); 1043 1044 // register for SM events 1045 sm_event_callback_registration.callback = &packet_handler; 1046 sm_add_event_handler(&sm_event_callback_registration); 1047 1048 // setup BASS Server 1049 broadcast_audio_scan_service_server_init(BASS_NUM_SOURCES, bass_sources, BASS_NUM_CLIENTS, bass_clients); 1050 broadcast_audio_scan_service_server_register_packet_handler(&bass_packet_handler); 1051 1052 // setup advertising and allow to receive periodic advertising sync trasnfers 1053 setup_advertising(); 1054 gap_periodic_advertising_sync_transfer_set_default_parameters(2, 0, 0x2000, 0); 1055 1056 // turn on! 1057 hci_power_control(HCI_POWER_ON); 1058 1059 btstack_stdin_setup(stdin_process); 1060 return 0; 1061 } 1062