mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-02 04:13:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

example: add le_audio_demo_util_sink and le_audio_demo_util_source

Browse Source
This commit is contained in:
Matthias Ringwald 2023-09-28 18:16:58 +02:00
parent 4bcc41884e
commit 24b69d49bc
4 changed files with 941 additions and 0 deletions

457
example/le_audio_demo_util_sink.c Normal file

@ -0,0 +1,457 @@
/*
* Copyright (C) {copyright_year} BlueKitchen GmbH
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holders nor the names of
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
* 4. Any redistribution, use, or modification is done solely for
* personal benefit and not for any commercial purpose or for
* monetary gain.
*
* THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
* GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* Please inquire about commercial licensing options at
* contact@bluekitchen-gmbh.com
*
*/
#define BTSTACK_FILE__ "le_audio_demo_util_sink.c"
#include "le_audio_demo_util_sink.h"
#include "btstack_bool.h"
#include "btstack_config.h"
#include <btstack_debug.h>
#include <printf.h>
#include "hci.h"
#include "btstack_audio.h"
#include "btstack_lc3_google.h"
#include "btstack_lc3plus_fraunhofer.h"
#include "hxcmod.h"
#include "mods/mod.h"
#ifdef HAVE_POSIX_FILE_IO
#include "wav_util.h"
#include "btstack_ring_buffer.h"
#endif
//#define DEBUG_PLC
#ifdef DEBUG_PLC
#define printf_plc(...) printf(__VA_ARGS__)
#else
#define printf_plc(...) (void)(0);
#endif
#define MAX_CHANNELS 2
#define MAX_SAMPLES_PER_FRAME 480
#define MAX_LC3_FRAME_BYTES 155
// playback
#define MAX_NUM_LC3_FRAMES 15
#define MAX_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)
#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"
// SINK
static const char * le_audio_demo_sink_filename_wav;
static btstack_lc3_frame_duration_t le_audio_demo_sink_frame_duration;
static hci_iso_type_t le_audio_demo_sink_type;
static uint32_t le_audio_demo_sink_sampling_frequency_hz;
static uint16_t le_audio_demo_sink_num_samples_per_frame;
static uint8_t le_audio_demo_sink_num_streams;
static uint8_t le_audio_demo_sink_num_channels_per_stream;
static uint8_t le_audio_demo_sink_num_channels;
static uint16_t le_audio_demo_sink_octets_per_frame;
static uint16_t le_audio_demo_sink_iso_interval_1250us;
static uint8_t le_audio_demo_sink_flush_timeout;
static uint8_t le_audio_demo_sink_pre_transmission_offset;
// 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;
// PLC
static bool stream_last_packet_received[MAX_CHANNELS];
static uint16_t stream_last_packet_sequence[MAX_CHANNELS];
static uint16_t group_last_packet_sequence;
static bool group_last_packet_received;
static uint32_t le_audio_demo_sink_lc3_frames;
static uint32_t samples_received;
static uint32_t samples_played;
static uint32_t samples_dropped;
static btstack_timer_source_t next_packet_timer;
// lc3 decoder
static bool le_audio_demo_lc3plus_decoder_requested = false;
static const btstack_lc3_decoder_t * lc3_decoder;
static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME];
static bool have_pcm[MAX_CHANNELS];
static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS];
#ifdef HAVE_LC3PLUS
static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_CHANNELS];
#endif
static void * decoder_contexts[MAX_CHANNELS];
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) / (le_audio_demo_sink_num_channels * 2));
samples_played += num_samples;
uint32_t bytes_needed = num_samples * le_audio_demo_sink_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 < le_audio_demo_sink_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, le_audio_demo_sink_sampling_frequency_hz, le_audio_demo_sink_frame_duration, le_audio_demo_sink_octets_per_frame);
}
btstack_assert(le_audio_demo_sink_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME);
}
static void store_samples_in_ringbuffer(void){
// check if we have all channels
uint8_t channel;
for (channel = 0; channel < le_audio_demo_sink_num_channels; channel++){
if (have_pcm[channel] == false) return;
}
#ifdef HAVE_POSIX_FILE_IO
// write wav samples
wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, pcm);
#endif
// store samples in playback buffer
uint32_t bytes_to_store = le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame * 2;
samples_received += le_audio_demo_sink_num_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 += le_audio_demo_sink_num_samples_per_frame;
}
memset(have_pcm, 0, sizeof(have_pcm));
}
static void plc_do(uint8_t stream_index) {
// inject packet
uint8_t tmp_BEC_detect;
uint8_t BFI = 1;
uint8_t i;
for (i = 0; i < le_audio_demo_sink_num_channels_per_stream; i++){
uint8_t effective_channel = stream_index + i;
(void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], NULL, BFI,
&pcm[effective_channel], le_audio_demo_sink_num_channels,
&tmp_BEC_detect);
}
// and store in ringbuffer when PCM for all channels is available
store_samples_in_ringbuffer();
}
//
// Perform PLC for packets missing in previous intervals
//
// assumptions:
// - packet sequence number is monotonic increasing
// - if packet with seq nr x is received, all packets with smaller seq number are either received or missed
static void plc_check(uint16_t packet_sequence_number) {
while (group_last_packet_sequence != packet_sequence_number){
uint8_t i;
for (i=0;i<le_audio_demo_sink_num_streams;i++){
// deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start
if (stream_last_packet_received[i] == false){
printf_plc("- ISO #%u, very first packet missing\n", i);
have_pcm[i] = true;
store_samples_in_ringbuffer();
stream_last_packet_received[i] = true;
stream_last_packet_sequence[i] = group_last_packet_sequence;
continue;
}
// missing packet if big sequence counter is higher than bis sequence counter
if (btstack_time16_delta(group_last_packet_sequence, stream_last_packet_sequence[i]) > 0) {
printf_plc("- ISO #%u, PLC for %u\n", i, group_last_packet_sequence);
plc_do(i);
btstack_assert((stream_last_packet_sequence[i] + 1) == group_last_packet_sequence);
stream_last_packet_sequence[i] = group_last_packet_sequence;
}
}
group_last_packet_sequence++;
}
}
static void plc_timeout(btstack_timer_source_t * timer) {
// Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets
uint32_t frame_duration_ms = le_audio_demo_sink_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(group_last_packet_sequence + 1);
}
void le_audio_demo_util_sink_init(const char * filename_wav){
le_audio_demo_sink_filename_wav = filename_wav;
}
void le_audio_demo_util_sink_enable_lc3plus(bool enable){
le_audio_demo_lc3plus_decoder_requested = enable;
}
void le_audio_demo_util_sink_configure_general(uint8_t num_streams, uint8_t num_channels_per_stream,
uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
uint32_t iso_interval_1250us) {
le_audio_demo_sink_sampling_frequency_hz = sampling_frequency_hz;
le_audio_demo_sink_frame_duration = frame_duration;
le_audio_demo_sink_octets_per_frame = octets_per_frame;
le_audio_demo_sink_iso_interval_1250us = iso_interval_1250us;
le_audio_demo_sink_num_streams = num_streams;
le_audio_demo_sink_num_channels_per_stream = num_channels_per_stream;
le_audio_demo_sink_num_channels = le_audio_demo_sink_num_streams * le_audio_demo_sink_num_channels_per_stream;
btstack_assert((le_audio_demo_sink_num_channels == 1) || (le_audio_demo_sink_num_channels == 2));
le_audio_demo_sink_lc3_frames = 0;
le_audio_demo_sink_num_samples_per_frame = btstack_lc3_samples_per_frame(le_audio_demo_sink_sampling_frequency_hz, le_audio_demo_sink_frame_duration);
// switch to lc3plus if requested and possible
bool use_lc3plus_decoder = le_audio_demo_lc3plus_decoder_requested && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US);
// init decoder
setup_lc3_decoder();
printf("Configure: %u streams, %u channels per stream, sampling rate %u, samples per frame %u, lc3plus %u\n",
num_streams, num_channels_per_stream, sampling_frequency_hz, le_audio_demo_sink_num_samples_per_frame, use_lc3plus_decoder);
#ifdef HAVE_POSIX_FILE_IO
// create wav file
printf("WAV file: %s\n", le_audio_demo_sink_filename_wav);
wav_writer_open(le_audio_demo_sink_filename_wav, le_audio_demo_sink_num_channels, le_audio_demo_sink_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) * le_audio_demo_sink_num_channels * 2;
// start playback
const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance();
if (sink != NULL){
sink->init(le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz, le_audio_connection_sink_playback);
sink->start_stream();
}
}
void le_audio_demo_util_sink_configure_unicast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
uint32_t iso_interval_1250us, uint8_t flush_timeout){
le_audio_demo_sink_type = HCI_ISO_TYPE_CIS;
le_audio_demo_sink_flush_timeout = flush_timeout;
le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz,
frame_duration, octets_per_frame, iso_interval_1250us);
}
void le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
uint32_t iso_interval_1250us, uint8_t pre_transmission_offset) {
le_audio_demo_sink_type = HCI_ISO_TYPE_BIS;
le_audio_demo_sink_pre_transmission_offset = pre_transmission_offset;
le_audio_demo_util_sink_configure_unicast(num_streams, num_channels_per_stream, sampling_frequency_hz, frame_duration,
octets_per_frame, iso_interval_1250us, pre_transmission_offset);
}
void le_audio_demo_util_sink_receive(uint8_t stream_index, 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 != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame) {
printf("ISO Length %u != %u * %u\n", iso_sdu_length, le_audio_demo_sink_num_channels_per_stream, le_audio_demo_sink_octets_per_frame);
return;
}
// start with first packet on first stream
if (group_last_packet_received == false){
if (stream_index != 0){
printf("Ignore first packet for second stream\n");
return;
}
group_last_packet_received = true;
group_last_packet_sequence = packet_sequence_number;
}
if (stream_last_packet_received[stream_index]) {
printf_plc("ISO #%u, receive %u\n", stream_index, packet_sequence_number);
int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number,
stream_last_packet_sequence[stream_index]);
if (packet_sequence_delta < 1) {
// drop delayed packet that had already been generated by PLC
printf_plc("- dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n",
packet_sequence_number, stream_last_packet_sequence[stream_index]);
return;
}
// simple check
if (packet_sequence_number != stream_last_packet_sequence[stream_index] + 1) {
printf_plc("- ISO #%u, missing %u\n", stream_index, stream_last_packet_sequence[stream_index] + 1);
}
} else {
printf_plc("ISO %u, first packet seq number %u\n", stream_index, packet_sequence_number);
stream_last_packet_received[stream_index] = true;
}
plc_check(packet_sequence_number);
uint8_t i;
for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){
// decode codec frame
uint8_t tmp_BEC_detect;
uint8_t BFI = 0;
uint8_t effective_channel = stream_index + i;
(void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &packet[offset], BFI,
&pcm[effective_channel], le_audio_demo_sink_num_channels,
&tmp_BEC_detect);
offset += le_audio_demo_sink_octets_per_frame;
have_pcm[stream_index + i] = true;
}
store_samples_in_ringbuffer();
log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2));
le_audio_demo_sink_lc3_frames++;
// PLC
uint32_t frame_duration_ms = le_audio_demo_sink_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 >= le_audio_demo_sink_sampling_frequency_hz){
printf("LC3 Frames: %4u - samples received %5u, played %5u, dropped %5u\n", le_audio_demo_sink_lc3_frames, samples_received, samples_played, samples_dropped);
samples_received = 0;
samples_dropped = 0;
samples_played = 0;
}
stream_last_packet_sequence[stream_index] = packet_sequence_number;
}
/**
* @brief Close sink: close wav file, stop playback
*/
void le_audio_demo_util_sink_close(void){
#ifdef HAVE_POSIX_FILE_IO
printf("Close WAV file\n");
wav_writer_close();
#endif
// stop playback
const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance();
if (sink != NULL){
sink->stop_stream();
}
// stop timer
btstack_run_loop_remove_timer(&next_packet_timer);
}

