diff --git a/test/pts/CMakeLists.txt b/test/pts/CMakeLists.txt index 8ded2f31b..43b0e82db 100644 --- a/test/pts/CMakeLists.txt +++ b/test/pts/CMakeLists.txt @@ -27,6 +27,12 @@ if(FDK_AAC_FOUND) add_compile_definitions(HAVE_AAC_FDK) endif() +# ldac +pkg_check_modules(LDAC ldacBT-enc) +include_directories(${LDAC_INCLUDE_DIRS}) +link_directories(${LDAC_LIBRARY_DIRS}) +link_libraries(${LDAC_LIBRARIES}) + # enable optional features add_compile_definitions(ENABLE_TESTING_SUPPORT ENABLE_LE_SIGNED_WRITE) diff --git a/test/pts/avdtp_source_test.c b/test/pts/avdtp_source_test.c index 07c89be11..d521c6bc4 100644 --- a/test/pts/avdtp_source_test.c +++ b/test/pts/avdtp_source_test.c @@ -50,13 +50,17 @@ #include #endif +#include +#define A2DP_CODEC_VENDOR_ID_SONY 0x12d +#define A2DP_SONY_CODEC_LDAC 0xaa + #define AVDTP_MAX_SEP_NUM 10 #define AVDTP_MAX_MEDIA_CODEC_CONFIG_LEN 16 #define AVDTP_MAX_MEDIA_CODEC_CAPABILITES_EVENT_LEN 100 #define VOLUME_REDUCTION 2 -#define AUDIO_TIMEOUT_MS 10 +#define AUDIO_TIMEOUT_MS 5 #define TABLE_SIZE_441HZ 100 static const int16_t sine_int16[] = { @@ -105,6 +109,14 @@ typedef struct { int vbr; } avdtp_media_codec_capabilities_aac_t; +typedef struct { + int reconfigure; + int channel_mode; + int num_channels; + int num_samples; + int sampling_frequency; +} avdtp_media_codec_configuration_ldac_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"; @@ -133,6 +145,7 @@ typedef struct { uint8_t codec_storage[1030]; uint16_t codec_storage_count; uint8_t codec_ready_to_send; + uint16_t codec_num_frames; } a2dp_media_sending_context_t; @@ -141,6 +154,8 @@ static HANDLE_AACENCODER handleAAC; static AACENC_InfoStruct aacinf; #endif +HANDLE_LDAC_BT handleLDAC; + static a2dp_media_sending_context_t media_tracker; static int current_sample_rate = 44100; @@ -189,11 +204,20 @@ static uint8_t media_aac_codec_capabilities[] = { }; #endif +static uint8_t media_ldac_codec_capabilities[] = { + 0x2D, 0x1, 0x0, 0x0, + 0xAA, 0, + 0x3C, + 0x7 +}; + // configurations for local stream endpoints static uint8_t local_stream_endpoint_sbc_media_codec_configuration[4]; #ifdef HAVE_AAC_FDK 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 int a2dp_sample_rate(void){ return current_sample_rate; @@ -256,7 +280,32 @@ static void a2dp_demo_send_media_packet_aac(void) { } #endif +static void a2dp_demo_send_media_packet_ldac(void) { + uint8_t num_frames = media_tracker.codec_num_frames; + media_tracker.codec_storage[0] = num_frames; // frames in first byte + 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]; + vendor_id |= codec_info[1] << 8; + vendor_id |= codec_info[2] << 16; + vendor_id |= codec_info[3] << 24; + return vendor_id; +} + +static uint16_t get_codec_id(const uint8_t *codec_info) { + uint16_t codec_id = 0; + codec_id |= codec_info[4]; + codec_id |= codec_info[5] << 8; + return codec_id; +} + static void a2dp_demo_send_media_packet(void) { + adtvp_media_codec_capabilities_t local_cap; switch (remote_seps[selected_remote_sep_index].sep.capabilities.media_codec.media_codec_type){ case AVDTP_CODEC_SBC: a2dp_demo_send_media_packet_sbc(); @@ -264,6 +313,13 @@ static void a2dp_demo_send_media_packet(void) { case AVDTP_CODEC_MPEG_2_4_AAC: a2dp_demo_send_media_packet_aac(); break; + 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(); + break; default: // TODO: printf("Send media payload for %s not implemented yet\n", codec_name_for_type(sc.local_stream_endpoint->media_codec_type)); @@ -364,7 +420,35 @@ static int fill_aac_audio_buffer(a2dp_media_sending_context_t *context) { } #endif +static int a2dp_demo_fill_ldac_audio_buffer(a2dp_media_sending_context_t *context) { + int total_samples_read = 0; + unsigned int num_audio_samples_per_ldac_buffer = LDACBT_ENC_LSU; + int consumed; + int encoded = 0; + int frames; + + // reserve first byte for number of frames + if (context->codec_storage_count == 0) + context->codec_storage_count = 1; + + int16_t pcm_frame[LDACBT_MAX_LSU * 2]; + while (context->samples_ready >= num_audio_samples_per_ldac_buffer && encoded == 0) { + produce_sine_audio((int16_t *) pcm_frame, num_audio_samples_per_ldac_buffer); + + if (ldacBT_encode(handleLDAC, pcm_frame, &consumed, &context->codec_storage[context->codec_storage_count], &encoded, &frames) != 0) { + printf("LDAC encoding error: %d\n", ldacBT_get_error_code(handleLDAC)); + } + consumed = consumed / (2 * ldac_configuration.num_channels); + total_samples_read += consumed; + context->codec_storage_count += encoded; + context->codec_num_frames += frames; + context->samples_ready -= consumed; + } + return total_samples_read; +} + 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); btstack_run_loop_set_timer(&context->audio_timer, AUDIO_TIMEOUT_MS); btstack_run_loop_add_timer(&context->audio_timer); @@ -414,6 +498,23 @@ static void avdtp_audio_timeout_handler(btstack_timer_source_t * timer){ #endif case AVDTP_CODEC_ATRAC_FAMILY: break; + 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 + if (local_vendor_id == A2DP_CODEC_VENDOR_ID_SONY && local_codec_id == A2DP_SONY_CODEC_LDAC) { + if (context->codec_ready_to_send) + return; + a2dp_demo_fill_ldac_audio_buffer(context); + + if (context->codec_storage_count > 1) { + // schedule sending + context->codec_ready_to_send = 1; + a2dp_source_stream_endpoint_request_can_send_now(context->avdtp_cid, context->local_seid); + } + } + break; default: break; } @@ -544,6 +645,27 @@ static void setup_atrac_codec_config(uint8_t local_remote_seid_index) { media_codec_config_len = 4; } +static void setup_non_a2dp_codec_config(uint8_t local_remote_seid_index) { + const uint8_t * packet = remote_seps[local_remote_seid_index].media_codec_event; + uint8_t remote_seid = avdtp_subevent_signaling_media_codec_sbc_capability_get_remote_seid(packet); + 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); + // LDAC + if (vendor_id == A2DP_CODEC_VENDOR_ID_SONY && codec_id == A2DP_SONY_CODEC_LDAC) { + printf("Setup LDAC\n"); + media_codec_config_data[0] = 0x2D; + media_codec_config_data[1] = 0x1; + media_codec_config_data[2] = 0x0; + media_codec_config_data[3] = 0x0; + media_codec_config_data[4] = 0xAA; + media_codec_config_data[5] = 0; + media_codec_config_data[6] = 0x4; + media_codec_config_data[7] = 1; + media_codec_config_len = 8; + } +} + 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); @@ -570,6 +692,9 @@ static bool setup_remote_config(uint8_t local_remote_seid_index){ case AVDTP_CODEC_ATRAC_FAMILY: setup_atrac_codec_config(local_remote_seid_index); break; + case AVDTP_CODEC_NON_A2DP: + setup_non_a2dp_codec_config(local_remote_seid_index); + break; default: printf("Setup config for codec %s not implemented yet\n", codec_name_for_type(codec_type)); return false; @@ -602,6 +727,39 @@ static int convert_aac_vbr(int vbr) { } #endif +static int convert_ldac_sampling_frequency(uint8_t frequency_bitmap) { + switch (frequency_bitmap) { + case 1 << 0: + return 192000; + case 1 << 1: + return 176400; + case 1 << 2: + return 96000; + case 1 << 3: + return 88200; + case 1 << 4: + return 48000; + case 1 << 5: + return 44100; + default: + printf("invalid ldac sampling frequency %d\n", frequency_bitmap); + return 0; + } +} + +static int convert_ldac_num_channels(uint8_t channel_mode) { + switch (channel_mode) { + case 1 << 0: // stereo + case 1 << 1: // dual channel + return 2; + case 1 << 2: + return 1; + default: + printf("error ldac channel mode\n"); + 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; @@ -614,6 +772,8 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe uint16_t avdtp_cid; uint8_t remote_seid; int local_remote_seid_index; + uint32_t vendor_id; + uint16_t codec_id; switch (packet[2]){ case AVDTP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED: @@ -780,7 +940,15 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe (void) memcpy(remote_seps[local_remote_seid_index].media_codec_event, packet, size); remote_seps[local_remote_seid_index].sep.capabilities.media_codec.media_codec_type = AVDTP_CODEC_NON_A2DP; remote_seps[local_remote_seid_index].have_media_codec_apabilities = true; - printf("CAPABILITY - MEDIA_CODEC: OTHER, remote seid %u: \n", remote_seid); + const uint8_t *media_info = avdtp_subevent_signaling_media_codec_other_capability_get_media_codec_information(packet); + vendor_id = get_vendor_id(media_info); + codec_id = get_codec_id(media_info); + + if (vendor_id == A2DP_CODEC_VENDOR_ID_SONY && codec_id == A2DP_SONY_CODEC_LDAC) + printf("CAPABILITY - LDAC, remote seid %u\n", remote_seid); + else + printf("CAPABILITY - MEDIA_CODEC: OTHER, remote seid %u: \n", remote_seid); + break; case AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION:{ @@ -912,10 +1080,45 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe case AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AUDIO_CONFIGURATION: case AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_ATRAC_CONFIGURATION: - case AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION: // TODO: handle other configuration event 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; + case AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION: + printf("Received other configuration\n"); + uint8_t *codec_info = (uint8_t *) a2dp_subevent_signaling_media_codec_other_configuration_get_media_codec_information(packet); + + vendor_id = get_vendor_id(codec_info); + codec_id = get_codec_id(codec_info); + + // LDAC + if (vendor_id == A2DP_CODEC_VENDOR_ID_SONY && codec_id == A2DP_SONY_CODEC_LDAC) { + 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]; + ldac_configuration.sampling_frequency = convert_ldac_sampling_frequency(ldac_configuration.sampling_frequency); + ldac_configuration.num_channels = convert_ldac_num_channels(ldac_configuration.channel_mode); + printf("A2DP Source: Received LDAC configuration! Sampling frequency: %d, channel mode: %d channels: %d\n", + ldac_configuration.sampling_frequency, ldac_configuration.channel_mode, ldac_configuration.num_channels); + + handleLDAC = ldacBT_get_handle(); + if (handleLDAC == NULL) { + printf("Failed to get LDAC handle\n"); + break; + } + + // init ldac encoder + int mtu = 679; // minimal required mtu + if (ldacBT_init_handle_encode(handleLDAC, mtu, LDACBT_EQMID_SQ, ldac_configuration.channel_mode, + LDACBT_SMPL_FMT_S16, ldac_configuration.sampling_frequency) == -1) { + printf("Couldn't initialize LDAC encoder: %d\n", ldacBT_get_error_code(handleLDAC)); + break; + } + + current_sample_rate = ldac_configuration.sampling_frequency; + } 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; case AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW: a2dp_demo_send_media_packet(); @@ -1204,6 +1407,13 @@ int btstack_main(int argc, const char * argv[]){ // - MPEG1/2 Layer 3 // .. + // - LDAC + stream_endpoint = a2dp_source_create_stream_endpoint(AVDTP_AUDIO, AVDTP_CODEC_NON_A2DP, (uint8_t *) media_ldac_codec_capabilities, sizeof(media_ldac_codec_capabilities), (uint8_t*) local_stream_endpoint_ldac_media_codec_configuration, sizeof(local_stream_endpoint_ldac_media_codec_configuration)); + btstack_assert(stream_endpoint != NULL); + stream_endpoint->media_codec_configuration_info = local_stream_endpoint_ldac_media_codec_configuration; + stream_endpoint->media_codec_configuration_len = sizeof(local_stream_endpoint_ldac_media_codec_configuration); + avdtp_source_register_delay_reporting_category(avdtp_local_seid(stream_endpoint)); + // Initialize SDP sdp_init(); memset(sdp_avdtp_source_service_buffer, 0, sizeof(sdp_avdtp_source_service_buffer));