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_encoder"
18
19#include "a2dp_vendor_aptx_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.h"
28#include "bt_common.h"
29#include "osi/include/log.h"
30#include "osi/include/osi.h"
31
32//
33// Encoder for aptX Source Codec
34//
35
36//
37// The aptX encoder shared library, and the functions to use
38//
39static const char* APTX_ENCODER_LIB_NAME = "libaptX_encoder.so";
40static void* aptx_encoder_lib_handle = NULL;
41
42static const char* APTX_ENCODER_INIT_NAME = "aptxbtenc_init";
43typedef int (*tAPTX_ENCODER_INIT)(void* state, short endian);
44
45static const char* APTX_ENCODER_ENCODE_STEREO_NAME = "aptxbtenc_encodestereo";
46typedef int (*tAPTX_ENCODER_ENCODE_STEREO)(void* state, void* pcmL, void* pcmR,
47                                           void* buffer);
48
49static const char* APTX_ENCODER_SIZEOF_PARAMS_NAME = "SizeofAptxbtenc";
50typedef int (*tAPTX_ENCODER_SIZEOF_PARAMS)(void);
51
52static tAPTX_ENCODER_INIT aptx_encoder_init_func;
53static tAPTX_ENCODER_ENCODE_STEREO aptx_encoder_encode_stereo_func;
54static tAPTX_ENCODER_SIZEOF_PARAMS aptx_encoder_sizeof_params_func;
55
56// offset
57#if (BTA_AV_CO_CP_SCMS_T == TRUE)
58#define A2DP_APTX_OFFSET (AVDT_MEDIA_OFFSET + 1)
59#else
60// no RTP header for aptX classic
61#define A2DP_APTX_OFFSET (AVDT_MEDIA_OFFSET - AVDT_MEDIA_HDR_SIZE)
62#endif
63
64#define A2DP_APTX_MAX_PCM_BYTES_PER_READ 1024
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_bytes;
71  uint32_t frame_size_counter;
72} tAPTX_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_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_FRAMING_PARAMS framing_params;
98  void* aptx_encoder_state;
99  a2dp_aptx_encoder_stats_t stats;
100} tA2DP_APTX_ENCODER_CB;
101
102static tA2DP_APTX_ENCODER_CB a2dp_aptx_encoder_cb;
103
104static void a2dp_vendor_aptx_encoder_update(uint16_t peer_mtu,
105                                            A2dpCodecConfig* a2dp_codec_config,
106                                            bool* p_restart_input,
107                                            bool* p_restart_output,
108                                            bool* p_config_updated);
109static void aptx_init_framing_params(tAPTX_FRAMING_PARAMS* framing_params);
110static void aptx_update_framing_params(tAPTX_FRAMING_PARAMS* framing_params);
111static size_t aptx_encode_16bit(tAPTX_FRAMING_PARAMS* framing_params,
112                                size_t* data_out_index, uint16_t* data16_in,
113                                uint8_t* data_out);
114
115bool A2DP_VendorLoadEncoderAptx(void) {
116  if (aptx_encoder_lib_handle != NULL) return true;  // Already loaded
117
118  // Open the encoder library
119  aptx_encoder_lib_handle = dlopen(APTX_ENCODER_LIB_NAME, RTLD_NOW);
120  if (aptx_encoder_lib_handle == NULL) {
121    LOG_ERROR(LOG_TAG, "%s: cannot open aptX encoder library %s: %s", __func__,
122              APTX_ENCODER_LIB_NAME, dlerror());
123    return false;
124  }
125
126  aptx_encoder_init_func = (tAPTX_ENCODER_INIT)dlsym(aptx_encoder_lib_handle,
127                                                     APTX_ENCODER_INIT_NAME);
128  if (aptx_encoder_init_func == NULL) {
129    LOG_ERROR(LOG_TAG,
130              "%s: cannot find function '%s' in the encoder library: %s",
131              __func__, APTX_ENCODER_INIT_NAME, dlerror());
132    A2DP_VendorUnloadEncoderAptx();
133    return false;
134  }
135
136  aptx_encoder_encode_stereo_func = (tAPTX_ENCODER_ENCODE_STEREO)dlsym(
137      aptx_encoder_lib_handle, APTX_ENCODER_ENCODE_STEREO_NAME);
138  if (aptx_encoder_encode_stereo_func == NULL) {
139    LOG_ERROR(LOG_TAG,
140              "%s: cannot find function '%s' in the encoder library: %s",
141              __func__, APTX_ENCODER_ENCODE_STEREO_NAME, dlerror());
142    A2DP_VendorUnloadEncoderAptx();
143    return false;
144  }
145
146  aptx_encoder_sizeof_params_func = (tAPTX_ENCODER_SIZEOF_PARAMS)dlsym(
147      aptx_encoder_lib_handle, APTX_ENCODER_SIZEOF_PARAMS_NAME);
148  if (aptx_encoder_sizeof_params_func == NULL) {
149    LOG_ERROR(LOG_TAG,
150              "%s: cannot find function '%s' in the encoder library: %s",
151              __func__, APTX_ENCODER_SIZEOF_PARAMS_NAME, dlerror());
152    A2DP_VendorUnloadEncoderAptx();
153    return false;
154  }
155
156  return true;
157}
158
159void A2DP_VendorUnloadEncoderAptx(void) {
160  aptx_encoder_init_func = NULL;
161  aptx_encoder_encode_stereo_func = NULL;
162  aptx_encoder_sizeof_params_func = NULL;
163
164  if (aptx_encoder_lib_handle != NULL) {
165    dlclose(aptx_encoder_lib_handle);
166    aptx_encoder_lib_handle = NULL;
167  }
168}
169
170void a2dp_vendor_aptx_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_encoder_cb, 0, sizeof(a2dp_aptx_encoder_cb));
176
177  a2dp_aptx_encoder_cb.stats.session_start_us = time_get_os_boottime_us();
178
179  a2dp_aptx_encoder_cb.read_callback = read_callback;
180  a2dp_aptx_encoder_cb.enqueue_callback = enqueue_callback;
181  a2dp_aptx_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
182  a2dp_aptx_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
183  a2dp_aptx_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
184  a2dp_aptx_encoder_cb.timestamp = 0;
185
186  /* aptX encoder config */
187  a2dp_aptx_encoder_cb.use_SCMS_T = false;  // TODO: should be a parameter
188#if (BTA_AV_CO_CP_SCMS_T == TRUE)
189  a2dp_aptx_encoder_cb.use_SCMS_T = true;
190#endif
191
192  a2dp_aptx_encoder_cb.aptx_encoder_state =
193      osi_malloc(aptx_encoder_sizeof_params_func());
194  if (a2dp_aptx_encoder_cb.aptx_encoder_state != NULL) {
195    aptx_encoder_init_func(a2dp_aptx_encoder_cb.aptx_encoder_state, 0);
196  } else {
197    LOG_ERROR(LOG_TAG, "%s: Cannot allocate aptX encoder state", __func__);
198    // TODO: Return an error?
199  }
200
201  // NOTE: Ignore the restart_input / restart_output flags - this initization
202  // happens when the connection is (re)started.
203  bool restart_input = false;
204  bool restart_output = false;
205  bool config_updated = false;
206  a2dp_vendor_aptx_encoder_update(a2dp_aptx_encoder_cb.peer_mtu,
207                                  a2dp_codec_config, &restart_input,
208                                  &restart_output, &config_updated);
209}
210
211bool A2dpCodecConfigAptx::updateEncoderUserConfig(
212    const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params, bool* p_restart_input,
213    bool* p_restart_output, bool* p_config_updated) {
214  a2dp_aptx_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
215  a2dp_aptx_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
216  a2dp_aptx_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
217  a2dp_aptx_encoder_cb.timestamp = 0;
218
219  if (a2dp_aptx_encoder_cb.peer_mtu == 0) {
220    LOG_ERROR(LOG_TAG,
221              "%s: Cannot update the codec encoder for %s: "
222              "invalid peer MTU",
223              __func__, name().c_str());
224    return false;
225  }
226
227  a2dp_vendor_aptx_encoder_update(a2dp_aptx_encoder_cb.peer_mtu, this,
228                                  p_restart_input, p_restart_output,
229                                  p_config_updated);
230  return true;
231}
232
233// Update the A2DP aptX encoder.
234// |peer_mtu| is the peer MTU.
235// |a2dp_codec_config| is the A2DP codec to use for the update.
236static void a2dp_vendor_aptx_encoder_update(uint16_t peer_mtu,
237                                            A2dpCodecConfig* a2dp_codec_config,
238                                            bool* p_restart_input,
239                                            bool* p_restart_output,
240                                            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 = &a2dp_aptx_encoder_cb.feeding_params;
257  p_feeding_params->sample_rate =
258      A2DP_VendorGetTrackSampleRateAptx(p_codec_info);
259  p_feeding_params->bits_per_sample =
260      a2dp_codec_config->getAudioBitsPerSample();
261  p_feeding_params->channel_count =
262      A2DP_VendorGetTrackChannelCountAptx(p_codec_info);
263  LOG_DEBUG(LOG_TAG, "%s: sample_rate=%u bits_per_sample=%u channel_count=%u",
264            __func__, p_feeding_params->sample_rate,
265            p_feeding_params->bits_per_sample, p_feeding_params->channel_count);
266
267  aptx_init_framing_params(&a2dp_aptx_encoder_cb.framing_params);
268}
269
270void a2dp_vendor_aptx_encoder_cleanup(void) {
271  osi_free(a2dp_aptx_encoder_cb.aptx_encoder_state);
272  memset(&a2dp_aptx_encoder_cb, 0, sizeof(a2dp_aptx_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_init_framing_params(tAPTX_FRAMING_PARAMS* framing_params) {
280  framing_params->sleep_time_ns = 0;
281  framing_params->pcm_reads = 0;
282  framing_params->pcm_bytes_per_read = 0;
283  framing_params->aptx_bytes = 0;
284  framing_params->frame_size_counter = 0;
285
286  if (a2dp_aptx_encoder_cb.feeding_params.sample_rate == 48000) {
287    if (a2dp_aptx_encoder_cb.use_SCMS_T) {
288      framing_params->sleep_time_ns = 13000000;
289    } else {
290      framing_params->sleep_time_ns = 14000000;
291    }
292  } else {
293    // Assume the sample rate is 44100
294    if (a2dp_aptx_encoder_cb.use_SCMS_T) {
295      framing_params->sleep_time_ns = 14000000;
296    } else {
297      framing_params->sleep_time_ns = 15000000;
298    }
299  }
300
301  LOG_DEBUG(LOG_TAG, "%s: sleep_time_ns = %" PRIu64, __func__,
302            framing_params->sleep_time_ns);
303}
304
305//
306// Set frame size and transmission interval needed to stream the required
307// sample rate using 2-DH5 packets for aptX and 2-DH3 packets for aptX-LL.
308// With SCMS-T enabled we need to reserve room for extra headers added later.
309// Packets are always sent at equals time intervals but to achieve the
310// required sample rate, the frame size needs to change on occasion.
311//
312// Also need to specify how many of the required PCM samples are read at a
313// time:
314//     aptx_bytes = pcm_reads * pcm_bytes_per_read / 4
315// and
316//     number of aptX samples produced = pcm_bytes_per_read / 16
317//
318static void aptx_update_framing_params(tAPTX_FRAMING_PARAMS* framing_params) {
319  if (a2dp_aptx_encoder_cb.feeding_params.sample_rate == 48000) {
320    if (a2dp_aptx_encoder_cb.use_SCMS_T) {
321      framing_params->aptx_bytes = 624;
322      framing_params->pcm_bytes_per_read = 208;
323      framing_params->pcm_reads = 12;
324    } else {
325      framing_params->aptx_bytes = 672;
326      framing_params->pcm_bytes_per_read = 224;
327      framing_params->pcm_reads = 12;
328    }
329  } else {
330    // Assume the sample rate is 44100
331    if (a2dp_aptx_encoder_cb.use_SCMS_T) {
332      if (++framing_params->frame_size_counter < 20) {
333        framing_params->aptx_bytes = 616;
334        framing_params->pcm_bytes_per_read = 224;
335        framing_params->pcm_reads = 11;
336      } else {
337        framing_params->aptx_bytes = 644;
338        framing_params->pcm_bytes_per_read = 368;
339        framing_params->pcm_reads = 7;
340        framing_params->frame_size_counter = 0;
341      }
342    } else {
343      if (++framing_params->frame_size_counter < 8) {
344        framing_params->aptx_bytes = 660;
345        framing_params->pcm_bytes_per_read = 240;
346        framing_params->pcm_reads = 11;
347      } else {
348        framing_params->aptx_bytes = 672;
349        framing_params->pcm_bytes_per_read = 224;
350        framing_params->pcm_reads = 12;
351        framing_params->frame_size_counter = 0;
352      }
353    }
354  }
355
356  LOG_VERBOSE(LOG_TAG,
357              "%s: sleep_time_ns = %" PRIu64
358              " aptx_bytes = %u "
359              "pcm_bytes_per_read = %u pcm_reads = %u frame_size_counter = %u",
360              __func__, framing_params->sleep_time_ns,
361              framing_params->aptx_bytes, framing_params->pcm_bytes_per_read,
362              framing_params->pcm_reads, framing_params->frame_size_counter);
363}
364
365void a2dp_vendor_aptx_feeding_reset(void) {
366  aptx_init_framing_params(&a2dp_aptx_encoder_cb.framing_params);
367}
368
369void a2dp_vendor_aptx_feeding_flush(void) {
370  aptx_init_framing_params(&a2dp_aptx_encoder_cb.framing_params);
371}
372
373period_ms_t a2dp_vendor_aptx_get_encoder_interval_ms(void) {
374  return a2dp_aptx_encoder_cb.framing_params.sleep_time_ns / (1000 * 1000);
375}
376
377void a2dp_vendor_aptx_send_frames(uint64_t timestamp_us) {
378  tAPTX_FRAMING_PARAMS* framing_params = &a2dp_aptx_encoder_cb.framing_params;
379
380  // Prepare the packet to send
381  BT_HDR* p_buf = (BT_HDR*)osi_malloc(BT_DEFAULT_BUFFER_SIZE);
382  p_buf->offset = A2DP_APTX_OFFSET;
383  p_buf->len = 0;
384  p_buf->layer_specific = 0;
385
386  uint8_t* encoded_ptr = (uint8_t*)(p_buf + 1);
387  encoded_ptr += p_buf->offset;
388
389  aptx_update_framing_params(framing_params);
390
391  //
392  // Read the PCM data and encode it
393  //
394  LOG_VERBOSE(LOG_TAG, "%s: %u PCM reads of size %u", __func__,
395              framing_params->pcm_reads, framing_params->pcm_bytes_per_read);
396  size_t encoded_ptr_index = 0;
397  size_t pcm_bytes_encoded = 0;
398  a2dp_aptx_encoder_cb.stats.media_read_total_expected_packets++;
399  a2dp_aptx_encoder_cb.stats.media_read_total_expected_reads_count +=
400      framing_params->pcm_reads;
401  a2dp_aptx_encoder_cb.stats.media_read_total_expected_read_bytes +=
402      framing_params->pcm_reads * framing_params->pcm_bytes_per_read;
403  for (size_t reads = 0; reads < framing_params->pcm_reads; reads++) {
404    uint16_t read_buffer16[A2DP_APTX_MAX_PCM_BYTES_PER_READ / sizeof(uint16_t)];
405    size_t pcm_bytes_read = a2dp_aptx_encoder_cb.read_callback(
406        (uint8_t*)read_buffer16, framing_params->pcm_bytes_per_read);
407    a2dp_aptx_encoder_cb.stats.media_read_total_actual_read_bytes +=
408        pcm_bytes_read;
409    if (pcm_bytes_read < framing_params->pcm_bytes_per_read) {
410      LOG_WARN(LOG_TAG,
411               "%s: underflow at PCM reading iteration %zu: read %zu "
412               "instead of %d",
413               __func__, reads, pcm_bytes_read,
414               framing_params->pcm_bytes_per_read);
415      break;
416    }
417    a2dp_aptx_encoder_cb.stats.media_read_total_actual_reads_count++;
418    pcm_bytes_encoded += aptx_encode_16bit(framing_params, &encoded_ptr_index,
419                                           read_buffer16, encoded_ptr);
420  }
421
422  // Compute the number of encoded bytes
423  const int COMPRESSION_RATIO = 4;
424  size_t encoded_bytes = pcm_bytes_encoded / COMPRESSION_RATIO;
425  p_buf->len += encoded_bytes;
426  LOG_VERBOSE(LOG_TAG, "%s: encoded %zu PCM bytes to %zu", __func__,
427              pcm_bytes_encoded, encoded_bytes);
428
429  // Update the RTP timestamp
430  *((uint32_t*)(p_buf + 1)) = a2dp_aptx_encoder_cb.timestamp;
431  const uint8_t BYTES_PER_FRAME = 2;
432  uint32_t rtp_timestamp =
433      (pcm_bytes_encoded / a2dp_aptx_encoder_cb.feeding_params.channel_count) /
434      BYTES_PER_FRAME;
435  a2dp_aptx_encoder_cb.timestamp += rtp_timestamp;
436
437  if (p_buf->len > 0) {
438    a2dp_aptx_encoder_cb.enqueue_callback(p_buf, 1);
439  } else {
440    a2dp_aptx_encoder_cb.stats.media_read_total_dropped_packets++;
441    osi_free(p_buf);
442  }
443}
444
445static size_t aptx_encode_16bit(tAPTX_FRAMING_PARAMS* framing_params,
446                                size_t* data_out_index, uint16_t* data16_in,
447                                uint8_t* data_out) {
448  size_t pcm_bytes_encoded = 0;
449  size_t frame = 0;
450
451  for (size_t aptx_samples = 0;
452       aptx_samples < framing_params->pcm_bytes_per_read / 16; aptx_samples++) {
453    uint32_t pcmL[4];
454    uint32_t pcmR[4];
455    uint16_t encoded_sample[2];
456
457    for (size_t i = 0, j = frame; i < 4; i++, j++) {
458      pcmL[i] = (uint16_t) * (data16_in + (2 * j));
459      pcmR[i] = (uint16_t) * (data16_in + ((2 * j) + 1));
460    }
461
462    aptx_encoder_encode_stereo_func(a2dp_aptx_encoder_cb.aptx_encoder_state,
463                                    &pcmL, &pcmR, &encoded_sample);
464
465    data_out[*data_out_index + 0] = (uint8_t)((encoded_sample[0] >> 8) & 0xff);
466    data_out[*data_out_index + 1] = (uint8_t)((encoded_sample[0] >> 0) & 0xff);
467    data_out[*data_out_index + 2] = (uint8_t)((encoded_sample[1] >> 8) & 0xff);
468    data_out[*data_out_index + 3] = (uint8_t)((encoded_sample[1] >> 0) & 0xff);
469
470    frame += 4;
471    pcm_bytes_encoded += 16;
472    *data_out_index += 4;
473  }
474
475  return pcm_bytes_encoded;
476}
477
478period_ms_t A2dpCodecConfigAptx::encoderIntervalMs() const {
479  return a2dp_vendor_aptx_get_encoder_interval_ms();
480}
481
482void A2dpCodecConfigAptx::debug_codec_dump(int fd) {
483  a2dp_aptx_encoder_stats_t* stats = &a2dp_aptx_encoder_cb.stats;
484
485  A2dpCodecConfig::debug_codec_dump(fd);
486
487  dprintf(fd,
488          "  Packet counts (expected/dropped)                        : %zu / "
489          "%zu\n",
490          stats->media_read_total_expected_packets,
491          stats->media_read_total_dropped_packets);
492
493  dprintf(fd,
494          "  PCM read counts (expected/actual)                       : %zu / "
495          "%zu\n",
496          stats->media_read_total_expected_reads_count,
497          stats->media_read_total_actual_reads_count);
498
499  dprintf(fd,
500          "  PCM read bytes (expected/actual)                        : %zu / "
501          "%zu\n",
502          stats->media_read_total_expected_read_bytes,
503          stats->media_read_total_actual_read_bytes);
504}
505