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