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