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 "bluetooth-a2dp"
18
19 #include "a2dp_vendor_aptx_hd_encoder.h"
20
21 #include <bluetooth/log.h>
22 #include <dlfcn.h>
23 #include <inttypes.h>
24 #include <string.h>
25
26 #include "a2dp_vendor.h"
27 #include "a2dp_vendor_aptx_hd.h"
28 #include "aptXHDbtenc.h"
29 #include "common/time_util.h"
30 #include "internal_include/bt_target.h"
31 #include "os/log.h"
32 #include "osi/include/allocator.h"
33 #include "stack/include/bt_hdr.h"
34
35 using namespace bluetooth;
36
37 //
38 // Encoder for aptX-HD Source Codec
39 //
40
41 static const tAPTX_HD_API aptx_hd_api = {
42 .init_func = aptxhdbtenc_init,
43 .encode_stereo_func = aptxhdbtenc_encodestereo,
44 .sizeof_params_func = SizeofAptxhdbtenc,
45 };
46
47 // offset
48 #define A2DP_APTX_HD_OFFSET AVDT_MEDIA_OFFSET
49
50 #define A2DP_APTX_HD_MAX_PCM_BYTES_PER_READ 4096
51
52 typedef struct {
53 uint64_t sleep_time_ns;
54 uint32_t pcm_reads;
55 uint32_t pcm_bytes_per_read;
56 uint32_t aptx_hd_bytes;
57 uint32_t frame_size_counter;
58 } tAPTX_HD_FRAMING_PARAMS;
59
60 typedef struct {
61 uint64_t session_start_us;
62
63 size_t media_read_total_expected_packets;
64 size_t media_read_total_expected_reads_count;
65 size_t media_read_total_expected_read_bytes;
66
67 size_t media_read_total_dropped_packets;
68 size_t media_read_total_actual_reads_count;
69 size_t media_read_total_actual_read_bytes;
70 } a2dp_aptx_hd_encoder_stats_t;
71
72 typedef struct {
73 a2dp_source_read_callback_t read_callback;
74 a2dp_source_enqueue_callback_t enqueue_callback;
75
76 bool use_SCMS_T;
77 tA2DP_ENCODER_INIT_PEER_PARAMS peer_params;
78 uint32_t timestamp; // Timestamp for the A2DP frames
79
80 tA2DP_FEEDING_PARAMS feeding_params;
81 tAPTX_HD_FRAMING_PARAMS framing_params;
82 void* aptx_hd_encoder_state;
83 a2dp_aptx_hd_encoder_stats_t stats;
84 } tA2DP_APTX_HD_ENCODER_CB;
85
86 static tA2DP_APTX_HD_ENCODER_CB a2dp_aptx_hd_encoder_cb;
87
88 static void a2dp_vendor_aptx_hd_encoder_update(
89 A2dpCodecConfig* a2dp_codec_config, bool* p_restart_input,
90 bool* p_restart_output, bool* p_config_updated);
91 static void aptx_hd_init_framing_params(
92 tAPTX_HD_FRAMING_PARAMS* framing_params);
93 static void aptx_hd_update_framing_params(
94 tAPTX_HD_FRAMING_PARAMS* framing_params);
95 static size_t aptx_hd_encode_24bit(tAPTX_HD_FRAMING_PARAMS* framing_params,
96 size_t* data_out_index, uint32_t* data32_in,
97 uint8_t* data_out);
98
99 /*******************************************************************************
100 *
101 * Function A2DP_VendorLoadEncoderAptxHd
102 *
103 * Description This function will try to load the aptx HD encoder library.
104 *
105 * Returns LOAD_SUCCESS on success
106 * LOAD_ERROR_MISSING_CODEC on missing library
107 * LOAD_ERROR_VERSION_MISMATCH on symbol loading error
108 *
109 ******************************************************************************/
A2DP_VendorLoadEncoderAptxHd(void)110 tLOADING_CODEC_STATUS A2DP_VendorLoadEncoderAptxHd(void) {
111 // Nothing to do - the library is statically linked
112 return LOAD_SUCCESS;
113 }
114
A2DP_VendorCopyAptxHdApi(tAPTX_HD_API & external_api)115 bool A2DP_VendorCopyAptxHdApi(tAPTX_HD_API& external_api) {
116 external_api = aptx_hd_api;
117 return true;
118 }
119
A2DP_VendorUnloadEncoderAptxHd(void)120 void A2DP_VendorUnloadEncoderAptxHd(void) {
121 // nothing to do
122 }
123
a2dp_vendor_aptx_hd_encoder_init(const tA2DP_ENCODER_INIT_PEER_PARAMS * p_peer_params,A2dpCodecConfig * a2dp_codec_config,a2dp_source_read_callback_t read_callback,a2dp_source_enqueue_callback_t enqueue_callback)124 void a2dp_vendor_aptx_hd_encoder_init(
125 const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params,
126 A2dpCodecConfig* a2dp_codec_config,
127 a2dp_source_read_callback_t read_callback,
128 a2dp_source_enqueue_callback_t enqueue_callback) {
129 memset(&a2dp_aptx_hd_encoder_cb, 0, sizeof(a2dp_aptx_hd_encoder_cb));
130
131 a2dp_aptx_hd_encoder_cb.stats.session_start_us =
132 bluetooth::common::time_get_os_boottime_us();
133
134 a2dp_aptx_hd_encoder_cb.read_callback = read_callback;
135 a2dp_aptx_hd_encoder_cb.enqueue_callback = enqueue_callback;
136 a2dp_aptx_hd_encoder_cb.peer_params = *p_peer_params;
137 a2dp_aptx_hd_encoder_cb.timestamp = 0;
138
139 /* aptX-HD encoder config */
140 a2dp_aptx_hd_encoder_cb.use_SCMS_T = false;
141
142 a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state =
143 osi_malloc(aptx_hd_api.sizeof_params_func());
144 if (a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state != NULL) {
145 aptx_hd_api.init_func(a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state, 0);
146 } else {
147 log::error("Cannot allocate aptX-HD encoder state");
148 // TODO: Return an error?
149 }
150
151 // NOTE: Ignore the restart_input / restart_output flags - this initization
152 // happens when the audio session is (re)started.
153 bool restart_input = false;
154 bool restart_output = false;
155 bool config_updated = false;
156 a2dp_vendor_aptx_hd_encoder_update(a2dp_codec_config, &restart_input,
157 &restart_output, &config_updated);
158 }
159
160 // Update the A2DP aptX-HD encoder.
161 // |a2dp_codec_config| is the A2DP codec to use for the update.
a2dp_vendor_aptx_hd_encoder_update(A2dpCodecConfig * a2dp_codec_config,bool * p_restart_input,bool * p_restart_output,bool * p_config_updated)162 static void a2dp_vendor_aptx_hd_encoder_update(
163 A2dpCodecConfig* a2dp_codec_config, bool* p_restart_input,
164 bool* p_restart_output, bool* p_config_updated) {
165 uint8_t codec_info[AVDT_CODEC_SIZE];
166
167 *p_restart_input = false;
168 *p_restart_output = false;
169 *p_config_updated = false;
170 if (!a2dp_codec_config->copyOutOtaCodecConfig(codec_info)) {
171 log::error("Cannot update the codec encoder for {}: invalid codec config",
172 a2dp_codec_config->name());
173 return;
174 }
175 const uint8_t* p_codec_info = codec_info;
176
177 // The feeding parameters
178 tA2DP_FEEDING_PARAMS* p_feeding_params =
179 &a2dp_aptx_hd_encoder_cb.feeding_params;
180 p_feeding_params->sample_rate =
181 A2DP_VendorGetTrackSampleRateAptxHd(p_codec_info);
182 p_feeding_params->bits_per_sample =
183 a2dp_codec_config->getAudioBitsPerSample();
184 p_feeding_params->channel_count =
185 A2DP_VendorGetTrackChannelCountAptxHd(p_codec_info);
186 log::info("sample_rate={} bits_per_sample={} channel_count={}",
187 p_feeding_params->sample_rate, p_feeding_params->bits_per_sample,
188 p_feeding_params->channel_count);
189 a2dp_vendor_aptx_hd_feeding_reset();
190 }
191
a2dp_vendor_aptx_hd_encoder_cleanup(void)192 void a2dp_vendor_aptx_hd_encoder_cleanup(void) {
193 osi_free(a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state);
194 memset(&a2dp_aptx_hd_encoder_cb, 0, sizeof(a2dp_aptx_hd_encoder_cb));
195 }
196
197 //
198 // Initialize the framing parameters, and set those that don't change
199 // while streaming (e.g., 'sleep_time_ns').
200 //
aptx_hd_init_framing_params(tAPTX_HD_FRAMING_PARAMS * framing_params)201 static void aptx_hd_init_framing_params(
202 tAPTX_HD_FRAMING_PARAMS* framing_params) {
203 framing_params->sleep_time_ns = 0;
204 framing_params->pcm_reads = 0;
205 framing_params->pcm_bytes_per_read = 0;
206 framing_params->aptx_hd_bytes = 0;
207 framing_params->frame_size_counter = 0;
208
209 framing_params->sleep_time_ns = 9000000;
210
211 log::info("sleep_time_ns={}", framing_params->sleep_time_ns);
212 }
213
214 //
215 // Set frame size and transmission interval needed to stream the required
216 // sample rate using 2-DH5 packets for aptX and 2-DH3 packets for aptX-LL.
217 // With SCMS-T enabled we need to reserve room for extra headers added later.
218 // Packets are always sent at equals time intervals but to achieve the
219 // required sample rate, the frame size needs to change on occasion.
220 //
221 // Also need to specify how many of the required PCM samples are read at a
222 // time:
223 // aptx_bytes = pcm_reads * pcm_bytes_per_read / 4
224 // and
225 // number of aptX samples produced = pcm_bytes_per_read / 16
226 //
aptx_hd_update_framing_params(tAPTX_HD_FRAMING_PARAMS * framing_params)227 static void aptx_hd_update_framing_params(
228 tAPTX_HD_FRAMING_PARAMS* framing_params) {
229 if (a2dp_aptx_hd_encoder_cb.feeding_params.sample_rate == 48000) {
230 framing_params->aptx_hd_bytes = 648;
231 framing_params->pcm_bytes_per_read = 24;
232 framing_params->pcm_reads = 108;
233 } else {
234 // Assume the sample rate is 44100
235
236 //
237 // Total of 80 iterations:
238 // - Iteration 80: packet size 648, with 108 reads of 24 PCM bytes
239 // - Iterations 20, 40, 60: packet size 612, with 102 reads of 24 PCM bytes
240 // - All other iterations: packet size 594, with 99 reads of 24 PCM bytes
241 //
242 if (framing_params->frame_size_counter + 1 == 80) {
243 framing_params->aptx_hd_bytes = 648;
244 framing_params->pcm_bytes_per_read = 24;
245 framing_params->pcm_reads = 108;
246 } else if (((framing_params->frame_size_counter + 1) % 20) == 0) {
247 framing_params->aptx_hd_bytes = 612;
248 framing_params->pcm_bytes_per_read = 24;
249 framing_params->pcm_reads = 102;
250 } else {
251 framing_params->aptx_hd_bytes = 594;
252 framing_params->pcm_bytes_per_read = 24;
253 framing_params->pcm_reads = 99;
254 }
255 framing_params->frame_size_counter++;
256 if (framing_params->frame_size_counter == 80)
257 framing_params->frame_size_counter = 0;
258 }
259
260 log::verbose(
261 "sleep_time_ns={} aptx_hd_bytes={} pcm_bytes_per_read={} pcm_reads={} "
262 "frame_size_counter={}",
263 framing_params->sleep_time_ns, framing_params->aptx_hd_bytes,
264 framing_params->pcm_bytes_per_read, framing_params->pcm_reads,
265 framing_params->frame_size_counter);
266 }
267
a2dp_vendor_aptx_hd_feeding_reset(void)268 void a2dp_vendor_aptx_hd_feeding_reset(void) {
269 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params);
270 }
271
a2dp_vendor_aptx_hd_feeding_flush(void)272 void a2dp_vendor_aptx_hd_feeding_flush(void) {
273 aptx_hd_init_framing_params(&a2dp_aptx_hd_encoder_cb.framing_params);
274 }
275
a2dp_vendor_aptx_hd_get_encoder_interval_ms(void)276 uint64_t a2dp_vendor_aptx_hd_get_encoder_interval_ms(void) {
277 return a2dp_aptx_hd_encoder_cb.framing_params.sleep_time_ns / (1000 * 1000);
278 }
279
a2dp_vendor_aptx_hd_get_effective_frame_size()280 int a2dp_vendor_aptx_hd_get_effective_frame_size() {
281 return a2dp_aptx_hd_encoder_cb.peer_params.peer_mtu;
282 }
283
a2dp_vendor_aptx_hd_send_frames(uint64_t timestamp_us)284 void a2dp_vendor_aptx_hd_send_frames(uint64_t timestamp_us) {
285 tAPTX_HD_FRAMING_PARAMS* framing_params =
286 &a2dp_aptx_hd_encoder_cb.framing_params;
287
288 // Prepare the packet to send
289 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BT_DEFAULT_BUFFER_SIZE);
290 p_buf->offset = A2DP_APTX_HD_OFFSET;
291 p_buf->len = 0;
292 p_buf->layer_specific = 0;
293
294 uint8_t* encoded_ptr = (uint8_t*)(p_buf + 1);
295 encoded_ptr += p_buf->offset;
296
297 aptx_hd_update_framing_params(framing_params);
298
299 //
300 // Read the PCM data and encode it
301 //
302 uint32_t
303 read_buffer32[A2DP_APTX_HD_MAX_PCM_BYTES_PER_READ / sizeof(uint32_t)];
304 uint32_t expected_read_bytes =
305 framing_params->pcm_reads * framing_params->pcm_bytes_per_read;
306 size_t encoded_ptr_index = 0;
307 size_t pcm_bytes_encoded = 0;
308 uint32_t bytes_read = 0;
309
310 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_packets++;
311 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_reads_count++;
312 a2dp_aptx_hd_encoder_cb.stats.media_read_total_expected_read_bytes +=
313 expected_read_bytes;
314
315 log::verbose("PCM read of size {}", expected_read_bytes);
316 bytes_read = a2dp_aptx_hd_encoder_cb.read_callback((uint8_t*)read_buffer32,
317 expected_read_bytes);
318 a2dp_aptx_hd_encoder_cb.stats.media_read_total_actual_read_bytes +=
319 bytes_read;
320 if (bytes_read < expected_read_bytes) {
321 log::warn("underflow at PCM reading: read {} bytes instead of {}",
322 bytes_read, expected_read_bytes);
323 a2dp_aptx_hd_encoder_cb.stats.media_read_total_dropped_packets++;
324 osi_free(p_buf);
325 return;
326 }
327 a2dp_aptx_hd_encoder_cb.stats.media_read_total_actual_reads_count++;
328
329 for (uint32_t reads = 0, offset = 0; reads < framing_params->pcm_reads;
330 reads++, offset +=
331 framing_params->pcm_bytes_per_read / sizeof(uint32_t)) {
332 pcm_bytes_encoded +=
333 aptx_hd_encode_24bit(framing_params, &encoded_ptr_index,
334 read_buffer32 + offset, encoded_ptr);
335 }
336
337 // Compute the number of encoded bytes
338 const int COMPRESSION_RATIO = 4;
339 size_t encoded_bytes = pcm_bytes_encoded / COMPRESSION_RATIO;
340 p_buf->len += encoded_bytes;
341 log::verbose("encoded {} PCM bytes to {}", pcm_bytes_encoded, encoded_bytes);
342
343 // Update the RTP timestamp
344 *((uint32_t*)(p_buf + 1)) = a2dp_aptx_hd_encoder_cb.timestamp;
345 const uint8_t BYTES_PER_FRAME = 3;
346 uint32_t rtp_timestamp =
347 (pcm_bytes_encoded /
348 a2dp_aptx_hd_encoder_cb.feeding_params.channel_count) /
349 BYTES_PER_FRAME;
350
351 // Timestamp will wrap over to 0 if stream continues on long enough
352 // (>25H @ 48KHz). The parameters are promoted to 64bit to ensure that
353 // no unsigned overflow is triggered as ubsan is always enabled.
354 a2dp_aptx_hd_encoder_cb.timestamp =
355 ((uint64_t)a2dp_aptx_hd_encoder_cb.timestamp + rtp_timestamp) & UINT32_MAX;
356
357 if (p_buf->len > 0) {
358 a2dp_aptx_hd_encoder_cb.enqueue_callback(p_buf, 1, bytes_read);
359 } else {
360 a2dp_aptx_hd_encoder_cb.stats.media_read_total_dropped_packets++;
361 osi_free(p_buf);
362 }
363 }
364
aptx_hd_encode_24bit(tAPTX_HD_FRAMING_PARAMS * framing_params,size_t * data_out_index,uint32_t * data32_in,uint8_t * data_out)365 static size_t aptx_hd_encode_24bit(tAPTX_HD_FRAMING_PARAMS* framing_params,
366 size_t* data_out_index, uint32_t* data32_in,
367 uint8_t* data_out) {
368 size_t pcm_bytes_encoded = 0;
369 const uint8_t* p = (const uint8_t*)(data32_in);
370
371 for (size_t aptx_hd_samples = 0;
372 aptx_hd_samples < framing_params->pcm_bytes_per_read / 24;
373 aptx_hd_samples++) {
374 uint32_t pcmL[4];
375 uint32_t pcmR[4];
376 uint32_t encoded_sample[2];
377
378 // Expand from AUDIO_FORMAT_PCM_24_BIT_PACKED data (3 bytes per sample)
379 // into AUDIO_FORMAT_PCM_8_24_BIT (4 bytes per sample).
380 for (size_t i = 0; i < 4; i++) {
381 pcmL[i] = ((p[0] << 0) | (p[1] << 8) | (((int8_t)p[2]) << 16));
382 p += 3;
383 pcmR[i] = ((p[0] << 0) | (p[1] << 8) | (((int8_t)p[2]) << 16));
384 p += 3;
385 }
386
387 aptx_hd_api.encode_stereo_func(
388 a2dp_aptx_hd_encoder_cb.aptx_hd_encoder_state, &pcmL, &pcmR,
389 &encoded_sample);
390
391 uint8_t* encoded_ptr = (uint8_t*)&encoded_sample[0];
392 data_out[*data_out_index + 0] = *(encoded_ptr + 2);
393 data_out[*data_out_index + 1] = *(encoded_ptr + 1);
394 data_out[*data_out_index + 2] = *(encoded_ptr + 0);
395 data_out[*data_out_index + 3] = *(encoded_ptr + 6);
396 data_out[*data_out_index + 4] = *(encoded_ptr + 5);
397 data_out[*data_out_index + 5] = *(encoded_ptr + 4);
398
399 pcm_bytes_encoded += 24;
400 *data_out_index += 6;
401 }
402
403 return pcm_bytes_encoded;
404 }
405
debug_codec_dump(int fd)406 void A2dpCodecConfigAptxHd::debug_codec_dump(int fd) {
407 a2dp_aptx_hd_encoder_stats_t* stats = &a2dp_aptx_hd_encoder_cb.stats;
408
409 A2dpCodecConfig::debug_codec_dump(fd);
410
411 dprintf(fd, " Encoder interval (ms): %" PRIu64 "\n",
412 a2dp_vendor_aptx_hd_get_encoder_interval_ms());
413 dprintf(fd, " Effective MTU: %d\n",
414 a2dp_vendor_aptx_hd_get_effective_frame_size());
415 dprintf(fd,
416 " Packet counts (expected/dropped) : %zu / "
417 "%zu\n",
418 stats->media_read_total_expected_packets,
419 stats->media_read_total_dropped_packets);
420
421 dprintf(fd,
422 " PCM read counts (expected/actual) : %zu / "
423 "%zu\n",
424 stats->media_read_total_expected_reads_count,
425 stats->media_read_total_actual_reads_count);
426
427 dprintf(fd,
428 " PCM read bytes (expected/actual) : %zu / "
429 "%zu\n",
430 stats->media_read_total_expected_read_bytes,
431 stats->media_read_total_actual_read_bytes);
432 }
433