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 <unistd.h> 52 #include <inttypes.h> 53 #include <fcntl.h> // open 54 #include <errno.h> 55 56 #include "ad_parser.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 73 #ifdef HAVE_POSIX_FILE_IO 74 #include "wav_util.h" 75 #endif 76 77 // max config 78 #define MAX_NUM_BIS 2 79 #define MAX_SAMPLES_PER_FRAME 480 80 81 // playback 82 #define MAX_NUM_LC3_FRAMES 5 83 #define MAX_BYTES_PER_SAMPLE 4 84 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_BYTES_PER_SAMPLE) 85 86 // analysis 87 #define PACKET_PREFIX_LEN 10 88 89 #define ANSI_COLOR_RED "\x1b[31m" 90 #define ANSI_COLOR_GREEN "\x1b[32m" 91 #define ANSI_COLOR_YELLOW "\x1b[33m" 92 #define ANSI_COLOR_BLUE "\x1b[34m" 93 #define ANSI_COLOR_MAGENTA "\x1b[35m" 94 #define ANSI_COLOR_CYAN "\x1b[36m" 95 #define ANSI_COLOR_RESET "\x1b[0m" 96 97 static void show_usage(void); 98 99 static const char * filename_wav = "le_audio_broadcast_sink.wav"; 100 101 static enum { 102 APP_W4_WORKING, 103 APP_W4_BROADCAST_ADV, 104 APP_W4_PA_AND_BIG_INFO, 105 APP_W4_BIG_SYNC_ESTABLISHED, 106 APP_STREAMING, 107 APP_IDLE 108 } app_state = APP_W4_WORKING; 109 110 // 111 static btstack_packet_callback_registration_t hci_event_callback_registration; 112 113 static bool have_base; 114 static bool have_big_info; 115 116 uint32_t last_samples_report_ms; 117 uint16_t samples_received; 118 uint16_t samples_dropped; 119 uint16_t frames_per_second[MAX_NUM_BIS]; 120 121 // remote info 122 static char remote_name[20]; 123 static bd_addr_t remote; 124 static bd_addr_type_t remote_type; 125 static uint8_t remote_sid; 126 static bool count_mode; 127 static bool pts_mode; 128 static bool nrf5340_audio_demo; 129 130 131 // broadcast info 132 static const uint8_t big_handle = 1; 133 static hci_con_handle_t sync_handle; 134 static hci_con_handle_t bis_con_handles[MAX_NUM_BIS]; 135 static unsigned int next_bis_index; 136 static uint8_t encryption; 137 static uint8_t broadcast_code [] = {0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01}; 138 139 // analysis 140 static bool last_packet_received_big; 141 static uint16_t last_packet_sequence_big; 142 static bool last_packet_received[MAX_NUM_BIS]; 143 static uint16_t last_packet_sequence[MAX_NUM_BIS]; 144 static uint32_t last_packet_time_ms[MAX_NUM_BIS]; 145 static uint8_t last_packet_prefix[MAX_NUM_BIS * PACKET_PREFIX_LEN]; 146 147 // BIG Sync 148 static le_audio_big_sync_t big_sync_storage; 149 static le_audio_big_sync_params_t big_sync_params; 150 151 // lc3 writer 152 static uint32_t lc3_frames; 153 154 // lc3 codec config 155 static uint16_t sampling_frequency_hz; 156 static btstack_lc3_frame_duration_t frame_duration; 157 static uint16_t number_samples_per_frame; 158 static uint16_t octets_per_frame; 159 static uint8_t num_bis; 160 161 // lc3 decoder 162 static bool request_lc3plus_decoder = false; 163 static bool use_lc3plus_decoder = false; 164 static const btstack_lc3_decoder_t * lc3_decoder; 165 static int16_t pcm[MAX_NUM_BIS * MAX_SAMPLES_PER_FRAME]; 166 167 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_NUM_BIS]; 168 #ifdef HAVE_LC3PLUS 169 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_NUM_BIS]; 170 #endif 171 static void * decoder_contexts[MAX_NR_BIS]; 172 173 // playback 174 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; 175 static btstack_ring_buffer_t playback_buffer; 176 177 static btstack_timer_source_t next_packet_timer; 178 static uint16_t cached_iso_sdu_len; 179 static bool have_pcm[MAX_NUM_BIS]; 180 181 static void le_audio_broadcast_sink_playback(int16_t * buffer, uint16_t num_samples){ 182 // called from lower-layer but guaranteed to be on main thread 183 uint32_t bytes_needed = num_samples * num_bis * 2; 184 185 static bool underrun = true; 186 187 log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / ( num_bis * 2)); 188 189 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)){ 190 memset(buffer, 0, bytes_needed); 191 if (underrun == false){ 192 log_info("Playback underrun"); 193 underrun = true; 194 } 195 return; 196 } 197 198 if (underrun){ 199 underrun = false; 200 log_info("Playback started"); 201 } 202 uint32_t bytes_read; 203 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 204 btstack_assert(bytes_read == bytes_needed); 205 } 206 207 static void setup_lc3_decoder(void){ 208 uint8_t channel; 209 for (channel = 0 ; channel < num_bis ; channel++){ 210 // pick decoder 211 void * decoder_context = NULL; 212 #ifdef HAVE_LC3PLUS 213 if (use_lc3plus_decoder){ 214 decoder_context = &fraunhofer_decoder_contexts[channel]; 215 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); 216 } 217 else 218 #endif 219 { 220 decoder_context = &google_decoder_contexts[channel]; 221 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); 222 } 223 decoder_contexts[channel] = decoder_context; 224 lc3_decoder->configure(decoder_context, sampling_frequency_hz, frame_duration); 225 } 226 number_samples_per_frame = lc3_decoder->get_number_samples_per_frame(decoder_contexts[0]); 227 btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 228 } 229 230 static void close_files(void){ 231 #ifdef HAVE_POSIX_FILE_IO 232 printf("Close files\n"); 233 wav_writer_close(); 234 #endif 235 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 236 if (sink != NULL){ 237 sink->stop_stream(); 238 sink->close(); 239 } 240 } 241 242 static void handle_periodic_advertisement(const uint8_t * packet, uint16_t size){ 243 // nRF534_audio quirk - no BASE in periodic advertisement 244 if (nrf5340_audio_demo){ 245 // hard coded config LC3 246 // default: mono bitrate 96000, 10 ms with USB audio source, 120 octets per frame 247 count_mode = 0; 248 pts_mode = 0; 249 num_bis = 1; 250 sampling_frequency_hz = 48000; 251 frame_duration = BTSTACK_LC3_FRAME_DURATION_10000US; 252 octets_per_frame = 120; 253 have_base = true; 254 return; 255 } 256 257 // periodic advertisement contains the BASE 258 // TODO: BASE might be split across multiple advertisements 259 const uint8_t * adv_data = hci_subevent_le_periodic_advertising_report_get_data(packet); 260 uint16_t adv_size = hci_subevent_le_periodic_advertising_report_get_data_length(packet); 261 uint8_t adv_status = hci_subevent_le_periodic_advertising_report_get_data_status(packet); 262 263 if (adv_status != 0) { 264 printf("Periodic Advertisement (status %u): ", adv_status); 265 printf_hexdump(adv_data, adv_size); 266 return; 267 } 268 269 ad_context_t context; 270 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 271 uint8_t data_type = ad_iterator_get_data_type(&context); 272 // TODO: avoid out-of-bounds read 273 // uint8_t data_size = ad_iterator_get_data_len(&context); 274 const uint8_t * data = ad_iterator_get_data(&context); 275 uint16_t uuid; 276 switch (data_type){ 277 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 278 uuid = little_endian_read_16(data, 0); 279 if (uuid == ORG_BLUETOOTH_SERVICE_BASIC_AUDIO_ANNOUNCEMENT_SERVICE){ 280 have_base = true; 281 // Level 1: Group Level 282 const uint8_t * base_data = &data[2]; 283 // TODO: avoid out-of-bounds read 284 // uint16_t base_len = data_size - 2; 285 printf("BASE:\n"); 286 uint32_t presentation_delay = little_endian_read_24(base_data, 0); 287 printf("- presentation delay: %"PRIu32" us\n", presentation_delay); 288 uint8_t num_subgroups = base_data[3]; 289 printf("- num subgroups: %u\n", num_subgroups); 290 uint8_t i; 291 uint16_t offset = 4; 292 for (i=0;i<num_subgroups;i++){ 293 // Level 2: Subgroup Level 294 num_bis = base_data[offset++]; 295 printf(" - num bis[%u]: %u\n", i, num_bis); 296 // codec_id: coding format = 0x06, vendor and coded id = 0 297 offset += 5; 298 uint8_t codec_specific_configuration_length = base_data[offset++]; 299 const uint8_t * codec_specific_configuration = &base_data[offset]; 300 printf(" - codec specific config[%u]: ", i); 301 printf_hexdump(codec_specific_configuration, codec_specific_configuration_length); 302 // parse config to get sampling frequency and frame duration 303 uint8_t codec_offset = 0; 304 while ((codec_offset + 1) < codec_specific_configuration_length){ 305 uint8_t ltv_len = codec_specific_configuration[codec_offset++]; 306 uint8_t ltv_type = codec_specific_configuration[codec_offset]; 307 const uint32_t sampling_frequency_map[] = { 8000, 11025, 16000, 22050, 24000, 32000, 44100, 48000, 88200, 96000, 176400, 192000, 384000 }; 308 uint8_t sampling_frequency_index; 309 uint8_t frame_duration_index; 310 switch (ltv_type){ 311 case 0x01: // sampling frequency 312 sampling_frequency_index = codec_specific_configuration[codec_offset+1]; 313 // TODO: check range 314 sampling_frequency_hz = sampling_frequency_map[sampling_frequency_index - 1]; 315 printf(" - sampling frequency[%u]: %u\n", i, sampling_frequency_hz); 316 break; 317 case 0x02: // 0 = 7.5, 1 = 10 ms 318 frame_duration_index = codec_specific_configuration[codec_offset+1]; 319 frame_duration = (frame_duration_index == 0) ? BTSTACK_LC3_FRAME_DURATION_7500US : BTSTACK_LC3_FRAME_DURATION_10000US; 320 printf(" - frame duration[%u]: %s ms\n", i, (frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US) ? "7.5" : "10"); 321 break; 322 case 0x04: // octets per coding frame 323 octets_per_frame = little_endian_read_16(codec_specific_configuration, codec_offset+1); 324 printf(" - octets per codec frame[%u]: %u\n", i, octets_per_frame); 325 break; 326 default: 327 break; 328 } 329 codec_offset += ltv_len; 330 } 331 // 332 offset += codec_specific_configuration_length; 333 uint8_t metadata_length = base_data[offset++]; 334 const uint8_t * meta_data = &base_data[offset]; 335 offset += metadata_length; 336 printf(" - meta data[%u]: ", i); 337 printf_hexdump(meta_data, metadata_length); 338 uint8_t k; 339 for (k=0;k<num_bis;k++){ 340 // Level 3: BIS Level 341 uint8_t bis_index = base_data[offset++]; 342 printf(" - bis index[%u][%u]: %u\n", i, k, bis_index); 343 uint8_t codec_specific_configuration_length2 = base_data[offset++]; 344 const uint8_t * codec_specific_configuration2 = &base_data[offset]; 345 printf(" - codec specific config[%u][%u]: ", i, k); 346 printf_hexdump(codec_specific_configuration2, codec_specific_configuration_length2); 347 offset += codec_specific_configuration_length2; 348 } 349 } 350 } 351 break; 352 default: 353 break; 354 } 355 } 356 } 357 358 static void handle_big_info(const uint8_t * packet, uint16_t size){ 359 printf("BIG Info advertising report\n"); 360 sync_handle = hci_subevent_le_biginfo_advertising_report_get_sync_handle(packet); 361 encryption = hci_subevent_le_biginfo_advertising_report_get_encryption(packet); 362 if (encryption) { 363 printf("Stream is encrypted\n"); 364 } 365 have_big_info = true; 366 } 367 368 static void enter_create_big_sync(void){ 369 // stop scanning 370 gap_stop_scan(); 371 372 // switch to lc3plus if requested and possible 373 use_lc3plus_decoder = request_lc3plus_decoder && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); 374 375 // init decoder 376 setup_lc3_decoder(); 377 378 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); 379 380 #ifdef HAVE_POSIX_FILE_IO 381 // create wav file 382 printf("WAV file: %s\n", filename_wav); 383 wav_writer_open(filename_wav, num_bis, sampling_frequency_hz); 384 #endif 385 386 // init playback buffer 387 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 388 389 // start playback 390 // PTS 8.2 sends stereo at half speed for stereo, for now playback at half speed 391 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 392 if (sink != NULL){ 393 uint16_t playback_speed; 394 if ((num_bis > 1) && pts_mode){ 395 playback_speed = sampling_frequency_hz / num_bis; 396 printf("PTS workaround: playback at %u hz\n", playback_speed); 397 } else { 398 playback_speed = sampling_frequency_hz; 399 }; 400 sink->init(num_bis, sampling_frequency_hz, le_audio_broadcast_sink_playback); 401 sink->start_stream(); 402 } 403 404 big_sync_params.big_handle = big_handle; 405 big_sync_params.sync_handle = sync_handle; 406 big_sync_params.encryption = encryption; 407 if (encryption) { 408 memcpy(big_sync_params.broadcast_code, &broadcast_code[0], 16); 409 } else { 410 memset(big_sync_params.broadcast_code, 0, 16); 411 } 412 big_sync_params.mse = 0; 413 big_sync_params.big_sync_timeout_10ms = 100; 414 big_sync_params.num_bis = num_bis; 415 uint8_t i; 416 printf("BIG Create Sync for BIS: "); 417 for (i=0;i<num_bis;i++){ 418 big_sync_params.bis_indices[i] = i + 1; 419 printf("%u ", big_sync_params.bis_indices[i]); 420 } 421 printf("\n"); 422 app_state = APP_W4_BIG_SYNC_ESTABLISHED; 423 gap_big_sync_create(&big_sync_storage, &big_sync_params); 424 } 425 426 static void start_scanning() { 427 app_state = APP_W4_BROADCAST_ADV; 428 have_base = false; 429 have_big_info = false; 430 gap_set_scan_params(1, 0x30, 0x30, 0); 431 gap_start_scan(); 432 printf("Start scan..\n"); 433 } 434 435 static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 436 UNUSED(channel); 437 if (packet_type != HCI_EVENT_PACKET) return; 438 switch (packet[0]) { 439 case BTSTACK_EVENT_STATE: 440 switch(btstack_event_state_get_state(packet)) { 441 case HCI_STATE_WORKING: 442 app_state = APP_IDLE; 443 #ifdef ENABLE_DEMO_MODE 444 start_scanning(); 445 #else 446 show_usage(); 447 #endif 448 break; 449 case HCI_STATE_OFF: 450 printf("Goodbye\n"); 451 exit(0); 452 break; 453 default: 454 break; 455 } 456 break; 457 case GAP_EVENT_EXTENDED_ADVERTISING_REPORT: 458 { 459 if (app_state != APP_W4_BROADCAST_ADV) break; 460 461 gap_event_extended_advertising_report_get_address(packet, remote); 462 uint8_t adv_size = gap_event_extended_advertising_report_get_data_length(packet); 463 const uint8_t * adv_data = gap_event_extended_advertising_report_get_data(packet); 464 465 ad_context_t context; 466 bool found = false; 467 remote_name[0] = '\0'; 468 uint16_t uuid; 469 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 470 uint8_t data_type = ad_iterator_get_data_type(&context); 471 uint8_t size = ad_iterator_get_data_len(&context); 472 const uint8_t *data = ad_iterator_get_data(&context); 473 switch (data_type){ 474 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 475 uuid = little_endian_read_16(data, 0); 476 if (uuid == ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_ANNOUNCEMENT_SERVICE){ 477 found = true; 478 } 479 break; 480 case BLUETOOTH_DATA_TYPE_SHORTENED_LOCAL_NAME: 481 case BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME: 482 size = btstack_min(sizeof(remote_name) - 1, size); 483 memcpy(remote_name, data, size); 484 remote_name[size] = 0; 485 // support for nRF5340 Audio DK 486 if (strncmp("NRF5340", remote_name, 7) == 0){ 487 nrf5340_audio_demo = true; 488 found = true; 489 } 490 break; 491 default: 492 break; 493 } 494 } 495 if (!found) break; 496 remote_type = gap_event_extended_advertising_report_get_address_type(packet); 497 remote_sid = gap_event_extended_advertising_report_get_advertising_sid(packet); 498 pts_mode = strncmp("PTS-", remote_name, 4) == 0; 499 count_mode = strncmp("COUNT", remote_name, 5) == 0; 500 printf("Remote Broadcast sink found, addr %s, name: '%s' (pts-mode: %u, count: %u)\n", bd_addr_to_str(remote), remote_name, pts_mode, count_mode); 501 // ignore other advertisements 502 gap_whitelist_add(remote_type, remote); 503 gap_set_scan_params(1, 0x30, 0x30, 1); 504 // sync to PA 505 gap_periodic_advertiser_list_clear(); 506 gap_periodic_advertiser_list_add(remote_type, remote, remote_sid); 507 app_state = APP_W4_PA_AND_BIG_INFO; 508 printf("Start Periodic Advertising Sync\n"); 509 gap_periodic_advertising_create_sync(0x01, remote_sid, remote_type, remote, 0, 1000, 0); 510 break; 511 } 512 513 case HCI_EVENT_LE_META: 514 switch(hci_event_le_meta_get_subevent_code(packet)) { 515 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_SYNC_ESTABLISHMENT: 516 sync_handle = hci_subevent_le_periodic_advertising_sync_establishment_get_sync_handle(packet); 517 printf("Periodic advertising sync with handle 0x%04x established\n", sync_handle); 518 break; 519 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_REPORT: 520 if (have_base) break; 521 handle_periodic_advertisement(packet, size); 522 if (have_base & have_big_info){ 523 enter_create_big_sync(); 524 } 525 break; 526 case HCI_SUBEVENT_LE_BIGINFO_ADVERTISING_REPORT: 527 if (have_big_info) break; 528 handle_big_info(packet, size); 529 if (have_base & have_big_info){ 530 enter_create_big_sync(); 531 } 532 break; 533 case HCI_SUBEVENT_LE_BIG_SYNC_LOST: 534 printf("BIG Sync Lost\n"); 535 { 536 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 537 if (sink != NULL) { 538 sink->stop_stream(); 539 sink->close(); 540 } 541 } 542 // start over 543 start_scanning(); 544 break; 545 default: 546 break; 547 } 548 break; 549 case HCI_EVENT_META_GAP: 550 switch (hci_event_gap_meta_get_subevent_code(packet)){ 551 case GAP_SUBEVENT_BIG_SYNC_CREATED: { 552 printf("BIG Sync created with BIS Connection handles: "); 553 uint8_t i; 554 for (i=0;i<num_bis;i++){ 555 bis_con_handles[i] = gap_subevent_big_sync_created_get_bis_con_handles(packet, i); 556 printf("0x%04x ", bis_con_handles[i]); 557 } 558 app_state = APP_STREAMING; 559 last_packet_received_big = false; 560 last_samples_report_ms = btstack_run_loop_get_time_ms(); 561 memset(last_packet_sequence, 0, sizeof(last_packet_sequence)); 562 memset(last_packet_received, 0, sizeof(last_packet_received)); 563 memset(pcm, 0, sizeof(pcm)); 564 printf("Start receiving\n"); 565 break; 566 } 567 case GAP_SUBEVENT_BIG_SYNC_STOPPED: 568 printf("BIG Sync stopped, big_handle 0x%02x\n", gap_subevent_big_sync_stopped_get_big_handle(packet)); 569 break; 570 default: 571 break; 572 } 573 break; 574 default: 575 break; 576 } 577 } 578 579 static void store_samples_in_ringbuffer(void){ 580 // check if we have all channels 581 uint8_t bis_channel; 582 for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ 583 if (have_pcm[bis_channel] == false) return; 584 } 585 #ifdef HAVE_POSIX_FILE_IO 586 // write wav samples 587 wav_writer_write_int16(num_bis * number_samples_per_frame, pcm); 588 #endif 589 // store samples in playback buffer 590 uint32_t bytes_to_store = num_bis * number_samples_per_frame * 2; 591 samples_received += number_samples_per_frame; 592 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 593 btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm, bytes_to_store); 594 } else { 595 printf("Samples dropped\n"); 596 samples_dropped += number_samples_per_frame; 597 } 598 // reset 599 for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ 600 have_pcm[bis_channel] = false; 601 } 602 } 603 604 static void plc_do(uint8_t bis_channel) {// inject packet 605 uint8_t tmp_BEC_detect; 606 uint8_t BFI = 1; 607 (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], NULL, cached_iso_sdu_len, BFI, 608 &pcm[bis_channel], num_bis, 609 &tmp_BEC_detect); 610 611 printf("PLC channel %u - packet sequence %u\n", bis_channel, last_packet_sequence[bis_channel]); 612 613 have_pcm[bis_channel] = true; 614 store_samples_in_ringbuffer(); 615 } 616 617 static void plc_missing(uint16_t packet_sequence_number) { 618 while (btstack_time16_delta(packet_sequence_number, last_packet_sequence_big) > 1){ 619 uint8_t i; 620 for (i=0;i<num_bis;i++){ 621 // deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start 622 if (last_packet_received[i] == false){ 623 printf("PLC Missing: very first packet BIS %u missing\n", i); 624 have_pcm[i] = true; 625 store_samples_in_ringbuffer(); 626 627 last_packet_received[i] = true; 628 last_packet_sequence[i] = last_packet_sequence_big; 629 continue; 630 } 631 632 // missing packet if big sequence counter is higher than bis sequence counter 633 if (btstack_time16_delta(last_packet_sequence_big, last_packet_sequence[i]) > 0) { 634 plc_do(i); 635 last_packet_sequence[i] = last_packet_sequence_big; 636 } 637 } 638 last_packet_sequence_big++; 639 } 640 } 641 642 static void plc_timeout(btstack_timer_source_t * timer) { 643 if (app_state != APP_STREAMING) return; 644 645 // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets 646 uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 647 btstack_run_loop_set_timer(timer, frame_duration_ms); 648 btstack_run_loop_set_timer_handler(timer, plc_timeout); 649 btstack_run_loop_add_timer(timer); 650 651 // assume no packet received in iso interval 652 plc_missing(last_packet_sequence_big + 1); 653 } 654 655 static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 656 657 if (app_state != APP_STREAMING) return; 658 659 uint16_t header = little_endian_read_16(packet, 0); 660 hci_con_handle_t con_handle = header & 0x0fff; 661 uint8_t pb_flag = (header >> 12) & 3; 662 uint8_t ts_flag = (header >> 14) & 1; 663 uint16_t iso_load_len = little_endian_read_16(packet, 2); 664 665 uint16_t offset = 4; 666 uint32_t time_stamp = 0; 667 if (ts_flag){ 668 uint32_t time_stamp = little_endian_read_32(packet, offset); 669 offset += 4; 670 } 671 672 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 673 674 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 675 offset += 2; 676 677 uint16_t header_2 = little_endian_read_16(packet, offset); 678 uint16_t iso_sdu_length = header_2 & 0x3fff; 679 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 680 offset += 2; 681 682 if (iso_sdu_length == 0) return; 683 684 // infer channel from con handle - only works for up to 2 channels 685 uint8_t bis_channel = (con_handle == bis_con_handles[0]) ? 0 : 1; 686 687 if (count_mode){ 688 // check for missing packet 689 uint16_t last_seq_no = last_packet_sequence[bis_channel]; 690 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); 691 if (packet_missed){ 692 // print last packet 693 printf("\n"); 694 printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[bis_channel], bis_channel); 695 printf_hexdump(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); 696 last_seq_no++; 697 698 printf(ANSI_COLOR_RED); 699 while (last_seq_no < packet_sequence_number){ 700 printf("%04x %u MISSING\n", last_seq_no, bis_channel); 701 last_seq_no++; 702 } 703 printf(ANSI_COLOR_RESET); 704 705 // print current packet 706 printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, bis_channel); 707 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); 708 } 709 710 // cache current packet 711 memcpy(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); 712 713 } else { 714 715 if (last_packet_received[bis_channel]) { 716 int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, 717 last_packet_sequence[bis_channel]); 718 if (packet_sequence_delta < 1) { 719 // drop delayed packet that had already been generated by PLC 720 printf("Dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n", 721 packet_sequence_number, last_packet_sequence[bis_channel]); 722 return; 723 } 724 } else { 725 if (!last_packet_received_big) { 726 // track sequence number of very first received packet 727 last_packet_received_big = true; 728 last_packet_sequence_big = packet_sequence_number; 729 } 730 printf("BIS %u, first packet seq number %u\n", bis_channel, packet_sequence_number); 731 last_packet_received[bis_channel] = true; 732 } 733 734 // assert no channel is more than one packet behind 735 plc_missing(packet_sequence_number); 736 737 // decode codec frame 738 uint8_t tmp_BEC_detect; 739 uint8_t BFI = 0; 740 (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], &packet[offset], iso_sdu_length, BFI, 741 &pcm[bis_channel], num_bis, 742 &tmp_BEC_detect); 743 have_pcm[bis_channel] = true; 744 store_samples_in_ringbuffer(); 745 746 lc3_frames++; 747 frames_per_second[bis_channel]++; 748 749 // PLC 750 cached_iso_sdu_len = iso_sdu_length; 751 uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 752 uint32_t timeout_ms = frame_duration_ms * 5 / 2; 753 btstack_run_loop_remove_timer(&next_packet_timer); 754 btstack_run_loop_set_timer(&next_packet_timer, timeout_ms); 755 btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); 756 btstack_run_loop_add_timer(&next_packet_timer); 757 758 uint32_t time_ms = btstack_run_loop_get_time_ms(); 759 if (btstack_time_delta(time_ms, last_samples_report_ms) >= 1000){ 760 last_samples_report_ms = time_ms; 761 printf("LC3 Frames: %4u - ", (int) (lc3_frames / num_bis)); 762 uint8_t i; 763 for (i=0;i<num_bis;i++){ 764 printf("%u ", frames_per_second[i]); 765 frames_per_second[i] = 0; 766 } 767 printf(" frames per second, dropped %u of %u\n", samples_dropped, samples_received); 768 samples_received = 0; 769 samples_dropped = 0; 770 } 771 } 772 773 last_packet_time_ms[bis_channel] = receive_time_ms; 774 last_packet_sequence[bis_channel] = packet_sequence_number; 775 } 776 777 static void show_usage(void){ 778 printf("\n--- LE Audio Broadcast Sink Test Console ---\n"); 779 printf("s - start scanning\n"); 780 #ifdef HAVE_LC3PLUS 781 printf("q - use LC3plus decoder if 10 ms ISO interval is used\n"); 782 #endif 783 printf("t - terminate BIS streams\n"); 784 printf("x - close files and exit\n"); 785 printf("---\n"); 786 } 787 788 static void stdin_process(char c){ 789 switch (c){ 790 case 's': 791 if (app_state != APP_IDLE) break; 792 start_scanning(); 793 break; 794 #ifdef HAVE_LC3PLUS 795 case 'q': 796 printf("Use LC3plus decoder for 10 ms ISO interval...\n"); 797 request_lc3plus_decoder = true; 798 break; 799 #endif 800 case 't': 801 switch (app_state){ 802 case APP_STREAMING: 803 case APP_W4_BIG_SYNC_ESTABLISHED: 804 app_state = APP_IDLE; 805 close_files(); 806 printf("Terminate BIG SYNC\n"); 807 gap_big_sync_terminate(big_handle); 808 break; 809 default: 810 break; 811 } 812 break; 813 case 'x': 814 close_files(); 815 printf("Shutdown...\n"); 816 hci_power_control(HCI_POWER_OFF); 817 break; 818 case '\n': 819 case '\r': 820 break; 821 default: 822 show_usage(); 823 break; 824 825 } 826 } 827 828 int btstack_main(int argc, const char * argv[]); 829 int btstack_main(int argc, const char * argv[]){ 830 (void) argv; 831 (void) argc; 832 833 // register for HCI events 834 hci_event_callback_registration.callback = &packet_handler; 835 hci_add_event_handler(&hci_event_callback_registration); 836 837 // register for ISO Packet 838 hci_register_iso_packet_handler(&iso_packet_handler); 839 840 // turn on! 841 hci_power_control(HCI_POWER_ON); 842 843 btstack_stdin_setup(stdin_process); 844 return 0; 845 } 846