1 /* 2 * Copyright (C) {copyright_year} 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 BLUEKITCHEN 24 * GMBH 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_demo_util_sink.c" 39 40 #include <stdio.h> 41 #include <inttypes.h> 42 43 #include "le_audio_demo_util_sink.h" 44 45 #include "btstack_bool.h" 46 #include "btstack_config.h" 47 #include <btstack_debug.h> 48 #include <stdio.h> 49 50 #include "hci.h" 51 #include "btstack_audio.h" 52 #include "btstack_lc3_google.h" 53 #include "btstack_lc3plus_fraunhofer.h" 54 55 #include "btstack_sample_rate_compensation.h" 56 #include "btstack_resample.h" 57 58 #include "hxcmod.h" 59 #include "mods/mod.h" 60 61 #include "btstack_ring_buffer.h" 62 #ifdef HAVE_POSIX_FILE_IO 63 #include "wav_util.h" 64 #endif 65 66 //#define DEBUG_PLC 67 #ifdef DEBUG_PLC 68 #define printf_plc(...) { \ 69 printf(__VA_ARGS__); \ 70 log_info(__VA_ARGS__);\ 71 } 72 #else 73 #define printf_plc(...) (void)(0); 74 #endif 75 76 #define MAX_CHANNELS 2 77 #define MAX_SAMPLES_PER_FRAME 480 78 #define MAX_LC3_FRAME_BYTES 155 79 80 // playback 81 #define MAX_NUM_LC3_FRAMES 15 82 #define MAX_BYTES_PER_SAMPLE 4 83 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_CHANNELS * MAX_BYTES_PER_SAMPLE) 84 #define PLAYBACK_START_MS (MAX_NUM_LC3_FRAMES * 20 / 3) 85 86 // analysis 87 #define PACKET_PREFIX_LEN 10 88 89 #define ANSI_COLOR_RED "\x1b[31m" 90 #define ANSI_COLOR_GREEN "\x1b[32m" 91 #define ANSI_COLOR_YELLOW "\x1b[33m" 92 #define ANSI_COLOR_BLUE "\x1b[34m" 93 #define ANSI_COLOR_MAGENTA "\x1b[35m" 94 #define ANSI_COLOR_CYAN "\x1b[36m" 95 #define ANSI_COLOR_RESET "\x1b[0m" 96 97 // statistics 98 static uint16_t last_packet_sequence[MAX_CHANNELS]; 99 static uint32_t last_packet_time_ms[MAX_CHANNELS]; 100 static uint8_t last_packet_prefix[MAX_CHANNELS * PACKET_PREFIX_LEN]; 101 102 // SINK 103 104 static enum { 105 LE_AUDIO_SINK_IDLE, 106 LE_AUDIO_SINK_INIT, 107 LE_AUDIO_SINK_CONFIGURED, 108 } le_audio_demo_util_sink_state = LE_AUDIO_SINK_IDLE; 109 110 static const char * le_audio_demo_sink_filename_wav; 111 static btstack_sample_rate_compensation_t sample_rate_compensation; 112 static uint32_t le_audio_demo_sink_received_samples; 113 static btstack_resample_t resample_instance; 114 static bool sink_receive_streaming; 115 116 static int16_t pcm_resample[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME * 2]; 117 118 119 static btstack_lc3_frame_duration_t le_audio_demo_sink_frame_duration; 120 static hci_iso_type_t le_audio_demo_sink_type; 121 122 static uint32_t le_audio_demo_sink_sampling_frequency_hz; 123 static uint16_t le_audio_demo_sink_num_samples_per_frame; 124 static uint8_t le_audio_demo_sink_num_streams; 125 static uint8_t le_audio_demo_sink_num_channels_per_stream; 126 static uint8_t le_audio_demo_sink_num_channels; 127 static uint16_t le_audio_demo_sink_octets_per_frame; 128 static uint16_t le_audio_demo_sink_iso_interval_1250us; 129 static uint8_t le_audio_demo_sink_flush_timeout; 130 static uint8_t le_audio_demo_sink_pre_transmission_offset; 131 132 // playback 133 static uint16_t playback_start_threshold_bytes; 134 static bool playback_active; 135 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; 136 static btstack_ring_buffer_t playback_buffer; 137 138 // PLC 139 static bool stream_last_packet_received[MAX_CHANNELS]; 140 static uint16_t stream_last_packet_sequence[MAX_CHANNELS]; 141 static uint16_t group_last_packet_sequence; 142 static bool group_last_packet_received; 143 static uint16_t plc_timeout_initial_ms; 144 static uint16_t plc_timeout_subsequent_ms; 145 146 static uint32_t le_audio_demo_sink_lc3_frames; 147 static uint32_t le_audio_demo_sink_zero_frames; 148 static uint32_t samples_received; 149 static uint32_t samples_played; 150 static uint32_t samples_dropped; 151 152 static btstack_timer_source_t next_packet_timer; 153 154 // lc3 decoder 155 static bool le_audio_demo_lc3plus_decoder_requested = false; 156 static const btstack_lc3_decoder_t * lc3_decoder; 157 static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; 158 static bool have_pcm[MAX_CHANNELS]; 159 160 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS]; 161 #ifdef HAVE_LC3PLUS 162 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_CHANNELS]; 163 #endif 164 static void * decoder_contexts[MAX_CHANNELS]; 165 166 static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_samples){ 167 // called from lower-layer but guaranteed to be on main thread 168 log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); 169 170 samples_played += num_samples; 171 172 uint32_t bytes_needed = num_samples * le_audio_demo_sink_num_channels * 2; 173 if (playback_active == false){ 174 if (btstack_ring_buffer_bytes_available(&playback_buffer) >= playback_start_threshold_bytes) { 175 log_info("Playback started"); 176 playback_active = true; 177 } 178 } else { 179 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)) { 180 log_info("Playback underrun"); 181 printf("Playback Underrun\n"); 182 // empty buffer 183 uint32_t bytes_read; 184 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 185 playback_active = false; 186 } 187 } 188 189 if (playback_active){ 190 uint32_t bytes_read; 191 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 192 btstack_assert(bytes_read == bytes_needed); 193 } else { 194 memset(buffer, 0, bytes_needed); 195 } 196 } 197 198 static void store_samples_in_ringbuffer(void){ 199 // check if we have all channels 200 uint8_t channel; 201 for (channel = 0; channel < le_audio_demo_sink_num_channels; channel++){ 202 if (have_pcm[channel] == false) return; 203 } 204 #ifdef HAVE_POSIX_FILE_IO 205 // write wav samples 206 wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, pcm); 207 #endif 208 209 // count for samplerate compensation 210 le_audio_demo_sink_received_samples += le_audio_demo_sink_num_samples_per_frame; 211 212 // store samples in playback buffer 213 samples_received += le_audio_demo_sink_num_samples_per_frame; 214 uint32_t resampled_frames = btstack_resample_block(&resample_instance, pcm, le_audio_demo_sink_num_samples_per_frame, pcm_resample); 215 uint32_t bytes_to_store = resampled_frames * le_audio_demo_sink_num_channels * 2; 216 217 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 218 btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm_resample, bytes_to_store); 219 log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); 220 } else { 221 printf("Samples dropped\n"); 222 samples_dropped += le_audio_demo_sink_num_samples_per_frame; 223 } 224 memset(have_pcm, 0, sizeof(have_pcm)); 225 } 226 227 static void plc_do(uint8_t stream_index) { 228 // inject packet 229 uint8_t tmp_BEC_detect; 230 uint8_t BFI = 1; 231 uint8_t i; 232 for (i = 0; i < le_audio_demo_sink_num_channels_per_stream; i++){ 233 uint8_t effective_channel = stream_index + i; 234 (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], NULL, BFI, 235 &pcm[effective_channel], le_audio_demo_sink_num_channels, 236 &tmp_BEC_detect); 237 have_pcm[i] = true; 238 } 239 // and store in ringbuffer when PCM for all channels is available 240 store_samples_in_ringbuffer(); 241 } 242 243 // 244 // Perform PLC for packets missing in previous intervals 245 // 246 // assumptions: 247 // - packet sequence number is monotonic increasing 248 // - if packet with seq nr x is received, all packets with smaller seq number are either received or missed 249 static void plc_check(uint16_t packet_sequence_number) { 250 while (group_last_packet_sequence != packet_sequence_number){ 251 uint8_t i; 252 for (i=0;i<le_audio_demo_sink_num_streams;i++){ 253 // deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start 254 if (stream_last_packet_received[i] == false){ 255 printf_plc("- ISO #%u, very first packet missing\n", i); 256 have_pcm[i] = true; 257 store_samples_in_ringbuffer(); 258 259 stream_last_packet_received[i] = true; 260 stream_last_packet_sequence[i] = group_last_packet_sequence; 261 continue; 262 } 263 264 // missing packet if group sequence counter is higher than stream sequence counter 265 if (btstack_time16_delta(group_last_packet_sequence, stream_last_packet_sequence[i]) > 0) { 266 printf_plc("- ISO #%u, PLC for %u\n", i, group_last_packet_sequence); 267 #ifndef DEBUG_PLC 268 log_info("PLC for packet 0x%04x, stream #%u", group_last_packet_sequence, i); 269 #endif 270 plc_do(i); 271 btstack_assert((stream_last_packet_sequence[i] + 1) == group_last_packet_sequence); 272 stream_last_packet_sequence[i] = group_last_packet_sequence; 273 } 274 } 275 group_last_packet_sequence++; 276 } 277 } 278 279 static void plc_timeout(btstack_timer_source_t * timer) { 280 // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets 281 btstack_run_loop_set_timer(timer, plc_timeout_subsequent_ms); 282 btstack_run_loop_set_timer_handler(timer, plc_timeout); 283 btstack_run_loop_add_timer(timer); 284 285 switch (le_audio_demo_sink_type){ 286 case HCI_ISO_TYPE_CIS: 287 // assume no packet received in iso interval => FT packets missed 288 printf_plc("PLC: timeout cis, group %u, FT %u", group_last_packet_sequence, le_audio_demo_sink_flush_timeout); 289 plc_check(group_last_packet_sequence + le_audio_demo_sink_flush_timeout); 290 break; 291 case HCI_ISO_TYPE_BIS: 292 // assume PTO not used => 1 packet missed 293 plc_check(group_last_packet_sequence + 1); 294 break; 295 default: 296 btstack_unreachable(); 297 break; 298 } 299 } 300 301 void le_audio_demo_util_sink_init(const char * filename_wav){ 302 le_audio_demo_sink_filename_wav = filename_wav; 303 le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT; 304 } 305 306 void le_audio_demo_util_sink_enable_lc3plus(bool enable){ 307 le_audio_demo_lc3plus_decoder_requested = enable; 308 } 309 310 static void setup_lc3_decoder(bool use_lc3plus_decoder){ 311 uint8_t channel; 312 for (channel = 0 ; channel < le_audio_demo_sink_num_channels ; channel++){ 313 // pick decoder 314 void * decoder_context = NULL; 315 #ifdef HAVE_LC3PLUS 316 if (use_lc3plus_decoder){ 317 decoder_context = &fraunhofer_decoder_contexts[channel]; 318 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); 319 } 320 else 321 #endif 322 { 323 decoder_context = &google_decoder_contexts[channel]; 324 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); 325 } 326 decoder_contexts[channel] = decoder_context; 327 lc3_decoder->configure(decoder_context, le_audio_demo_sink_sampling_frequency_hz, le_audio_demo_sink_frame_duration, le_audio_demo_sink_octets_per_frame); 328 } 329 btstack_assert(le_audio_demo_sink_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 330 } 331 332 void le_audio_demo_util_sink_configure_general(uint8_t num_streams, uint8_t num_channels_per_stream, 333 uint32_t sampling_frequency_hz, 334 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 335 uint32_t iso_interval_1250us) { 336 le_audio_demo_sink_sampling_frequency_hz = sampling_frequency_hz; 337 le_audio_demo_sink_frame_duration = frame_duration; 338 le_audio_demo_sink_octets_per_frame = octets_per_frame; 339 le_audio_demo_sink_iso_interval_1250us = iso_interval_1250us; 340 le_audio_demo_sink_num_streams = num_streams; 341 le_audio_demo_sink_num_channels_per_stream = num_channels_per_stream; 342 343 sink_receive_streaming = false; 344 le_audio_demo_util_sink_state = LE_AUDIO_SINK_CONFIGURED; 345 346 le_audio_demo_sink_num_channels = le_audio_demo_sink_num_streams * le_audio_demo_sink_num_channels_per_stream; 347 btstack_assert((le_audio_demo_sink_num_channels == 1) || (le_audio_demo_sink_num_channels == 2)); 348 349 le_audio_demo_sink_lc3_frames = 0; 350 351 group_last_packet_received = false; 352 uint8_t i; 353 for (i=0;i<MAX_CHANNELS;i++){ 354 stream_last_packet_received[i] = false; 355 have_pcm[i] = false; 356 } 357 358 le_audio_demo_sink_num_samples_per_frame = btstack_lc3_samples_per_frame(le_audio_demo_sink_sampling_frequency_hz, le_audio_demo_sink_frame_duration); 359 360 // switch to lc3plus if requested and possible 361 bool use_lc3plus_decoder = le_audio_demo_lc3plus_decoder_requested && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); 362 363 // init decoder 364 setup_lc3_decoder(use_lc3plus_decoder); 365 366 printf("Configure: %u streams, %u channels per stream, sampling rate %u, samples per frame %u, lc3plus %u\n", 367 num_streams, num_channels_per_stream, sampling_frequency_hz, le_audio_demo_sink_num_samples_per_frame, use_lc3plus_decoder); 368 369 #ifdef HAVE_POSIX_FILE_IO 370 // create wav file 371 printf("WAV file: %s\n", le_audio_demo_sink_filename_wav); 372 wav_writer_open(le_audio_demo_sink_filename_wav, le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz); 373 #endif 374 375 // init playback buffer 376 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 377 378 // calc start threshold in bytes for PLAYBACK_START_MS 379 playback_start_threshold_bytes = (sampling_frequency_hz / 1000 * PLAYBACK_START_MS) * le_audio_demo_sink_num_channels * 2; 380 381 // sample rate compensation 382 le_audio_demo_sink_received_samples = 0; 383 384 // start playback 385 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 386 if (sink != NULL){ 387 btstack_sample_rate_compensation_reset( &sample_rate_compensation, btstack_run_loop_get_time_ms() ); 388 btstack_resample_init(&resample_instance, le_audio_demo_sink_num_channels_per_stream); 389 sink->init(le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz, le_audio_connection_sink_playback); 390 sink->start_stream(); 391 } 392 } 393 394 void le_audio_demo_util_sink_configure_unicast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 395 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 396 uint32_t iso_interval_1250us, uint8_t flush_timeout){ 397 le_audio_demo_sink_type = HCI_ISO_TYPE_CIS; 398 le_audio_demo_sink_flush_timeout = flush_timeout; 399 400 // set playback start: FT * ISO Interval + max(10 ms, 1/2 ISO Interval) 401 uint16_t playback_start_ms = flush_timeout * (iso_interval_1250us * 5 / 4) + btstack_max(10, iso_interval_1250us * 5 / 8); 402 uint16_t playback_start_samples = sampling_frequency_hz / 1000 * playback_start_ms; 403 playback_start_threshold_bytes = playback_start_samples * num_streams * num_channels_per_stream * 2; 404 printf("Playback: start %u ms (%u samples, %u bytes)\n", playback_start_ms, playback_start_samples, playback_start_threshold_bytes); 405 406 // set subsequent plc timeout: FT * ISO Interval 407 plc_timeout_subsequent_ms = flush_timeout * iso_interval_1250us * 5 / 4; 408 409 // set initial plc timeout:FT * ISO Interval + 4 ms 410 plc_timeout_initial_ms = plc_timeout_subsequent_ms + 4; 411 412 printf("PLC: initial timeout %u ms\n", plc_timeout_initial_ms); 413 printf("PLC: subsequent timeout %u ms\n", plc_timeout_subsequent_ms); 414 415 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz, 416 frame_duration, octets_per_frame, iso_interval_1250us); 417 } 418 419 void le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 420 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 421 uint32_t iso_interval_1250us, uint8_t pre_transmission_offset) { 422 le_audio_demo_sink_type = HCI_ISO_TYPE_BIS; 423 le_audio_demo_sink_pre_transmission_offset = pre_transmission_offset; 424 425 // set playback start: ISO Interval + 10 ms 426 uint16_t playback_start_ms = (iso_interval_1250us * 5 / 4) + 10; 427 uint16_t playback_start_samples = sampling_frequency_hz / 1000 * playback_start_ms; 428 playback_start_threshold_bytes = playback_start_samples * num_streams * num_channels_per_stream * 2; 429 printf("Playback: start %u ms (%u samples, %u bytes)\n", playback_start_ms, playback_start_samples, playback_start_threshold_bytes); 430 431 // set subsequent plc timeout: ISO Interval 432 plc_timeout_subsequent_ms = iso_interval_1250us * 5 / 4; 433 434 // set initial plc timeout: ISO Interval + 4 ms 435 plc_timeout_initial_ms = plc_timeout_subsequent_ms + 4; 436 437 printf("PLC: initial timeout %u ms\n", plc_timeout_initial_ms); 438 printf("PLC: subsequent timeout %u ms\n", plc_timeout_subsequent_ms); 439 440 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz, frame_duration, octets_per_frame, iso_interval_1250us); 441 } 442 443 void le_audio_demo_util_sink_count(uint8_t stream_index, uint8_t *packet, uint16_t size) { 444 // check for missing packet 445 uint16_t header = little_endian_read_16(packet, 0); 446 uint8_t ts_flag = (header >> 14) & 1; 447 448 uint16_t offset = 4; 449 uint32_t time_stamp = 0; 450 if (ts_flag){ 451 time_stamp = little_endian_read_32(packet, offset); 452 offset += 4; 453 } 454 455 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 456 457 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 458 offset += 4; 459 460 uint16_t last_seq_no = last_packet_sequence[stream_index]; 461 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); 462 if (packet_missed){ 463 // print last packet 464 printf("\n"); 465 printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[stream_index], stream_index); 466 printf_hexdump(&last_packet_prefix[le_audio_demo_sink_num_streams*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); 467 last_seq_no++; 468 469 printf(ANSI_COLOR_RED); 470 while (last_seq_no < packet_sequence_number){ 471 printf("%04x %u MISSING\n", last_seq_no, stream_index); 472 last_seq_no++; 473 } 474 printf(ANSI_COLOR_RESET); 475 476 // print current packet 477 printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, stream_index); 478 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); 479 } 480 481 // cache current packet 482 memcpy(&last_packet_prefix[le_audio_demo_sink_num_streams*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); 483 } 484 485 void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint16_t size) { 486 487 if (le_audio_demo_util_sink_state != LE_AUDIO_SINK_CONFIGURED) return; 488 489 uint16_t header = little_endian_read_16(packet, 0); 490 hci_con_handle_t con_handle = header & 0x0fff; 491 uint8_t pb_flag = (header >> 12) & 3; 492 uint8_t ts_flag = (header >> 14) & 1; 493 uint16_t iso_load_len = little_endian_read_16(packet, 2); 494 495 uint16_t offset = 4; 496 uint32_t time_stamp = 0; 497 if (ts_flag){ 498 time_stamp = little_endian_read_32(packet, offset); 499 offset += 4; 500 } 501 502 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 503 504 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 505 offset += 2; 506 507 uint16_t header_2 = little_endian_read_16(packet, offset); 508 uint16_t iso_sdu_length = header_2 & 0x3fff; 509 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 510 offset += 2; 511 512 // avoid warning for (yet) unused fields 513 UNUSED(con_handle); 514 UNUSED(pb_flag); 515 UNUSED(iso_load_len); 516 UNUSED(packet_status_flag); 517 UNUSED(time_stamp); 518 519 // start with first packet on first stream 520 if (group_last_packet_received == false){ 521 if (stream_index != 0){ 522 printf("Ignore first packet for second stream\n"); 523 return; 524 } 525 group_last_packet_received = true; 526 group_last_packet_sequence = packet_sequence_number; 527 } 528 529 if (stream_last_packet_received[stream_index]) { 530 printf_plc("ISO #%u, receive %u\n", stream_index, packet_sequence_number); 531 532 int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, 533 stream_last_packet_sequence[stream_index]); 534 if (packet_sequence_delta < 1) { 535 // drop delayed packet that had already been generated by PLC 536 printf_plc("- dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n", 537 packet_sequence_number, stream_last_packet_sequence[stream_index]); 538 return; 539 } 540 // simple check 541 if (packet_sequence_number != stream_last_packet_sequence[stream_index] + 1) { 542 printf_plc("- ISO #%u, missing %u\n", stream_index, stream_last_packet_sequence[stream_index] + 1); 543 } 544 } else { 545 printf_plc("ISO %u, first packet seq number %u\n", stream_index, packet_sequence_number); 546 stream_last_packet_received[stream_index] = true; 547 } 548 549 if (sink_receive_streaming){ 550 plc_check(packet_sequence_number); 551 } 552 553 // either empty packets or num channels * num octets 554 if ((iso_sdu_length != 0) && (iso_sdu_length != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame)) { 555 printf("ISO Length %u != %u * %u\n", iso_sdu_length, le_audio_demo_sink_num_channels_per_stream, le_audio_demo_sink_octets_per_frame); 556 log_info("ISO Length %u != %u * %u", iso_sdu_length, le_audio_demo_sink_num_channels_per_stream, le_audio_demo_sink_octets_per_frame); 557 return; 558 } 559 560 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 561 if( (sink != NULL) && (iso_sdu_length>0)) { 562 if (!sink_receive_streaming && playback_active) { 563 btstack_sample_rate_compensation_init(&sample_rate_compensation, receive_time_ms, 564 le_audio_demo_sink_sampling_frequency_hz, FLOAT_TO_Q15(1.f)); 565 sink_receive_streaming = true; 566 } 567 } 568 569 if (iso_sdu_length == 0) { 570 if (sink_receive_streaming){ 571 // empty packet -> generate silence 572 memset(pcm, 0, sizeof(pcm)); 573 uint8_t i; 574 for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++) { 575 have_pcm[stream_index + i] = true; 576 } 577 le_audio_demo_sink_zero_frames++; 578 // pause detection (1000 ms for 10 ms, 750 ms for 7.5 ms frames) 579 if (le_audio_demo_sink_zero_frames > 100){ 580 printf("Pause detected, stopping audio\n"); 581 log_info("Pause detected, stopping audio"); 582 // pause/reset audio 583 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 584 sink_receive_streaming = false; 585 playback_active = false; 586 } 587 } 588 } else { 589 // regular packet -> decode codec frame 590 le_audio_demo_sink_zero_frames = 0; 591 uint8_t i; 592 for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){ 593 uint8_t tmp_BEC_detect; 594 uint8_t BFI = 0; 595 uint8_t effective_channel = stream_index + i; 596 (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &packet[offset], BFI, 597 &pcm[effective_channel], le_audio_demo_sink_num_channels, 598 &tmp_BEC_detect); 599 offset += le_audio_demo_sink_octets_per_frame; 600 have_pcm[stream_index + i] = true; 601 } 602 } 603 604 store_samples_in_ringbuffer(); 605 606 if( (sink != NULL) && (iso_sdu_length>0)) { 607 if( sink_receive_streaming ) { 608 uint32_t resampling_factor = btstack_sample_rate_compensation_update( &sample_rate_compensation, receive_time_ms, 609 le_audio_demo_sink_received_samples, sink->get_samplerate() ); 610 btstack_resample_set_factor(&resample_instance, resampling_factor); 611 le_audio_demo_sink_received_samples = 0; 612 } 613 } 614 615 le_audio_demo_sink_lc3_frames++; 616 617 // PLC 618 btstack_run_loop_remove_timer(&next_packet_timer); 619 btstack_run_loop_set_timer(&next_packet_timer, plc_timeout_initial_ms); 620 btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); 621 btstack_run_loop_add_timer(&next_packet_timer); 622 623 if (samples_received >= le_audio_demo_sink_sampling_frequency_hz){ 624 printf("LC3 Frames: %4u - samples received %5u, played %5u, dropped %5u\n", le_audio_demo_sink_lc3_frames, samples_received, samples_played, samples_dropped); 625 samples_received = 0; 626 samples_dropped = 0; 627 samples_played = 0; 628 } 629 630 stream_last_packet_sequence[stream_index] = packet_sequence_number; 631 } 632 633 /** 634 * @brief Close sink: close wav file, stop playback 635 */ 636 void le_audio_demo_util_sink_close(void){ 637 #ifdef HAVE_POSIX_FILE_IO 638 printf("Close WAV file\n"); 639 wav_writer_close(); 640 #endif 641 // stop playback 642 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 643 if (sink != NULL){ 644 sink->stop_stream(); 645 } 646 le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT; 647 sink_receive_streaming = false; 648 // stop timer 649 btstack_run_loop_remove_timer(&next_packet_timer); 650 } 651