106
example/le_audio_demo_util_sink.h Normal file

@ -0,0 +1,106 @@
/*
* Copyright (C) 2023 BlueKitchen GmbH
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holders nor the names of
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
* GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
*/
/**
* @brief Send/receive isochronous audio, used by le_audio_* demos
*/
#ifndef LE_AUDIO_DEMO_UTIL_SINK_H
#define LE_AUDIO_DEMO_UTIL_SINK_H
#include <stdint.h>
#include "bluetooth.h"
#include "btstack_bool.h"
#include "btstack_lc3.h"
#if defined __cplusplus
extern "C" {
#endif
/**
* @brief Init sink functionality
* @param filename_wav
*/
void le_audio_demo_util_sink_init(const char * filename_wav);
/**
* @brief Enable LC3plus if available for 10 ms frame duration
* @param enable
*/
void le_audio_demo_util_sink_enable_lc3plus(bool enable);
/**
* @brief Configure unicast sink
* Supported num_streams x num_channels_per_stream configurations: 1 x 1, 1 x 2, 2 x 1
* @param num_streams
* @param num_channels_per_stream
* @param sampling_frequency_hz
* @param frame_duration
* @param octets_per_frame
* @param iso_interval_1250us
* @param flush_timeout
*/
void le_audio_demo_util_sink_configure_unicast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
uint32_t iso_interval_1250us, uint8_t flush_timeout);
/**
* @brief Configure broadcast sink
* Supported num_streams x num_channels_per_stream configurations: 1 x 1, 1 x 2, 2 x 1
* @param num_streams
* @param num_channels_per_stream
* @param sampling_frequency_hz
* @param frame_duration
* @param octets_per_frame
* @param iso_interval_1250us
* @param pre_transmission_offset
*/
void le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
uint32_t iso_interval_1250us, uint8_t pre_transmission_offset);
/**
* @brief Process ISO packet
* @param stream_index
* @param packet
* @param size
*/
void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint16_t size);
/**
* @brief Close sink: close wav file, stop playbacl
*/
void le_audio_demo_util_sink_close(void);
#if defined __cplusplus
}
#endif
#endif // LE_AUDIO_DEMO_UTIL_SINK_H

