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 #include "btstack_fsm.h" 58 59 #include "hxcmod.h" 60 #include "mods/mod.h" 61 62 #include "btstack_ring_buffer.h" 63 #ifdef HAVE_POSIX_FILE_IO 64 #include "wav_util.h" 65 #endif 66 67 #define MAX_CHANNELS 2 68 #define MAX_SAMPLES_PER_FRAME 480 69 #define MAX_LC3_FRAME_BYTES 155 70 71 // playback 72 #define MAX_NUM_LC3_FRAMES (15*2) 73 #define MAX_BYTES_PER_SAMPLE 4 74 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_CHANNELS * MAX_BYTES_PER_SAMPLE) 75 #define PLAYBACK_START_MS (MAX_NUM_LC3_FRAMES * 20 / 3) 76 77 // analysis 78 #define PACKET_PREFIX_LEN 10 79 80 #define ANSI_COLOR_RED "\x1b[31m" 81 #define ANSI_COLOR_GREEN "\x1b[32m" 82 #define ANSI_COLOR_YELLOW "\x1b[33m" 83 #define ANSI_COLOR_BLUE "\x1b[34m" 84 #define ANSI_COLOR_MAGENTA "\x1b[35m" 85 #define ANSI_COLOR_CYAN "\x1b[36m" 86 #define ANSI_COLOR_RESET "\x1b[0m" 87 88 // statistics 89 static uint16_t last_packet_sequence[MAX_CHANNELS]; 90 static uint32_t last_packet_time_ms[MAX_CHANNELS]; 91 static uint8_t last_packet_prefix[MAX_CHANNELS * PACKET_PREFIX_LEN]; 92 93 // SINK 94 95 static enum { 96 LE_AUDIO_SINK_IDLE, 97 LE_AUDIO_SINK_INIT, 98 LE_AUDIO_SINK_CONFIGURED, 99 } le_audio_demo_util_sink_state = LE_AUDIO_SINK_IDLE; 100 101 static const char * le_audio_demo_sink_filename_wav; 102 static btstack_sample_rate_compensation_t sample_rate_compensation; 103 static uint32_t le_audio_demo_sink_received_samples; 104 static btstack_resample_t resample_instance; 105 static bool sink_receive_streaming; 106 107 static int16_t pcm_resample[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME * 2]; 108 109 110 static btstack_lc3_frame_duration_t le_audio_demo_sink_frame_duration; 111 static hci_iso_type_t le_audio_demo_sink_type; 112 113 static uint32_t le_audio_demo_sink_sampling_frequency_hz; 114 static uint16_t le_audio_demo_sink_num_samples_per_frame; 115 static uint8_t le_audio_demo_sink_num_streams; 116 static uint8_t le_audio_demo_sink_num_channels_per_stream; 117 static uint8_t le_audio_demo_sink_num_channels; 118 static uint16_t le_audio_demo_sink_octets_per_frame; 119 static uint16_t le_audio_demo_sink_iso_interval_1250us; 120 static uint8_t le_audio_demo_sink_flush_timeout; 121 static uint8_t le_audio_demo_sink_pre_transmission_offset; 122 123 // playback 124 static uint16_t playback_start_threshold_bytes; 125 static bool playback_active; 126 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; 127 static btstack_ring_buffer_t playback_buffer; 128 129 // PLC 130 static uint32_t le_audio_demo_sink_lc3_frames; 131 static uint32_t samples_received; 132 static uint32_t samples_played; 133 static uint32_t samples_dropped; 134 135 // Audio FSM 136 #define TRAN( target ) btstack_fsm_transit( &me->super, (btstack_fsm_state_handler_t)target ) 137 138 typedef struct { 139 btstack_fsm_t super; 140 uint32_t receive_time_ms; 141 uint32_t last_receive_time_ms; 142 uint32_t zero_frames; 143 uint32_t have_pcm; 144 uint32_t received_samples; 145 } audio_processing_t; 146 147 typedef struct { 148 btstack_fsm_event_t super; 149 uint16_t sequence_number; 150 uint16_t size; 151 uint32_t receive_time_ms; 152 uint8_t *data; 153 uint8_t stream; 154 } data_event_t; 155 156 typedef struct { 157 btstack_fsm_event_t super; 158 uint32_t time_ms; 159 } time_event_t; 160 161 audio_processing_t audio_processing; 162 163 enum EventSignals { 164 DATA_SIG = BTSTACK_FSM_USER_SIG, 165 TIME_SIG 166 }; 167 168 #define AUDIO_FSM_DEBUGx 169 #ifdef AUDIO_FSM_DEBUG 170 #define ENUM_TO_TEXT(sig) [sig] = #sig 171 #define audio_fsm_debug(format, ...) \ 172 printf( format __VA_OPT__(,) __VA_ARGS__) 173 174 const char * const sigToString[] = { 175 ENUM_TO_TEXT(BTSTACK_FSM_INIT_SIG), 176 ENUM_TO_TEXT(BTSTACK_FSM_ENTRY_SIG), 177 ENUM_TO_TEXT(BTSTACK_FSM_EXIT_SIG), 178 ENUM_TO_TEXT(DATA_SIG), 179 ENUM_TO_TEXT(TIME_SIG), 180 }; 181 #else 182 #define audio_fsm_debug(...) 183 #endif 184 185 static btstack_fsm_state_t audio_processing_initial( audio_processing_t * const me, btstack_fsm_event_t const * const e ); 186 static btstack_fsm_state_t audio_processing_waiting( audio_processing_t * const me, btstack_fsm_event_t const * const e ); 187 static btstack_fsm_state_t audio_processing_streaming( audio_processing_t * const me, btstack_fsm_event_t const * const e ); 188 static bool audio_processing_is_streaming( audio_processing_t * const me ); 189 190 static btstack_timer_source_t next_packet_timer; 191 192 // lc3 decoder 193 static bool le_audio_demo_lc3plus_decoder_requested = false; 194 static const btstack_lc3_decoder_t * lc3_decoder; 195 static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; 196 197 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS]; 198 #ifdef HAVE_LC3PLUS 199 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_CHANNELS]; 200 #endif 201 static void * decoder_contexts[MAX_CHANNELS]; 202 203 static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_samples){ 204 // called from lower-layer but guaranteed to be on main thread 205 log_info("Playback: need %u, have %" PRIu32 "", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); 206 207 samples_played += num_samples; 208 209 uint32_t bytes_needed = num_samples * le_audio_demo_sink_num_channels * 2; 210 if (playback_active == false){ 211 if (btstack_ring_buffer_bytes_available(&playback_buffer) >= playback_start_threshold_bytes) { 212 log_info("Playback started"); 213 printf("Playback started\n"); 214 playback_active = true; 215 } 216 } else { 217 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)) { 218 if( audio_processing_is_streaming( &audio_processing ) ) { 219 log_info("Playback underrun"); 220 printf("Playback Underrun\n"); 221 } else { 222 log_info("Playback stopped"); 223 printf("Playback stopped\n"); 224 } 225 // empty buffer 226 uint32_t bytes_read; 227 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 228 playback_active = false; 229 } 230 } 231 232 if (playback_active){ 233 uint32_t bytes_read; 234 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 235 btstack_assert(bytes_read == bytes_needed); 236 } else { 237 memset(buffer, 0, bytes_needed); 238 } 239 } 240 241 void le_audio_demo_util_sink_enable_lc3plus(bool enable){ 242 le_audio_demo_lc3plus_decoder_requested = enable; 243 } 244 245 static void setup_lc3_decoder(bool use_lc3plus_decoder){ 246 UNUSED(use_lc3plus_decoder); 247 248 uint8_t channel; 249 for (channel = 0 ; channel < le_audio_demo_sink_num_channels ; channel++){ 250 // pick decoder 251 void * decoder_context = NULL; 252 #ifdef HAVE_LC3PLUS 253 if (use_lc3plus_decoder){ 254 decoder_context = &fraunhofer_decoder_contexts[channel]; 255 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); 256 } 257 else 258 #endif 259 { 260 decoder_context = &google_decoder_contexts[channel]; 261 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); 262 } 263 decoder_contexts[channel] = decoder_context; 264 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); 265 } 266 btstack_assert(le_audio_demo_sink_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 267 } 268 269 void le_audio_demo_util_sink_configure_general(uint8_t num_streams, uint8_t num_channels_per_stream, 270 uint32_t sampling_frequency_hz, 271 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 272 uint32_t iso_interval_1250us) { 273 le_audio_demo_sink_sampling_frequency_hz = sampling_frequency_hz; 274 le_audio_demo_sink_frame_duration = frame_duration; 275 le_audio_demo_sink_octets_per_frame = octets_per_frame; 276 le_audio_demo_sink_iso_interval_1250us = iso_interval_1250us; 277 le_audio_demo_sink_num_streams = num_streams; 278 le_audio_demo_sink_num_channels_per_stream = num_channels_per_stream; 279 280 sink_receive_streaming = false; 281 le_audio_demo_util_sink_state = LE_AUDIO_SINK_CONFIGURED; 282 283 le_audio_demo_sink_num_channels = le_audio_demo_sink_num_streams * le_audio_demo_sink_num_channels_per_stream; 284 btstack_assert((le_audio_demo_sink_num_channels == 1) || (le_audio_demo_sink_num_channels == 2)); 285 286 le_audio_demo_sink_lc3_frames = 0; 287 288 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); 289 290 // switch to lc3plus if requested and possible 291 bool use_lc3plus_decoder = le_audio_demo_lc3plus_decoder_requested && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); 292 293 // init decoder 294 setup_lc3_decoder(use_lc3plus_decoder); 295 296 printf("Configure: %u streams, %u channels per stream, sampling rate %" PRIu32 ", samples per frame %u, lc3plus %u\n", 297 num_streams, num_channels_per_stream, sampling_frequency_hz, le_audio_demo_sink_num_samples_per_frame, use_lc3plus_decoder); 298 299 #ifdef HAVE_POSIX_FILE_IO 300 // create wav file 301 printf("WAV file: %s\n", le_audio_demo_sink_filename_wav); 302 wav_writer_open(le_audio_demo_sink_filename_wav, le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz); 303 #endif 304 305 // init playback buffer 306 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 307 308 // calc start threshold in bytes for PLAYBACK_START_MS 309 playback_start_threshold_bytes = (sampling_frequency_hz / 1000 * PLAYBACK_START_MS) * le_audio_demo_sink_num_channels * 2; 310 311 // sample rate compensation 312 le_audio_demo_sink_received_samples = 0; 313 314 // start playback 315 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 316 if (sink != NULL){ 317 btstack_sample_rate_compensation_reset( &sample_rate_compensation, btstack_run_loop_get_time_ms() ); 318 btstack_resample_init(&resample_instance, le_audio_demo_sink_num_channels); 319 sink->init(le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz, le_audio_connection_sink_playback); 320 sink->start_stream(); 321 } 322 } 323 324 void le_audio_demo_util_sink_configure_unicast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 325 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 326 uint32_t iso_interval_1250us, uint8_t flush_timeout){ 327 le_audio_demo_sink_type = HCI_ISO_TYPE_CIS; 328 le_audio_demo_sink_flush_timeout = flush_timeout; 329 330 // set playback start: FT * ISO Interval + max(10 ms, 1/2 ISO Interval) 331 uint16_t playback_start_ms = flush_timeout * (iso_interval_1250us * 5 / 4) + btstack_max(10, iso_interval_1250us * 5 / 8); 332 uint16_t playback_start_samples = sampling_frequency_hz / 1000 * playback_start_ms; 333 playback_start_threshold_bytes = playback_start_samples * num_streams * num_channels_per_stream * 2; 334 printf("Playback: start %u ms (%u samples, %u bytes)\n", playback_start_ms, playback_start_samples, playback_start_threshold_bytes); 335 336 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz, 337 frame_duration, octets_per_frame, iso_interval_1250us); 338 } 339 340 void le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 341 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 342 uint32_t iso_interval_1250us, uint8_t pre_transmission_offset) { 343 le_audio_demo_sink_type = HCI_ISO_TYPE_BIS; 344 le_audio_demo_sink_pre_transmission_offset = pre_transmission_offset; 345 346 // set playback start: ISO Interval + 10 ms 347 uint16_t playback_start_ms = (iso_interval_1250us * 5 / 4) + 10; 348 uint16_t playback_start_samples = sampling_frequency_hz / 1000 * playback_start_ms; 349 playback_start_threshold_bytes = playback_start_samples * num_streams * num_channels_per_stream * 2; 350 printf("Playback: start %u ms (%u samples, %u bytes)\n", playback_start_ms, playback_start_samples, playback_start_threshold_bytes); 351 352 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz, frame_duration, octets_per_frame, iso_interval_1250us); 353 } 354 355 void le_audio_demo_util_sink_count(uint8_t stream_index, uint8_t *packet, uint16_t size) { 356 UNUSED(size); 357 // check for missing packet 358 uint16_t header = little_endian_read_16(packet, 0); 359 uint8_t ts_flag = (header >> 14) & 1; 360 361 uint16_t offset = 4; 362 uint32_t time_stamp = 0; 363 if (ts_flag){ 364 time_stamp = little_endian_read_32(packet, offset); 365 offset += 4; 366 } 367 368 UNUSED(time_stamp); 369 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 370 371 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 372 offset += 4; 373 374 uint16_t last_seq_no = last_packet_sequence[stream_index]; 375 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); 376 if (packet_missed){ 377 // print last packet 378 printf("\n"); 379 printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[stream_index], stream_index); 380 printf_hexdump(&last_packet_prefix[le_audio_demo_sink_num_streams*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); 381 last_seq_no++; 382 383 printf(ANSI_COLOR_RED); 384 while (last_seq_no < packet_sequence_number){ 385 printf("%04x %u MISSING\n", last_seq_no, stream_index); 386 last_seq_no++; 387 } 388 printf(ANSI_COLOR_RESET); 389 390 // print current packet 391 printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, stream_index); 392 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); 393 } 394 395 // cache current packet 396 memcpy(&last_packet_prefix[le_audio_demo_sink_num_streams*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); 397 } 398 399 static btstack_fsm_state_t audio_processing_initial( audio_processing_t * const me, btstack_fsm_event_t const * const e ) { 400 UNUSED(e); 401 audio_fsm_debug("%s\n", __FUNCTION__ ); 402 return TRAN(audio_processing_waiting); 403 } 404 405 static btstack_fsm_state_t audio_processing_waiting( audio_processing_t * const me, btstack_fsm_event_t const * const e ) { 406 audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]); 407 btstack_fsm_state_t status; 408 switch(e->sig) { 409 case BTSTACK_FSM_ENTRY_SIG: { 410 status = BTSTACK_FSM_HANDLED_STATUS; 411 break; 412 } 413 case BTSTACK_FSM_EXIT_SIG: { 414 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 415 416 btstack_sample_rate_compensation_init(&sample_rate_compensation, me->last_receive_time_ms, 417 le_audio_demo_sink_sampling_frequency_hz, FLOAT_TO_Q15(1.f)); 418 me->zero_frames = 0; 419 me->received_samples = 0; 420 btstack_resample_init( &resample_instance, le_audio_demo_sink_num_channels ); 421 me->have_pcm = 0; 422 status = BTSTACK_FSM_HANDLED_STATUS; 423 break; 424 } 425 case DATA_SIG: { 426 data_event_t *data_event = (data_event_t*)e; 427 // nothing to do here 428 if( data_event->data == NULL ) { 429 status = BTSTACK_FSM_IGNORED_STATUS; 430 break; 431 } 432 433 // ignore empty data at start 434 if( data_event->size == 0 ) { 435 status = BTSTACK_FSM_IGNORED_STATUS; 436 break; 437 } 438 439 // always start at first stream 440 if( data_event->stream > 0 ) { 441 status = BTSTACK_FSM_IGNORED_STATUS; 442 break; 443 } 444 445 me->last_receive_time_ms = data_event->receive_time_ms; 446 status = TRAN(audio_processing_streaming); 447 break; 448 } 449 default: { 450 status = BTSTACK_FSM_IGNORED_STATUS; 451 break; 452 } 453 } 454 return status; 455 } 456 457 static void audio_processing_resample( audio_processing_t * const me, data_event_t *e ) { 458 // mark current packet as handled 459 e->data = NULL; 460 if( me->have_pcm != (uint32_t)((1<<(le_audio_demo_sink_num_streams*le_audio_demo_sink_num_channels_per_stream))-1) ) { 461 return; 462 } 463 464 int16_t *data_in = pcm; 465 int16_t *data_out = pcm_resample; 466 #ifdef HAVE_POSIX_FILE_IO 467 // write wav samples 468 wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, data_in); 469 #endif 470 471 // count for samplerate compensation 472 me->received_samples += le_audio_demo_sink_num_samples_per_frame; 473 474 // store samples in playback buffer 475 samples_received += le_audio_demo_sink_num_samples_per_frame; 476 uint32_t resampled_frames = btstack_resample_block(&resample_instance, data_in, le_audio_demo_sink_num_samples_per_frame, data_out); 477 uint32_t bytes_to_store = resampled_frames * le_audio_demo_sink_num_channels * 2; 478 479 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 480 btstack_ring_buffer_write(&playback_buffer, (uint8_t *)data_out, bytes_to_store); 481 log_info("Samples in playback buffer %5" PRIu32 "", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); 482 } else { 483 printf("Samples dropped\n"); 484 samples_dropped += le_audio_demo_sink_num_samples_per_frame; 485 } 486 me->have_pcm = 0; 487 } 488 489 static btstack_fsm_state_t audio_processing_decode( audio_processing_t * const me, btstack_fsm_event_t const * const e ) { 490 audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]); 491 btstack_fsm_state_t status; 492 switch(e->sig) { 493 case BTSTACK_FSM_ENTRY_SIG: { 494 btstack_assert( (le_audio_demo_sink_num_streams*le_audio_demo_sink_num_channels_per_stream) < (sizeof(me->have_pcm)*8)); 495 status = BTSTACK_FSM_HANDLED_STATUS; 496 break; 497 } 498 case BTSTACK_FSM_EXIT_SIG: { 499 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 500 if( sink == NULL ) { 501 502 status = BTSTACK_FSM_HANDLED_STATUS; 503 break; 504 } 505 uint32_t resampling_factor = btstack_sample_rate_compensation_update( &sample_rate_compensation, me->receive_time_ms, 506 me->received_samples, sink->get_samplerate() ); 507 btstack_resample_set_factor(&resample_instance, resampling_factor); 508 me->received_samples = 0; 509 510 status = BTSTACK_FSM_HANDLED_STATUS; 511 break; 512 } 513 case DATA_SIG: { 514 data_event_t *data_event = (data_event_t*)e; 515 uint8_t *data_in = data_event->data; 516 int16_t *data_out = pcm; 517 uint16_t offset = 0; 518 uint8_t BFI = 0; 519 if (data_event->size != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame) { 520 // incorrect size. we assume that we received this packet on time but cannot decode it, so we use PLC 521 BFI = 1; 522 printf("predict audio\n"); 523 } 524 uint8_t i; 525 for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){ 526 uint8_t tmp_BEC_detect; 527 uint8_t effective_channel = (data_event->stream * le_audio_demo_sink_num_channels_per_stream) + i; 528 (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &data_in[offset], BFI, 529 &data_out[effective_channel], le_audio_demo_sink_num_channels, 530 &tmp_BEC_detect); 531 offset += le_audio_demo_sink_octets_per_frame; 532 audio_fsm_debug("effective_channel: %d\n", effective_channel ); 533 if( (me->have_pcm & (1<<effective_channel)) ) { 534 audio_fsm_debug("de-syncroniced, resync\n"); 535 status = TRAN(audio_processing_waiting); 536 break; 537 } 538 me->have_pcm |= (1<<effective_channel); 539 } 540 audio_processing_resample( me, data_event ); 541 status = TRAN(audio_processing_streaming); 542 break; 543 } 544 default: { 545 status = BTSTACK_FSM_IGNORED_STATUS; 546 break; 547 } 548 } 549 return status; 550 } 551 552 static btstack_fsm_state_t audio_processing_streaming( audio_processing_t * const me, btstack_fsm_event_t const * const e ) { 553 audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]); 554 555 btstack_fsm_state_t status; 556 switch(e->sig) { 557 case BTSTACK_FSM_ENTRY_SIG: { 558 status = BTSTACK_FSM_HANDLED_STATUS; 559 break; 560 } 561 case BTSTACK_FSM_EXIT_SIG: { 562 me->last_receive_time_ms = me->receive_time_ms; 563 status = BTSTACK_FSM_HANDLED_STATUS; 564 break; 565 } 566 case TIME_SIG: { 567 time_event_t *time = (time_event_t*)e; 568 printf("time: %" PRId32 " - %" PRId32 " %" PRId32 "\n", time->time_ms, me->last_receive_time_ms, time->time_ms-me->last_receive_time_ms ); 569 // we were last called ages ago, so just start waiting again 570 if( btstack_time_delta( time->time_ms, me->last_receive_time_ms ) > 100) { 571 status = TRAN(audio_processing_waiting); 572 break; 573 } 574 status = BTSTACK_FSM_HANDLED_STATUS; 575 break; 576 } 577 case DATA_SIG: { 578 data_event_t *data_event = (data_event_t*)e; 579 me->receive_time_ms = data_event->receive_time_ms; 580 581 // done processing this data 582 if( data_event->data == NULL ) { 583 status = BTSTACK_FSM_HANDLED_STATUS; 584 break; 585 } 586 587 if( btstack_time_delta( data_event->receive_time_ms, me->last_receive_time_ms ) > 100) { 588 status = TRAN(audio_processing_waiting); 589 break; 590 } 591 592 if( me->zero_frames > 10 ) { 593 status = TRAN(audio_processing_waiting); 594 break; 595 } 596 597 // track consecutive audio frames without data 598 if( data_event->size == 0 ) { 599 me->zero_frames++; 600 } else { 601 me->zero_frames = 0; 602 } 603 604 // will decode and/or predict missing data 605 status = TRAN(audio_processing_decode); 606 break; 607 } 608 default: { 609 status = BTSTACK_FSM_IGNORED_STATUS; 610 break; 611 } 612 } 613 return status; 614 } 615 616 static void audio_processing_constructor( audio_processing_t *me) { 617 btstack_fsm_constructor(&me->super, (btstack_fsm_state_handler_t)&audio_processing_initial); 618 btstack_fsm_init(&me->super, NULL); 619 } 620 621 static void audio_processing_task( audio_processing_t *me, btstack_fsm_event_t const *e ) { 622 btstack_fsm_dispatch_until(&me->super, e); 623 } 624 625 static bool audio_processing_is_streaming( audio_processing_t *me ) { 626 btstack_fsm_t *fsm = &me->super; 627 time_event_t const time_event = { { TIME_SIG }, btstack_run_loop_get_time_ms() }; 628 audio_processing_task( me, &time_event.super ); 629 return fsm->state == (btstack_fsm_state_handler_t)&audio_processing_streaming; 630 } 631 632 void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint16_t size) { 633 UNUSED(size); 634 635 if (le_audio_demo_util_sink_state != LE_AUDIO_SINK_CONFIGURED) return; 636 637 uint16_t header = little_endian_read_16(packet, 0); 638 hci_con_handle_t con_handle = header & 0x0fff; 639 uint8_t pb_flag = (header >> 12) & 3; 640 uint8_t ts_flag = (header >> 14) & 1; 641 uint16_t iso_load_len = little_endian_read_16(packet, 2); 642 643 uint16_t offset = 4; 644 uint32_t time_stamp = 0; 645 if (ts_flag){ 646 time_stamp = little_endian_read_32(packet, offset); 647 offset += 4; 648 } 649 650 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 651 652 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 653 offset += 2; 654 655 uint16_t header_2 = little_endian_read_16(packet, offset); 656 uint16_t iso_sdu_length = header_2 & 0x3fff; 657 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 658 offset += 2; 659 660 // avoid warning for (yet) unused fields 661 UNUSED(con_handle); 662 UNUSED(pb_flag); 663 UNUSED(iso_load_len); 664 UNUSED(packet_status_flag); 665 UNUSED(time_stamp); 666 667 data_event_t const data_event = { 668 .super.sig = DATA_SIG, 669 .sequence_number = packet_sequence_number, 670 .stream = stream_index, 671 .data = &packet[offset], 672 .size = iso_sdu_length, 673 .receive_time_ms = receive_time_ms, 674 }; 675 676 audio_fsm_debug("new data\n stream_index: %d\n", stream_index); 677 audio_processing_task( &audio_processing, &data_event.super ); 678 679 le_audio_demo_sink_lc3_frames++; 680 681 if (samples_received >= 10 * le_audio_demo_sink_sampling_frequency_hz){ 682 printf("LC3 Frames: %4" PRIu32 " - samples received %5" PRIu32 ", played %5" PRIu32 ", dropped %5" PRIu32 "\n", le_audio_demo_sink_lc3_frames, samples_received, samples_played, samples_dropped); 683 samples_received = 0; 684 samples_dropped = 0; 685 samples_played = 0; 686 } 687 } 688 689 void le_audio_demo_util_sink_init(const char * filename_wav){ 690 le_audio_demo_sink_filename_wav = filename_wav; 691 le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT; 692 audio_processing_constructor( &audio_processing ); 693 } 694 695 /** 696 * @brief Close sink: close wav file, stop playback 697 */ 698 void le_audio_demo_util_sink_close(void){ 699 #ifdef HAVE_POSIX_FILE_IO 700 printf("Close WAV file\n"); 701 wav_writer_close(); 702 #endif 703 // stop playback 704 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 705 if (sink != NULL){ 706 sink->stop_stream(); 707 } 708 le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT; 709 sink_receive_streaming = false; 710 // stop timer 711 btstack_run_loop_remove_timer(&next_packet_timer); 712 } 713