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