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