1/* 2 * Copyright 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17#define LOG_TAG "a2dp_vendor_aptx_hd_encoder" 18 19#include "a2dp_vendor_aptx_hd_encoder.h" 20 21#include <dlfcn.h> 22#include <inttypes.h> 23#include <stdio.h> 24#include <string.h> 25 26#include "a2dp_vendor.h" 27#include "a2dp_vendor_aptx_hd.h" 28#include "bt_common.h" 29#include "osi/include/log.h" 30#include "osi/include/osi.h" 31 32// 33// Encoder for aptX-HD Source Codec 34// 35 36// 37// The aptX-HD encoder shared library, and the functions to use 38// 39static const char* APTX_HD_ENCODER_LIB_NAME = "libaptXHD_encoder.so"; 40static void* aptx_hd_encoder_lib_handle = NULL; 41 42static const char* APTX_HD_ENCODER_INIT_NAME = "aptxhdbtenc_init"; 43typedef int (*tAPTX_HD_ENCODER_INIT)(void* state, short endian); 44 45static const char* APTX_HD_ENCODER_ENCODE_STEREO_NAME = 46 "aptxhdbtenc_encodestereo"; 47typedef int (*tAPTX_HD_ENCODER_ENCODE_STEREO)(void* state, void* pcmL, 48 void* pcmR, void* buffer); 49 50static const char* APTX_HD_ENCODER_SIZEOF_PARAMS_NAME = "SizeofAptxhdbtenc"; 51typedef int (*tAPTX_HD_ENCODER_SIZEOF_PARAMS)(void); 52 53static tAPTX_HD_ENCODER_INIT aptx_hd_encoder_init_func; 54static tAPTX_HD_ENCODER_ENCODE_STEREO aptx_hd_encoder_encode_stereo_func; 55static tAPTX_HD_ENCODER_SIZEOF_PARAMS aptx_hd_encoder_sizeof_params_func; 56 57// offset 58#if (BTA_AV_CO_CP_SCMS_T == TRUE) 59#define A2DP_APTX_HD_OFFSET (AVDT_MEDIA_OFFSET + 1) 60#else 61#define A2DP_APTX_HD_OFFSET AVDT_MEDIA_OFFSET 62#endif 63 64#define A2DP_APTX_HD_MAX_PCM_BYTES_PER_READ 4096 65 66typedef struct { 67 uint64_t sleep_time_ns; 68 uint32_t pcm_reads; 69 uint32_t pcm_bytes_per_read; 70 uint32_t aptx_hd_bytes; 71 uint32_t frame_size_counter; 72} tAPTX_HD_FRAMING_PARAMS; 73 74typedef struct { 75 uint64_t session_start_us; 76 77 size_t media_read_total_expected_packets; 78 size_t media_read_total_expected_reads_count; 79 size_t media_read_total_expected_read_bytes; 80 81 size_t media_read_total_dropped_packets; 82 size_t media_read_total_actual_reads_count; 83 size_t media_read_total_actual_read_bytes; 84} a2dp_aptx_hd_encoder_stats_t; 85 86typedef struct { 87 a2dp_source_read_callback_t read_callback; 88 a2dp_source_enqueue_callback_t enqueue_callback; 89 90 bool use_SCMS_T; 91 bool is_peer_edr; // True if the peer device supports EDR 92 bool peer_supports_3mbps; // True if the peer device supports 3Mbps EDR 93 uint16_t peer_mtu; // MTU of the A2DP peer 94 uint32_t timestamp; // Timestamp for the A2DP frames 95 96 tA2DP_FEEDING_PARAMS feeding_params; 97 tAPTX_HD_FRAMING_PARAMS framing_params; 98 void* aptx_hd_encoder_state; 99 a2dp_aptx_hd_encoder_stats_t stats; 100} tA2DP_APTX_HD_ENCODER_CB; 101 102static tA2DP_APTX_HD_ENCODER_CB a2dp_aptx_hd_encoder_cb; 103 104static void a2dp_vendor_aptx_hd_encoder_update( 105 uint16_t peer_mtu, A2dpCodecConfig* a2dp_codec_config, 106 bool* p_restart_input, bool* p_restart_output, bool* p_config_updated); 107static void aptx_hd_init_framing_params( 108 tAPTX_HD_FRAMING_PARAMS* framing_params); 109static void aptx_hd_update_framing_params( 110 tAPTX_HD_FRAMING_PARAMS* framing_params); 111static size_t aptx_hd_encode_24bit(tAPTX_HD_FRAMING_PARAMS* framing_params, 112 size_t* data_out_index, uint32_t* data32_in, 113 uint8_t* data_out); 114 115bool A2DP_VendorLoadEncoderAptxHd(void) { 116 if (aptx_hd_encoder_lib_handle != NULL) return true; // Already loaded 117 118 // Open the encoder library 119 aptx_hd_encoder_lib_handle = dlopen(APTX_HD_ENCODER_LIB_NAME, RTLD_NOW); 120 if (aptx_hd_encoder_lib_handle == NULL) { 121 LOG_ERROR(LOG_TAG, "%s: cannot open aptX-HD encoder library %s: %s", 122 __func__, APTX_HD_ENCODER_LIB_NAME, dlerror()); 123 return false; 124 } 125 126 aptx_hd_encoder_init_func = (tAPTX_HD_ENCODER_INIT)dlsym( 127 aptx_hd_encoder_lib_handle, APTX_HD_ENCODER_INIT_NAME); 128 if (aptx_hd_encoder_init_func == NULL) { 129 LOG_ERROR(LOG_TAG, 130 "%s: cannot find function '%s' in the encoder library: %s", 131 __func__, APTX_HD_ENCODER_INIT_NAME, dlerror()); 132 A2DP_VendorUnloadEncoderAptxHd(); 133 return false; 134 } 135 136 aptx_hd_encoder_encode_stereo_func = (tAPTX_HD_ENCODER_ENCODE_STEREO)dlsym( 137 aptx_hd_encoder_lib_handle, APTX_HD_ENCODER_ENCODE_STEREO_NAME); 138 if (aptx_hd_encoder_encode_stereo_func == NULL) { 139 LOG_ERROR(LOG_TAG, 140 "%s: cannot find function '%s' in the encoder library: %s", 141 __func__, APTX_HD_ENCODER_ENCODE_STEREO_NAME, dlerror()); 142 A2DP_VendorUnloadEncoderAptxHd(); 143 return false; 144 } 145 146 aptx_hd_encoder_sizeof_params_func = (tAPTX_HD_ENCODER_SIZEOF_PARAMS)dlsym( 147 aptx_hd_encoder_lib_handle, APTX_HD_ENCODER_SIZEOF_PARAMS_NAME); 148 if (aptx_hd_encoder_sizeof_params_func == NULL) { 149 LOG_ERROR(LOG_TAG, 150 "%s: cannot find function '%s' in the encoder library: %s", 151 __func__, APTX_HD_ENCODER_SIZEOF_PARAMS_NAME, dlerror()); 152 A2DP_VendorUnloadEncoderAptxHd(); 153 return false; 154 } 155 156 return true; 157} 158 159void A2DP_VendorUnloadEncoderAptxHd(void) { 160 aptx_hd_encoder_init_func = NULL; 161 aptx_hd_encoder_encode_stereo_func = NULL; 162 aptx_hd_encoder_sizeof_params_func = NULL; 163 164 if (aptx_hd_encoder_lib_handle != NULL) { 165 dlclose(aptx_hd_encoder_lib_handle); 166 aptx_hd_encoder_lib_handle = NULL; 167 } 168} 169 170void a2dp_vendor_aptx_hd_encoder_init( 171 const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params, 172 A2dpCodecConfig* a2dp_codec_config, 173 a2dp_source_read_callback_t read_callback, 174 a2dp_source_enqueue_callback_t enqueue_callback) { 175 memset(&a2dp_aptx_hd_encoder_cb, 0, sizeof(a2dp_aptx_hd_encoder_cb)); 176 177 a2dp_aptx_hd_encoder_cb.stats.session_start_us = time_get_os_boottime_us(); 178 179 a2dp_aptx_hd_encoder_cb.read_callback = read_callback; 180 a2dp_aptx_hd_encoder_cb.enqueue_callback = enqueue_callback; 181 a2dp_aptx_hd_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr; 182 a2dp_aptx_hd_encoder_cb.peer_supports_3mbps = 183 p_peer_params->peer_supports_3mbps; 184 a2dp_aptx_hd_encoder_cb.peer_mtu = p_peer_params->peer_mtu; 185 a2dp_aptx_hd_encoder_cb.timestamp = 0; 186 187 /* aptX-HD encoder config */ 188 a2dp_aptx_hd_encoder_cb.use_SCMS_T = false; // TODO: should be a parameter 189#if (BTA_AV_CO_CP_SCMS_T == TRUE) 190 a2dp_aptx_hd_encoder_cb.use_SCMS_T = true; 191#endif 192 193 a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state = 194 osi_malloc(aptx_hd_encoder_sizeof_params_func()); 195 if (a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state != NULL) { 196 aptx_hd_encoder_init_func(a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state, 0); 197 } else { 198 LOG_ERROR(LOG_TAG, "%s: Cannot allocate aptX-HD encoder state", __func__); 199 // TODO: Return an error? 200 } 201 202 // NOTE: Ignore the restart_input / restart_output flags - this initization 203 // happens when the connection is (re)started. 204 bool restart_input = false; 205 bool restart_output = false; 206 bool config_updated = false; 207 a2dp_vendor_aptx_hd_encoder_update(a2dp_aptx_hd_encoder_cb.peer_mtu, 208 a2dp_codec_config, &restart_input, 209 &restart_output, &config_updated); 210} 211 212bool A2dpCodecConfigAptxHd::updateEncoderUserConfig( 213 const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params, bool* p_restart_input, 214 bool* p_restart_output, bool* p_config_updated) { 215 a2dp_aptx_hd_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr; 216 a2dp_aptx_hd_encoder_cb.peer_supports_3mbps = 217 p_peer_params->peer_supports_3mbps; 218 a2dp_aptx_hd_encoder_cb.peer_mtu = p_peer_params->peer_mtu; 219 a2dp_aptx_hd_encoder_cb.timestamp = 0; 220 221 if (a2dp_aptx_hd_encoder_cb.peer_mtu == 0) { 222 LOG_ERROR(LOG_TAG, 223 "%s: Cannot update the codec encoder for %s: " 224 "invalid peer MTU", 225 __func__, name().c_str()); 226 return false; 227 } 228 229 a2dp_vendor_aptx_hd_encoder_update(a2dp_aptx_hd_encoder_cb.peer_mtu, this, 230 p_restart_input, p_restart_output, 231 p_config_updated); 232 return true; 233} 234 235// Update the A2DP aptX-HD encoder. 236// |peer_mtu| is the peer MTU. 237// |a2dp_codec_config| is the A2DP codec to use for the update. 238static void a2dp_vendor_aptx_hd_encoder_update( 239 uint16_t peer_mtu, A2dpCodecConfig* a2dp_codec_config, 240 bool* p_restart_input, bool* p_restart_output, bool* p_config_updated) { 241 uint8_t codec_info[AVDT_CODEC_SIZE]; 242 243 *p_restart_input = false; 244 *p_restart_output = false; 245 *p_config_updated = false; 246 if (!a2dp_codec_config->copyOutOtaCodecConfig(codec_info)) { 247 LOG_ERROR(LOG_TAG, 248 "%s: Cannot update the codec encoder for %s: " 249 "invalid codec config", 250 __func__, a2dp_codec_config->name().c_str()); 251 return; 252 } 253 const uint8_t* p_codec_info = codec_info; 254 255 // The feeding parameters 256 tA2DP_FEEDING_PARAMS* p_feeding_params = 257 &a2dp_aptx_hd_encoder_cb.feeding_params; 258 p_feeding_params->sample_rate = 259 A2DP_VendorGetTrackSampleRateAptxHd(p_codec_info); 260 p_feeding_params->bits_per_sample = 261 a2dp_codec_config->getAudioBitsPerSample(); 262 p_feeding_params->channel_count = 263 A2DP_VendorGetTrackChannelCountAptxHd(p_codec_info); 264 LOG_DEBUG(LOG_TAG, "%s: sample_rate=%u bits_per_sample=%u channel_count=%u", 265 __func__, p_feeding_params->sample_rate, 266 p_feeding_params->bits_per_sample, p_feeding_params->channel_count); 267 a2dp_vendor_aptx_hd_feeding_reset(); 268} 269 270void a2dp_vendor_aptx_hd_encoder_cleanup(void) { 271 osi_free(a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state); 272 memset(&a2dp_aptx_hd_encoder_cb, 0, sizeof(a2dp_aptx_hd_encoder_cb)); 273} 274 275// 276// Initialize the framing parameters, and set those that don't change 277// while streaming (e.g., 'sleep_time_ns'). 278// 279static void aptx_hd_init_framing_params( 280 tAPTX_HD_FRAMING_PARAMS* framing_params) { 281 framing_params->sleep_time_ns = 0; 282 framing_params->pcm_reads = 0; 283 framing_params->pcm_bytes_per_read = 0; 284 framing_params->aptx_hd_bytes = 0; 285 framing_params->frame_size_counter = 0; 286 287 framing_params->sleep_time_ns = 9000000; 288 289 LOG_DEBUG(LOG_TAG, "%s: sleep_time_ns = %" PRIu64, __func__, 290 framing_params->sleep_time_ns); 291} 292 293// 294// Set frame size and transmission interval needed to stream the required 295// sample rate using 2-DH5 packets for aptX and 2-DH3 packets for aptX-LL. 296// With SCMS-T enabled we need to reserve room for extra headers added later. 297// Packets are always sent at equals time intervals but to achieve the 298// required sample rate, the frame size needs to change on occasion. 299// 300// Also need to specify how many of the required PCM samples are read at a 301// time: 302// aptx_bytes = pcm_reads * pcm_bytes_per_read / 4 303// and 304// number of aptX samples produced = pcm_bytes_per_read / 16 305// 306static void aptx_hd_update_framing_params( 307 tAPTX_HD_FRAMING_PARAMS* framing_params) { 308 if (a2dp_aptx_hd_encoder_cb.feeding_params.sample_rate == 48000) { 309 framing_params->aptx_hd_bytes = 648; 310 framing_params->pcm_bytes_per_read = 24; 311 framing_params->pcm_reads = 108; 312 } else { 313 // Assume the sample rate is 44100 314 315 // 316 // Total of 80 iterations: 317 // - Iteration 80: packet size 648, with 108 reads of 24 PCM bytes 318 // - Iterations 20, 40, 60: packet size 612, with 102 reads of 24 PCM bytes 319 // - All other iterations: packet size 594, with 99 reads of 24 PCM bytes 320 // 321 if (framing_params->frame_size_counter + 1 == 80) { 322 framing_params->aptx_hd_bytes = 648; 323 framing_params->pcm_bytes_per_read = 24; 324 framing_params->pcm_reads = 108; 325 } else if (((framing_params->frame_size_counter + 1) % 20) == 0) { 326 framing_params->aptx_hd_bytes = 612; 327 framing_params->pcm_bytes_per_read = 24; 328 framing_params->pcm_reads = 102; 329 } else { 330 framing_params->aptx_hd_bytes = 594; 331 framing_params->pcm_bytes_per_read = 24; 332 framing_params->pcm_reads = 99; 333 } 334 framing_params->frame_size_counter++; 335 if (framing_params->frame_size_counter == 80) 336 framing_params->frame_size_counter = 0; 337 } 338 339 LOG_VERBOSE(LOG_TAG, 340 "%s: sleep_time_ns = %" PRIu64 341 " aptx_hd_bytes = %u " 342 "pcm_bytes_per_read = %u pcm_reads = %u frame_size_counter = %u", 343 __func__, framing_params->sleep_time_ns, 344 framing_params->aptx_hd_bytes, framing_params->pcm_bytes_per_read, 345 framing_params->pcm_reads, framing_params->frame_size_counter); 346} 347 348void a2dp_vendor_aptx_hd_feeding_reset(void) { 349 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params); 350} 351 352void a2dp_vendor_aptx_hd_feeding_flush(void) { 353 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params); 354} 355 356period_ms_t a2dp_vendor_aptx_hd_get_encoder_interval_ms(void) { 357 return a2dp_aptx_hd_encoder_cb.framing_params.sleep_time_ns / (1000 * 1000); 358} 359 360void a2dp_vendor_aptx_hd_send_frames(uint64_t timestamp_us) { 361 tAPTX_HD_FRAMING_PARAMS* framing_params = 362 &a2dp_aptx_hd_encoder_cb.framing_params; 363 364 // Prepare the packet to send 365 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BT_DEFAULT_BUFFER_SIZE); 366 p_buf->offset = A2DP_APTX_HD_OFFSET; 367 p_buf->len = 0; 368 p_buf->layer_specific = 0; 369 370 uint8_t* encoded_ptr = (uint8_t*)(p_buf + 1); 371 encoded_ptr += p_buf->offset; 372 373 aptx_hd_update_framing_params(framing_params); 374 375 // 376 // Read the PCM data and encode it 377 // 378 uint32_t 379 read_buffer32[A2DP_APTX_HD_MAX_PCM_BYTES_PER_READ / sizeof(uint32_t)]; 380 uint32_t expected_read_bytes = 381 framing_params->pcm_reads * framing_params->pcm_bytes_per_read; 382 size_t encoded_ptr_index = 0; 383 size_t pcm_bytes_encoded = 0; 384 uint32_t bytes_read = 0; 385 386 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_packets++; 387 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_reads_count++; 388 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_read_bytes += 389 expected_read_bytes; 390 391 LOG_VERBOSE(LOG_TAG, "%s: PCM read of size %u", __func__, 392 expected_read_bytes); 393 bytes_read = a2dp_aptx_hd_encoder_cb.read_callback((uint8_t*)read_buffer32, 394 expected_read_bytes); 395 a2dp_aptx_hd_encoder_cb.stats.media_read_total_actual_read_bytes += 396 bytes_read; 397 if (bytes_read < expected_read_bytes) { 398 LOG_WARN(LOG_TAG, 399 "%s: underflow at PCM reading: read %u bytes instead of %u", 400 __func__, bytes_read, expected_read_bytes); 401 a2dp_aptx_hd_encoder_cb.stats.media_read_total_dropped_packets++; 402 osi_free(p_buf); 403 return; 404 } 405 a2dp_aptx_hd_encoder_cb.stats.media_read_total_actual_reads_count++; 406 407 for (uint32_t reads = 0, offset = 0; reads < framing_params->pcm_reads; 408 reads++, offset += 409 framing_params->pcm_bytes_per_read / sizeof(uint32_t)) { 410 pcm_bytes_encoded += 411 aptx_hd_encode_24bit(framing_params, &encoded_ptr_index, 412 read_buffer32 + offset, encoded_ptr); 413 } 414 415 // Compute the number of encoded bytes 416 const int COMPRESSION_RATIO = 4; 417 size_t encoded_bytes = pcm_bytes_encoded / COMPRESSION_RATIO; 418 p_buf->len += encoded_bytes; 419 LOG_VERBOSE(LOG_TAG, "%s: encoded %zu PCM bytes to %zu", __func__, 420 pcm_bytes_encoded, encoded_bytes); 421 422 // Update the RTP timestamp 423 *((uint32_t*)(p_buf + 1)) = a2dp_aptx_hd_encoder_cb.timestamp; 424 const uint8_t BYTES_PER_FRAME = 3; 425 uint32_t rtp_timestamp = 426 (pcm_bytes_encoded / 427 a2dp_aptx_hd_encoder_cb.feeding_params.channel_count) / 428 BYTES_PER_FRAME; 429 a2dp_aptx_hd_encoder_cb.timestamp += rtp_timestamp; 430 431 if (p_buf->len > 0) { 432 a2dp_aptx_hd_encoder_cb.enqueue_callback(p_buf, 1, bytes_read); 433 } else { 434 a2dp_aptx_hd_encoder_cb.stats.media_read_total_dropped_packets++; 435 osi_free(p_buf); 436 } 437} 438 439static size_t aptx_hd_encode_24bit(tAPTX_HD_FRAMING_PARAMS* framing_params, 440 size_t* data_out_index, uint32_t* data32_in, 441 uint8_t* data_out) { 442 size_t pcm_bytes_encoded = 0; 443 const uint8_t* p = (const uint8_t*)(data32_in); 444 445 for (size_t aptx_hd_samples = 0; 446 aptx_hd_samples < framing_params->pcm_bytes_per_read / 24; 447 aptx_hd_samples++) { 448 uint32_t pcmL[4]; 449 uint32_t pcmR[4]; 450 uint32_t encoded_sample[2]; 451 452 // Expand from AUDIO_FORMAT_PCM_24_BIT_PACKED data (3 bytes per sample) 453 // into AUDIO_FORMAT_PCM_8_24_BIT (4 bytes per sample). 454 for (size_t i = 0; i < 4; i++) { 455 pcmL[i] = ((p[0] << 0) | (p[1] << 8) | (((int8_t)p[2]) << 16)); 456 p += 3; 457 pcmR[i] = ((p[0] << 0) | (p[1] << 8) | (((int8_t)p[2]) << 16)); 458 p += 3; 459 } 460 461 aptx_hd_encoder_encode_stereo_func( 462 a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state, &pcmL, &pcmR, 463 &encoded_sample); 464 465 uint8_t* encoded_ptr = (uint8_t*)&encoded_sample[0]; 466 data_out[*data_out_index + 0] = *(encoded_ptr + 2); 467 data_out[*data_out_index + 1] = *(encoded_ptr + 1); 468 data_out[*data_out_index + 2] = *(encoded_ptr + 0); 469 data_out[*data_out_index + 3] = *(encoded_ptr + 6); 470 data_out[*data_out_index + 4] = *(encoded_ptr + 5); 471 data_out[*data_out_index + 5] = *(encoded_ptr + 4); 472 473 pcm_bytes_encoded += 24; 474 *data_out_index += 6; 475 } 476 477 return pcm_bytes_encoded; 478} 479 480period_ms_t A2dpCodecConfigAptxHd::encoderIntervalMs() const { 481 return a2dp_vendor_aptx_hd_get_encoder_interval_ms(); 482} 483 484int A2dpCodecConfigAptxHd::getEffectiveMtu() const { 485 return a2dp_aptx_hd_encoder_cb.peer_mtu; 486} 487 488void A2dpCodecConfigAptxHd::debug_codec_dump(int fd) { 489 a2dp_aptx_hd_encoder_stats_t* stats = &a2dp_aptx_hd_encoder_cb.stats; 490 491 A2dpCodecConfig::debug_codec_dump(fd); 492 493 dprintf(fd, 494 " Packet counts (expected/dropped) : %zu / " 495 "%zu\n", 496 stats->media_read_total_expected_packets, 497 stats->media_read_total_dropped_packets); 498 499 dprintf(fd, 500 " PCM read counts (expected/actual) : %zu / " 501 "%zu\n", 502 stats->media_read_total_expected_reads_count, 503 stats->media_read_total_actual_reads_count); 504 505 dprintf(fd, 506 " PCM read bytes (expected/actual) : %zu / " 507 "%zu\n", 508 stats->media_read_total_expected_read_bytes, 509 stats->media_read_total_actual_read_bytes); 510} 511