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