285
example/le_audio_demo_util_source.c Normal file

@ -0,0 +1,285 @@
/*
* Copyright (C) {copyright_year} BlueKitchen GmbH
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holders nor the names of
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
* 4. Any redistribution, use, or modification is done solely for
* personal benefit and not for any commercial purpose or for
* monetary gain.
*
* THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
* GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* Please inquire about commercial licensing options at
* contact@bluekitchen-gmbh.com
*
*/
#define BTSTACK_FILE__ "le_audio_demo_util_source.c"
#include "le_audio_demo_util_source.h"
#include "btstack_bool.h"
#include "btstack_config.h"
#include <btstack_debug.h>
#include <printf.h>
#include "hci.h"
#include "btstack_audio.h"
#include "btstack_lc3_google.h"
#include "btstack_lc3plus_fraunhofer.h"
#include "hxcmod.h"
#include "mods/mod.h"
#ifdef HAVE_POSIX_FILE_IO
#include "wav_util.h"
#include "btstack_ring_buffer.h"
#endif
//#define DEBUG_PLC
#ifdef DEBUG_PLC
#define printf_plc(...) printf(__VA_ARGS__)
#else
#define printf_plc(...) (void)(0);
#endif
#define MAX_CHANNELS 2
#define MAX_SAMPLES_PER_FRAME 480
#define MAX_LC3_FRAME_BYTES 155
// playback
#define MAX_NUM_LC3_FRAMES 15
#define MAX_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)
#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"
// analysis
#define PACKET_PREFIX_LEN 10
// SOURCE
// 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 uint16_t le_audio_demo_source_octets_per_frame;
static uint16_t le_audio_demo_source_packet_sequence_numbers[MAX_CHANNELS];
static uint8_t le_audio_demo_source_iso_frame_counter;
static uint32_t le_audio_demo_source_sampling_frequency_hz;
static btstack_lc3_frame_duration_t le_audio_demo_source_frame_duration;
static uint8_t le_audio_demo_source_num_channels;
static uint8_t le_audio_demo_source_num_streams;
static uint8_t le_audio_demo_source_num_channels_per_stream;
static uint16_t le_audio_demo_source_num_samples_per_frame;
static uint8_t le_audio_demo_source_iso_payload[MAX_CHANNELS * MAX_LC3_FRAME_BYTES];
// lc3 encoder
static const btstack_lc3_encoder_t * le_audio_demo_source_lc3_encoder;
static btstack_lc3_encoder_google_t le_audio_demo_source_encoder_contexts[MAX_CHANNELS];
static int16_t le_audio_demo_source_pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME];
// sine generator
static uint8_t le_audio_demo_source_sine_step;
static uint16_t le_audio_demo_source_sine_phases[MAX_CHANNELS];
// mod player
static bool le_audio_demo_source_hxcmod_initialized;
static modcontext le_audio_demo_source_hxcmod_context;
static tracker_buffer_state le_audio_demo_source_hxcmod_trkbuf;
void le_audio_demo_util_source_init(void){
}
static void le_audio_demo_source_setup_lc3_encoder(void){
uint8_t channel;
for (channel = 0 ; channel < le_audio_demo_source_num_channels ; channel++){
btstack_lc3_encoder_google_t * context = &le_audio_demo_source_encoder_contexts[channel];
le_audio_demo_source_lc3_encoder = btstack_lc3_encoder_google_init_instance(context);
le_audio_demo_source_lc3_encoder->configure(context, le_audio_demo_source_sampling_frequency_hz, le_audio_demo_source_frame_duration, le_audio_demo_source_octets_per_frame);
}
printf("LC3 Encoder config: %u hz, frame duration %s ms, num samples %u, num octets %u\n",
le_audio_demo_source_sampling_frequency_hz, le_audio_demo_source_frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? "7.5" : "10",
le_audio_demo_source_num_samples_per_frame, le_audio_demo_source_octets_per_frame);
}
static void le_audio_demo_source_setup_mod_player(void){
if (!le_audio_demo_source_hxcmod_initialized) {
le_audio_demo_source_hxcmod_initialized = hxcmod_init(&le_audio_demo_source_hxcmod_context);
btstack_assert(le_audio_demo_source_hxcmod_initialized != 0);
}
hxcmod_unload(&le_audio_demo_source_hxcmod_context);
hxcmod_setcfg(&le_audio_demo_source_hxcmod_context, le_audio_demo_source_sampling_frequency_hz, 16, 1, 1, 1);
hxcmod_load(&le_audio_demo_source_hxcmod_context, (void *) &mod_data, mod_len);
}
void le_audio_demo_util_source_configure(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame) {
le_audio_demo_source_sampling_frequency_hz = sampling_frequency_hz;
le_audio_demo_source_frame_duration = frame_duration;
le_audio_demo_source_octets_per_frame = octets_per_frame;
le_audio_demo_source_num_streams = num_streams;
le_audio_demo_source_num_channels_per_stream = num_channels_per_stream;
le_audio_demo_source_num_channels = num_streams * num_channels_per_stream;
btstack_assert((le_audio_demo_source_num_channels == 1) || (le_audio_demo_source_num_channels == 2));
le_audio_demo_source_num_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration);
btstack_assert(le_audio_demo_source_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME);
// setup encoder
le_audio_demo_source_setup_lc3_encoder();
// setup sine generator
if (sampling_frequency_hz == 44100){
le_audio_demo_source_sine_step = 2;
} else {
le_audio_demo_source_sine_step = 96000 / sampling_frequency_hz;
}
// setup mod player
le_audio_demo_source_setup_mod_player();
};
void le_audio_demo_util_source_generate_iso_frame(le_audio_demo_source_generator generator) {
btstack_assert(le_audio_demo_source_octets_per_frame != 0);
uint16_t sample;
bool encode_pcm = true;
switch (generator){
case AUDIO_SOURCE_COUNTER:
encode_pcm = false;
memset(le_audio_demo_source_iso_payload, le_audio_demo_source_iso_frame_counter++, sizeof(le_audio_demo_source_iso_payload));
break;
case AUDIO_SOURCE_SINE:
// generate sine wave for all channels
for (sample = 0 ; sample < le_audio_demo_source_num_samples_per_frame ; sample++){
uint8_t i;
for (i = 0; i < le_audio_demo_source_num_channels; i++) {
int16_t value = sine_int16[le_audio_demo_source_sine_phases[i]] / 4;
le_audio_demo_source_pcm[sample * le_audio_demo_source_num_channels + i] = value;
le_audio_demo_source_sine_phases[i] += le_audio_demo_source_sine_step * (1 + i); // second channel, double frequency
if (le_audio_demo_source_sine_phases[i] >= (sizeof(sine_int16) / sizeof(int16_t))) {
le_audio_demo_source_sine_phases[i] = 0;
}
}
}
break;
case AUDIO_SOURCE_MODPLAYER:
// mod player configured for stereo
hxcmod_fillbuffer(&le_audio_demo_source_hxcmod_context, (unsigned short *) le_audio_demo_source_pcm, le_audio_demo_source_num_samples_per_frame, &le_audio_demo_source_hxcmod_trkbuf);
if (le_audio_demo_source_num_channels == 1) {
// stereo -> mono
uint8_t i;
for (i=0;i<le_audio_demo_source_num_samples_per_frame;i++){
le_audio_demo_source_pcm[i] = (le_audio_demo_source_pcm[2*i] / 2) + (le_audio_demo_source_pcm[2*i+1] / 2);
}
}
break;
default:
btstack_unreachable();
break;
}
if (encode_pcm){
uint8_t i;
for (i=0;i<le_audio_demo_source_num_channels;i++){
le_audio_demo_source_lc3_encoder->encode_signed_16(&le_audio_demo_source_encoder_contexts[i], &le_audio_demo_source_pcm[i], le_audio_demo_source_num_channels, &le_audio_demo_source_iso_payload[i * MAX_LC3_FRAME_BYTES]);
}
}
};
void le_audio_demo_util_source_send(uint8_t stream_index, hci_con_handle_t con_handle){
btstack_assert(le_audio_demo_source_octets_per_frame != 0);
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, ((uint16_t) con_handle) | (2 << 12));
// len
little_endian_store_16(buffer, 2, 0 + 4 + le_audio_demo_source_num_channels_per_stream * le_audio_demo_source_octets_per_frame);
// TimeStamp if TS flag is set
// packet seq nr
little_endian_store_16(buffer, 4, le_audio_demo_source_packet_sequence_numbers[stream_index]);
// iso sdu len
little_endian_store_16(buffer, 6, le_audio_demo_source_num_channels_per_stream * le_audio_demo_source_octets_per_frame);
uint16_t offset = 8;
// copy encoded payload
uint8_t i;
for (i=0; i<le_audio_demo_source_num_channels_per_stream;i++) {
memcpy(&buffer[offset], &le_audio_demo_source_iso_payload[i * MAX_LC3_FRAME_BYTES], le_audio_demo_source_octets_per_frame);
offset += le_audio_demo_source_octets_per_frame;
}
// send
hci_send_iso_packet_buffer(offset);
le_audio_demo_source_packet_sequence_numbers[stream_index]++;
}
void le_audio_demo_util_source_close(void){
}

