From 3128eefde709b64f6c84916d252934f3825918d0 Mon Sep 17 00:00:00 2001 From: Dirk Helbig Date: Thu, 23 Mar 2023 15:47:27 +0100 Subject: [PATCH] a2dp_sink_demo: relocated samplerate compensation init to fix measurement bias btstack_config esp32: enabled samplerate compensation by default btstack_audio_esp32_v5: allow the playback handler to catch up in case of unintended delays --- example/a2dp_sink_demo.c | 34 ++++++--- .../btstack/btstack_audio_esp32_v5.c | 69 ++++++++----------- .../btstack/include/btstack_config.h | 2 + 3 files changed, 55 insertions(+), 50 deletions(-) diff --git a/example/a2dp_sink_demo.c b/example/a2dp_sink_demo.c index a462bf401..420fe98e9 100644 --- a/example/a2dp_sink_demo.c +++ b/example/a2dp_sink_demo.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2016 BlueKitchen GmbH + * Copyright (C) 2023 BlueKitchen GmbH * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -139,6 +139,9 @@ static btstack_ring_buffer_t decoded_audio_ring_buffer; static int media_initialized = 0; static int audio_stream_started; +#ifdef HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE +static int l2cap_stream_started; +#endif static btstack_resample_t resample_instance; // temp storage of lower-layer request for audio samples @@ -405,9 +408,6 @@ static void handle_pcm_data(int16_t * data, int num_audio_frames, int num_channe static int media_processing_init(media_codec_configuration_sbc_t * configuration){ if (media_initialized) return 0; -#ifdef HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE - btstack_sample_rate_compensation_init( &sample_rate_compensation, btstack_run_loop_get_time_ms(), configuration->sampling_frequency, FLOAT_TO_Q15(1.f) ); -#endif btstack_sbc_decoder_init(&state, mode, handle_pcm_data, NULL); #ifdef STORE_TO_WAV_FILE @@ -431,6 +431,7 @@ static int media_processing_init(media_codec_configuration_sbc_t * configuration static void media_processing_start(void){ if (!media_initialized) return; + #ifdef HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE btstack_sample_rate_compensation_reset( &sample_rate_compensation, btstack_run_loop_get_time_ms() ); #endif @@ -444,8 +445,13 @@ static void media_processing_start(void){ static void media_processing_pause(void){ if (!media_initialized) return; + // stop audio playback audio_stream_started = 0; +#ifdef HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE + l2cap_stream_started = 0; +#endif + const btstack_audio_sink_t * audio = btstack_audio_sink_get_instance(); if (audio){ audio->stop_stream(); @@ -457,9 +463,13 @@ static void media_processing_pause(void){ static void media_processing_close(void){ if (!media_initialized) return; + media_initialized = 0; audio_stream_started = 0; sbc_frame_size = 0; +#ifdef HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE + l2cap_stream_started = 0; +#endif #ifdef STORE_TO_WAV_FILE wav_writer_close(); @@ -498,18 +508,20 @@ static void handle_l2cap_media_data_packet(uint8_t seid, uint8_t *packet, uint16 avdtp_sbc_codec_header_t sbc_header; if (!read_sbc_header(packet, size, &pos, &sbc_header)) return; + int packet_length = size-pos; + uint8_t *packet_begin = packet+pos; + const btstack_audio_sink_t * audio = btstack_audio_sink_get_instance(); // process data right away if there's no audio implementation active, e.g. on posix systems to store as .wav if (!audio){ - btstack_sbc_decoder_process_data(&state, 0, packet+pos, size-pos); + btstack_sbc_decoder_process_data(&state, 0, packet_begin, packet_length); return; } // store sbc frame size for buffer management - sbc_frame_size = (size-pos)/ sbc_header.num_frames; - - int status = btstack_ring_buffer_write(&sbc_frame_ring_buffer, packet+pos, size-pos); + sbc_frame_size = packet_length / sbc_header.num_frames; + int status = btstack_ring_buffer_write(&sbc_frame_ring_buffer, packet_begin, packet_length); if (status != ERROR_CODE_SUCCESS){ printf("Error storing samples in SBC ring buffer!!!\n"); } @@ -517,11 +529,15 @@ static void handle_l2cap_media_data_packet(uint8_t seid, uint8_t *packet, uint16 // decide on audio sync drift based on number of sbc frames in queue int sbc_frames_in_buffer = btstack_ring_buffer_bytes_available(&sbc_frame_ring_buffer) / sbc_frame_size; #ifdef HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE + if( !l2cap_stream_started && audio_stream_started ) { + l2cap_stream_started = 1; + btstack_sample_rate_compensation_init( &sample_rate_compensation, btstack_run_loop_get_time_ms(), a2dp_sink_demo_a2dp_connection.sbc_configuration.sampling_frequency, FLOAT_TO_Q15(1.f) ); + } // update sample rate compensation if( audio_stream_started && (audio != NULL)) { uint32_t resampling_factor = btstack_sample_rate_compensation_update( &sample_rate_compensation, btstack_run_loop_get_time_ms(), sbc_header.num_frames*128, audio->get_samplerate() ); btstack_resample_set_factor(&resample_instance, resampling_factor); -// printf("sbc buffer level : %d\n", btstack_ring_buffer_bytes_available(&sbc_frame_ring_buffer)); +// printf("sbc buffer level : %"PRIu32"\n", btstack_ring_buffer_bytes_available(&sbc_frame_ring_buffer)); } #else uint32_t resampling_factor; diff --git a/port/esp32/components/btstack/btstack_audio_esp32_v5.c b/port/esp32/components/btstack/btstack_audio_esp32_v5.c index 52583d016..a3bb3bbf7 100644 --- a/port/esp32/components/btstack/btstack_audio_esp32_v5.c +++ b/port/esp32/components/btstack/btstack_audio_esp32_v5.c @@ -95,6 +95,7 @@ i2s_chan_handle_t rx_handle = NULL; #define BTSTACK_AUDIO_I2S_WS GPIO_NUM_25 #define BTSTACK_AUDIO_I2S_OUT GPIO_NUM_26 #define BTSTACK_AUDIO_I2S_IN GPIO_NUM_35 +#define HEADPHONE_DETECT GPIO_NUM_19 #endif // prototypes @@ -106,7 +107,7 @@ static void btstack_audio_esp32_source_process_buffer(void); #define BTSTACK_AUDIO_I2S_NUM (I2S_NUM_0) #define DRIVER_ISR_INTERVAL_MS 10 // dma interrupt cycle time in ms -#define DMA_BUFFER_COUNT 2 +#define DMA_BUFFER_COUNT 6 #define BYTES_PER_SAMPLE_STEREO 4 // one DMA buffer for max sample rate @@ -198,51 +199,35 @@ static void btstack_audio_esp32_stream_stop(void){ static IRAM_ATTR bool i2s_rx_callback(i2s_chan_handle_t handle, i2s_event_data_t *event, void *user_ctx) { - atomic_store_explicit(&isr_bytes_read, event->size, memory_order_relaxed); - if( isr_bytes_read > 0 ) { + size_t block_size = event->size; + atomic_fetch_add_explicit(&isr_bytes_read, block_size, memory_order_relaxed); + if( block_size > 0 ) { btstack_run_loop_poll_data_sources_from_irq(); return true; } return false; } -volatile int32_t delta; -volatile uint32_t count = 0; static IRAM_ATTR bool i2s_tx_callback(i2s_chan_handle_t handle, i2s_event_data_t *event, void *user_ctx) { -// isr_bytes_written = event->size; - static uint32_t last = 0; -// static uint32_t count = 0; - uint32_t current = btstack_run_loop_get_time_ms(); - if( last == 0 ) { - last = current; - } - count += event->size/BYTES_PER_SAMPLE_STEREO; - delta = current-last; - if( delta >= 1000 ) { - last = current; - count = 0; - } - - atomic_store_explicit(&isr_bytes_written, event->size, memory_order_relaxed); - if( isr_bytes_written > 0 ) { + size_t block_size = event->size; + atomic_fetch_add_explicit(&isr_bytes_written, block_size, memory_order_relaxed); + if( block_size > 0 ) { btstack_run_loop_poll_data_sources_from_irq(); return true; } return false; } -static uint8_t btstack_audio_esp32_buffer[MAX_DMA_BUFFER_SAMPLES * BYTES_PER_SAMPLE_STEREO]; +static uint8_t btstack_audio_esp32_buffer[MAX_DMA_BUFFER_SAMPLES * BYTES_PER_SAMPLE_STEREO * DMA_BUFFER_COUNT]; static void i2s_data_process(btstack_data_source_t *ds, btstack_data_source_callback_type_t callback_type) { size_t bytes_written = 0; size_t bytes_read = 0; uint32_t write_block_size = atomic_load_explicit(&isr_bytes_written, memory_order_relaxed); uint32_t read_block_size = atomic_load_explicit(&isr_bytes_read, memory_order_relaxed); -// ESP_LOGW(LOG_TAG, "d:%5"PRIu32" c:%5"PRIu32"", delta, count ); switch (callback_type){ case DATA_SOURCE_CALLBACK_POLL: { -// ESP_LOGW(LOG_TAG, "%s: w:%5"PRIu32" r:%5"PRIu32"", __FUNCTION__, write_block_size, read_block_size ); uint16_t num_samples = write_block_size/BYTES_PER_SAMPLE_STEREO; if( num_samples > 0 ) { (*btstack_audio_esp32_sink_playback_callback)((int16_t *) btstack_audio_esp32_buffer, num_samples); @@ -254,15 +239,23 @@ static void i2s_data_process(btstack_data_source_t *ds, btstack_data_source_call buffer16[2*i+1] = buffer16[i]; } } - ESP_ERROR_CHECK(i2s_channel_write(tx_handle, btstack_audio_esp32_buffer, write_block_size, &bytes_written, 0 )); - btstack_assert( bytes_written == write_block_size ); - atomic_store_explicit(&isr_bytes_written, 0, memory_order_relaxed); + esp_err_t ret = i2s_channel_write(tx_handle, btstack_audio_esp32_buffer, write_block_size, &bytes_written, 0 ); + if( ret == ESP_OK ) { + btstack_assert( bytes_written == write_block_size ); + } else if( ret == ESP_ERR_TIMEOUT ) { + ESP_LOGW(LOG_TAG, "audio output buffer underrun"); + } + atomic_fetch_sub_explicit( &isr_bytes_written, write_block_size, memory_order_relaxed ); } num_samples = read_block_size/BYTES_PER_SAMPLE_STEREO; if( num_samples > 0 ) { - ESP_ERROR_CHECK(i2s_channel_read(rx_handle, btstack_audio_esp32_buffer, read_block_size, &bytes_read, 0 )); - btstack_assert( bytes_read == read_block_size ); + esp_err_t ret = i2s_channel_read(rx_handle, btstack_audio_esp32_buffer, read_block_size, &bytes_read, 0 ); + if( ret == ESP_OK ) { + btstack_assert( bytes_read == read_block_size ); + } else if( ret == ESP_ERR_TIMEOUT ) { + ESP_LOGW(LOG_TAG, "audio input buffer overrun"); + } // drop second channel if configured for mono int16_t * buffer16 = (int16_t *)btstack_audio_esp32_buffer; if (btstack_audio_esp32_source_num_channels == 1){ @@ -271,7 +264,7 @@ static void i2s_data_process(btstack_data_source_t *ds, btstack_data_source_call } } (*btstack_audio_esp32_source_recording_callback)((int16_t *) btstack_audio_esp32_buffer, num_samples); - atomic_store_explicit(&isr_bytes_read, 0, memory_order_relaxed); + atomic_fetch_sub_explicit(&isr_bytes_read, read_block_size, memory_order_relaxed); } break; } @@ -288,13 +281,8 @@ static void i2s_data_process(btstack_data_source_t *ds, btstack_data_source_call * recv_buffer_size > dma_desc_num * dma_buffer_size = 3 * 4092 = 12276 bytes * */ -static void btstack_audio_esp32_init(void){ - - ESP_LOGW(LOG_TAG, "audio init"); - - // de-register driver if already installed - if (btstack_audio_esp32_i2s_installed){ - ESP_LOGW(LOG_TAG, "allready initialized, skipping init!\n"); +static void btstack_audio_esp32_init(void) { + if (btstack_audio_esp32_i2s_installed) { return; } @@ -354,8 +342,8 @@ static void btstack_audio_esp32_init(void){ ESP_ERROR_CHECK(i2s_channel_register_event_callback(rx_handle, &cbs, NULL)); ESP_ERROR_CHECK(i2s_channel_register_event_callback(tx_handle, &cbs, NULL)); -// log_info("i2s init samplerate %" PRIu32 ", samples per DMA buffer: %u", -// btstack_audio_esp32_sink_samplerate, btstack_audio_esp32_samples_per_dma_buffer); + log_info("i2s init samplerate %" PRIu32 ", samples per DMA buffer: %" PRIu32, + btstack_audio_esp32_sink_samplerate, chan_cfg.dma_frame_num); #ifdef CONFIG_ESP_LYRAT_V4_3_BOARD btstack_audio_esp32_es8388_init(); @@ -364,7 +352,6 @@ static void btstack_audio_esp32_init(void){ btstack_audio_esp32_i2s_installed = true; btstack_run_loop_set_data_source_handler(&transport_data_source, i2s_data_process); - ESP_LOGW(LOG_TAG, "audio init done."); } static void btstack_audio_esp32_deinit(void){ @@ -375,7 +362,6 @@ static void btstack_audio_esp32_deinit(void){ || (btstack_audio_esp32_source_state != BTSTACK_AUDIO_ESP32_OFF); if (still_needed) return; - // uninstall driver log_info("i2s close"); ESP_ERROR_CHECK(i2s_del_channel(rx_handle)); @@ -394,6 +380,7 @@ static int btstack_audio_esp32_sink_init( btstack_assert(playback != NULL); btstack_assert((1 <= channels) && (channels <= 2)); + btstack_assert( samplerate <= 48000 ); // store config btstack_audio_esp32_sink_playback_callback = playback; diff --git a/port/esp32/components/btstack/include/btstack_config.h b/port/esp32/components/btstack/include/btstack_config.h index 2bef1efe7..1239f485d 100644 --- a/port/esp32/components/btstack/include/btstack_config.h +++ b/port/esp32/components/btstack/include/btstack_config.h @@ -15,6 +15,8 @@ #define HAVE_FREERTOS_TASK_NOTIFICATIONS #define HAVE_MALLOC +#define HAVE_BTSTACK_AUDIO_EFFECTIVE_SAMPLERATE + // HCI Controller to Host Flow Control #define ENABLE_HCI_CONTROLLER_TO_HOST_FLOW_CONTROL