1 /* 2 * Copyright (C) 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 // 39 static const char* APTX_HD_ENCODER_LIB_NAME = "libaptXHD_encoder.so"; 40 static void* aptx_hd_encoder_lib_handle = NULL; 41 42 static const char* APTX_HD_ENCODER_INIT_NAME = "aptxhdbtenc_init"; 43 typedef int (*tAPTX_HD_ENCODER_INIT)(void* state, short endian); 44 45 static const char* APTX_HD_ENCODER_ENCODE_STEREO_NAME = 46 "aptxhdbtenc_encodestereo"; 47 typedef int (*tAPTX_HD_ENCODER_ENCODE_STEREO)(void* state, void* pcmL, 48 void* pcmR, void* buffer); 49 50 static const char* APTX_HD_ENCODER_SIZEOF_PARAMS_NAME = "SizeofAptxhdbtenc"; 51 typedef int (*tAPTX_HD_ENCODER_SIZEOF_PARAMS)(void); 52 53 static tAPTX_HD_ENCODER_INIT aptx_hd_encoder_init_func; 54 static tAPTX_HD_ENCODER_ENCODE_STEREO aptx_hd_encoder_encode_stereo_func; 55 static 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 1024 65 66 typedef 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 74 typedef 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 86 typedef 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 102 static tA2DP_APTX_HD_ENCODER_CB a2dp_aptx_hd_encoder_cb; 103 104 static 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); 107 static void aptx_hd_init_framing_params( 108 tAPTX_HD_FRAMING_PARAMS* framing_params); 109 static void aptx_hd_update_framing_params( 110 tAPTX_HD_FRAMING_PARAMS* framing_params); 111 static 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 115 bool 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 159 void 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 170 void 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 212 bool 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. 238 static 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 268 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params); 269 } 270 271 void a2dp_vendor_aptx_hd_encoder_cleanup(void) { 272 osi_free(a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state); 273 memset(&a2dp_aptx_hd_encoder_cb, 0, sizeof(a2dp_aptx_hd_encoder_cb)); 274 } 275 276 // 277 // Initialize the framing parameters, and set those that don't change 278 // while streaming (e.g., 'sleep_time_ns'). 279 // 280 static void aptx_hd_init_framing_params( 281 tAPTX_HD_FRAMING_PARAMS* framing_params) { 282 framing_params->sleep_time_ns = 0; 283 framing_params->pcm_reads = 0; 284 framing_params->pcm_bytes_per_read = 0; 285 framing_params->aptx_hd_bytes = 0; 286 framing_params->frame_size_counter = 0; 287 288 framing_params->sleep_time_ns = 9000000; 289 290 LOG_DEBUG(LOG_TAG, "%s: sleep_time_ns = %" PRIu64, __func__, 291 framing_params->sleep_time_ns); 292 } 293 294 // 295 // Set frame size and transmission interval needed to stream the required 296 // sample rate using 2-DH5 packets for aptX and 2-DH3 packets for aptX-LL. 297 // With SCMS-T enabled we need to reserve room for extra headers added later. 298 // Packets are always sent at equals time intervals but to achieve the 299 // required sample rate, the frame size needs to change on occasion. 300 // 301 // Also need to specify how many of the required PCM samples are read at a 302 // time: 303 // aptx_bytes = pcm_reads * pcm_bytes_per_read / 4 304 // and 305 // number of aptX samples produced = pcm_bytes_per_read / 16 306 // 307 static void aptx_hd_update_framing_params( 308 tAPTX_HD_FRAMING_PARAMS* framing_params) { 309 if (a2dp_aptx_hd_encoder_cb.feeding_params.sample_rate == 48000) { 310 framing_params->aptx_hd_bytes = 648; 311 framing_params->pcm_bytes_per_read = 24; 312 framing_params->pcm_reads = 108; 313 } else { 314 // Assume the sample rate is 44100 315 316 // 317 // Total of 80 iterations: 318 // - Iteration 80: packet size 648, with 108 reads of 24 PCM bytes 319 // - Iterations 20, 40, 60: packet size 612, with 102 reads of 24 PCM bytes 320 // - All other iterations: packet size 594, with 99 reads of 24 PCM bytes 321 // 322 if (framing_params->frame_size_counter + 1 == 80) { 323 framing_params->aptx_hd_bytes = 648; 324 framing_params->pcm_bytes_per_read = 24; 325 framing_params->pcm_reads = 108; 326 } else if (((framing_params->frame_size_counter + 1) % 20) == 0) { 327 framing_params->aptx_hd_bytes = 612; 328 framing_params->pcm_bytes_per_read = 24; 329 framing_params->pcm_reads = 102; 330 } else { 331 framing_params->aptx_hd_bytes = 594; 332 framing_params->pcm_bytes_per_read = 24; 333 framing_params->pcm_reads = 99; 334 } 335 framing_params->frame_size_counter++; 336 if (framing_params->frame_size_counter == 80) 337 framing_params->frame_size_counter = 0; 338 } 339 340 LOG_VERBOSE(LOG_TAG, 341 "%s: sleep_time_ns = %" PRIu64 342 " aptx_hd_bytes = %u " 343 "pcm_bytes_per_read = %u pcm_reads = %u frame_size_counter = %u", 344 __func__, framing_params->sleep_time_ns, 345 framing_params->aptx_hd_bytes, framing_params->pcm_bytes_per_read, 346 framing_params->pcm_reads, framing_params->frame_size_counter); 347 } 348 349 void a2dp_vendor_aptx_hd_feeding_reset(void) { 350 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params); 351 } 352 353 void a2dp_vendor_aptx_hd_feeding_flush(void) { 354 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params); 355 } 356 357 period_ms_t a2dp_vendor_aptx_hd_get_encoder_interval_ms(void) { 358 return a2dp_aptx_hd_encoder_cb.framing_params.sleep_time_ns / (1000 * 1000); 359 } 360 361 void a2dp_vendor_aptx_hd_send_frames(uint64_t timestamp_us) { 362 tAPTX_HD_FRAMING_PARAMS* framing_params = 363 &a2dp_aptx_hd_encoder_cb.framing_params; 364 365 // Prepare the packet to send 366 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BT_DEFAULT_BUFFER_SIZE); 367 p_buf->offset = A2DP_APTX_HD_OFFSET; 368 p_buf->len = 0; 369 p_buf->layer_specific = 0; 370 371 uint8_t* encoded_ptr = (uint8_t*)(p_buf + 1); 372 encoded_ptr += p_buf->offset; 373 374 aptx_hd_update_framing_params(framing_params); 375 376 // 377 // Read the PCM data and encode it 378 // 379 LOG_VERBOSE(LOG_TAG, "%s: %u PCM reads of size %u", __func__, 380 framing_params->pcm_reads, framing_params->pcm_bytes_per_read); 381 size_t encoded_ptr_index = 0; 382 size_t pcm_bytes_encoded = 0; 383 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_packets++; 384 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_reads_count += 385 framing_params->pcm_reads; 386 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_read_bytes += 387 framing_params->pcm_reads * framing_params->pcm_bytes_per_read; 388 for (size_t reads = 0; reads < framing_params->pcm_reads; reads++) { 389 uint32_t 390 read_buffer32[A2DP_APTX_HD_MAX_PCM_BYTES_PER_READ / sizeof(uint32_t)]; 391 size_t pcm_bytes_read = a2dp_aptx_hd_encoder_cb.read_callback( 392 (uint8_t*)read_buffer32, framing_params->pcm_bytes_per_read); 393 a2dp_aptx_hd_encoder_cb.stats.media_read_total_actual_read_bytes += 394 pcm_bytes_read; 395 if (pcm_bytes_read < framing_params->pcm_bytes_per_read) { 396 LOG_WARN(LOG_TAG, 397 "%s: underflow at PCM reading iteration %zu: read %zu " 398 "instead of %d", 399 __func__, reads, pcm_bytes_read, 400 framing_params->pcm_bytes_per_read); 401 break; 402 } 403 a2dp_aptx_hd_encoder_cb.stats.media_read_total_actual_reads_count++; 404 pcm_bytes_encoded += aptx_hd_encode_24bit( 405 framing_params, &encoded_ptr_index, read_buffer32, encoded_ptr); 406 } 407 408 // Compute the number of encoded bytes 409 const int COMPRESSION_RATIO = 4; 410 size_t encoded_bytes = pcm_bytes_encoded / COMPRESSION_RATIO; 411 p_buf->len += encoded_bytes; 412 LOG_VERBOSE(LOG_TAG, "%s: encoded %zu PCM bytes to %zu", __func__, 413 pcm_bytes_encoded, encoded_bytes); 414 415 // Update the RTP timestamp 416 *((uint32_t*)(p_buf + 1)) = a2dp_aptx_hd_encoder_cb.timestamp; 417 const uint8_t BYTES_PER_FRAME = 3; 418 uint32_t rtp_timestamp = 419 (pcm_bytes_encoded / 420 a2dp_aptx_hd_encoder_cb.feeding_params.channel_count) / 421 BYTES_PER_FRAME; 422 a2dp_aptx_hd_encoder_cb.timestamp += rtp_timestamp; 423 424 if (p_buf->len > 0) { 425 a2dp_aptx_hd_encoder_cb.enqueue_callback(p_buf, 1); 426 } else { 427 a2dp_aptx_hd_encoder_cb.stats.media_read_total_dropped_packets++; 428 osi_free(p_buf); 429 } 430 } 431 432 static size_t aptx_hd_encode_24bit(tAPTX_HD_FRAMING_PARAMS* framing_params, 433 size_t* data_out_index, uint32_t* data32_in, 434 uint8_t* data_out) { 435 size_t pcm_bytes_encoded = 0; 436 const uint8_t* p = (const uint8_t*)(data32_in); 437 438 for (size_t aptx_hd_samples = 0; 439 aptx_hd_samples < framing_params->pcm_bytes_per_read / 24; 440 aptx_hd_samples++) { 441 uint32_t pcmL[4]; 442 uint32_t pcmR[4]; 443 uint32_t encoded_sample[2]; 444 445 // Expand from AUDIO_FORMAT_PCM_24_BIT_PACKED data (3 bytes per sample) 446 // into AUDIO_FORMAT_PCM_8_24_BIT (4 bytes per sample). 447 for (size_t i = 0; i < 4; i++) { 448 pcmL[i] = ((p[0] << 0) | (p[1] << 8) | (((int8_t)p[2]) << 16)); 449 p += 3; 450 pcmR[i] = ((p[0] << 0) | (p[1] << 8) | (((int8_t)p[2]) << 16)); 451 p += 3; 452 } 453 454 aptx_hd_encoder_encode_stereo_func( 455 a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state, &pcmL, &pcmR, 456 &encoded_sample); 457 458 uint8_t* encoded_ptr = (uint8_t*)&encoded_sample[0]; 459 data_out[*data_out_index + 0] = *(encoded_ptr + 2); 460 data_out[*data_out_index + 1] = *(encoded_ptr + 1); 461 data_out[*data_out_index + 2] = *(encoded_ptr + 0); 462 data_out[*data_out_index + 3] = *(encoded_ptr + 6); 463 data_out[*data_out_index + 4] = *(encoded_ptr + 5); 464 data_out[*data_out_index + 5] = *(encoded_ptr + 4); 465 466 pcm_bytes_encoded += 24; 467 *data_out_index += 6; 468 } 469 470 return pcm_bytes_encoded; 471 } 472 473 period_ms_t A2dpCodecConfigAptxHd::encoderIntervalMs() const { 474 return a2dp_vendor_aptx_hd_get_encoder_interval_ms(); 475 } 476 477 void A2dpCodecConfigAptxHd::debug_codec_dump(int fd) { 478 a2dp_aptx_hd_encoder_stats_t* stats = &a2dp_aptx_hd_encoder_cb.stats; 479 480 A2dpCodecConfig::debug_codec_dump(fd); 481 482 dprintf(fd, 483 " Packet counts (expected/dropped) : %zu / " 484 "%zu\n", 485 stats->media_read_total_expected_packets, 486 stats->media_read_total_dropped_packets); 487 488 dprintf(fd, 489 " PCM read counts (expected/actual) : %zu / " 490 "%zu\n", 491 stats->media_read_total_expected_reads_count, 492 stats->media_read_total_actual_reads_count); 493 494 dprintf(fd, 495 " PCM read bytes (expected/actual) : %zu / " 496 "%zu\n", 497 stats->media_read_total_expected_read_bytes, 498 stats->media_read_total_actual_read_bytes); 499 } 500