93
example/le_audio_demo_util_source.h Normal file

@ -0,0 +1,93 @@
/*
* Copyright (C) {copyright_year} BlueKitchen GmbH
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the copyright holders nor the names of
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
* GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
* THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
*/
/**
* @brief Send/receive isochronous audio, used by le_audio_* demos
*/
#ifndef LE_AUDIO_DEMO_UTIL_SOURCE_H
#define LE_AUDIO_DEMO_UTIL_SOURCE_H
#include <stdint.h>
#include "bluetooth.h"
#include "btstack_bool.h"
#include "btstack_lc3.h"
#if defined __cplusplus
extern "C" {
#endif
typedef enum {
AUDIO_SOURCE_COUNTER,
AUDIO_SOURCE_SINE,
AUDIO_SOURCE_MODPLAYER
} le_audio_demo_source_generator;
/**
* @brief Init source functionality
*/
void le_audio_demo_util_source_init(void);
/**
* @brief Configure audio source
* Supported num_streams x num_channels_per_stream configurations: 1 x 1, 1 x 2, 2 x 1
* @param num_streams
* @param num_channels_per_stream
* @param sampling_frequency_hz
* @param frame_duration
* @param octets_per_frame
*/
void le_audio_demo_util_source_configure(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame);
/**
* @brief Generate audio and encode as ISO frame
* @param channel
* @param generator
*/
void le_audio_demo_util_source_generate_iso_frame(le_audio_demo_source_generator generator);
/**
* @brief Send prepared ISO packet
* @param stream_index
* @param con_handle
*/
void le_audio_demo_util_source_send(uint8_t stream_index, hci_con_handle_t con_handle);
/**
* @brief Close source
*/
void le_audio_demo_util_source_close(void);
#if defined __cplusplus
}
#endif
#endif // LE_AUDIO_DEMO_UTIL_SOURCE_H