diff --git a/test/le_audio/CMakeLists.txt b/test/le_audio/CMakeLists.txt index cfe7931a8..48fb993ac 100644 --- a/test/le_audio/CMakeLists.txt +++ b/test/le_audio/CMakeLists.txt @@ -37,7 +37,7 @@ file(GLOB SOURCES_SRC "../../src/*.c" "../../src/*.cpp" "../../example/sco file(GLOB SOURCES_BLE "../../src/ble/*.c") file(GLOB SOURCES_GATT "../../src/ble/gatt-service/*.c") file(GLOB SOURCES_CLASSIC "../../src/classic/*.c") -file(GLOB SOURCES_LE_AUDIO "../../src/le-audio/*.c" "../../src/le-audio/gatt-service/*.c") +file(GLOB SOURCES_LE_AUDIO "../../src/le-audio/*.c" "../../src/le-audio/gatt-service/*.c" "../../example/le_audio_demo_util_*".c) file(GLOB SOURCES_MESH "../../src/mesh/*.c") file(GLOB SOURCES_MD5 "../../3rd-party/md5/md5.c") file(GLOB SOURCES_UECC "../../3rd-party/micro-ecc/uECC.c") diff --git a/test/le_audio/le_audio_broadcast_sink.c b/test/le_audio/le_audio_broadcast_sink.c index 5a60f9501..ff85e2ee5 100644 --- a/test/le_audio/le_audio_broadcast_sink.c +++ b/test/le_audio/le_audio_broadcast_sink.c @@ -73,17 +73,7 @@ #include "le-audio/gatt-service/broadcast_audio_scan_service_server.h" #include "le_audio_broadcast_sink.h" - -#ifdef HAVE_POSIX_FILE_IO -#include "wav_util.h" -#endif - -// #define DEBUG_PLC -#ifdef DEBUG_PLC -#define printf_plc(...) printf(__VA_ARGS__) -#else -#define printf_plc(...) (void)(0); -#endif +#include "le_audio_demo_util_sink.h" // max config #define MAX_NUM_BIS 2 @@ -94,17 +84,6 @@ #define MAX_BYTES_PER_SAMPLE 4 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_BYTES_PER_SAMPLE) -// analysis -#define PACKET_PREFIX_LEN 10 - -#define ANSI_COLOR_RED "\x1b[31m" -#define ANSI_COLOR_GREEN "\x1b[32m" -#define ANSI_COLOR_YELLOW "\x1b[33m" -#define ANSI_COLOR_BLUE "\x1b[34m" -#define ANSI_COLOR_MAGENTA "\x1b[35m" -#define ANSI_COLOR_CYAN "\x1b[36m" -#define ANSI_COLOR_RESET "\x1b[0m" - static void show_usage(void); static const char * filename_wav = "le_audio_broadcast_sink.wav"; @@ -160,15 +139,12 @@ static btstack_packet_callback_registration_t sm_event_callback_registration; static bool have_base; static bool have_big_info; -static bool have_past; static bool have_broadcast_code; static bool standalone_mode; static uint32_t bis_sync_mask; -uint32_t last_samples_report_ms; uint16_t samples_received; uint16_t samples_dropped; -uint16_t frames_per_second[MAX_NUM_BIS]; // remote info static char remote_name[20]; @@ -184,27 +160,15 @@ static bool nrf5340_audio_demo; static const uint8_t big_handle = 1; static hci_con_handle_t sync_handle; static hci_con_handle_t bis_con_handles[MAX_NUM_BIS]; -static unsigned int next_bis_index; static uint8_t encryption; static uint8_t broadcast_code[16]; static btstack_timer_source_t broadcast_sink_pa_sync_timer; -// analysis -static bool last_packet_received_big; -static uint16_t last_packet_sequence_big; -static bool last_packet_received[MAX_NUM_BIS]; -static uint16_t last_packet_sequence[MAX_NUM_BIS]; -static uint32_t last_packet_time_ms[MAX_NUM_BIS]; -static uint8_t last_packet_prefix[MAX_NUM_BIS * PACKET_PREFIX_LEN]; - // BIG Sync static le_audio_big_sync_t big_sync_storage; static le_audio_big_sync_params_t big_sync_params; -// lc3 writer -static uint32_t lc3_frames; - // lc3 codec config static uint16_t sampling_frequency_hz; static btstack_lc3_frame_duration_t frame_duration; @@ -212,87 +176,6 @@ static uint16_t number_samples_per_frame; static uint16_t octets_per_frame; static uint8_t num_bis; -// lc3 decoder -static bool request_lc3plus_decoder = false; -static bool use_lc3plus_decoder = false; -static const btstack_lc3_decoder_t * lc3_decoder; -static int16_t pcm[MAX_NUM_BIS * MAX_SAMPLES_PER_FRAME]; - -static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_NUM_BIS]; -#ifdef HAVE_LC3PLUS -static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_NUM_BIS]; -#endif -static void * decoder_contexts[MAX_NR_BIS]; - -// playback -static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; -static btstack_ring_buffer_t playback_buffer; - -static btstack_timer_source_t next_packet_timer; -static uint16_t cached_iso_sdu_len; -static bool have_pcm[MAX_NUM_BIS]; - -static void le_audio_broadcast_sink_playback(int16_t * buffer, uint16_t num_samples){ - // called from lower-layer but guaranteed to be on main thread - uint32_t bytes_needed = num_samples * num_bis * 2; - - static bool underrun = true; - - log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / ( num_bis * 2)); - - if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)){ - memset(buffer, 0, bytes_needed); - if (underrun == false){ - log_info("Playback underrun"); - underrun = true; - } - return; - } - - if (underrun){ - underrun = false; - log_info("Playback started"); - } - uint32_t bytes_read; - btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); - btstack_assert(bytes_read == bytes_needed); -} - -static void setup_lc3_decoder(void){ - uint8_t channel; - for (channel = 0 ; channel < num_bis ; channel++){ - // pick decoder - void * decoder_context = NULL; -#ifdef HAVE_LC3PLUS - if (use_lc3plus_decoder){ - decoder_context = &fraunhofer_decoder_contexts[channel]; - lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); - } - else -#endif - { - decoder_context = &google_decoder_contexts[channel]; - lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); - } - decoder_contexts[channel] = decoder_context; - lc3_decoder->configure(decoder_context, sampling_frequency_hz, frame_duration, octets_per_frame); - } - number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); - btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); -} - -static void close_files(void){ -#ifdef HAVE_POSIX_FILE_IO - printf("Close files\n"); - wav_writer_close(); -#endif - const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); - if (sink != NULL){ - sink->stop_stream(); - sink->close(); - } -} - static void handle_periodic_advertisement(const uint8_t * packet, uint16_t size){ // nRF534_audio quirk - no BASE in periodic advertisement if (nrf5340_audio_demo){ @@ -420,37 +303,11 @@ static void enter_create_big_sync(void){ // stop scanning gap_stop_scan(); - // switch to lc3plus if requested and possible - use_lc3plus_decoder = request_lc3plus_decoder && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); - - // init decoder - setup_lc3_decoder(); - - printf("Configure: %u channels, sampling rate %u, samples per frame %u, lc3plus %u\n", num_bis, sampling_frequency_hz, number_samples_per_frame, use_lc3plus_decoder); - -#ifdef HAVE_POSIX_FILE_IO - // create wav file - printf("WAV file: %s\n", filename_wav); - wav_writer_open(filename_wav, num_bis, sampling_frequency_hz); -#endif - - // init playback buffer - btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); - - // start playback - // PTS 8.2 sends stereo at half speed for stereo, for now playback at half speed - const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); - if (sink != NULL){ - uint16_t playback_speed; - if ((num_bis > 1) && pts_mode){ - playback_speed = sampling_frequency_hz / num_bis; - printf("PTS workaround: playback at %u hz\n", playback_speed); - } else { - playback_speed = sampling_frequency_hz; - }; - sink->init(num_bis, sampling_frequency_hz, le_audio_broadcast_sink_playback); - sink->start_stream(); - } + // init sink + uint16_t iso_interval_1250us = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 6 : 8; + uint8_t pre_transmission_offset = 1; + le_audio_demo_util_sink_configure_broadcast(num_bis, 1, sampling_frequency_hz, frame_duration, octets_per_frame, + iso_interval_1250us, pre_transmission_offset); big_sync_params.big_handle = big_handle; big_sync_params.sync_handle = sync_handle; @@ -665,11 +522,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack } printf("\n"); app_state = APP_STREAMING; - last_packet_received_big = false; - last_samples_report_ms = btstack_run_loop_get_time_ms(); - memset(last_packet_sequence, 0, sizeof(last_packet_sequence)); - memset(last_packet_received, 0, sizeof(last_packet_received)); - memset(pcm, 0, sizeof(pcm)); printf("Start receiving\n"); // update BIS Sync state @@ -679,7 +531,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack } case GAP_SUBEVENT_BIG_SYNC_STOPPED: printf("BIG Sync stopped, big_handle 0x%02x\n", gap_subevent_big_sync_stopped_get_big_handle(packet)); - btstack_run_loop_remove_timer(&next_packet_timer); break; default: break; @@ -694,219 +545,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack } } -static void store_samples_in_ringbuffer(void){ - // check if we have all channels - uint8_t bis_channel; - for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ - if (have_pcm[bis_channel] == false) return; - } -#ifdef HAVE_POSIX_FILE_IO - // write wav samples - wav_writer_write_int16(num_bis * number_samples_per_frame, pcm); -#endif - // store samples in playback buffer - uint32_t bytes_to_store = num_bis * number_samples_per_frame * 2; - samples_received += number_samples_per_frame; - if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { - btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm, bytes_to_store); - } else { - //printf("Samples dropped\n"); - samples_dropped += number_samples_per_frame; - } - // reset - for (bis_channel = 0; bis_channel < num_bis ; bis_channel++){ - have_pcm[bis_channel] = false; - } -} - -static void plc_do(uint8_t bis_channel) {// inject packet - uint8_t tmp_BEC_detect; - uint8_t BFI = 1; - (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], NULL, BFI, - &pcm[bis_channel], num_bis, - &tmp_BEC_detect); - - have_pcm[bis_channel] = true; - 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 (last_packet_sequence_big != packet_sequence_number){ - uint8_t i; - for (i=0;i 0) { - printf_plc("- BIS #%u, PLC for %u\n", i, last_packet_sequence_big); - plc_do(i); - btstack_assert((last_packet_sequence[i] + 1) == last_packet_sequence_big); - last_packet_sequence[i] = last_packet_sequence_big; - } - } - last_packet_sequence_big++; - } -} - -static void plc_timeout(btstack_timer_source_t * timer) { - if (app_state != APP_STREAMING) return; - - // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets - uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; - btstack_run_loop_set_timer(timer, frame_duration_ms); - btstack_run_loop_set_timer_handler(timer, plc_timeout); - btstack_run_loop_add_timer(timer); - - // assume no packet received in iso interval - plc_check(last_packet_sequence_big + 1); -} - -static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ - - if (app_state != APP_STREAMING) return; - - uint16_t header = little_endian_read_16(packet, 0); - hci_con_handle_t con_handle = header & 0x0fff; - uint8_t pb_flag = (header >> 12) & 3; - uint8_t ts_flag = (header >> 14) & 1; - uint16_t iso_load_len = little_endian_read_16(packet, 2); - - uint16_t offset = 4; - uint32_t time_stamp = 0; - if (ts_flag){ - uint32_t time_stamp = little_endian_read_32(packet, offset); - offset += 4; - } - - uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); - - uint16_t packet_sequence_number = little_endian_read_16(packet, offset); - offset += 2; - - uint16_t header_2 = little_endian_read_16(packet, offset); - uint16_t iso_sdu_length = header_2 & 0x3fff; - uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); - offset += 2; - - if (iso_sdu_length == 0) return; - - // infer channel from con handle - only works for up to 2 channels - uint8_t bis_channel = (con_handle == bis_con_handles[0]) ? 0 : 1; - - if (count_mode){ - // check for missing packet - uint16_t last_seq_no = last_packet_sequence[bis_channel]; - bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); - if (packet_missed){ - // print last packet - printf("\n"); - printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[bis_channel], bis_channel); - printf_hexdump(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); - last_seq_no++; - - printf(ANSI_COLOR_RED); - while (last_seq_no < packet_sequence_number){ - printf("%04x %u MISSING\n", last_seq_no, bis_channel); - last_seq_no++; - } - printf(ANSI_COLOR_RESET); - - // print current packet - printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, bis_channel); - printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); - } - - // cache current packet - memcpy(&last_packet_prefix[num_bis*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); - - } else { - - - if (last_packet_received[bis_channel]) { - - printf_plc("BIS #%u, receive %u\n", bis_channel, packet_sequence_number); - - int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, - last_packet_sequence[bis_channel]); - 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, last_packet_sequence[bis_channel]); - return; - } - // simple check - if (packet_sequence_number != last_packet_sequence[bis_channel] + 1) { - printf_plc("- BIS #%u, missing %u\n", bis_channel, last_packet_sequence[bis_channel] + 1); - } - } else { - - printf_plc("BIS %u, first packet seq number %u\n", bis_channel, packet_sequence_number); - - if (!last_packet_received_big) { - // track sequence number of very first received packet - last_packet_received_big = true; - last_packet_sequence_big = packet_sequence_number; - } - last_packet_received[bis_channel] = true; - } - - plc_check(packet_sequence_number); - - // decode codec frame - uint8_t tmp_BEC_detect; - uint8_t BFI = 0; - (void) lc3_decoder->decode_signed_16(decoder_contexts[bis_channel], &packet[offset], BFI, - &pcm[bis_channel], num_bis, - &tmp_BEC_detect); - have_pcm[bis_channel] = true; - store_samples_in_ringbuffer(); - - lc3_frames++; - frames_per_second[bis_channel]++; - - // PLC - cached_iso_sdu_len = iso_sdu_length; - uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; - uint32_t timeout_ms = frame_duration_ms * 5 / 2; - btstack_run_loop_remove_timer(&next_packet_timer); - btstack_run_loop_set_timer(&next_packet_timer, timeout_ms); - btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); - btstack_run_loop_add_timer(&next_packet_timer); - - uint32_t time_ms = btstack_run_loop_get_time_ms(); - if (btstack_time_delta(time_ms, last_samples_report_ms) >= 1000){ - last_samples_report_ms = time_ms; - printf("LC3 Frames: %4u - ", (int) (lc3_frames / num_bis)); - uint8_t i; - for (i=0;i LE_AUDIO_PA_SYNC_STATE_NO_PAST\n"); diff --git a/test/le_audio/le_audio_broadcast_source.c b/test/le_audio/le_audio_broadcast_source.c index 440b08337..48f3fa9f8 100644 --- a/test/le_audio/le_audio_broadcast_source.c +++ b/test/le_audio/le_audio_broadcast_source.c @@ -60,6 +60,7 @@ #include "hxcmod.h" #include "mods/mod.h" +#include "le_audio_demo_util_source.h" // PTS mode // #define PTS_MODE @@ -118,42 +119,6 @@ static const le_periodic_advertising_parameters_t periodic_params = { .periodic_advertising_properties = 0 }; -// input signal: pre-computed int16 sine wave, 96000 Hz at 300 Hz -static const int16_t sine_int16[] = { - 0, 643, 1286, 1929, 2571, 3212, 3851, 4489, 5126, 5760, - 6393, 7022, 7649, 8273, 8894, 9512, 10126, 10735, 11341, 11943, - 12539, 13131, 13718, 14300, 14876, 15446, 16011, 16569, 17121, 17666, - 18204, 18736, 19260, 19777, 20286, 20787, 21280, 21766, 22242, 22710, - 23170, 23620, 24062, 24494, 24916, 25329, 25732, 26126, 26509, 26882, - 27245, 27597, 27938, 28269, 28589, 28898, 29196, 29482, 29757, 30021, - 30273, 30513, 30742, 30958, 31163, 31356, 31537, 31705, 31862, 32006, - 32137, 32257, 32364, 32458, 32540, 32609, 32666, 32710, 32742, 32761, - 32767, 32761, 32742, 32710, 32666, 32609, 32540, 32458, 32364, 32257, - 32137, 32006, 31862, 31705, 31537, 31356, 31163, 30958, 30742, 30513, - 30273, 30021, 29757, 29482, 29196, 28898, 28589, 28269, 27938, 27597, - 27245, 26882, 26509, 26126, 25732, 25329, 24916, 24494, 24062, 23620, - 23170, 22710, 22242, 21766, 21280, 20787, 20286, 19777, 19260, 18736, - 18204, 17666, 17121, 16569, 16011, 15446, 14876, 14300, 13718, 13131, - 12539, 11943, 11341, 10735, 10126, 9512, 8894, 8273, 7649, 7022, - 6393, 5760, 5126, 4489, 3851, 3212, 2571, 1929, 1286, 643, - 0, -643, -1286, -1929, -2571, -3212, -3851, -4489, -5126, -5760, - -6393, -7022, -7649, -8273, -8894, -9512, -10126, -10735, -11341, -11943, - -12539, -13131, -13718, -14300, -14876, -15446, -16011, -16569, -17121, -17666, - -18204, -18736, -19260, -19777, -20286, -20787, -21280, -21766, -22242, -22710, - -23170, -23620, -24062, -24494, -24916, -25329, -25732, -26126, -26509, -26882, - -27245, -27597, -27938, -28269, -28589, -28898, -29196, -29482, -29757, -30021, - -30273, -30513, -30742, -30958, -31163, -31356, -31537, -31705, -31862, -32006, - -32137, -32257, -32364, -32458, -32540, -32609, -32666, -32710, -32742, -32761, - -32767, -32761, -32742, -32710, -32666, -32609, -32540, -32458, -32364, -32257, - -32137, -32006, -31862, -31705, -31537, -31356, -31163, -30958, -30742, -30513, - -30273, -30021, -29757, -29482, -29196, -28898, -28589, -28269, -27938, -27597, - -27245, -26882, -26509, -26126, -25732, -25329, -24916, -24494, -24062, -23620, - -23170, -22710, -22242, -21766, -21280, -20787, -20286, -19777, -19260, -18736, - -18204, -17666, -17121, -16569, -16011, -15446, -14876, -14300, -13718, -13131, - -12539, -11943, -11341, -10735, -10126, -9512, -8894, -8273, -7649, -7022, - -6393, -5760, -5126, -4489, -3851, -3212, -2571, -1929, -1286, -643, -}; - static bd_addr_t remote; static const char * remote_addr_string = "00:1B:DC:08:E2:72"; @@ -188,35 +153,16 @@ static uint16_t number_samples_per_frame; static uint16_t octets_per_frame; static uint8_t num_bis = 1; -// lc3 encoder -static const btstack_lc3_encoder_t * lc3_encoder; -static btstack_lc3_encoder_google_t encoder_contexts[MAX_NUM_BIS]; -static int16_t pcm[MAX_NUM_BIS * MAX_SAMPLES_PER_FRAME]; -static uint8_t iso_payload[MAX_NUM_BIS * MAX_LC3_FRAME_BYTES]; -static uint32_t time_generation_ms; - // codec menu static uint8_t menu_sampling_frequency; static uint8_t menu_variant; -// mod player -static int hxcmod_initialized; -static modcontext mod_context; -static tracker_buffer_state trkbuf; - -// sine generator -static uint8_t sine_step; -static uint16_t sine_phases[MAX_NUM_BIS]; - // encryption static uint8_t encryption = 0; static uint8_t broadcast_code [] = {0x01, 0x02, 0x68, 0x05, 0x53, 0xF1, 0x41, 0x5A, 0xA2, 0x65, 0xBB, 0xAF, 0xC6, 0xEA, 0x03, 0xB8, }; // audio producer -static enum { - AUDIO_SOURCE_SINE, - AUDIO_SOURCE_MODPLAYER -} audio_source = AUDIO_SOURCE_MODPLAYER; +static le_audio_demo_source_generator audio_source = AUDIO_SOURCE_MODPLAYER; static enum { APP_IDLE, @@ -295,130 +241,6 @@ static void print_config(void) { audio_source == AUDIO_SOURCE_SINE ? "Sine" : "Modplayer", encryption ? " (encrypted)" : ""); } -static void setup_lc3_encoder(void){ - uint8_t channel; - for (channel = 0 ; channel < num_bis ; channel++){ - btstack_lc3_encoder_google_t * context = &encoder_contexts[channel]; - lc3_encoder = btstack_lc3_encoder_google_init_instance(context); - lc3_encoder->configure(context, sampling_frequency_hz, frame_duration, octets_per_frame); - } - number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); - btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); - printf("LC3 Encoder config: %u hz, frame duration %s ms, num samples %u, num octets %u\n", - sampling_frequency_hz, frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? "7.5" : "10", - number_samples_per_frame, octets_per_frame); -} - -static void setup_mod_player(void){ - if (!hxcmod_initialized) { - hxcmod_initialized = hxcmod_init(&mod_context); - btstack_assert(hxcmod_initialized != 0); - } - hxcmod_unload(&mod_context); - hxcmod_setcfg(&mod_context, sampling_frequency_hz, 16, 1, 1, 1); - hxcmod_load(&mod_context, (void *) &mod_data, mod_len); -} - -static void generate_audio(void){ - uint32_t start_ms = btstack_run_loop_get_time_ms(); - uint16_t sample; - switch (audio_source) { - case AUDIO_SOURCE_SINE: - // generate sine wave for all channels - for (sample = 0 ; sample < number_samples_per_frame ; sample++){ - uint8_t channel; - for (channel = 0; channel < num_bis; channel++) { - int16_t value = sine_int16[sine_phases[channel]] / 4; - pcm[sample * num_bis + channel] = value; - sine_phases[channel] += sine_step * (1+channel); // second channel, double frequency - if (sine_phases[channel] >= (sizeof(sine_int16) / sizeof(int16_t))) { - sine_phases[channel] = 0; - } - } - } - break; - case AUDIO_SOURCE_MODPLAYER: - // mod player configured for stereo - hxcmod_fillbuffer(&mod_context, (unsigned short *) pcm, number_samples_per_frame, &trkbuf); - if (num_bis == 1) { - // stereo -> mono - uint16_t i; - for (i=0;iencode_signed_16(&encoder_contexts[bis_index], &pcm[bis_index], num_bis, &iso_payload[bis_index * MAX_LC3_FRAME_BYTES]); -} - - -static void send_iso_packet(uint8_t bis_index) { - -#ifdef COUNT_MODE - if (bis_index == 0) { - uint32_t now = btstack_run_loop_get_time_ms(); - if (send_last_ms != 0) { - uint16_t send_interval_ms = now - send_last_ms; - if (send_interval_ms >= MAX_PACKET_INTERVAL_BINS_MS) { - printf("ERROR: send interval %u\n", send_interval_ms); - } else { - send_time_bins[send_interval_ms]++; - } - } - send_last_ms = now; - } -#endif - bool ok = hci_reserve_packet_buffer(); - btstack_assert(ok); - uint8_t * buffer = hci_get_outgoing_packet_buffer(); - // complete SDU, no TimeStamp - little_endian_store_16(buffer, 0, bis_con_handles[bis_index] | (2 << 12)); - // len - little_endian_store_16(buffer, 2, 0 + 4 + octets_per_frame); - // TimeStamp if TS flag is set - // packet seq nr - little_endian_store_16(buffer, 4, packet_sequence_numbers[bis_index]); - // iso sdu len - little_endian_store_16(buffer, 6, octets_per_frame); -#ifdef COUNT_MODE - // test data: bis_index, counter - buffer[8] = bis_index; - memset(&buffer[9], iso_frame_counter, octets_per_frame - 1); -#else - // copy encoded payload - memcpy(&buffer[8], &iso_payload[bis_index * MAX_LC3_FRAME_BYTES], octets_per_frame); -#endif - // send - hci_send_iso_packet_buffer(4 + 0 + 4 + octets_per_frame); - -#ifdef HAVE_POSIX_FILE_IO - if (((packet_sequence_numbers[bis_index] & 0x7f) == 0) && (bis_index == 0)) { - printf("Encoding time: %u\n", time_generation_ms); - } -#endif - - packet_sequence_numbers[bis_index]++; -} - -static void generate_audio_and_encode(void){ - uint8_t i; - generate_audio(); - for (i = 0; i < num_bis; i++) { - encode(i); - bis_has_data[i] = true; - } -} - static void setup_advertising() { bd_addr_t local_addr; gap_local_bd_addr(local_addr); @@ -472,8 +294,14 @@ static void start_broadcast() {// use values from table octets_per_frame = codec_configurations[menu_sampling_frequency].variants[menu_variant].octets_per_frame; frame_duration = codec_configurations[menu_sampling_frequency].variants[menu_variant].frame_duration; - // get num samples per frame - setup_lc3_encoder(); + number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); + + printf("LC3 Encoder config: %u hz, frame duration %s ms, num samples %u, num octets %u\n", + sampling_frequency_hz, frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? "7.5" : "10", + number_samples_per_frame, octets_per_frame); + + le_audio_demo_util_source_configure(num_bis, 1, sampling_frequency_hz, frame_duration, octets_per_frame); + le_audio_demo_util_source_generate_iso_frame(audio_source); // setup base uint8_t codec_id[] = { 0x06, 0x00, 0x00, 0x00, 0x00 }; @@ -508,16 +336,6 @@ static void start_broadcast() {// use values from table } period_adv_data_len = le_audio_base_builder_get_ad_data_size(&builder); - // setup mod player - setup_mod_player(); - - // setup sine generator - if (sampling_frequency_hz == 44100){ - sine_step = 2; - } else { - sine_step = 96000 / sampling_frequency_hz; - } - // setup extended and periodic advertising setup_advertising(); @@ -564,7 +382,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack app_state = APP_STREAMING; printf("Start streaming\n"); - generate_audio_and_encode(); hci_request_bis_can_send_now_events(big_params.big_handle); break; default: @@ -573,10 +390,10 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack break; case HCI_EVENT_BIS_CAN_SEND_NOW: bis_index = hci_event_bis_can_send_now_get_bis_index(packet); - send_iso_packet(bis_index); + le_audio_demo_util_source_send(bis_index, bis_con_handles[bis_index]); bis_index++; if (bis_index == num_bis){ - generate_audio_and_encode(); + le_audio_demo_util_source_generate_iso_frame(audio_source); hci_request_bis_can_send_now_events(big_params.big_handle); } break; @@ -669,6 +486,9 @@ int btstack_main(int argc, const char * argv[]){ hci_event_callback_registration.callback = &packet_handler; hci_add_event_handler(&hci_event_callback_registration); + // setup audio processing + le_audio_demo_util_source_init(); + // turn on! hci_power_control(HCI_POWER_ON); diff --git a/test/le_audio/le_audio_unicast_sink.c b/test/le_audio/le_audio_unicast_sink.c index aaff8a2fe..a4cacad29 100644 --- a/test/le_audio/le_audio_unicast_sink.c +++ b/test/le_audio/le_audio_unicast_sink.c @@ -49,65 +49,24 @@ #include #include #include -#include -#include -#include // open -#include #include "ad_parser.h" #include "bluetooth_data_types.h" #include "bluetooth_company_id.h" -#include "bluetooth_gatt.h" #include "btstack_debug.h" -#include "btstack_audio.h" #include "btstack_event.h" -#include "btstack_run_loop.h" -#include "btstack_ring_buffer.h" #include "btstack_stdin.h" -#include "btstack_util.h" #include "gap.h" #include "hci.h" -#include "hci_cmd.h" -#include "btstack_lc3.h" -#include "btstack_lc3_google.h" -#include "btstack_lc3plus_fraunhofer.h" - -#ifdef HAVE_POSIX_FILE_IO -#include "wav_util.h" -#endif - -//#define DEBUG_PLC -#ifdef DEBUG_PLC -#define printf_plc(...) printf(__VA_ARGS__) -#else -#define printf_plc(...) (void)(0); -#endif +#include "le_audio_demo_util_sink.h" +#include "le_audio_demo_util_source.h" // max config #define MAX_CHANNELS 2 #define MAX_SAMPLES_PER_FRAME 480 -// playback -#define MAX_NUM_LC3_FRAMES 15 -#define MAX_BYTES_PER_SAMPLE 4 -#define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_BYTES_PER_SAMPLE) -#define PLAYBACK_START_MS (MAX_NUM_LC3_FRAMES * 20 / 3) - -// analysis -#define PACKET_PREFIX_LEN 10 - -#define ANSI_COLOR_RED "\x1b[31m" -#define ANSI_COLOR_GREEN "\x1b[32m" -#define ANSI_COLOR_YELLOW "\x1b[33m" -#define ANSI_COLOR_BLUE "\x1b[34m" -#define ANSI_COLOR_MAGENTA "\x1b[35m" -#define ANSI_COLOR_CYAN "\x1b[36m" -#define ANSI_COLOR_RESET "\x1b[0m" - static void show_usage(void); -static const char * filename_wav = "le_audio_unicast_sink.wav"; - static enum { APP_W4_WORKING, APP_W4_SOURCE_ADV, @@ -126,19 +85,16 @@ static bd_addr_t remote_addr; static bd_addr_type_t remote_type; static hci_con_handle_t remote_handle; -static bool count_mode; -static bool pts_mode; - static le_audio_cig_t cig; static le_audio_cig_params_t cig_params; // iso info static bool framed_pdus; static uint16_t frame_duration_us; + static uint8_t num_cis; static hci_con_handle_t cis_con_handles[MAX_CHANNELS]; static bool cis_established[MAX_CHANNELS]; -static unsigned int next_cis_index; // lc3 codec config static uint16_t sampling_frequency_hz; @@ -147,134 +103,8 @@ static uint16_t number_samples_per_frame; static uint16_t octets_per_frame; static uint8_t num_channels; -// lc3 decoder -static bool request_lc3plus_decoder = false; -static bool use_lc3plus_decoder = false; -static const btstack_lc3_decoder_t * lc3_decoder; -static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; - -static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS]; -#ifdef HAVE_LC3PLUS -static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_CHANNELS]; -#endif -static void * decoder_contexts[MAX_NR_BIS]; - -// playback -static uint16_t playback_start_threshold_bytes; -static bool playback_active; -static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; -static btstack_ring_buffer_t playback_buffer; - -// statistics -static bool last_packet_received[MAX_CHANNELS]; -static uint16_t last_packet_sequence[MAX_CHANNELS]; -static uint32_t last_packet_time_ms[MAX_CHANNELS]; -static uint8_t last_packet_prefix[MAX_CHANNELS * PACKET_PREFIX_LEN]; - -static uint32_t lc3_frames; -static uint32_t samples_received; -static uint32_t samples_played; -static uint32_t samples_dropped; -static uint32_t last_samples_report_ms; - -// PLC state -#define PLC_GUARD_MS 1 - -static btstack_timer_source_t next_packet_timer; -static uint16_t cached_iso_sdu_len; - -static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_samples){ - // called from lower-layer but guaranteed to be on main thread - log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / (num_channels * 2)); - - samples_played += num_samples; - - uint32_t bytes_needed = num_samples * num_channels * 2; - if (playback_active == false){ - if (btstack_ring_buffer_bytes_available(&playback_buffer) >= playback_start_threshold_bytes) { - log_info("Playback started"); - playback_active = true; - } - } else { - if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)) { - log_info("Playback underrun"); - printf("Playback Underrun\n"); - // empty buffer - uint32_t bytes_read; - btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); - playback_active = false; - } - } - - if (playback_active){ - uint32_t bytes_read; - btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); - btstack_assert(bytes_read == bytes_needed); - } else { - memset(buffer, 0, bytes_needed); - } -} - -static void setup_lc3_decoder(void){ - uint8_t channel; - for (channel = 0 ; channel < num_channels ; channel++){ - // pick decoder - void * decoder_context = NULL; -#ifdef HAVE_LC3PLUS - if (use_lc3plus_decoder){ - decoder_context = &fraunhofer_decoder_contexts[channel]; - lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); - } - else -#endif - { - decoder_context = &google_decoder_contexts[channel]; - lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); - } - decoder_contexts[channel] = decoder_context; - lc3_decoder->configure(decoder_context, sampling_frequency_hz, frame_duration, octets_per_frame); - } - number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); - btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); -} - -static void close_files(void){ -#ifdef HAVE_POSIX_FILE_IO - printf("Close files\n"); - wav_writer_close(); -#endif -} - -static void enter_streaming(void){ - - // switch to lc3plus if requested and possible - use_lc3plus_decoder = request_lc3plus_decoder && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); - - // init decoder - setup_lc3_decoder(); - - printf("Configure: %u channels, sampling rate %u, samples per frame %u, lc3plus %u\n", num_channels, sampling_frequency_hz, number_samples_per_frame, use_lc3plus_decoder); - -#ifdef HAVE_POSIX_FILE_IO - // create wav file - printf("WAV file: %s\n", filename_wav); - wav_writer_open(filename_wav, num_channels, sampling_frequency_hz); -#endif - - // init playback buffer - btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); - - // calc start threshold in bytes for PLAYBACK_START_MS - playback_start_threshold_bytes = (sampling_frequency_hz / 1000 * PLAYBACK_START_MS) * num_channels * 2; - - // start playback - const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); - if (sink != NULL){ - sink->init(num_channels, sampling_frequency_hz, le_audio_connection_sink_playback); - sink->start_stream(); - } - -} +// microphone +static bool microphone_enable; static void start_scanning() { app_state = APP_W4_SOURCE_ADV; @@ -306,9 +136,10 @@ static void create_cig() { cig_params.max_transport_latency_c_to_p = 40; cig_params.max_transport_latency_p_to_c = 40; uint8_t i; + uint16_t max_sdu_c_to_p = microphone_enable ? octets_per_frame : 0; for (i=0; i < num_cis; i++){ cig_params.cis_params[i].cis_id = i; - cig_params.cis_params[i].max_sdu_c_to_p = 0; + cig_params.cis_params[i].max_sdu_c_to_p = max_sdu_c_to_p; cig_params.cis_params[i].max_sdu_p_to_c = num_channels * octets_per_frame; cig_params.cis_params[i].phy_c_to_p = 2; // 2M cig_params.cis_params[i].phy_p_to_c = 2; // 2M @@ -320,11 +151,27 @@ static void create_cig() { gap_cig_create(&cig, &cig_params); } +static void enter_streaming(void){ + // init source + if (microphone_enable){ + le_audio_demo_util_source_configure(1, 1, sampling_frequency_hz, frame_duration, octets_per_frame); + le_audio_demo_util_source_generate_iso_frame(AUDIO_SOURCE_SINE); + } + // init sink + uint16_t iso_interval_1250us = frame_duration_us / 1250; + uint8_t flush_timeout = 1; + le_audio_demo_util_sink_configure_unicast(1, num_channels, sampling_frequency_hz, frame_duration, octets_per_frame, + iso_interval_1250us, flush_timeout); + printf("Configure: %u channels, sampling rate %u, samples per frame %u\n", num_channels, sampling_frequency_hz, number_samples_per_frame); +} + + static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ UNUSED(channel); bd_addr_t event_addr; hci_con_handle_t cis_handle; unsigned int i; + uint8_t status; if (packet_type != HCI_EVENT_PACKET) return; switch (packet[0]) { case BTSTACK_EVENT_STATE: @@ -348,15 +195,7 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack case HCI_EVENT_DISCONNECTION_COMPLETE: if (hci_event_disconnection_complete_get_connection_handle(packet) == remote_handle){ printf("Disconnected, back to scanning\n"); - // stop timer - btstack_run_loop_remove_timer(&next_packet_timer); - // close files - close_files(); - // stop playback - const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); - if (sink != NULL){ - sink->stop_stream(); - } + le_audio_demo_util_sink_close(); start_scanning(); } break; @@ -386,8 +225,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack if (subtype != 0) break; // flags uint8_t flags = data[3]; - pts_mode = (flags & 1) != 0; - count_mode = (flags & 2) != 0; // num channels num_channels = data[4]; if (num_channels > 2) break; @@ -413,9 +250,7 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack } if (!found) break; remote_type = gap_event_advertising_report_get_address_type(packet); - pts_mode = false; - count_mode = false; - printf("Remote Unicast source found, addr %s, name: '%s' (pts-mode: %u, count: %u)\n", bd_addr_to_str(remote_addr), remote_name, pts_mode, count_mode); + printf("Remote Unicast source found, addr %s, name: '%s'\n", bd_addr_to_str(remote_addr), remote_name); // stop scanning app_state = APP_W4_CIS_CREATED; gap_stop_scan(); @@ -446,12 +281,7 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack printf("0x%04x ", cis_con_handles[i]); } printf("\n"); - next_cis_index = 0; - - memset(last_packet_sequence, 0, sizeof(last_packet_sequence)); - memset(last_packet_received, 0, sizeof(last_packet_received)); - memset(pcm, 0, sizeof(pcm)); - + printf("Create CIS\n"); hci_con_handle_t acl_connection_handles[MAX_CHANNELS]; for (i=0; i < num_cis; i++){ @@ -462,207 +292,48 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack } break; case GAP_SUBEVENT_CIS_CREATED: - // only look for cis handle + status = gap_subevent_big_created_get_status(packet); cis_handle = gap_subevent_cis_created_get_cis_con_handle(packet); - for (i=0; i < num_cis; i++){ - if (cis_handle == cis_con_handles[i]){ - cis_established[i] = true; + if (status == ERROR_CODE_SUCCESS){ + // only look for cis handle + for (i=0; i < num_cis; i++){ + if (cis_handle == cis_con_handles[i]){ + cis_established[i] = true; + } } - } - // check for complete - bool complete = true; - for (i=0; i < num_cis; i++) { - complete &= cis_established[i]; - } - if (complete) { - printf("All CIS Established\n"); - next_cis_index = 0; - app_state = APP_STREAMING; + // check for complete + bool complete = true; + for (i=0; i < num_cis; i++) { + complete &= cis_established[i]; + } + if (complete) { + printf("All CIS Established\n"); + app_state = APP_STREAMING; + // start sending + if (microphone_enable){ + hci_request_cis_can_send_now_events(cis_con_handles[0]); + } + } + } else { + printf("CIS Create failed with status 0x%02x for con handle 0x%04x\n", status, cis_handle); } break; default: break; } break; + case HCI_EVENT_CIS_CAN_SEND_NOW: + le_audio_demo_util_source_send(0, cis_con_handles[0]); + le_audio_demo_util_source_generate_iso_frame(AUDIO_SOURCE_SINE); + hci_request_cis_can_send_now_events(cis_con_handles[0]); + break; default: break; } } -static void store_samples_in_ringbuffer(void){ -#ifdef HAVE_POSIX_FILE_IO - // write wav samples - wav_writer_write_int16(num_channels * number_samples_per_frame, pcm); -#endif - // store samples in playback buffer - uint32_t bytes_to_store = num_channels * number_samples_per_frame * 2; - samples_received += number_samples_per_frame; - if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { - btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm, bytes_to_store); - } else { - printf("Samples dropped\n"); - samples_dropped += number_samples_per_frame; - } -} - -static void plc_do(void) { - // inject packet - uint8_t tmp_BEC_detect; - uint8_t BFI = 1; - uint8_t channel; - for (channel = 0; channel < num_channels; channel++){ - (void) lc3_decoder->decode_signed_16(decoder_contexts[channel], NULL, BFI, - &pcm[channel], num_channels, - &tmp_BEC_detect); - } - - store_samples_in_ringbuffer(); -} - -static void plc_timeout(btstack_timer_source_t * timer) { - // set timer again - uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; - btstack_run_loop_set_timer(&next_packet_timer, frame_duration_ms); - btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); - btstack_run_loop_add_timer(&next_packet_timer); - - printf_plc("- ISO, PLC for timeout\n"); - plc_do(); -} - -static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ - - uint16_t header = little_endian_read_16(packet, 0); - hci_con_handle_t con_handle = header & 0x0fff; - uint8_t pb_flag = (header >> 12) & 3; - uint8_t ts_flag = (header >> 14) & 1; - uint16_t iso_load_len = little_endian_read_16(packet, 2); - - uint16_t offset = 4; - uint32_t time_stamp = 0; - if (ts_flag){ - uint32_t time_stamp = little_endian_read_32(packet, offset); - offset += 4; - } - - uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); - - uint16_t packet_sequence_number = little_endian_read_16(packet, offset); - offset += 2; - - uint16_t header_2 = little_endian_read_16(packet, offset); - uint16_t iso_sdu_length = header_2 & 0x3fff; - uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); - offset += 2; - - if (iso_sdu_length == 0) return; - - if (iso_sdu_length != num_channels * octets_per_frame) { - printf("ISO Length %u != %u * %u\n", iso_sdu_length, num_channels, octets_per_frame); - } - - // infer channel from con handle - only works for up to 2 channels - uint8_t cis_channel = (con_handle == cis_con_handles[0]) ? 0 : 1; - - if (count_mode){ - // check for missing packet - uint16_t last_seq_no = last_packet_sequence[cis_channel]; - uint32_t now = btstack_run_loop_get_time_ms(); - bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number); - if (packet_missed){ - // print last packet - printf("\n"); - printf("%04x %10u %u ", last_seq_no, last_packet_time_ms[cis_channel], cis_channel); - printf_hexdump(&last_packet_prefix[num_channels * PACKET_PREFIX_LEN], PACKET_PREFIX_LEN); - last_seq_no++; - - printf(ANSI_COLOR_RED); - while (last_seq_no < packet_sequence_number){ - printf("%04x %u MISSING\n", last_seq_no, cis_channel); - last_seq_no++; - } - printf(ANSI_COLOR_RESET); - - // print current packet - printf("%04x %10u %u ", packet_sequence_number, now, cis_channel); - printf_hexdump(&packet[offset], PACKET_PREFIX_LEN); - } - - // cache current packet - last_packet_time_ms[cis_channel] = now; - last_packet_sequence[cis_channel] = packet_sequence_number; - memcpy(&last_packet_prefix[num_channels * PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN); - - } else { - - if (last_packet_received[cis_channel]) { - printf_plc("ISO, receive %u\n", packet_sequence_number); - - int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, - last_packet_sequence[cis_channel]); - 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, last_packet_sequence[cis_channel]); - return; - } - // simple check - if (packet_sequence_number != last_packet_sequence[cis_channel] + 1) { - printf_plc("- BIS #%u, missing %u\n", cis_channel, last_packet_sequence[cis_channel] + 1); - } - } else { - printf_plc("BIS %u, first packet seq number %u\n", cis_channel, packet_sequence_number); - last_packet_received[cis_channel] = true; - } - - // PLC for missing packets - while (true) { - uint16_t expected = last_packet_sequence[cis_channel]+1; - if (expected == packet_sequence_number){ - break; - } - printf_plc("- ISO, PLC for %u\n", expected); - plc_do(); - last_packet_sequence[cis_channel] = expected; - } - - uint8_t channel; - for (channel = 0 ; channel < num_channels ; channel++){ - - // decode codec frame - uint8_t tmp_BEC_detect; - uint8_t BFI = 0; - (void) lc3_decoder->decode_signed_16(decoder_contexts[channel], &packet[offset], BFI, - &pcm[channel], num_channels, - &tmp_BEC_detect); - offset += octets_per_frame; - } - - store_samples_in_ringbuffer(); - - log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (num_channels * 2)); - - lc3_frames++; - - // PLC - cached_iso_sdu_len = iso_sdu_length; - uint32_t frame_duration_ms = frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; - uint32_t timeout_ms = frame_duration_ms * 5 / 2; - btstack_run_loop_remove_timer(&next_packet_timer); - btstack_run_loop_set_timer(&next_packet_timer, timeout_ms); - btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); - btstack_run_loop_add_timer(&next_packet_timer); - - if (samples_received >= sampling_frequency_hz){ - printf("LC3 Frames: %4u - samples received %5u, played %5u, dropped %5u\n", lc3_frames, samples_received, samples_played, samples_dropped); - samples_received = 0; - samples_dropped = 0; - samples_played = 0; - } - - last_packet_time_ms[cis_channel] = receive_time_ms; - last_packet_sequence[cis_channel] = packet_sequence_number; - } +static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { + le_audio_demo_util_sink_receive(0, packet, size); } static void show_usage(void){ @@ -671,6 +342,7 @@ static void show_usage(void){ #ifdef HAVE_LC3PLUS printf("q - use LC3plus decoder if 10 ms ISO interval is used\n"); #endif + printf("m - enable virtual microphone\n"); printf("---\n"); } @@ -682,9 +354,14 @@ static void stdin_process(char c){ break; #ifdef HAVE_LC3PLUS case 'q': - request_lc3plus_decoder = true; + printf("Use LC3 Plust for 10 ms frames\n"); + le_audio_demo_sink_enable_lc3plus(true); break; #endif + case 'm': + printf("Enable virtual microphone\n"); + microphone_enable = true; + break; case '\n': case '\r': break; @@ -707,6 +384,10 @@ int btstack_main(int argc, const char * argv[]){ // register for ISO Packet hci_register_iso_packet_handler(&iso_packet_handler); + // setup audio processing + le_audio_demo_util_sink_init("le_audio_unicast_sink.wav"); + le_audio_demo_util_source_init(); + // turn on! hci_power_control(HCI_POWER_ON); diff --git a/test/le_audio/le_audio_unicast_source.c b/test/le_audio/le_audio_unicast_source.c index 50ad452ac..60d5fa90f 100644 --- a/test/le_audio/le_audio_unicast_source.c +++ b/test/le_audio/le_audio_unicast_source.c @@ -51,22 +51,16 @@ #include "bluetooth_company_id.h" #include "btstack_stdin.h" #include "btstack_event.h" -#include "btstack_run_loop.h" #include "gap.h" #include "hci.h" -#include "hci_cmd.h" -#include "hci_dump.h" #include "btstack_lc3.h" #include "btstack_lc3_google.h" - -#include "hxcmod.h" -#include "mods/mod.h" +#include "le_audio_demo_util_source.h" +#include "le_audio_demo_util_sink.h" // max config #define MAX_CHANNELS 2 #define MAX_NUM_CIS 1 -#define MAX_SAMPLES_PER_FRAME 480 -#define MAX_LC3_FRAME_BYTES 155 static uint8_t adv_data[] = { @@ -85,56 +79,15 @@ static uint8_t adv_data[] = { 7, BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME, 'S', 'o', 'u', 'r', 'c', 'e' }; -// input signal: pre-computed int16 sine wave, 96000 Hz at 300 Hz -static const int16_t sine_int16[] = { - 0, 643, 1286, 1929, 2571, 3212, 3851, 4489, 5126, 5760, - 6393, 7022, 7649, 8273, 8894, 9512, 10126, 10735, 11341, 11943, - 12539, 13131, 13718, 14300, 14876, 15446, 16011, 16569, 17121, 17666, - 18204, 18736, 19260, 19777, 20286, 20787, 21280, 21766, 22242, 22710, - 23170, 23620, 24062, 24494, 24916, 25329, 25732, 26126, 26509, 26882, - 27245, 27597, 27938, 28269, 28589, 28898, 29196, 29482, 29757, 30021, - 30273, 30513, 30742, 30958, 31163, 31356, 31537, 31705, 31862, 32006, - 32137, 32257, 32364, 32458, 32540, 32609, 32666, 32710, 32742, 32761, - 32767, 32761, 32742, 32710, 32666, 32609, 32540, 32458, 32364, 32257, - 32137, 32006, 31862, 31705, 31537, 31356, 31163, 30958, 30742, 30513, - 30273, 30021, 29757, 29482, 29196, 28898, 28589, 28269, 27938, 27597, - 27245, 26882, 26509, 26126, 25732, 25329, 24916, 24494, 24062, 23620, - 23170, 22710, 22242, 21766, 21280, 20787, 20286, 19777, 19260, 18736, - 18204, 17666, 17121, 16569, 16011, 15446, 14876, 14300, 13718, 13131, - 12539, 11943, 11341, 10735, 10126, 9512, 8894, 8273, 7649, 7022, - 6393, 5760, 5126, 4489, 3851, 3212, 2571, 1929, 1286, 643, - 0, -643, -1286, -1929, -2571, -3212, -3851, -4489, -5126, -5760, - -6393, -7022, -7649, -8273, -8894, -9512, -10126, -10735, -11341, -11943, - -12539, -13131, -13718, -14300, -14876, -15446, -16011, -16569, -17121, -17666, - -18204, -18736, -19260, -19777, -20286, -20787, -21280, -21766, -22242, -22710, - -23170, -23620, -24062, -24494, -24916, -25329, -25732, -26126, -26509, -26882, - -27245, -27597, -27938, -28269, -28589, -28898, -29196, -29482, -29757, -30021, - -30273, -30513, -30742, -30958, -31163, -31356, -31537, -31705, -31862, -32006, - -32137, -32257, -32364, -32458, -32540, -32609, -32666, -32710, -32742, -32761, - -32767, -32761, -32742, -32710, -32666, -32609, -32540, -32458, -32364, -32257, - -32137, -32006, -31862, -31705, -31537, -31356, -31163, -30958, -30742, -30513, - -30273, -30021, -29757, -29482, -29196, -28898, -28589, -28269, -27938, -27597, - -27245, -26882, -26509, -26126, -25732, -25329, -24916, -24494, -24062, -23620, - -23170, -22710, -22242, -21766, -21280, -20787, -20286, -19777, -19260, -18736, - -18204, -17666, -17121, -16569, -16011, -15446, -14876, -14300, -13718, -13131, - -12539, -11943, -11341, -10735, -10126, -9512, -8894, -8273, -7649, -7022, - -6393, -5760, -5126, -4489, -3851, -3212, -2571, -1929, -1286, -643, -}; - static bd_addr_t remote; -static hci_con_handle_t remote_handle; static btstack_packet_callback_registration_t hci_event_callback_registration; static unsigned int next_cis_index; static hci_con_handle_t cis_con_handles[MAX_NUM_CIS]; -static uint16_t packet_sequence_numbers[MAX_NUM_CIS]; -static uint8_t framed_pdus; static bool cis_established[MAX_NUM_CIS]; static uint8_t iso_frame_counter; -static uint16_t frame_duration_us; static uint8_t num_cis; -static uint8_t iso_payload[MAX_CHANNELS * MAX_LC3_FRAME_BYTES]; // time stamping #ifdef COUNT_MODE @@ -150,30 +103,12 @@ static uint16_t number_samples_per_frame; static uint16_t octets_per_frame; static uint8_t num_channels = 1; -// lc3 encoder -static const btstack_lc3_encoder_t * lc3_encoder; -static btstack_lc3_encoder_google_t encoder_contexts[MAX_CHANNELS]; -static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; -static uint32_t time_generation_ms; - // codec menu static uint8_t menu_sampling_frequency; static uint8_t menu_variant; -// mod player -static int hxcmod_initialized; -static modcontext mod_context; -static tracker_buffer_state trkbuf; - -// sine generator -static uint8_t sine_step; -static uint16_t sine_phases[MAX_CHANNELS]; - // audio producer -static enum { - AUDIO_SOURCE_SINE, - AUDIO_SOURCE_MODPLAYER -} audio_source = AUDIO_SOURCE_MODPLAYER; +static le_audio_demo_source_generator audio_source = AUDIO_SOURCE_MODPLAYER; static enum { APP_W4_WORKING, @@ -253,149 +188,14 @@ static void print_config(void) { audio_source == AUDIO_SOURCE_SINE ? "Sine" : "Modplayer"); } -static void setup_lc3_encoder(void){ - uint8_t channel; - for (channel = 0 ; channel < num_channels ; channel++){ - btstack_lc3_encoder_google_t * context = &encoder_contexts[channel]; - lc3_encoder = btstack_lc3_encoder_google_init_instance(context); - lc3_encoder->configure(context, sampling_frequency_hz, frame_duration, octets_per_frame); - } - number_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); - btstack_assert(number_samples_per_frame <= MAX_SAMPLES_PER_FRAME); - printf("LC3 Encoder config: %u hz, frame duration %s ms, num samples %u, num octets %u\n", - sampling_frequency_hz, frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? "7.5" : "10", - number_samples_per_frame, octets_per_frame); -} - -static void setup_mod_player(void){ - if (!hxcmod_initialized) { - hxcmod_initialized = hxcmod_init(&mod_context); - btstack_assert(hxcmod_initialized != 0); - } - hxcmod_unload(&mod_context); - hxcmod_setcfg(&mod_context, sampling_frequency_hz, 16, 1, 1, 1); - hxcmod_load(&mod_context, (void *) &mod_data, mod_len); -} - -static void generate_audio(void){ - uint32_t start_ms = btstack_run_loop_get_time_ms(); - uint16_t sample; - switch (audio_source) { - case AUDIO_SOURCE_SINE: - // generate sine wave for all channels - for (sample = 0 ; sample < number_samples_per_frame ; sample++){ - uint8_t channel; - for (channel = 0; channel < num_channels; channel++) { - int16_t value = sine_int16[sine_phases[channel]] / 4; - pcm[sample * num_channels + channel] = value; - sine_phases[channel] += sine_step * (1+channel); // second channel, double frequency - if (sine_phases[channel] >= (sizeof(sine_int16) / sizeof(int16_t))) { - sine_phases[channel] = 0; - } - } - } - break; - case AUDIO_SOURCE_MODPLAYER: - // mod player configured for stereo - hxcmod_fillbuffer(&mod_context, (unsigned short *) pcm, number_samples_per_frame, &trkbuf); - if (num_channels == 1) { - // stereo -> mono - uint16_t i; - for (i=0;iencode_signed_16(&encoder_contexts[channel], &pcm[channel], num_channels, &iso_payload[channel * MAX_LC3_FRAME_BYTES]); -} - -static void send_iso_packet(uint8_t cis_index){ - -#ifdef COUNT_MODE - if (bis_index == 0) { - uint32_t now = btstack_run_loop_get_time_ms(); - if (send_last_ms != 0) { - uint16_t send_interval_ms = now - send_last_ms; - if (send_interval_ms >= MAX_PACKET_INTERVAL_BINS_MS) { - printf("ERROR: send interval %u\n", send_interval_ms); - } else { - send_time_bins[send_interval_ms]++; - } - } - send_last_ms = now; - } -#endif - bool ok = hci_reserve_packet_buffer(); - btstack_assert(ok); - uint8_t * buffer = hci_get_outgoing_packet_buffer(); - // complete SDU, no TimeStamp - little_endian_store_16(buffer, 0, cis_con_handles[cis_index] | (2 << 12)); - // len - little_endian_store_16(buffer, 2, 0 + 4 + num_channels * octets_per_frame); - // TimeStamp if TS flag is set - // packet seq nr - little_endian_store_16(buffer, 4, packet_sequence_numbers[cis_index]); - // iso sdu len - little_endian_store_16(buffer, 6, num_channels * octets_per_frame); -#ifdef COUNT_MODE - // test data: bis_index, counter - buffer[8] = bis_index; - memset(&buffer[9], iso_frame_counter, num_channels * octets_per_frame - 1); -#else - // copy encoded payload - uint8_t i; - uint16_t offset = 8; - for (i=0; i