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