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