mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-26 20:37:19 +00:00
Code Issues Packages Projects Releases Wiki Activity

test/le-audio: use example/le_audio_demo_uitil_*

Browse Source
This commit is contained in:
Matthias Ringwald 2023-10-04 15:00:41 +02:00
parent 24b69d49bc
commit bb81690e3b
5 changed files with 139 additions and 1163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
test/le_audio/CMakeLists.txt
View File

@ -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")

389
test/le_audio/le_audio_broadcast_sink.c
View File

@ -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<num_bis;i++){
// deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start
if (last_packet_received[i] == false){
printf_plc("- BIS #%u, very first packet missing\n", i);
have_pcm[i] = true;
store_samples_in_ringbuffer();
last_packet_received[i] = true;
last_packet_sequence[i] = last_packet_sequence_big;
continue;
}
// missing packet if big sequence counter is higher than bis sequence counter
if (btstack_time16_delta(last_packet_sequence_big, last_packet_sequence[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<num_bis;i++){
printf("%u ", frames_per_second[i]);
frames_per_second[i] = 0;
}
printf(" frames per second, dropped %u of %u\n", samples_dropped, samples_received);
samples_received = 0;
samples_dropped = 0;
}
}
last_packet_time_ms[bis_channel] = receive_time_ms;
last_packet_sequence[bis_channel] = packet_sequence_number;
}
static void show_usage(void){
printf("\n--- LE Audio Broadcast Sink Test Console ---\n");
printf("s - start scanning\n");
@ -941,8 +579,7 @@ static void stdin_process(char c){
case APP_STREAMING:
case APP_W4_BIG_SYNC_ESTABLISHED:
app_state = APP_IDLE;
btstack_run_loop_remove_timer(&next_packet_timer);
close_files();
le_audio_demo_util_sink_close();
printf("Terminate BIG SYNC\n");
gap_big_sync_terminate(big_handle);
break;
@ -960,6 +597,18 @@ static void stdin_process(char c){
}
}
static void iso_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
// get stream_index from con_handle
uint16_t header = little_endian_read_16(packet, 0);
hci_con_handle_t con_handle = header & 0x0fff;
uint8_t i;
for (i=0;i<num_bis;i++){
if (bis_con_handles[i] == con_handle) {
le_audio_demo_util_sink_receive(i, packet, size);
}
}
}
static void broadcast_sync_pa_sync_timeout_handler(btstack_timer_source_t * ts){
UNUSED(ts);
printf("PAST Timeout -> LE_AUDIO_PA_SYNC_STATE_NO_PAST\n");

210
test/le_audio/le_audio_broadcast_source.c
View File

@ -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;i<number_samples_per_frame;i++){
pcm[i] = (pcm[2*i] / 2) + (pcm[2*i+1] / 2);
}
}
break;
default:
btstack_unreachable();
break;
}
time_generation_ms = btstack_run_loop_get_time_ms() - start_ms;
iso_frame_counter++;
}
static void encode(uint8_t bis_index){
// encode as lc3
lc3_encoder->encode_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);

451
test/le_audio/le_audio_unicast_sink.c
View File

@ -49,65 +49,24 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <inttypes.h>
#include <fcntl.h> // open
#include <errno.h>
#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);

250
test/le_audio/le_audio_unicast_source.c
View File

@ -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;i<number_samples_per_frame;i++){
pcm[i] = (pcm[2*i] / 2) + (pcm[2*i+1] / 2);
}
}
break;
default:
btstack_unreachable();
break;
}
time_generation_ms = btstack_run_loop_get_time_ms() - start_ms;
iso_frame_counter++;
}
static void encode(uint8_t channel){
// encode as lc3
lc3_encoder->encode_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<num_channels;i++) {
memcpy(&buffer[offset], &iso_payload[i * MAX_LC3_FRAME_BYTES], octets_per_frame);
offset += octets_per_frame;
}
#endif
// send
hci_send_iso_packet_buffer(offset);
if (((packet_sequence_numbers[cis_index] & 0x7f) == 0) && (cis_index == 0)) {
printf("Encoding time: %u\n", time_generation_ms);
}
packet_sequence_numbers[cis_index]++;
}
static void generate_audio_and_encode(uint8_t cis_index){
generate_audio();
uint8_t i;
for (i=0; i<num_channels;i++) {
encode(i);
}
}
static void start_unicast() {// use values from table
static void start_unicast() {
// use values from table
sampling_frequency_hz = codec_configurations[menu_sampling_frequency].samplingrate_hz;
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();
// setup mod player
setup_mod_player();
// setup sine generator
if (sampling_frequency_hz == 44100){
sine_step = 2;
} else {
sine_step = 96000 / sampling_frequency_hz;
}
le_audio_demo_util_source_configure(1, num_channels, sampling_frequency_hz, frame_duration, octets_per_frame);
le_audio_demo_util_source_generate_iso_frame(audio_source);
// update adv / BASE
adv_data[4] = 0; // subtype
@ -422,7 +222,6 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
UNUSED(channel);
if (packet_type != HCI_EVENT_PACKET) return;
bd_addr_t event_addr;
hci_con_handle_t cis_con_handle;
uint8_t i;
@ -450,6 +249,14 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
break;
}
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
cis_con_handle = hci_event_disconnection_complete_get_connection_handle(packet);
for (i=0; i < num_cis; i++){
if (cis_con_handle == cis_con_handles[i]){
le_audio_demo_util_sink_close();
}
}
break;
case HCI_EVENT_LE_META:
switch(hci_event_le_meta_get_subevent_code(packet)){
case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
@ -481,9 +288,17 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
// ready to send
if (complete) {
printf("All CIS Established and ISO Path setup\n");
// init sink
uint16_t iso_interval_1250us = gap_subevent_cis_created_get_iso_interval_1250us(packet);
uint8_t flush_timeout = gap_subevent_cis_created_get_flush_timeout_c_to_p(packet);
le_audio_demo_util_sink_configure_unicast(1, 1, sampling_frequency_hz, frame_duration,
octets_per_frame,
iso_interval_1250us, flush_timeout);
next_cis_index = 0;
app_state = APP_STREAMING;
generate_audio_and_encode(0);
hci_request_cis_can_send_now_events(cis_con_handles[0]);
}
break;
@ -495,9 +310,9 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
for (i=0;i<num_cis;i++){
if (cis_con_handle == cis_con_handles[i]){
// allow to send
send_iso_packet(i);
generate_audio_and_encode(0);
hci_request_cis_can_send_now_events(cis_con_handles[0]);
le_audio_demo_util_source_send(i, cis_con_handle);
le_audio_demo_util_source_generate_iso_frame(audio_source);
hci_request_cis_can_send_now_events(cis_con_handle);
}
}
break;
@ -507,6 +322,10 @@ static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *pack
}
}
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){
printf("\n--- LE Audio Unicast Source Test Console ---\n");
print_config();
@ -585,6 +404,13 @@ int btstack_main(int argc, const char * argv[]){
hci_event_callback_registration.callback = &packet_handler;
hci_add_event_handler(&hci_event_callback_registration);
// register for ISO Packet
hci_register_iso_packet_handler(&iso_packet_handler);
// setup audio processing
le_audio_demo_util_sink_init("le_audio_unicast_source.wav");
le_audio_demo_util_source_init();
// turn on!
hci_power_control(HCI_POWER_ON);