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 "lc3.h" 70 #include "lc3_ehima.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 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 lc3_decoder_t * lc3_decoder; 155 static lc3_decoder_ehima_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 == 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 lc3_decoder_ehima_t * decoder_context = &decoder_contexts[channel]; 220 lc3_decoder = lc3_decoder_ehima_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 239 ad_context_t context; 240 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 241 uint8_t data_type = ad_iterator_get_data_type(&context); 242 uint8_t data_size = ad_iterator_get_data_len(&context); 243 const uint8_t * data = ad_iterator_get_data(&context); 244 uint16_t uuid; 245 switch (data_type){ 246 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 247 uuid = little_endian_read_16(data, 0); 248 if (uuid == ORG_BLUETOOTH_SERVICE_BASIC_AUDIO_ANNOUNCEMENT_SERVICE){ 249 have_base = true; 250 // Level 1: Group Level 251 const uint8_t * base_data = &data[2]; 252 uint16_t base_len = data_size - 2; 253 printf("BASE:\n"); 254 uint32_t presentation_delay = little_endian_read_24(base_data, 0); 255 printf("- presentation delay: %"PRIu32" us\n", presentation_delay); 256 uint8_t num_subgroups = base_data[3]; 257 printf("- num subgroups: %u\n", num_subgroups); 258 uint8_t i; 259 uint16_t offset = 4; 260 for (i=0;i<num_subgroups;i++){ 261 // Level 2: Subgroup Level 262 num_bis = base_data[offset++]; 263 printf(" - num bis[%u]: %u\n", i, num_bis); 264 // codec_id: coding format = 0x06, vendor and coded id = 0 265 offset += 5; 266 uint8_t codec_specific_configuration_length = base_data[offset++]; 267 const uint8_t * codec_specific_configuration = &base_data[offset]; 268 printf(" - codec specific config[%u]: ", i); 269 printf_hexdump(codec_specific_configuration, codec_specific_configuration_length); 270 // parse config to get sampling frequency and frame duration 271 uint8_t codec_offset = 0; 272 while ((codec_offset + 1) < codec_specific_configuration_length){ 273 uint8_t ltv_len = codec_specific_configuration[codec_offset++]; 274 uint8_t ltv_type = codec_specific_configuration[codec_offset]; 275 const uint32_t sampling_frequency_map[] = { 8000, 11025, 16000, 22050, 24000, 32000, 44100, 48000, 88200, 96000, 176400, 192000, 384000 }; 276 uint8_t sampling_frequency_index; 277 uint8_t frame_duration_index; 278 switch (ltv_type){ 279 case 0x01: // sampling frequency 280 sampling_frequency_index = codec_specific_configuration[codec_offset+1]; 281 // TODO: check range 282 sampling_frequency_hz = sampling_frequency_map[sampling_frequency_index - 1]; 283 printf(" - sampling frequency[%u]: %"PRIu32"\n", i, sampling_frequency_hz); 284 break; 285 case 0x02: // 0 = 7.5, 1 = 10 ms 286 frame_duration_index = codec_specific_configuration[codec_offset+1]; 287 frame_duration = (frame_duration_index == 0) ? LC3_FRAME_DURATION_7500US : LC3_FRAME_DURATION_10000US; 288 printf(" - frame duration[%u]: %s ms\n", i, (frame_duration == LC3_FRAME_DURATION_7500US) ? "7.5" : "10"); 289 break; 290 case 0x04: // octets per coding frame 291 octets_per_frame = little_endian_read_16(codec_specific_configuration, codec_offset+1); 292 printf(" - octets per codec frame[%u]: %u\n", i, octets_per_frame); 293 break; 294 default: 295 break; 296 } 297 codec_offset += ltv_len; 298 } 299 // 300 offset += codec_specific_configuration_length; 301 uint8_t metadata_length = base_data[offset++]; 302 const uint8_t * meta_data = &base_data[offset]; 303 offset += metadata_length; 304 printf(" - meta data[%u]: ", i); 305 printf_hexdump(meta_data, metadata_length); 306 uint8_t k; 307 for (k=0;k<num_bis;k++){ 308 // Level 3: BIS Level 309 uint8_t bis_index = base_data[offset++]; 310 printf(" - bis index[%u][%u]: %u\n", i, k, bis_index); 311 uint8_t codec_specific_configuration_length2 = base_data[offset++]; 312 const uint8_t * codec_specific_configuration2 = &base_data[offset]; 313 printf(" - codec specific config[%u][%u]: ", i, k); 314 printf_hexdump(codec_specific_configuration2, codec_specific_configuration_length2); 315 offset += codec_specific_configuration_length2; 316 } 317 } 318 } 319 break; 320 default: 321 break; 322 } 323 } 324 } 325 326 static void handle_big_info(const uint8_t * packet, uint16_t size){ 327 printf("BIG Info advertising report\n"); 328 sync_handle = hci_subevent_le_biginfo_advertising_report_get_sync_handle(packet); 329 have_big_info = true; 330 } 331 332 static void enter_create_big_sync(void){ 333 // stop scanning 334 gap_stop_scan(); 335 336 // init decoder 337 setup_lc3_decoder(); 338 339 printf("Configure: %u channels, sampling rate %u, samples per frame %u\n", num_bis, sampling_frequency_hz, number_samples_per_frame); 340 341 // create lc3 file 342 open_lc3_file(); 343 344 // create wav file 345 printf("WAV file: %s\n", filename_wav); 346 wav_writer_open(filename_wav, num_bis, sampling_frequency_hz); 347 348 // init playback buffer 349 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 350 351 // start playback 352 // PTS 8.2 sends stereo at half speed for stereo, for now playback at half speed 353 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 354 if (sink != NULL){ 355 uint32_t playback_speed; 356 if ((num_bis > 1) && pts_mode){ 357 playback_speed = sampling_frequency_hz / num_bis; 358 printf("PTS workaround: playback at %u hz\n", playback_speed); 359 } else { 360 playback_speed = sampling_frequency_hz; 361 }; 362 sink->init(num_bis, sampling_frequency_hz, le_audio_broadcast_sink_playback); 363 sink->start_stream(); 364 } 365 366 app_state = APP_CREATE_BIG_SYNC; 367 } 368 369 static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 370 UNUSED(channel); 371 if (packet_type != HCI_EVENT_PACKET) return; 372 unsigned int i; 373 switch (packet[0]) { 374 case BTSTACK_EVENT_STATE: 375 switch(btstack_event_state_get_state(packet)) { 376 case HCI_STATE_WORKING: 377 if (app_state != APP_W4_WORKING) break; 378 app_state = APP_W4_BROADCAST_ADV; 379 gap_set_scan_params(1, 0x30, 0x30, 0); 380 gap_start_scan(); 381 printf("Start scan..\n"); 382 break; 383 case HCI_STATE_OFF: 384 printf("Goodbye\n"); 385 exit(0); 386 break; 387 default: 388 break; 389 } 390 break; 391 case GAP_EVENT_EXTENDED_ADVERTISING_REPORT: 392 { 393 if (app_state != APP_W4_BROADCAST_ADV) break; 394 395 gap_event_extended_advertising_report_get_address(packet, remote); 396 uint8_t adv_size = gap_event_extended_advertising_report_get_data_length(packet); 397 const uint8_t * adv_data = gap_event_extended_advertising_report_get_data(packet); 398 399 ad_context_t context; 400 bool found = false; 401 remote_name[0] = '\0'; 402 uint16_t uuid; 403 for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)) { 404 uint8_t data_type = ad_iterator_get_data_type(&context); 405 uint8_t size = ad_iterator_get_data_len(&context); 406 const uint8_t *data = ad_iterator_get_data(&context); 407 switch (data_type){ 408 case BLUETOOTH_DATA_TYPE_SERVICE_DATA_16_BIT_UUID: 409 uuid = little_endian_read_16(data, 0); 410 if (uuid == ORG_BLUETOOTH_SERVICE_BROADCAST_AUDIO_ANNOUNCEMENT_SERVICE){ 411 found = true; 412 } 413 break; 414 case BLUETOOTH_DATA_TYPE_SHORTENED_LOCAL_NAME: 415 case BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME: 416 size = btstack_min(sizeof(remote_name) - 1, size); 417 memcpy(remote_name, data, size); 418 remote_name[size] = 0; 419 break; 420 default: 421 break; 422 } 423 } 424 if (!found) break; 425 remote_type = gap_event_extended_advertising_report_get_address_type(packet); 426 remote_sid = gap_event_extended_advertising_report_get_advertising_sid(packet); 427 pts_mode = strncmp("PTS-", remote_name, 4) == 0; 428 count_mode = strncmp("COUNT", remote_name, 5) == 0; 429 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); 430 // ignore other advertisements 431 gap_whitelist_add(remote_type, remote); 432 gap_set_scan_params(1, 0x30, 0x30, 1); 433 // sync to PA 434 gap_periodic_advertiser_list_clear(); 435 gap_periodic_advertiser_list_add(remote_type, remote, remote_sid); 436 app_state = APP_W4_PA_AND_BIG_INFO; 437 printf("Start Periodic Advertising Sync\n"); 438 gap_periodic_advertising_create_sync(0x01, remote_sid, remote_type, remote, 0, 1000, 0); 439 break; 440 } 441 442 case HCI_EVENT_LE_META: 443 switch(hci_event_le_meta_get_subevent_code(packet)) { 444 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_SYNC_ESTABLISHMENT: 445 printf("Periodic advertising sync established\n"); 446 break; 447 case HCI_SUBEVENT_LE_PERIODIC_ADVERTISING_REPORT: 448 if (have_base) break; 449 handle_periodic_advertisement(packet, size); 450 if (have_big_info){ 451 enter_create_big_sync(); 452 } 453 break; 454 case HCI_SUBEVENT_LE_BIGINFO_ADVERTISING_REPORT: 455 if (have_big_info) break; 456 handle_big_info(packet, size); 457 if (have_base){ 458 enter_create_big_sync(); 459 } 460 break; 461 case HCI_SUBEVENT_LE_BIG_SYNC_ESTABLISHED: 462 printf("BIG Sync Established\n"); 463 if (app_state == APP_W4_BIG_SYNC_ESTABLISHED){ 464 gap_stop_scan(); 465 gap_periodic_advertising_terminate_sync(sync_handle); 466 // update num_bis 467 num_bis = packet[16]; 468 for (i=0;i<num_bis;i++){ 469 bis_con_handles[i] = little_endian_read_16(packet, 17 + 2*i); 470 } 471 next_bis_index = 0; 472 app_state = APP_SET_ISO_PATHS; 473 } 474 break; 475 default: 476 break; 477 } 478 default: 479 break; 480 } 481 482 if (!hci_can_send_command_packet_now()) return; 483 484 const uint8_t broadcast_code[16] = { 0 }; 485 uint8_t bis_array[MAX_NUM_BIS]; 486 switch(app_state){ 487 case APP_CREATE_BIG_SYNC: 488 app_state = APP_W4_BIG_SYNC_ESTABLISHED; 489 printf("BIG Create Sync for BIS: "); 490 for (i=0;i<num_bis;i++){ 491 bis_array[i] = i + 1; 492 printf("%u ", bis_array[i]); 493 } 494 printf("\n"); 495 hci_send_cmd(&hci_le_big_create_sync, big_handle, sync_handle, 0, broadcast_code, 0, 100, num_bis, bis_array); 496 break; 497 case APP_SET_ISO_PATHS: 498 hci_send_cmd(&hci_le_setup_iso_data_path, bis_con_handles[next_bis_index++], 1, 0, 0, 0, 0, 0, 0, NULL); 499 if (next_bis_index == num_bis){ 500 app_state = APP_STREAMING; 501 last_samples_report_ms = btstack_run_loop_get_time_ms(); 502 } 503 break; 504 case APP_TERMINATE_BIG: 505 hci_send_cmd(&hci_le_big_terminate_sync, big_handle); 506 app_state = APP_IDLE; 507 printf("Shutdown...\n"); 508 hci_power_control(HCI_POWER_OFF); 509 break; 510 default: 511 break; 512 } 513 } 514 515 static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 516 517 uint16_t header = little_endian_read_16(packet, 0); 518 hci_con_handle_t con_handle = header & 0x0fff; 519 uint8_t pb_flag = (header >> 12) & 3; 520 uint8_t ts_flag = (header >> 14) & 1; 521 uint16_t iso_load_len = little_endian_read_16(packet, 2); 522 523 uint16_t offset = 4; 524 uint32_t time_stamp = 0; 525 if (ts_flag){ 526 uint32_t time_stamp = little_endian_read_32(packet, offset); 527 offset += 4; 528 } 529 530 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 531 offset += 2; 532 533 uint16_t header_2 = little_endian_read_16(packet, offset); 534 uint16_t iso_sdu_length = header_2 & 0x3fff; 535 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 536 offset += 2; 537 538 if (iso_sdu_length == 0) return; 539 540 // infer channel from con handle - only works for up to 2 channels 541 uint8_t bis_channel = (con_handle == bis_con_handles[0]) ? 0 : 1; 542 543 if (count_mode){ 544 // check for missing packet 545 uint16_t last_seq_no = last_packet_sequence[bis_channel]; 546 uint32_t now = btstack_run_loop_get_time_ms(); 547 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); 548 if (packet_missed){ 549 // print last packet 550 printf("\n"); 551 printf("%04x %10u %u ", last_seq_no, last_packet_time_ms[bis_channel], bis_channel); 552 printf_hexdump(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); 553 last_seq_no++; 554 555 printf(ANSI_COLOR_RED); 556 while (last_seq_no < packet_sequence_number){ 557 printf("%04x %u MISSING\n", last_seq_no, bis_channel); 558 last_seq_no++; 559 } 560 printf(ANSI_COLOR_RESET); 561 562 // print current packet 563 printf("%04x %10u %u ", packet_sequence_number, now, bis_channel); 564 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); 565 } 566 567 // cache current packet 568 last_packet_time_ms[bis_channel] = now; 569 last_packet_sequence[bis_channel] = packet_sequence_number; 570 memcpy(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); 571 572 } else { 573 574 if ((packet_sequence_number & 0x7c) == 0) { 575 printf("%04x %10u %u ", packet_sequence_number, btstack_run_loop_get_time_ms(), bis_channel); 576 printf_hexdump(&packet[offset], iso_sdu_length); 577 } 578 579 if (lc3_frames < DUMP_LEN_LC3_FRAMES) { 580 // store len header only for first bis 581 if (bis_channel == 0) { 582 uint8_t len_header[2]; 583 little_endian_store_16(len_header, 0, num_bis * iso_sdu_length); 584 write(dump_file, len_header, 2); 585 } 586 587 // store single channel codec frame 588 write(dump_file, &packet[offset], iso_sdu_length); 589 } 590 591 // decode codec frame 592 uint8_t tmp_BEC_detect; 593 uint8_t BFI = 0; 594 (void) lc3_decoder->decode(&decoder_contexts[bis_channel], &packet[offset], iso_sdu_length, BFI, 595 &pcm[bis_channel * MAX_SAMPLES_PER_FRAME], number_samples_per_frame, 596 &tmp_BEC_detect); 597 598 // interleave channel samples 599 if ((bis_channel + 1) == num_bis) { 600 uint16_t sample; 601 int16_t wav_frame[MAX_NUM_BIS]; 602 uint8_t wav_channel; 603 for (sample = 0; sample < number_samples_per_frame; sample++) { 604 for (wav_channel = 0; wav_channel < num_bis; wav_channel++) { 605 wav_frame[wav_channel] = pcm[wav_channel * MAX_SAMPLES_PER_FRAME + sample]; 606 } 607 608 // write wav sample 609 if (lc3_frames < DUMP_LEN_LC3_FRAMES) { 610 wav_writer_write_int16(num_bis, wav_frame); 611 } 612 613 // store sample in playback buffer 614 uint32_t bytes_to_store = num_bis * 2; 615 samples_received++; 616 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 617 btstack_ring_buffer_write(&playback_buffer, (uint8_t *) wav_frame, bytes_to_store); 618 } else { 619 samples_dropped++; 620 } 621 } 622 } 623 624 log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (num_bis * 2)); 625 626 lc3_frames++; 627 frames_per_second[bis_channel]++; 628 629 uint32_t time_ms = btstack_run_loop_get_time_ms(); 630 if (btstack_time_delta(time_ms, last_samples_report_ms) > 1000){ 631 last_samples_report_ms = time_ms; 632 printf("LC3 Frames: %4u - ", lc3_frames / num_bis); 633 uint8_t i; 634 for (i=0;i<num_bis;i++){ 635 printf("%u ", frames_per_second[i]); 636 frames_per_second[i] = 0; 637 } 638 printf(" frames per second, dropped %u of %u\n", samples_dropped, samples_received); 639 samples_received = 0; 640 samples_dropped = 0; 641 } 642 643 if (lc3_frames == DUMP_LEN_LC3_FRAMES){ 644 close_files(); 645 } 646 } 647 } 648 649 static void show_usage(void){ 650 printf("\n--- LE Audio Broadcast Sink Test Console ---\n"); 651 printf("x - close files and exit\n"); 652 printf("---\n"); 653 } 654 655 static void stdin_process(char c){ 656 switch (c){ 657 case 'x': 658 close_files(); 659 printf("Shutdown...\n"); 660 hci_power_control(HCI_POWER_OFF); 661 break; 662 case '\n': 663 case '\r': 664 break; 665 default: 666 show_usage(); 667 break; 668 669 } 670 } 671 672 int btstack_main(int argc, const char * argv[]); 673 int btstack_main(int argc, const char * argv[]){ 674 (void) argv; 675 (void) argc; 676 677 // register for HCI events 678 hci_event_callback_registration.callback = &packet_handler; 679 hci_add_event_handler(&hci_event_callback_registration); 680 681 // register for ISO Packet 682 hci_register_iso_packet_handler(&iso_packet_handler); 683 684 // turn on! 685 hci_power_control(HCI_POWER_ON); 686 687 btstack_stdin_setup(stdin_process); 688 return 0; 689 } 690