1 /******************************************************************************
2 *
3 * Copyright 2016 The Android Open Source Project
4 * Copyright 2009-2012 Broadcom Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at:
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 ******************************************************************************/
19
20 #define LOG_TAG "bluetooth-a2dp"
21
22 #include "a2dp_sbc_encoder.h"
23
24 #include <bluetooth/log.h>
25 #include <limits.h>
26 #include <string.h>
27
28 #include "a2dp_sbc.h"
29 #include "a2dp_sbc_up_sample.h"
30 #include "common/time_util.h"
31 #include "embdrv/sbc/encoder/include/sbc_encoder.h"
32 #include "internal_include/bt_target.h"
33 #include "osi/include/allocator.h"
34 #include "stack/include/bt_hdr.h"
35
36 /* Buffer pool */
37 #define A2DP_SBC_BUFFER_SIZE BT_DEFAULT_BUFFER_SIZE
38
39 // A2DP SBC encoder interval in milliseconds.
40 #define A2DP_SBC_ENCODER_INTERVAL_MS 20
41
42 /* High quality quality setting @ 44.1 khz */
43 #define A2DP_SBC_DEFAULT_BITRATE 328
44
45 #define A2DP_SBC_NON_EDR_MAX_RATE 229
46
47 #define A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK 3
48
49 #define A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1 119
50 #define A2DP_SBC_MAX_HQ_FRAME_SIZE_48 115
51
52 /* Define the bitrate step when trying to match bitpool value */
53 #define A2DP_SBC_BITRATE_STEP 5
54
55 /* Readability constants */
56 #define A2DP_SBC_FRAME_HEADER_SIZE_BYTES 4 // A2DP Spec v1.3, 12.4, Table 12.12
57 #define A2DP_SBC_SCALE_FACTOR_BITS 4 // A2DP Spec v1.3, 12.4, Table 12.13
58
59 /* offset */
60 #define A2DP_HDR_SIZE 1
61 #define A2DP_SBC_OFFSET (AVDT_MEDIA_OFFSET + A2DP_SBC_MPL_HDR_LEN)
62
63 using namespace bluetooth;
64
65 typedef struct {
66 uint32_t aa_frame_counter;
67 int32_t aa_feed_counter;
68 int32_t aa_feed_residue;
69 float counter;
70 uint32_t bytes_per_tick; /* pcm bytes read each media task tick */
71 uint64_t last_frame_us;
72 } tA2DP_SBC_FEEDING_STATE;
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
85 size_t media_read_total_expected_frames;
86 size_t media_read_total_dropped_frames;
87 } a2dp_sbc_encoder_stats_t;
88
89 typedef struct {
90 a2dp_source_read_callback_t read_callback;
91 a2dp_source_enqueue_callback_t enqueue_callback;
92 uint16_t TxAaMtuSize;
93 uint8_t tx_sbc_frames;
94 tA2DP_ENCODER_INIT_PEER_PARAMS peer_params;
95 uint32_t timestamp; /* Timestamp for the A2DP frames */
96 SBC_ENC_PARAMS sbc_encoder_params;
97 tA2DP_FEEDING_PARAMS feeding_params;
98 tA2DP_SBC_FEEDING_STATE feeding_state;
99 int16_t pcmBuffer[SBC_MAX_PCM_BUFFER_SIZE];
100
101 a2dp_sbc_encoder_stats_t stats;
102 } tA2DP_SBC_ENCODER_CB;
103
104 static tA2DP_SBC_ENCODER_CB a2dp_sbc_encoder_cb;
105
106 static void a2dp_sbc_encoder_update(A2dpCodecConfig* a2dp_codec_config,
107 bool* p_restart_input,
108 bool* p_restart_output,
109 bool* p_config_updated);
110 static bool a2dp_sbc_read_feeding(uint32_t* bytes);
111 static void a2dp_sbc_encode_frames(uint8_t nb_frame);
112 static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
113 uint8_t* num_of_frames,
114 uint64_t timestamp_us);
115 static uint16_t adjust_effective_mtu(
116 const tA2DP_ENCODER_INIT_PEER_PARAMS& peer_params);
117 static uint8_t calculate_max_frames_per_packet(void);
118 static uint16_t a2dp_sbc_source_rate(bool is_peer_edr);
119 static uint32_t a2dp_sbc_frame_length(void);
120
A2DP_LoadEncoderSbc(void)121 bool A2DP_LoadEncoderSbc(void) {
122 // Nothing to do - the library is statically linked
123 return true;
124 }
125
A2DP_UnloadEncoderSbc(void)126 void A2DP_UnloadEncoderSbc(void) {
127 // Nothing to do - the library is statically linked
128 }
129
a2dp_sbc_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)130 void a2dp_sbc_encoder_init(const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params,
131 A2dpCodecConfig* a2dp_codec_config,
132 a2dp_source_read_callback_t read_callback,
133 a2dp_source_enqueue_callback_t enqueue_callback) {
134 memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
135
136 a2dp_sbc_encoder_cb.stats.session_start_us =
137 bluetooth::common::time_get_os_boottime_us();
138
139 a2dp_sbc_encoder_cb.read_callback = read_callback;
140 a2dp_sbc_encoder_cb.enqueue_callback = enqueue_callback;
141 a2dp_sbc_encoder_cb.peer_params = *p_peer_params;
142 a2dp_sbc_encoder_cb.timestamp = 0;
143
144 // NOTE: Ignore the restart_input / restart_output flags - this initization
145 // happens when the audio session is (re)started.
146 bool restart_input = false;
147 bool restart_output = false;
148 bool config_updated = false;
149 a2dp_sbc_encoder_update(a2dp_codec_config, &restart_input, &restart_output,
150 &config_updated);
151 }
152
153 // Update the A2DP SBC encoder.
154 // |a2dp_codec_config| is the A2DP codec to use for the update.
a2dp_sbc_encoder_update(A2dpCodecConfig * a2dp_codec_config,bool * p_restart_input,bool * p_restart_output,bool * p_config_updated)155 static void a2dp_sbc_encoder_update(A2dpCodecConfig* a2dp_codec_config,
156 bool* p_restart_input,
157 bool* p_restart_output,
158 bool* p_config_updated) {
159 SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
160 uint8_t codec_info[AVDT_CODEC_SIZE];
161 uint16_t s16SamplingFreq;
162 int16_t s16BitPool = 0;
163 int16_t s16BitRate;
164 int16_t s16FrameLen;
165 uint8_t protect = 0;
166 int min_bitpool;
167 int max_bitpool;
168
169 *p_restart_input = false;
170 *p_restart_output = false;
171 *p_config_updated = false;
172 if (!a2dp_codec_config->copyOutOtaCodecConfig(codec_info)) {
173 log::error("Cannot update the codec encoder for {}: invalid codec config",
174 a2dp_codec_config->name());
175 return;
176 }
177 const uint8_t* p_codec_info = codec_info;
178 min_bitpool = A2DP_GetMinBitpoolSbc(p_codec_info);
179 max_bitpool = A2DP_GetMaxBitpoolSbc(p_codec_info);
180
181 // The feeding parameters
182 tA2DP_FEEDING_PARAMS* p_feeding_params = &a2dp_sbc_encoder_cb.feeding_params;
183 p_feeding_params->sample_rate = A2DP_GetTrackSampleRateSbc(p_codec_info);
184 p_feeding_params->bits_per_sample =
185 a2dp_codec_config->getAudioBitsPerSample();
186 p_feeding_params->channel_count = A2DP_GetTrackChannelCountSbc(p_codec_info);
187 log::info("sample_rate={} bits_per_sample={} channel_count={}",
188 p_feeding_params->sample_rate, p_feeding_params->bits_per_sample,
189 p_feeding_params->channel_count);
190 a2dp_sbc_feeding_reset();
191
192 // The codec parameters
193 p_encoder_params->s16ChannelMode = A2DP_GetChannelModeCodeSbc(p_codec_info);
194 p_encoder_params->s16NumOfSubBands =
195 A2DP_GetNumberOfSubbandsSbc(p_codec_info);
196 p_encoder_params->s16NumOfBlocks = A2DP_GetNumberOfBlocksSbc(p_codec_info);
197 p_encoder_params->s16AllocationMethod =
198 A2DP_GetAllocationMethodCodeSbc(p_codec_info);
199 p_encoder_params->s16SamplingFreq =
200 A2DP_GetSamplingFrequencyCodeSbc(p_codec_info);
201 p_encoder_params->s16NumOfChannels =
202 A2DP_GetTrackChannelCountSbc(p_codec_info);
203
204 // Reset invalid parameters
205 if (!p_encoder_params->s16NumOfSubBands) {
206 log::warn("SubBands are set to 0, resetting to max ({})",
207 SBC_MAX_NUM_OF_SUBBANDS);
208 p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
209 }
210 if (!p_encoder_params->s16NumOfBlocks) {
211 log::warn("Blocks are set to 0, resetting to max ({})",
212 SBC_MAX_NUM_OF_BLOCKS);
213 p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
214 }
215 if (!p_encoder_params->s16NumOfChannels) {
216 log::warn("Channels are set to 0, resetting to max ({})",
217 SBC_MAX_NUM_OF_CHANNELS);
218 p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
219 }
220
221 if (p_encoder_params->s16SamplingFreq == SBC_sf16000)
222 s16SamplingFreq = 16000;
223 else if (p_encoder_params->s16SamplingFreq == SBC_sf32000)
224 s16SamplingFreq = 32000;
225 else if (p_encoder_params->s16SamplingFreq == SBC_sf44100)
226 s16SamplingFreq = 44100;
227 else
228 s16SamplingFreq = 48000;
229
230 // Set the initial target bit rate
231 const tA2DP_ENCODER_INIT_PEER_PARAMS& peer_params =
232 a2dp_sbc_encoder_cb.peer_params;
233 p_encoder_params->u16BitRate = a2dp_sbc_source_rate(peer_params.is_peer_edr);
234
235 a2dp_sbc_encoder_cb.TxAaMtuSize = adjust_effective_mtu(peer_params);
236 log::info("MTU={}, peer_mtu={} min_bitpool={} max_bitpool={}",
237 a2dp_sbc_encoder_cb.TxAaMtuSize, peer_params.peer_mtu, min_bitpool,
238 max_bitpool);
239 log::info(
240 "ChannelMode={}, NumOfSubBands={}, NumOfBlocks={}, AllocationMethod={}, "
241 "BitRate={}, SamplingFreq={} BitPool={}",
242 p_encoder_params->s16ChannelMode, p_encoder_params->s16NumOfSubBands,
243 p_encoder_params->s16NumOfBlocks, p_encoder_params->s16AllocationMethod,
244 p_encoder_params->u16BitRate, s16SamplingFreq,
245 p_encoder_params->s16BitPool);
246
247 do {
248 if ((p_encoder_params->s16ChannelMode == SBC_JOINT_STEREO) ||
249 (p_encoder_params->s16ChannelMode == SBC_STEREO)) {
250 s16BitPool = (int16_t)((p_encoder_params->u16BitRate *
251 p_encoder_params->s16NumOfSubBands * 1000 /
252 s16SamplingFreq) -
253 ((32 + (4 * p_encoder_params->s16NumOfSubBands *
254 p_encoder_params->s16NumOfChannels) +
255 ((p_encoder_params->s16ChannelMode - 2) *
256 p_encoder_params->s16NumOfSubBands)) /
257 p_encoder_params->s16NumOfBlocks));
258
259 s16FrameLen = 4 +
260 (4 * p_encoder_params->s16NumOfSubBands *
261 p_encoder_params->s16NumOfChannels) /
262 8 +
263 (((p_encoder_params->s16ChannelMode - 2) *
264 p_encoder_params->s16NumOfSubBands) +
265 (p_encoder_params->s16NumOfBlocks * s16BitPool)) /
266 8;
267
268 s16BitRate = (8 * s16FrameLen * s16SamplingFreq) /
269 (p_encoder_params->s16NumOfSubBands *
270 p_encoder_params->s16NumOfBlocks * 1000);
271
272 if (s16BitRate > p_encoder_params->u16BitRate) s16BitPool--;
273
274 if (p_encoder_params->s16NumOfSubBands == 8)
275 s16BitPool = (s16BitPool > 255) ? 255 : s16BitPool;
276 else
277 s16BitPool = (s16BitPool > 128) ? 128 : s16BitPool;
278 } else {
279 s16BitPool =
280 (int16_t)(((p_encoder_params->s16NumOfSubBands *
281 p_encoder_params->u16BitRate * 1000) /
282 (s16SamplingFreq * p_encoder_params->s16NumOfChannels)) -
283 (((32 / p_encoder_params->s16NumOfChannels) +
284 (4 * p_encoder_params->s16NumOfSubBands)) /
285 p_encoder_params->s16NumOfBlocks));
286
287 p_encoder_params->s16BitPool =
288 (s16BitPool > (16 * p_encoder_params->s16NumOfSubBands))
289 ? (16 * p_encoder_params->s16NumOfSubBands)
290 : s16BitPool;
291 }
292
293 if (s16BitPool < 0) s16BitPool = 0;
294
295 log::verbose("bitpool candidate: {} ({} kbps)", s16BitPool,
296 p_encoder_params->u16BitRate);
297
298 if (s16BitPool > max_bitpool) {
299 log::verbose("computed bitpool too large ({})", s16BitPool);
300 /* Decrease bitrate */
301 p_encoder_params->u16BitRate -= A2DP_SBC_BITRATE_STEP;
302 /* Record that we have decreased the bitrate */
303 protect |= 1;
304 } else if (s16BitPool < min_bitpool) {
305 log::warn("computed bitpool too small ({})", s16BitPool);
306
307 /* Increase bitrate */
308 uint16_t previous_u16BitRate = p_encoder_params->u16BitRate;
309 p_encoder_params->u16BitRate += A2DP_SBC_BITRATE_STEP;
310 /* Record that we have increased the bitrate */
311 protect |= 2;
312 /* Check over-flow */
313 if (p_encoder_params->u16BitRate < previous_u16BitRate) protect |= 3;
314 } else {
315 break;
316 }
317 /* In case we have already increased and decreased the bitrate, just stop */
318 if (protect == 3) {
319 log::error("could not find bitpool in range");
320 break;
321 }
322 } while (true);
323
324 /* Finally update the bitpool in the encoder structure */
325 p_encoder_params->s16BitPool = s16BitPool;
326
327 log::info("final bit rate {}, final bit pool {}",
328 p_encoder_params->u16BitRate, p_encoder_params->s16BitPool);
329
330 /* Reset the SBC encoder */
331 SBC_Encoder_Init(&a2dp_sbc_encoder_cb.sbc_encoder_params);
332 a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
333 }
334
a2dp_sbc_encoder_cleanup(void)335 void a2dp_sbc_encoder_cleanup(void) {
336 memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
337 }
338
a2dp_sbc_feeding_reset(void)339 void a2dp_sbc_feeding_reset(void) {
340 /* By default, just clear the entire state */
341 memset(&a2dp_sbc_encoder_cb.feeding_state, 0,
342 sizeof(a2dp_sbc_encoder_cb.feeding_state));
343
344 a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick =
345 (a2dp_sbc_encoder_cb.feeding_params.sample_rate *
346 a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8 *
347 a2dp_sbc_encoder_cb.feeding_params.channel_count *
348 A2DP_SBC_ENCODER_INTERVAL_MS) /
349 1000;
350
351 log::info("PCM bytes per tick {}",
352 a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick);
353 }
354
a2dp_sbc_feeding_flush(void)355 void a2dp_sbc_feeding_flush(void) {
356 a2dp_sbc_encoder_cb.feeding_state.counter = 0.0f;
357 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
358 }
359
a2dp_sbc_get_encoder_interval_ms(void)360 uint64_t a2dp_sbc_get_encoder_interval_ms(void) {
361 return A2DP_SBC_ENCODER_INTERVAL_MS;
362 }
363
a2dp_sbc_get_effective_frame_size()364 int a2dp_sbc_get_effective_frame_size() {
365 return a2dp_sbc_encoder_cb.TxAaMtuSize;
366 }
367
a2dp_sbc_send_frames(uint64_t timestamp_us)368 void a2dp_sbc_send_frames(uint64_t timestamp_us) {
369 uint8_t nb_frame = 0;
370 uint8_t nb_iterations = 0;
371
372 a2dp_sbc_get_num_frame_iteration(&nb_iterations, &nb_frame, timestamp_us);
373 log::verbose("Sending {} frames per iteration, {} iterations", nb_frame,
374 nb_iterations);
375 if (nb_frame == 0) return;
376
377 for (uint8_t counter = 0; counter < nb_iterations; counter++) {
378 // Transcode frame and enqueue
379 a2dp_sbc_encode_frames(nb_frame);
380 }
381 }
382
383 // Obtains the number of frames to send and number of iterations
384 // to be used. |num_of_iterations| and |num_of_frames| parameters
385 // are used as output param for returning the respective values.
a2dp_sbc_get_num_frame_iteration(uint8_t * num_of_iterations,uint8_t * num_of_frames,uint64_t timestamp_us)386 static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
387 uint8_t* num_of_frames,
388 uint64_t timestamp_us) {
389 uint8_t nof = 0;
390 uint8_t noi = 1;
391
392 uint32_t projected_nof = 0;
393 uint32_t pcm_bytes_per_frame =
394 a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfSubBands *
395 a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfBlocks *
396 a2dp_sbc_encoder_cb.feeding_params.channel_count *
397 a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
398 log::verbose("pcm_bytes_per_frame {}", pcm_bytes_per_frame);
399
400 uint32_t us_this_tick = A2DP_SBC_ENCODER_INTERVAL_MS * 1000;
401 uint64_t now_us = timestamp_us;
402 if (a2dp_sbc_encoder_cb.feeding_state.last_frame_us != 0)
403 us_this_tick = (now_us - a2dp_sbc_encoder_cb.feeding_state.last_frame_us);
404 a2dp_sbc_encoder_cb.feeding_state.last_frame_us = now_us;
405
406 a2dp_sbc_encoder_cb.feeding_state.counter +=
407 (float)a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick *
408 (float)us_this_tick / (A2DP_SBC_ENCODER_INTERVAL_MS * 1000);
409
410 /* Calculate the number of frames pending for this media tick */
411 projected_nof =
412 a2dp_sbc_encoder_cb.feeding_state.counter / (float)pcm_bytes_per_frame;
413 // Update the stats
414 a2dp_sbc_encoder_cb.stats.media_read_total_expected_frames += projected_nof;
415
416 if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
417 log::warn("limiting frames to be sent from {} to {}", projected_nof,
418 MAX_PCM_FRAME_NUM_PER_TICK);
419
420 // Update the stats
421 size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
422 a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
423
424 projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
425 }
426
427 log::verbose("frames for available PCM data {}", projected_nof);
428
429 if (a2dp_sbc_encoder_cb.peer_params.is_peer_edr) {
430 if (!a2dp_sbc_encoder_cb.tx_sbc_frames) {
431 log::error("tx_sbc_frames not updated, update from here");
432 a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
433 }
434
435 nof = a2dp_sbc_encoder_cb.tx_sbc_frames;
436 if (!nof) {
437 log::error("number of frames not updated, set calculated values");
438 nof = projected_nof;
439 noi = 1;
440 } else {
441 if (nof < projected_nof) {
442 noi = projected_nof / nof; // number of iterations would vary
443 if (noi > A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK) {
444 log::error("Audio Congestion (iterations:{} > max ({}))", noi,
445 A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK);
446 noi = A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK;
447 a2dp_sbc_encoder_cb.feeding_state.counter =
448 noi * nof * (float)pcm_bytes_per_frame;
449 }
450 projected_nof = nof;
451 } else {
452 noi = 1; // number of iterations is 1
453 log::verbose("reducing frames for available PCM data");
454 nof = projected_nof;
455 }
456 }
457 } else {
458 // For BR cases nof will be same as the value retrieved at projected_nof
459 log::verbose("headset BR, number of frames {}", nof);
460 if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
461 log::error("Audio Congestion (frames: {} > max ({}))", projected_nof,
462 MAX_PCM_FRAME_NUM_PER_TICK);
463
464 // Update the stats
465 size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
466 a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
467
468 projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
469 a2dp_sbc_encoder_cb.feeding_state.counter =
470 (float)noi * (float)projected_nof * (float)pcm_bytes_per_frame;
471 }
472 nof = projected_nof;
473 }
474 a2dp_sbc_encoder_cb.feeding_state.counter -=
475 noi * nof * (float)pcm_bytes_per_frame;
476 log::verbose("effective num of frames {}, iterations {}", nof, noi);
477
478 *num_of_frames = nof;
479 *num_of_iterations = noi;
480 }
481
a2dp_sbc_encode_frames(uint8_t nb_frame)482 static void a2dp_sbc_encode_frames(uint8_t nb_frame) {
483 SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
484 uint8_t remain_nb_frame = nb_frame;
485 uint16_t blocm_x_subband =
486 p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
487
488 uint8_t last_frame_len = 0;
489
490 while (nb_frame) {
491 BT_HDR* p_buf = (BT_HDR*)osi_malloc(A2DP_SBC_BUFFER_SIZE);
492 uint32_t bytes_read = 0;
493
494 p_buf->offset = A2DP_SBC_OFFSET;
495 p_buf->len = 0;
496 p_buf->layer_specific = 0;
497 a2dp_sbc_encoder_cb.stats.media_read_total_expected_packets++;
498
499 do {
500 /* Fill allocated buffer with 0 */
501 memset(a2dp_sbc_encoder_cb.pcmBuffer, 0,
502 blocm_x_subband * p_encoder_params->s16NumOfChannels);
503 //
504 // Read the PCM data and encode it. If necessary, upsample the data.
505 //
506 uint32_t num_bytes = 0;
507 if (a2dp_sbc_read_feeding(&num_bytes)) {
508 uint8_t* output = (uint8_t*)(p_buf + 1) + p_buf->offset + p_buf->len;
509 int16_t* input = a2dp_sbc_encoder_cb.pcmBuffer;
510 uint16_t output_len = SBC_Encode(p_encoder_params, input, output);
511 last_frame_len = output_len;
512
513 /* Update SBC frame length */
514 p_buf->len += output_len;
515 nb_frame--;
516 p_buf->layer_specific++;
517
518 bytes_read += num_bytes;
519 } else {
520 log::warn("underflow {}, {}", nb_frame,
521 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
522 a2dp_sbc_encoder_cb.feeding_state.counter +=
523 nb_frame * p_encoder_params->s16NumOfSubBands *
524 p_encoder_params->s16NumOfBlocks *
525 a2dp_sbc_encoder_cb.feeding_params.channel_count *
526 a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
527 /* no more pcm to read */
528 nb_frame = 0;
529 }
530 } while (
531 ((p_buf->len + last_frame_len) < a2dp_sbc_encoder_cb.TxAaMtuSize) &&
532 (p_buf->layer_specific < 0x0F) && nb_frame);
533
534 if (p_buf->len) {
535 /*
536 * Timestamp of the media packet header represent the TS of the
537 * first SBC frame, i.e the timestamp before including this frame.
538 */
539 *((uint32_t*)(p_buf + 1)) = a2dp_sbc_encoder_cb.timestamp;
540
541 // Timestamp will wrap over to 0 if stream continues on long enough
542 // (>25H @ 48KHz). The parameters are promoted to 64bit to ensure that
543 // no unsigned overflow is triggered as ubsan is always enabled.
544 a2dp_sbc_encoder_cb.timestamp =
545 ((uint64_t)a2dp_sbc_encoder_cb.timestamp +
546 (p_buf->layer_specific * blocm_x_subband)) & UINT32_MAX;
547
548 uint8_t done_nb_frame = remain_nb_frame - nb_frame;
549 remain_nb_frame = nb_frame;
550 if (!a2dp_sbc_encoder_cb.enqueue_callback(p_buf, done_nb_frame,
551 bytes_read))
552 return;
553 } else {
554 a2dp_sbc_encoder_cb.stats.media_read_total_dropped_packets++;
555 osi_free(p_buf);
556 }
557 }
558 }
559
a2dp_sbc_read_feeding(uint32_t * bytes_read)560 static bool a2dp_sbc_read_feeding(uint32_t* bytes_read) {
561 SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
562 uint16_t blocm_x_subband =
563 p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
564 uint32_t read_size;
565 uint32_t sbc_sampling = 48000;
566 uint32_t src_samples;
567 uint16_t bytes_needed = blocm_x_subband * p_encoder_params->s16NumOfChannels *
568 a2dp_sbc_encoder_cb.feeding_params.bits_per_sample /
569 8;
570 static uint16_t up_sampled_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
571 SBC_MAX_NUM_OF_CHANNELS *
572 SBC_MAX_NUM_OF_SUBBANDS * 2];
573 static uint16_t read_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
574 SBC_MAX_NUM_OF_CHANNELS *
575 SBC_MAX_NUM_OF_SUBBANDS];
576 uint32_t src_size_used;
577 uint32_t dst_size_used;
578 bool fract_needed;
579 int32_t fract_max;
580 int32_t fract_threshold;
581 uint32_t nb_byte_read;
582
583 /* Get the SBC sampling rate */
584 switch (p_encoder_params->s16SamplingFreq) {
585 case SBC_sf48000:
586 sbc_sampling = 48000;
587 break;
588 case SBC_sf44100:
589 sbc_sampling = 44100;
590 break;
591 case SBC_sf32000:
592 sbc_sampling = 32000;
593 break;
594 case SBC_sf16000:
595 sbc_sampling = 16000;
596 break;
597 }
598
599 a2dp_sbc_encoder_cb.stats.media_read_total_expected_reads_count++;
600 if (sbc_sampling == a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
601 read_size =
602 bytes_needed - a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue;
603 a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
604 nb_byte_read = a2dp_sbc_encoder_cb.read_callback(
605 ((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer) +
606 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
607 read_size);
608 a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes +=
609 nb_byte_read;
610
611 *bytes_read = nb_byte_read;
612 if (nb_byte_read != read_size) {
613 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += nb_byte_read;
614 return false;
615 }
616 a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
617 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
618 return true;
619 }
620
621 /*
622 * Some Feeding PCM frequencies require to split the number of sample
623 * to read.
624 * E.g 128 / 6 = 21.3333 => read 22 and 21 and 21 => max = 2; threshold = 0
625 */
626 fract_needed = false; /* Default */
627 switch (a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
628 case 32000:
629 case 8000:
630 fract_needed = true;
631 fract_max = 2; /* 0, 1 and 2 */
632 fract_threshold = 0; /* Add one for the first */
633 break;
634 case 16000:
635 fract_needed = true;
636 fract_max = 2; /* 0, 1 and 2 */
637 fract_threshold = 1; /* Add one for the first two frames*/
638 break;
639 }
640
641 /* Compute number of sample to read from source */
642 src_samples = blocm_x_subband;
643 src_samples *= a2dp_sbc_encoder_cb.feeding_params.sample_rate;
644 src_samples /= sbc_sampling;
645
646 /* The previous division may have a remainder not null */
647 if (fract_needed) {
648 if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter <= fract_threshold) {
649 src_samples++; /* for every read before threshold add one sample */
650 }
651
652 /* do nothing if counter >= threshold */
653 a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter++; /* one more read */
654 if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter > fract_max) {
655 a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter = 0;
656 }
657 }
658
659 /* Compute number of bytes to read from source */
660 read_size = src_samples;
661 read_size *= a2dp_sbc_encoder_cb.feeding_params.channel_count;
662 read_size *= (a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8);
663 a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
664
665 /* Read Data from UIPC channel */
666 nb_byte_read =
667 a2dp_sbc_encoder_cb.read_callback((uint8_t*)read_buffer, read_size);
668 a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes += nb_byte_read;
669
670 if (nb_byte_read < read_size) {
671 if (nb_byte_read == 0) return false;
672
673 /* Fill the unfilled part of the read buffer with silence (0) */
674 memset(((uint8_t*)read_buffer) + nb_byte_read, 0, read_size - nb_byte_read);
675 nb_byte_read = read_size;
676 }
677 a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
678
679 /* Initialize PCM up-sampling engine */
680 a2dp_sbc_init_up_sample(a2dp_sbc_encoder_cb.feeding_params.sample_rate,
681 sbc_sampling,
682 a2dp_sbc_encoder_cb.feeding_params.bits_per_sample,
683 a2dp_sbc_encoder_cb.feeding_params.channel_count);
684
685 /*
686 * Re-sample the read buffer.
687 * The output PCM buffer will be stereo, 16 bit per sample.
688 */
689 dst_size_used = a2dp_sbc_up_sample(
690 (uint8_t*)read_buffer,
691 (uint8_t*)up_sampled_buffer +
692 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
693 nb_byte_read, sizeof(up_sampled_buffer) -
694 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
695 &src_size_used);
696
697 /* update the residue */
698 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += dst_size_used;
699
700 /* only copy the pcm sample when we have up-sampled enough PCM */
701 if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue < bytes_needed)
702 return false;
703
704 /* Copy the output pcm samples in SBC encoding buffer */
705 memcpy((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer, (uint8_t*)up_sampled_buffer,
706 bytes_needed);
707 /* update the residue */
708 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue -= bytes_needed;
709
710 if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue != 0) {
711 memcpy((uint8_t*)up_sampled_buffer,
712 (uint8_t*)up_sampled_buffer + bytes_needed,
713 a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
714 }
715 return true;
716 }
717
adjust_effective_mtu(const tA2DP_ENCODER_INIT_PEER_PARAMS & peer_params)718 static uint16_t adjust_effective_mtu(
719 const tA2DP_ENCODER_INIT_PEER_PARAMS& peer_params) {
720 uint16_t mtu_size = A2DP_SBC_BUFFER_SIZE - A2DP_SBC_OFFSET - sizeof(BT_HDR);
721 if (mtu_size > peer_params.peer_mtu) {
722 mtu_size = peer_params.peer_mtu;
723 }
724 log::verbose("original AVDTP MTU size: {}", mtu_size);
725 if (peer_params.is_peer_edr && !peer_params.peer_supports_3mbps) {
726 // This condition would be satisfied only if the remote device is
727 // EDR and supports only 2 Mbps, but the effective AVDTP MTU size
728 // exceeds the 2DH5 packet size.
729 log::verbose("The remote device is EDR but does not support 3 Mbps");
730 if (mtu_size > MAX_2MBPS_AVDTP_MTU) {
731 log::warn("Restricting AVDTP MTU size from {} to {}", mtu_size,
732 MAX_2MBPS_AVDTP_MTU);
733 mtu_size = MAX_2MBPS_AVDTP_MTU;
734 }
735 }
736 return mtu_size;
737 }
738
calculate_max_frames_per_packet(void)739 static uint8_t calculate_max_frames_per_packet(void) {
740 SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
741 uint16_t result = 0;
742 uint32_t frame_len;
743
744 a2dp_sbc_encoder_cb.TxAaMtuSize =
745 adjust_effective_mtu(a2dp_sbc_encoder_cb.peer_params);
746 const uint16_t& effective_mtu_size = a2dp_sbc_encoder_cb.TxAaMtuSize;
747
748 if (!p_encoder_params->s16NumOfSubBands) {
749 log::error("SubBands are set to 0, resetting to {}",
750 SBC_MAX_NUM_OF_SUBBANDS);
751 p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
752 }
753 if (!p_encoder_params->s16NumOfBlocks) {
754 log::error("Blocks are set to 0, resetting to {}", SBC_MAX_NUM_OF_BLOCKS);
755 p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
756 }
757 if (!p_encoder_params->s16NumOfChannels) {
758 log::error("Channels are set to 0, resetting to {}",
759 SBC_MAX_NUM_OF_CHANNELS);
760 p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
761 }
762
763 frame_len = a2dp_sbc_frame_length();
764
765 log::verbose("Effective Tx MTU to be considered: {}", effective_mtu_size);
766
767 switch (p_encoder_params->s16SamplingFreq) {
768 case SBC_sf44100:
769 if (frame_len == 0) {
770 log::error("Calculating frame length, resetting it to default {}",
771 A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1);
772 frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1;
773 }
774 result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
775 log::verbose("Max number of SBC frames: {}", result);
776 break;
777
778 case SBC_sf48000:
779 if (frame_len == 0) {
780 log::error("Calculating frame length, resetting it to default {}",
781 A2DP_SBC_MAX_HQ_FRAME_SIZE_48);
782 frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_48;
783 }
784 result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
785 log::verbose("Max number of SBC frames: {}", result);
786 break;
787
788 default:
789 log::error("Max number of SBC frames: {}", result);
790 break;
791 }
792 return result;
793 }
794
a2dp_sbc_source_rate(bool is_peer_edr)795 static uint16_t a2dp_sbc_source_rate(bool is_peer_edr) {
796 uint16_t rate = A2DP_SBC_DEFAULT_BITRATE;
797
798 /* restrict bitrate if a2dp link is non-edr */
799 if (!is_peer_edr) {
800 rate = A2DP_SBC_NON_EDR_MAX_RATE;
801 log::verbose("non-edr a2dp sink detected, restrict rate to {}", rate);
802 }
803
804 return rate;
805 }
806
a2dp_sbc_frame_length(void)807 static uint32_t a2dp_sbc_frame_length(void) {
808 SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
809 uint32_t frame_len = 0;
810
811 log::verbose(
812 "channel mode: {}, sub-band: {}, number of block: {}, bitpool: {}, "
813 "sampling frequency: {}, num channels: {}",
814 p_encoder_params->s16ChannelMode, p_encoder_params->s16NumOfSubBands,
815 p_encoder_params->s16NumOfBlocks, p_encoder_params->s16BitPool,
816 p_encoder_params->s16SamplingFreq, p_encoder_params->s16NumOfChannels);
817
818 switch (p_encoder_params->s16ChannelMode) {
819 case SBC_MONO:
820 FALLTHROUGH_INTENDED; /* FALLTHROUGH */
821 case SBC_DUAL:
822 frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
823 ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
824 p_encoder_params->s16NumOfSubBands *
825 p_encoder_params->s16NumOfChannels) /
826 CHAR_BIT) +
827 ((uint32_t)(p_encoder_params->s16NumOfBlocks *
828 p_encoder_params->s16NumOfChannels *
829 p_encoder_params->s16BitPool) /
830 CHAR_BIT);
831 break;
832 case SBC_STEREO:
833 frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
834 ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
835 p_encoder_params->s16NumOfSubBands *
836 p_encoder_params->s16NumOfChannels) /
837 CHAR_BIT) +
838 ((uint32_t)(p_encoder_params->s16NumOfBlocks *
839 p_encoder_params->s16BitPool) /
840 CHAR_BIT);
841 break;
842 case SBC_JOINT_STEREO:
843 frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
844 ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
845 p_encoder_params->s16NumOfSubBands *
846 p_encoder_params->s16NumOfChannels) /
847 CHAR_BIT) +
848 ((uint32_t)(p_encoder_params->s16NumOfSubBands +
849 (p_encoder_params->s16NumOfBlocks *
850 p_encoder_params->s16BitPool)) /
851 CHAR_BIT);
852 break;
853 default:
854 log::verbose("Invalid channel number: {}",
855 p_encoder_params->s16ChannelMode);
856 break;
857 }
858 log::verbose("calculated frame length: {}", frame_len);
859 return frame_len;
860 }
861
a2dp_sbc_get_bitrate()862 uint32_t a2dp_sbc_get_bitrate() {
863 SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
864 log::info("bit rate {}", p_encoder_params->u16BitRate);
865 return p_encoder_params->u16BitRate * 1000;
866 }
867
debug_codec_dump(int fd)868 void A2dpCodecConfigSbcSource::debug_codec_dump(int fd) {
869 a2dp_sbc_encoder_stats_t* stats = &a2dp_sbc_encoder_cb.stats;
870
871 A2dpCodecConfig::debug_codec_dump(fd);
872
873 uint8_t codec_info[AVDT_CODEC_SIZE];
874 if (copyOutOtaCodecConfig(codec_info)) {
875 dprintf(fd,
876 " SBC Block length : %d\n",
877 A2DP_GetNumberOfBlocksSbc(codec_info));
878 dprintf(fd,
879 " SBC Number of subbands : %d\n",
880 A2DP_GetNumberOfSubbandsSbc(codec_info));
881 dprintf(fd,
882 " SBC Allocation method : %d\n",
883 A2DP_GetAllocationMethodCodeSbc(codec_info));
884 dprintf(
885 fd,
886 " SBC Bitpool (min/max) : %d / %d\n",
887 A2DP_GetMinBitpoolSbc(codec_info), A2DP_GetMaxBitpoolSbc(codec_info));
888 }
889
890 dprintf(fd, " Encoder interval (ms): %" PRIu64 "\n",
891 a2dp_sbc_get_encoder_interval_ms());
892 dprintf(fd, " Effective MTU: %d\n", a2dp_sbc_get_effective_frame_size());
893 dprintf(fd,
894 " Packet counts (expected/dropped) : %zu / "
895 "%zu\n",
896 stats->media_read_total_expected_packets,
897 stats->media_read_total_dropped_packets);
898
899 dprintf(fd,
900 " PCM read counts (expected/actual) : %zu / "
901 "%zu\n",
902 stats->media_read_total_expected_reads_count,
903 stats->media_read_total_actual_reads_count);
904
905 dprintf(fd,
906 " PCM read bytes (expected/actual) : %zu / "
907 "%zu\n",
908 stats->media_read_total_expected_read_bytes,
909 stats->media_read_total_actual_read_bytes);
910
911 dprintf(fd,
912 " Frames counts (expected/dropped) : %zu / "
913 "%zu\n",
914 stats->media_read_total_expected_frames,
915 stats->media_read_total_dropped_frames);
916 }
917