pts/avdtp_source: add aptx and aptx hd encoder

This commit is contained in:
Bjoern Hartmann 2021-02-23 16:33:41 +01:00 committed by Matthias Ringwald
parent a9207bf9a2
commit f493d145a6
2 changed files with 229 additions and 10 deletions

View File

@ -37,6 +37,16 @@ if (LDAC_ENCODER_FOUND)
add_compile_definitions(HAVE_LDAC_ENCODER)
endif()
# openaptx
pkg_check_modules(APTX openaptx)
if (APTX_FOUND)
message("HAVE_APTX")
include_directories(${APTX_INCLUDE_DIRS})
link_directories(${APTX_LIBRARY_DIRS})
link_libraries(${APTX_LIBRARIES})
add_compile_definitions(HAVE_APTX)
endif()
# enable optional features
add_compile_definitions(ENABLE_TESTING_SUPPORT ENABLE_LE_SIGNED_WRITE)

View File

@ -57,6 +57,14 @@
#define A2DP_CODEC_VENDOR_ID_SONY 0x12d
#define A2DP_SONY_CODEC_LDAC 0xaa
#ifdef HAVE_APTX
#include <openaptx.h>
#endif
#define A2DP_CODEC_VENDOR_ID_APT_LTD 0x4f
#define A2DP_CODEC_VENDOR_ID_QUALCOMM 0xd7
#define A2DP_APT_LTD_CODEC_APTX 0x1
#define A2DP_QUALCOMM_CODEC_APTX_HD 0x24
#define AVDTP_MAX_SEP_NUM 10
#define AVDTP_MAX_MEDIA_CODEC_CONFIG_LEN 16
#define AVDTP_MAX_MEDIA_CODEC_CAPABILITES_EVENT_LEN 100
@ -120,6 +128,13 @@ typedef struct {
int sampling_frequency;
} avdtp_media_codec_configuration_ldac_t;
typedef struct {
int reconfigure;
int channel_mode;
int num_channels;
int sampling_frequency;
} avdtp_media_codec_configuration_aptx_t;
#ifdef HAVE_BTSTACK_STDIN
// mac 2011: static const char * device_addr_string = "04:0C:CE:E4:85:D3";
// pts: static const char * device_addr_string = "00:1B:DC:08:E2:72";
@ -161,6 +176,8 @@ static AACENC_InfoStruct aacinf;
HANDLE_LDAC_BT handleLDAC;
#endif
static struct aptx_context *aptx_handle;
static a2dp_media_sending_context_t media_tracker;
static int current_sample_rate = 44100;
@ -216,6 +233,19 @@ static uint8_t media_ldac_codec_capabilities[] = {
0x7
};
static uint8_t media_aptx_codec_capabilities[] = {
0x4F, 0x0, 0x0, 0x0,
0x1, 0,
0xFF,
};
static uint8_t media_aptxhd_codec_capabilities[] = {
0xD7, 0x0, 0x0, 0x0,
0x24, 0,
0xFF,
0, 0, 0, 0
};
// configurations for local stream endpoints
static uint8_t local_stream_endpoint_sbc_media_codec_configuration[4];
#ifdef HAVE_AAC_FDK
@ -223,6 +253,10 @@ static uint8_t local_stream_endpoint_aac_media_codec_configuration[6];
#endif
static uint8_t local_stream_endpoint_ldac_media_codec_configuration[8];
static avdtp_media_codec_configuration_ldac_t ldac_configuration;
static uint8_t local_stream_endpoint_aptx_media_codec_configuration[7];
static avdtp_media_codec_configuration_aptx_t aptx_configuration;
static uint8_t local_stream_endpoint_aptxhd_media_codec_configuration[11];
static avdtp_media_codec_configuration_aptx_t aptxhd_configuration;
static int a2dp_sample_rate(void){
return current_sample_rate;
@ -271,7 +305,6 @@ static void a2dp_demo_send_media_packet_sbc(void){
media_tracker.codec_ready_to_send = 0;
}
#ifdef HAVE_AAC_FDK
static void a2dp_demo_send_media_packet_aac(void) {
int bytes_to_send;
if (media_tracker.codec_storage_count > media_tracker.max_media_payload_size)
@ -283,7 +316,6 @@ static void a2dp_demo_send_media_packet_aac(void) {
memcpy(media_tracker.codec_storage, &media_tracker.codec_storage[bytes_to_send], media_tracker.codec_storage_count);
media_tracker.codec_ready_to_send = 0;
}
#endif
static void a2dp_demo_send_media_packet_ldac(void) {
uint8_t num_frames = media_tracker.codec_num_frames;
@ -293,6 +325,18 @@ static void a2dp_demo_send_media_packet_ldac(void) {
media_tracker.codec_ready_to_send = 0;
}
static void a2dp_send_aptx(void) {
a2dp_source_stream_send_media_packet(media_tracker.avdtp_cid, media_tracker.local_seid, media_tracker.codec_storage, media_tracker.codec_storage_count);
media_tracker.codec_storage_count = 0;
media_tracker.codec_ready_to_send = 0;
}
static void a2dp_send_aptx_hd(void) {
a2dp_source_stream_send_media_payload_rtp(media_tracker.avdtp_cid, media_tracker.local_seid, 0, media_tracker.codec_storage, media_tracker.codec_storage_count);
media_tracker.codec_storage_count = 0;
media_tracker.codec_ready_to_send = 0;
}
static uint32_t get_vendor_id(const uint8_t *codec_info) {
uint32_t vendor_id = 0;
vendor_id |= codec_info[0];
@ -318,15 +362,17 @@ static void a2dp_demo_send_media_packet(void) {
case AVDTP_CODEC_MPEG_2_4_AAC:
a2dp_demo_send_media_packet_aac();
break;
#ifdef HAVE_LDAC_ENCODER
case AVDTP_CODEC_NON_A2DP:
local_cap = sc.local_stream_endpoint->sep.capabilities.media_codec;
uint32_t local_vendor_id = get_vendor_id(local_cap.media_codec_information);
uint16_t local_codec_id = get_codec_id(local_cap.media_codec_information);
if (local_vendor_id == A2DP_CODEC_VENDOR_ID_SONY && local_codec_id == A2DP_SONY_CODEC_LDAC)
a2dp_demo_send_media_packet_ldac();
else if (local_vendor_id == A2DP_CODEC_VENDOR_ID_APT_LTD && local_codec_id == A2DP_APT_LTD_CODEC_APTX)
a2dp_send_aptx();
else if (local_vendor_id == A2DP_CODEC_VENDOR_ID_QUALCOMM && local_codec_id == A2DP_QUALCOMM_CODEC_APTX_HD)
a2dp_send_aptx_hd();
break;
#endif
default:
// TODO:
printf("Send media payload for %s not implemented yet\n", codec_name_for_type(sc.local_stream_endpoint->media_codec_type));
@ -456,6 +502,49 @@ static int a2dp_demo_fill_ldac_audio_buffer(a2dp_media_sending_context_t *contex
}
#endif
#ifdef HAVE_APTX
static int a2dp_demo_fill_aptx_audio_buffer(a2dp_media_sending_context_t *context) {
int total_num_bytes_read = 0;
unsigned int num_audio_samples_per_aptx_buffer = 4;
uint8_t out_size;
int channels;
adtvp_media_codec_capabilities_t local_cap = sc.local_stream_endpoint->sep.capabilities.media_codec;
uint32_t local_vendor_id = get_vendor_id(local_cap.media_codec_information);
uint16_t local_codec_id = get_codec_id(local_cap.media_codec_information);
if (local_vendor_id == A2DP_CODEC_VENDOR_ID_APT_LTD && local_codec_id == A2DP_APT_LTD_CODEC_APTX) {
out_size = 4;
channels = aptx_configuration.num_channels;
} else {
out_size = 6; // aptx hd
channels = aptxhd_configuration.num_channels;
}
uint16_t required_samples = num_audio_samples_per_aptx_buffer * channels;
int16_t pcm_frame16[4 * 2];
unsigned char pcm_frame8[4 * 2 * 3];
while (context->samples_ready >= num_audio_samples_per_aptx_buffer &&
(context->max_media_payload_size - context->codec_storage_count) >= out_size) {
produce_sine_audio((int16_t *) pcm_frame16, num_audio_samples_per_aptx_buffer);
// convert to 24-bit stored in bytes
for (int i = 0; i < 4 * 2; i++) {
pcm_frame8[i * 3 + 2] = (pcm_frame16[i] & 0xFF00) >> 8;
pcm_frame8[i * 3 + 1] = pcm_frame16[i] & 0xFF;
pcm_frame8[i * 3] = 0;
}
size_t written;
aptx_encode(aptx_handle, pcm_frame8, 4 * 3 * 2, &context->codec_storage[context->codec_storage_count], 6, &written);
total_num_bytes_read += num_audio_samples_per_aptx_buffer;
context->codec_storage_count += out_size; // LLRR for aptx or LLLRRR for aptx hd
context->samples_ready -= num_audio_samples_per_aptx_buffer;
}
return total_num_bytes_read;
}
#endif
static void avdtp_audio_timeout_handler(btstack_timer_source_t * timer){
adtvp_media_codec_capabilities_t local_cap;
a2dp_media_sending_context_t * context = (a2dp_media_sending_context_t *) btstack_run_loop_get_timer_context(timer);
@ -507,12 +596,12 @@ static void avdtp_audio_timeout_handler(btstack_timer_source_t * timer){
#endif
case AVDTP_CODEC_ATRAC_FAMILY:
break;
#ifdef HAVE_LDAC_ENCODER
case AVDTP_CODEC_NON_A2DP:
local_cap = sc.local_stream_endpoint->sep.capabilities.media_codec;
uint32_t local_vendor_id = get_vendor_id(local_cap.media_codec_information);
uint16_t local_codec_id = get_codec_id(local_cap.media_codec_information);
// LDAC
#ifdef HAVE_LDAC_ENCODER
if (local_vendor_id == A2DP_CODEC_VENDOR_ID_SONY && local_codec_id == A2DP_SONY_CODEC_LDAC) {
if (context->codec_ready_to_send)
return;
@ -524,8 +613,23 @@ static void avdtp_audio_timeout_handler(btstack_timer_source_t * timer){
a2dp_source_stream_endpoint_request_can_send_now(context->avdtp_cid, context->local_seid);
}
}
break;
#endif
#ifdef HAVE_APTX
// APTX / APTX HD
if ((local_vendor_id == A2DP_CODEC_VENDOR_ID_APT_LTD && local_codec_id == A2DP_APT_LTD_CODEC_APTX) ||
(local_vendor_id == A2DP_CODEC_VENDOR_ID_QUALCOMM && local_codec_id == A2DP_QUALCOMM_CODEC_APTX_HD)) {
if (context->codec_ready_to_send)
return;
a2dp_demo_fill_aptx_audio_buffer(context);
if ((context->codec_storage_count + 6) > context->max_media_payload_size) {
// schedule sending
context->codec_ready_to_send = 1;
a2dp_source_stream_endpoint_request_can_send_now(context->avdtp_cid, context->local_seid);
}
}
#endif
break;
default:
break;
}
@ -674,12 +778,44 @@ static void setup_non_a2dp_codec_config(uint8_t local_remote_seid_index) {
media_codec_config_data[6] = 0x4;
media_codec_config_data[7] = 1;
media_codec_config_len = 8;
} else if (vendor_id == A2DP_CODEC_VENDOR_ID_APT_LTD && codec_id == A2DP_APT_LTD_CODEC_APTX) {
printf("Setup aptx\n");
media_codec_config_data[0] = 0x4f;
media_codec_config_data[1] = 0x0;
media_codec_config_data[2] = 0x0;
media_codec_config_data[3] = 0x0;
media_codec_config_data[4] = 0x1;
media_codec_config_data[5] = 0x0;
media_codec_config_data[6] = 0x12;
media_codec_config_len = 7;
} else if (vendor_id == A2DP_CODEC_VENDOR_ID_QUALCOMM && codec_id == A2DP_QUALCOMM_CODEC_APTX_HD) {
printf("Setup aptx hd\n");
media_codec_config_data[0] = 0xD7;
media_codec_config_data[1] = 0x0;
media_codec_config_data[2] = 0x0;
media_codec_config_data[3] = 0x0;
media_codec_config_data[4] = 0x24;
media_codec_config_data[5] = 0x0;
media_codec_config_data[6] = 0x12;
media_codec_config_data[7] = 0x0;
media_codec_config_data[8] = 0x0;
media_codec_config_data[9] = 0x0;
media_codec_config_data[10] = 0x0;
media_codec_config_len = 11;
}
}
static bool setup_remote_config(uint8_t local_remote_seid_index){
avdtp_media_codec_type_t codec_type = remote_seps[local_remote_seid_index].sep.capabilities.media_codec.media_codec_type;
sc.local_stream_endpoint = avdtp_get_source_stream_endpoint_for_media_codec(codec_type);
if (codec_type == AVDTP_CODEC_NON_A2DP) {
const uint8_t * packet = remote_seps[local_remote_seid_index].media_codec_event;
const uint8_t *media_info = a2dp_subevent_signaling_media_codec_other_capability_get_media_codec_information(packet);
uint32_t vendor_id = get_vendor_id(media_info);
uint16_t codec_id = get_codec_id(media_info);
sc.local_stream_endpoint = avdtp_get_source_stream_endpoint_for_media_codec_other(vendor_id, codec_id);
} else {
sc.local_stream_endpoint = avdtp_get_source_stream_endpoint_for_media_codec(codec_type);
}
if (sc.local_stream_endpoint == NULL){
printf("No local endpoint provided with codec %s\n", codec_name_for_type(codec_type));
return false;
@ -771,6 +907,36 @@ static int convert_ldac_num_channels(uint8_t channel_mode) {
}
}
static int convert_aptx_sampling_frequency(uint8_t frequency_bitmap) {
switch (frequency_bitmap) {
case 1 << 4:
return 48000;
case 1 << 5:
return 44100;
case 1 << 6:
return 32000;
case 1 <<7:
return 16000;
default:
printf("invalid aptx sampling frequency %d\n", frequency_bitmap);
return 0;
}
}
static int convert_aptx_num_channels(uint8_t channel_mode) {
switch (channel_mode) {
case 1 << 0:
case 1 << 1:
case 1 << 2:
return 2;
case 1 << 3:
return 1;
default:
printf("invalid aptx channel mode %d\n", channel_mode);
return 0;
}
}
static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
if (packet_type != HCI_EVENT_PACKET) return;
if (hci_event_packet_get_type(packet) != HCI_EVENT_AVDTP_META) return;
@ -957,6 +1123,10 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
if (vendor_id == A2DP_CODEC_VENDOR_ID_SONY && codec_id == A2DP_SONY_CODEC_LDAC)
printf("CAPABILITY - LDAC, remote seid %u\n", remote_seid);
else if (vendor_id == A2DP_CODEC_VENDOR_ID_APT_LTD && codec_id == A2DP_APT_LTD_CODEC_APTX)
printf("CAPABILITY - APTX, remote seid %u\n", remote_seid);
else if (vendor_id == A2DP_CODEC_VENDOR_ID_QUALCOMM && codec_id == A2DP_QUALCOMM_CODEC_APTX_HD)
printf("CAPABILITY - APTX HD, remote seid %u\n", remote_seid);
else
printf("CAPABILITY - MEDIA_CODEC: OTHER, remote seid %u: \n", remote_seid);
@ -1101,9 +1271,9 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
vendor_id = get_vendor_id(codec_info);
codec_id = get_codec_id(codec_info);
#ifdef HAVE_LDAC_ENCODER
// LDAC
if (vendor_id == A2DP_CODEC_VENDOR_ID_SONY && codec_id == A2DP_SONY_CODEC_LDAC) {
#ifdef HAVE_LDAC_ENCODER
ldac_configuration.reconfigure = a2dp_subevent_signaling_media_codec_other_configuration_get_reconfigure(packet);
ldac_configuration.sampling_frequency = codec_info[6];
ldac_configuration.channel_mode = codec_info[7];
@ -1127,9 +1297,32 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
}
current_sample_rate = ldac_configuration.sampling_frequency;
} else
#endif
{
#ifdef HAVE_APTX
} else if (vendor_id == A2DP_CODEC_VENDOR_ID_APT_LTD && codec_id == A2DP_APT_LTD_CODEC_APTX) {
aptx_configuration.reconfigure = a2dp_subevent_signaling_media_codec_other_configuration_get_reconfigure(packet);
aptx_configuration.sampling_frequency = codec_info[6] & 0xF0;
aptx_configuration.channel_mode = codec_info[6] & 0x0F;
aptx_configuration.sampling_frequency = convert_aptx_sampling_frequency(aptx_configuration.sampling_frequency);
aptx_configuration.num_channels = convert_aptx_num_channels(aptx_configuration.channel_mode);
printf("A2DP Source: Received APTX configuration! Sampling frequency: %d, channel mode: %d channels: %d\n",
aptx_configuration.sampling_frequency, aptx_configuration.channel_mode, aptx_configuration.num_channels);
aptx_handle = aptx_init(0);
current_sample_rate = aptx_configuration.sampling_frequency;
} else if (vendor_id == A2DP_CODEC_VENDOR_ID_QUALCOMM && codec_id == A2DP_QUALCOMM_CODEC_APTX_HD) {
aptxhd_configuration.reconfigure = a2dp_subevent_signaling_media_codec_other_configuration_get_reconfigure(packet);
aptxhd_configuration.sampling_frequency = codec_info[6] & 0xF0;
aptxhd_configuration.channel_mode = codec_info[6] & 0x0F;
aptxhd_configuration.sampling_frequency = convert_aptx_sampling_frequency(aptxhd_configuration.sampling_frequency);
aptxhd_configuration.num_channels = convert_aptx_num_channels(aptxhd_configuration.channel_mode);
printf("A2DP Source: Received APTX HD configuration! Sampling frequency: %d, channel mode: %d channels: %d\n",
aptxhd_configuration.sampling_frequency, aptxhd_configuration.channel_mode, aptxhd_configuration.num_channels);
aptx_handle = aptx_init(1);
current_sample_rate = aptxhd_configuration.sampling_frequency;
#endif
} else {
printf("Config not handled for %s\n", codec_name_for_type(remote_seps[selected_remote_sep_index].sep.capabilities.media_codec.media_codec_type));
}
break;
@ -1430,6 +1623,22 @@ int btstack_main(int argc, const char * argv[]){
avdtp_source_register_delay_reporting_category(avdtp_local_seid(stream_endpoint));
#endif
#ifdef HAVE_APTX
// - APTX
stream_endpoint = a2dp_source_create_stream_endpoint(AVDTP_AUDIO, AVDTP_CODEC_NON_A2DP, (uint8_t *) media_aptx_codec_capabilities, sizeof(media_aptx_codec_capabilities), (uint8_t*) local_stream_endpoint_aptx_media_codec_configuration, sizeof(local_stream_endpoint_aptx_media_codec_configuration));
btstack_assert(stream_endpoint != NULL);
stream_endpoint->media_codec_configuration_info = local_stream_endpoint_aptx_media_codec_configuration;
stream_endpoint->media_codec_configuration_len = sizeof(local_stream_endpoint_aptx_media_codec_configuration);
avdtp_source_register_delay_reporting_category(avdtp_local_seid(stream_endpoint));
// - APTX HD
stream_endpoint = a2dp_source_create_stream_endpoint(AVDTP_AUDIO, AVDTP_CODEC_NON_A2DP, (uint8_t *) media_aptxhd_codec_capabilities, sizeof(media_aptxhd_codec_capabilities), (uint8_t*) local_stream_endpoint_aptxhd_media_codec_configuration, sizeof(local_stream_endpoint_aptxhd_media_codec_configuration));
btstack_assert(stream_endpoint != NULL);
stream_endpoint->media_codec_configuration_info = local_stream_endpoint_aptxhd_media_codec_configuration;
stream_endpoint->media_codec_configuration_len = sizeof(local_stream_endpoint_aptxhd_media_codec_configuration);
avdtp_source_register_delay_reporting_category(avdtp_local_seid(stream_endpoint));
#endif
// Initialize SDP
sdp_init();
memset(sdp_avdtp_source_service_buffer, 0, sizeof(sdp_avdtp_source_service_buffer));