From c18c19debd05a2c8ded036c3d6cb10b0dd23e87e Mon Sep 17 00:00:00 2001 From: Dirk Helbig Date: Thu, 18 Jan 2024 18:27:07 +0100 Subject: [PATCH] le_audio_demo_util_sink: reworked playback, PLC --- example/le_audio_demo_util_sink.c | 565 ++++++++++++++++-------------- 1 file changed, 307 insertions(+), 258 deletions(-) diff --git a/example/le_audio_demo_util_sink.c b/example/le_audio_demo_util_sink.c index d38d12dc6..cc671ab3d 100644 --- a/example/le_audio_demo_util_sink.c +++ b/example/le_audio_demo_util_sink.c @@ -54,6 +54,7 @@ #include "btstack_sample_rate_compensation.h" #include "btstack_resample.h" +#include "btstack_fsm.h" #include "hxcmod.h" #include "mods/mod.h" @@ -63,22 +64,12 @@ #include "wav_util.h" #endif -//#define DEBUG_PLC -#ifdef DEBUG_PLC -#define printf_plc(...) { \ - printf(__VA_ARGS__); \ - log_info(__VA_ARGS__);\ -} -#else -#define printf_plc(...) (void)(0); -#endif - #define MAX_CHANNELS 2 #define MAX_SAMPLES_PER_FRAME 480 #define MAX_LC3_FRAME_BYTES 155 // playback -#define MAX_NUM_LC3_FRAMES 15 +#define MAX_NUM_LC3_FRAMES (15*2) #define MAX_BYTES_PER_SAMPLE 4 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_CHANNELS * MAX_BYTES_PER_SAMPLE) #define PLAYBACK_START_MS (MAX_NUM_LC3_FRAMES * 20 / 3) @@ -136,26 +127,72 @@ static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; static btstack_ring_buffer_t playback_buffer; // PLC -static bool stream_last_packet_received[MAX_CHANNELS]; -static uint16_t stream_last_packet_sequence[MAX_CHANNELS]; -static uint16_t group_last_packet_sequence; -static bool group_last_packet_received; -static uint16_t plc_timeout_initial_ms; -static uint16_t plc_timeout_subsequent_ms; - static uint32_t le_audio_demo_sink_lc3_frames; -static uint32_t le_audio_demo_sink_zero_frames; static uint32_t samples_received; static uint32_t samples_played; static uint32_t samples_dropped; +// Audio FSM +#define TRAN( target ) btstack_fsm_transit( &me->super, (btstack_fsm_state_handler_t)target ) + +typedef struct { + btstack_fsm_t super; + uint32_t receive_time_ms; + uint32_t last_receive_time_ms; + uint32_t zero_frames; + uint32_t have_pcm; + uint32_t received_samples; +} audio_processing_t; + +typedef struct { + btstack_fsm_event_t super; + uint16_t sequence_number; + uint16_t size; + uint32_t receive_time_ms; + uint8_t *data; + uint8_t stream; +} data_event_t; + +typedef struct { + btstack_fsm_event_t super; + uint32_t time_ms; +} time_event_t; + +audio_processing_t audio_processing; + +enum EventSignals { + DATA_SIG = BTSTACK_FSM_USER_SIG, + TIME_SIG +}; + +#define AUDIO_FSM_DEBUG +#ifdef AUDIO_FSM_DEBUG +#define ENUM_TO_TEXT(sig) [sig] = #sig +#define audio_fsm_debug(format, ...) \ + printf( format __VA_OPT__(,) __VA_ARGS__) + +const char * const sigToString[] = { + ENUM_TO_TEXT(BTSTACK_FSM_INIT_SIG), + ENUM_TO_TEXT(BTSTACK_FSM_ENTRY_SIG), + ENUM_TO_TEXT(BTSTACK_FSM_EXIT_SIG), + ENUM_TO_TEXT(DATA_SIG), + ENUM_TO_TEXT(TIME_SIG), +}; +#else +#define audio_fsm_debug(...) +#endif + +static btstack_fsm_state_t audio_processing_initial( audio_processing_t * const me, btstack_fsm_event_t const * const e ); +static btstack_fsm_state_t audio_processing_waiting( audio_processing_t * const me, btstack_fsm_event_t const * const e ); +static btstack_fsm_state_t audio_processing_streaming( audio_processing_t * const me, btstack_fsm_event_t const * const e ); +static bool audio_processing_is_streaming( audio_processing_t * const me ); + static btstack_timer_source_t next_packet_timer; // lc3 decoder static bool le_audio_demo_lc3plus_decoder_requested = false; static const btstack_lc3_decoder_t * lc3_decoder; static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; -static bool have_pcm[MAX_CHANNELS]; static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS]; #ifdef HAVE_LC3PLUS @@ -173,12 +210,18 @@ static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_sam if (playback_active == false){ if (btstack_ring_buffer_bytes_available(&playback_buffer) >= playback_start_threshold_bytes) { log_info("Playback started"); + printf("Playback started\n"); playback_active = true; } } else { if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)) { - log_info("Playback underrun"); - printf("Playback Underrun\n"); + if( audio_processing_is_streaming( &audio_processing ) ) { + log_info("Playback underrun"); + printf("Playback Underrun\n"); + } else { + log_info("Playback stopped"); + printf("Playback stopped\n"); + } // empty buffer uint32_t bytes_read; btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); @@ -195,114 +238,6 @@ static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_sam } } -static void store_samples_in_ringbuffer(void){ - // check if we have all channels - uint8_t channel; - for (channel = 0; channel < le_audio_demo_sink_num_channels; channel++){ - if (have_pcm[channel] == false) return; - } -#ifdef HAVE_POSIX_FILE_IO - // write wav samples - wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, pcm); -#endif - - // count for samplerate compensation - le_audio_demo_sink_received_samples += le_audio_demo_sink_num_samples_per_frame; - - // store samples in playback buffer - samples_received += le_audio_demo_sink_num_samples_per_frame; - uint32_t resampled_frames = btstack_resample_block(&resample_instance, pcm, le_audio_demo_sink_num_samples_per_frame, pcm_resample); - uint32_t bytes_to_store = resampled_frames * le_audio_demo_sink_num_channels * 2; - - if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { - btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm_resample, bytes_to_store); - log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); - } else { - printf("Samples dropped\n"); - samples_dropped += le_audio_demo_sink_num_samples_per_frame; - } - memset(have_pcm, 0, sizeof(have_pcm)); -} - -static void plc_do(uint8_t stream_index) { - // inject packet - uint8_t tmp_BEC_detect; - uint8_t BFI = 1; - uint8_t i; - for (i = 0; i < le_audio_demo_sink_num_channels_per_stream; i++){ - uint8_t effective_channel = stream_index + i; - (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], NULL, BFI, - &pcm[effective_channel], le_audio_demo_sink_num_channels, - &tmp_BEC_detect); - have_pcm[i] = true; - } - // and store in ringbuffer when PCM for all channels is available - store_samples_in_ringbuffer(); -} - -// -// Perform PLC for packets missing in previous intervals -// -// assumptions: -// - packet sequence number is monotonic increasing -// - if packet with seq nr x is received, all packets with smaller seq number are either received or missed -static void plc_check(uint16_t packet_sequence_number) { - while (group_last_packet_sequence != packet_sequence_number){ - uint8_t i; - for (i=0;i 0) { - printf_plc("- ISO #%u, PLC for %u\n", i, group_last_packet_sequence); -#ifndef DEBUG_PLC - log_info("PLC for packet 0x%04x, stream #%u", group_last_packet_sequence, i); -#endif - plc_do(i); - btstack_assert((stream_last_packet_sequence[i] + 1) == group_last_packet_sequence); - stream_last_packet_sequence[i] = group_last_packet_sequence; - } - } - group_last_packet_sequence++; - } -} - -static void plc_timeout(btstack_timer_source_t * timer) { - // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets - btstack_run_loop_set_timer(timer, plc_timeout_subsequent_ms); - btstack_run_loop_set_timer_handler(timer, plc_timeout); - btstack_run_loop_add_timer(timer); - - switch (le_audio_demo_sink_type){ - case HCI_ISO_TYPE_CIS: - // assume no packet received in iso interval => FT packets missed - printf_plc("PLC: timeout cis, group %u, FT %u", group_last_packet_sequence, le_audio_demo_sink_flush_timeout); - plc_check(group_last_packet_sequence + le_audio_demo_sink_flush_timeout); - break; - case HCI_ISO_TYPE_BIS: - // assume PTO not used => 1 packet missed - plc_check(group_last_packet_sequence + 1); - break; - default: - btstack_unreachable(); - break; - } -} - -void le_audio_demo_util_sink_init(const char * filename_wav){ - le_audio_demo_sink_filename_wav = filename_wav; - le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT; -} - void le_audio_demo_util_sink_enable_lc3plus(bool enable){ le_audio_demo_lc3plus_decoder_requested = enable; } @@ -348,13 +283,6 @@ void le_audio_demo_util_sink_configure_general(uint8_t num_streams, uint8_t num_ le_audio_demo_sink_lc3_frames = 0; - group_last_packet_received = false; - uint8_t i; - for (i=0;isig]); + btstack_fsm_state_t status; + switch(e->sig) { + case BTSTACK_FSM_ENTRY_SIG: { + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case BTSTACK_FSM_EXIT_SIG: { + btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); + + btstack_sample_rate_compensation_init(&sample_rate_compensation, me->last_receive_time_ms, + le_audio_demo_sink_sampling_frequency_hz, FLOAT_TO_Q15(1.f)); + me->zero_frames = 0; + me->received_samples = 0; + btstack_resample_init( &resample_instance, le_audio_demo_sink_num_channels ); + me->have_pcm = 0; + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case DATA_SIG: { + data_event_t *data_event = (data_event_t*)e; + // nothing to do here + if( data_event->data == NULL ) { + status = BTSTACK_FSM_IGNORED_STATUS; + break; + } + + // ignore empty data at start + if( data_event->size == 0 ) { + status = BTSTACK_FSM_IGNORED_STATUS; + break; + } + + // always start at first stream + if( data_event->stream > 0 ) { + status = BTSTACK_FSM_IGNORED_STATUS; + break; + } + + me->last_receive_time_ms = data_event->receive_time_ms; + status = TRAN(audio_processing_streaming); + break; + } + default: { + status = BTSTACK_FSM_IGNORED_STATUS; + break; + } + } + return status; +} + +static void audio_processing_resample( audio_processing_t * const me, data_event_t *e ) { + if( me->have_pcm != ((1<<(le_audio_demo_sink_num_streams*le_audio_demo_sink_num_channels_per_stream))-1) ) { + return; + } + + int16_t *data_in = pcm; + int16_t *data_out = pcm_resample; +#ifdef HAVE_POSIX_FILE_IO + // write wav samples + wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, data_in); +#endif + + // count for samplerate compensation + me->received_samples += le_audio_demo_sink_num_samples_per_frame; + + // store samples in playback buffer + samples_received += le_audio_demo_sink_num_samples_per_frame; + uint32_t resampled_frames = btstack_resample_block(&resample_instance, data_in, le_audio_demo_sink_num_samples_per_frame, data_out); + uint32_t bytes_to_store = resampled_frames * le_audio_demo_sink_num_channels * 2; + + if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { + btstack_ring_buffer_write(&playback_buffer, (uint8_t *)data_out, bytes_to_store); + log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); + } else { + printf("Samples dropped\n"); + samples_dropped += le_audio_demo_sink_num_samples_per_frame; + } + e->data = NULL; + me->have_pcm = 0; +} + +static btstack_fsm_state_t audio_processing_decode( audio_processing_t * const me, btstack_fsm_event_t const * const e ) { + audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]); + btstack_fsm_state_t status; + switch(e->sig) { + case BTSTACK_FSM_ENTRY_SIG: { + btstack_assert( (le_audio_demo_sink_num_streams*le_audio_demo_sink_num_channels_per_stream) < (sizeof(me->have_pcm)*8)); + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case BTSTACK_FSM_EXIT_SIG: { + const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); + if( sink == NULL ) { + + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + uint32_t resampling_factor = btstack_sample_rate_compensation_update( &sample_rate_compensation, me->receive_time_ms, + me->received_samples, sink->get_samplerate() ); + btstack_resample_set_factor(&resample_instance, resampling_factor); + me->received_samples = 0; + + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case DATA_SIG: { + data_event_t *data_event = (data_event_t*)e; + uint8_t *data_in = data_event->data; + int16_t *data_out = pcm; + uint16_t offset = 0; + uint8_t BFI = 0; + if (data_event->size != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame) { + // incorrect size. we assume that we received this packet on time but cannot decode it, so we use PLC + BFI = 1; + printf("predict audio\n"); + } + uint8_t i; + for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){ + uint8_t tmp_BEC_detect; + uint8_t effective_channel = (data_event->stream * le_audio_demo_sink_num_channels_per_stream) + i; + (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &data_in[offset], BFI, + &data_out[effective_channel], le_audio_demo_sink_num_channels, + &tmp_BEC_detect); + offset += le_audio_demo_sink_octets_per_frame; + btstack_assert( !(me->have_pcm & (1<have_pcm |= (1<sig]); + + btstack_fsm_state_t status; + switch(e->sig) { + case BTSTACK_FSM_ENTRY_SIG: { + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case BTSTACK_FSM_EXIT_SIG: { + me->last_receive_time_ms = me->receive_time_ms; + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case TIME_SIG: { + time_event_t *time = (time_event_t*)e; + printf("time: %d - %d %d\n", time->time_ms, me->last_receive_time_ms, time->time_ms-me->last_receive_time_ms ); + // we were last called ages ago, so just start waiting again + if( btstack_time_delta( time->time_ms, me->last_receive_time_ms ) > 100) { + status = TRAN(audio_processing_waiting); + break; + } + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + case DATA_SIG: { + data_event_t *data_event = (data_event_t*)e; + me->receive_time_ms = data_event->receive_time_ms; + + // done processing this data + if( data_event->data == NULL ) { + status = BTSTACK_FSM_HANDLED_STATUS; + break; + } + + if( btstack_time_delta( data_event->receive_time_ms, me->last_receive_time_ms ) > 100) { + status = TRAN(audio_processing_waiting); + break; + } + + if( me->zero_frames > 10 ) { + status = TRAN(audio_processing_waiting); + break; + } + + // track consecutive audio frames without data + if( data_event->size == 0 ) { + me->zero_frames++; + } else { + me->zero_frames = 0; + } + + // will decode and/or predict missing data + status = TRAN(audio_processing_decode); + break; + } + default: { + status = BTSTACK_FSM_IGNORED_STATUS; + break; + } + } + return status; +} + +static void audio_processing_constructor( audio_processing_t *me) { + btstack_fsm_constructor(&me->super, (btstack_fsm_state_handler_t)&audio_processing_initial); + btstack_fsm_init(&me->super, NULL); +} + +static void audio_processing_task( audio_processing_t *me, btstack_fsm_event_t const *e ) { + btstack_fsm_dispatch_until(&me->super, e); +} + +static bool audio_processing_is_streaming( audio_processing_t *me ) { + btstack_fsm_t *fsm = &me->super; + time_event_t const time_event = { TIME_SIG, btstack_run_loop_get_time_ms() }; + audio_processing_task( me, &time_event.super ); + return fsm->state == (btstack_fsm_state_handler_t)&audio_processing_streaming; +} + +void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint16_t size) { if (le_audio_demo_util_sink_state != LE_AUDIO_SINK_CONFIGURED) return; uint16_t header = little_endian_read_16(packet, 0); @@ -516,117 +651,31 @@ void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint UNUSED(packet_status_flag); UNUSED(time_stamp); - // start with first packet on first stream - if (group_last_packet_received == false){ - if (stream_index != 0){ - printf("Ignore first packet for second stream\n"); - return; - } - group_last_packet_received = true; - group_last_packet_sequence = packet_sequence_number; - } - - if (stream_last_packet_received[stream_index]) { - printf_plc("ISO #%u, receive %u\n", stream_index, packet_sequence_number); - - int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, - stream_last_packet_sequence[stream_index]); - if (packet_sequence_delta < 1) { - // drop delayed packet that had already been generated by PLC - printf_plc("- dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n", - packet_sequence_number, stream_last_packet_sequence[stream_index]); - return; - } - // simple check - if (packet_sequence_number != stream_last_packet_sequence[stream_index] + 1) { - printf_plc("- ISO #%u, missing %u\n", stream_index, stream_last_packet_sequence[stream_index] + 1); - } - } else { - printf_plc("ISO %u, first packet seq number %u\n", stream_index, packet_sequence_number); - stream_last_packet_received[stream_index] = true; - } - - if (sink_receive_streaming){ - plc_check(packet_sequence_number); - } - - const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); - if( (sink != NULL) && (iso_sdu_length>0)) { - if (!sink_receive_streaming && playback_active) { - btstack_sample_rate_compensation_init(&sample_rate_compensation, receive_time_ms, - le_audio_demo_sink_sampling_frequency_hz, FLOAT_TO_Q15(1.f)); - sink_receive_streaming = true; - } - } - - if (iso_sdu_length == 0) { - if (sink_receive_streaming){ - // empty packet -> generate silence - memset(pcm, 0, sizeof(pcm)); - uint8_t i; - for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++) { - uint8_t effective_channel = (stream_index * le_audio_demo_sink_num_channels_per_stream) + i; - have_pcm[effective_channel] = true; - } - le_audio_demo_sink_zero_frames++; - // pause detection (1000 ms for 10 ms, 750 ms for 7.5 ms frames) - if (le_audio_demo_sink_zero_frames > 100){ - printf("Pause detected, stopping audio\n"); - log_info("Pause detected, stopping audio"); - // pause/reset audio - btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); - sink_receive_streaming = false; - playback_active = false; - } - } - } else { - // regular packet -> decode codec frame if size ok - uint8_t BFI = 0; - if (iso_sdu_length != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame) { - // incorrect size. we assume that we received this packet on time but cannot decode it, so we use PLC - BFI = 1; - } - le_audio_demo_sink_zero_frames = 0; - uint8_t i; - for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){ - uint8_t tmp_BEC_detect; - uint8_t BFI = 0; - uint8_t effective_channel = (stream_index * le_audio_demo_sink_num_channels_per_stream) + i; - (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &packet[offset], BFI, - &pcm[effective_channel], le_audio_demo_sink_num_channels, - &tmp_BEC_detect); - offset += le_audio_demo_sink_octets_per_frame; - have_pcm[effective_channel] = true; - } - } - - store_samples_in_ringbuffer(); - - if( (sink != NULL) && (iso_sdu_length>0)) { - if( sink_receive_streaming ) { - uint32_t resampling_factor = btstack_sample_rate_compensation_update( &sample_rate_compensation, receive_time_ms, - le_audio_demo_sink_received_samples, sink->get_samplerate() ); - btstack_resample_set_factor(&resample_instance, resampling_factor); - le_audio_demo_sink_received_samples = 0; - } - } + data_event_t const data_event = { + .super.sig = DATA_SIG, + .sequence_number = packet_sequence_number, + .stream = stream_index, + .data = &packet[offset], + .size = iso_sdu_length, + .receive_time_ms = receive_time_ms, + }; + audio_fsm_debug("new data\n"); + audio_processing_task( &audio_processing, &data_event.super ); le_audio_demo_sink_lc3_frames++; - // PLC - btstack_run_loop_remove_timer(&next_packet_timer); - btstack_run_loop_set_timer(&next_packet_timer, plc_timeout_initial_ms); - btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); - btstack_run_loop_add_timer(&next_packet_timer); - if (samples_received >= le_audio_demo_sink_sampling_frequency_hz){ printf("LC3 Frames: %4u - samples received %5u, played %5u, dropped %5u\n", le_audio_demo_sink_lc3_frames, samples_received, samples_played, samples_dropped); samples_received = 0; samples_dropped = 0; samples_played = 0; } +} - stream_last_packet_sequence[stream_index] = packet_sequence_number; +void le_audio_demo_util_sink_init(const char * filename_wav){ + le_audio_demo_sink_filename_wav = filename_wav; + le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT; + audio_processing_constructor( &audio_processing ); } /**