sandy
/
PL286
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1 Commit
1 Branch
0 Tags
18 GiB
Tree: 99da0f2c72
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '99da0f2c72'
${ noResults }
PL286/system/bt/stack/a2dp/a2dp_sbc_encoder.cc

951 lines
36 KiB
Raw Blame History

/******************************************************************************
*
* Copyright (C) 2016 The Android Open Source Project
* Copyright (C) 2009-2012 Broadcom Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************/
#define LOG_TAG "a2dp_sbc_encoder"
#include "a2dp_sbc_encoder.h"
#include <limits.h>
#include <stdio.h>
#include <string.h>
#include "a2dp_sbc.h"
#include "a2dp_sbc_up_sample.h"
#include "bt_common.h"
#include "embdrv/sbc/encoder/include/sbc_encoder.h"
#include "osi/include/log.h"
#include "osi/include/osi.h"
#if defined(MTK_VND_A2DP_PKT_LEN) && (MTK_VND_A2DP_PKT_LEN == TRUE)
/** M: Firmware inform host to adjust a2dp packet length as 2-DH5/3-DH5 by RSSI.
** Then host will modify a2dp pacekt length of SBC codec after receive vendor event. @{ */
#include "mediatek/stack/include/a2dp_sbc_encoder.h"
/** @} */
#endif
/* Buffer pool */
#define A2DP_SBC_BUFFER_SIZE BT_DEFAULT_BUFFER_SIZE
// A2DP SBC encoder interval in milliseconds.
#define A2DP_SBC_ENCODER_INTERVAL_MS 20
/* High quality quality setting @ 44.1 khz */
#define A2DP_SBC_DEFAULT_BITRATE 328
#define A2DP_SBC_NON_EDR_MAX_RATE 229
/*
* 2DH5 payload size of:
* 679 bytes - (4 bytes L2CAP Header + 12 bytes AVDTP Header)
*/
#define MAX_2MBPS_AVDTP_MTU 663
#define A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK 3
#define A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1 119
#define A2DP_SBC_MAX_HQ_FRAME_SIZE_48 115
/* Define the bitrate step when trying to match bitpool value */
#define A2DP_SBC_BITRATE_STEP 5
/* Readability constants */
#define A2DP_SBC_FRAME_HEADER_SIZE_BYTES 4 // A2DP Spec v1.3, 12.4, Table 12.12
#define A2DP_SBC_SCALE_FACTOR_BITS 4 // A2DP Spec v1.3, 12.4, Table 12.13
/* offset */
#if (BTA_AV_CO_CP_SCMS_T == TRUE)
/* A2DP header will contain a CP header of size 1 */
#define A2DP_HDR_SIZE 2
#define A2DP_SBC_OFFSET (AVDT_MEDIA_OFFSET + A2DP_SBC_MPL_HDR_LEN + 1)
#else
#define A2DP_HDR_SIZE 1
#define A2DP_SBC_OFFSET (AVDT_MEDIA_OFFSET + A2DP_SBC_MPL_HDR_LEN)
#endif
typedef struct {
uint32_t aa_frame_counter;
int32_t aa_feed_counter;
int32_t aa_feed_residue;
uint32_t counter;
uint32_t bytes_per_tick; /* pcm bytes read each media task tick */
uint64_t last_frame_us;
} tA2DP_SBC_FEEDING_STATE;
typedef struct {
uint64_t session_start_us;
size_t media_read_total_expected_packets;
size_t media_read_total_expected_reads_count;
size_t media_read_total_expected_read_bytes;
size_t media_read_total_dropped_packets;
size_t media_read_total_actual_reads_count;
size_t media_read_total_actual_read_bytes;
size_t media_read_total_expected_frames;
size_t media_read_total_dropped_frames;
} a2dp_sbc_encoder_stats_t;
typedef struct {
a2dp_source_read_callback_t read_callback;
a2dp_source_enqueue_callback_t enqueue_callback;
uint16_t TxAaMtuSize;
uint8_t tx_sbc_frames;
bool is_peer_edr; /* True if the peer device supports EDR */
bool peer_supports_3mbps; /* True if the peer device supports 3Mbps EDR */
uint16_t peer_mtu; /* MTU of the A2DP peer */
uint32_t timestamp; /* Timestamp for the A2DP frames */
SBC_ENC_PARAMS sbc_encoder_params;
tA2DP_FEEDING_PARAMS feeding_params;
tA2DP_SBC_FEEDING_STATE feeding_state;
int16_t pcmBuffer[SBC_MAX_PCM_BUFFER_SIZE];
a2dp_sbc_encoder_stats_t stats;
} tA2DP_SBC_ENCODER_CB;
static tA2DP_SBC_ENCODER_CB a2dp_sbc_encoder_cb;
static void a2dp_sbc_encoder_update(uint16_t peer_mtu,
A2dpCodecConfig* a2dp_codec_config,
bool* p_restart_input,
bool* p_restart_output,
bool* p_config_updated);
static bool a2dp_sbc_read_feeding(void);
static void a2dp_sbc_encode_frames(uint8_t nb_frame);
static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
uint8_t* num_of_frames,
uint64_t timestamp_us);
static uint8_t calculate_max_frames_per_packet(void);
static uint16_t a2dp_sbc_source_rate(void);
static uint32_t a2dp_sbc_frame_length(void);
bool A2DP_LoadEncoderSbc(void) {
// Nothing to do - the library is statically linked
return true;
}
void A2DP_UnloadEncoderSbc(void) {
// Nothing to do - the library is statically linked
}
void 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) {
memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
a2dp_sbc_encoder_cb.stats.session_start_us = time_get_os_boottime_us();
a2dp_sbc_encoder_cb.read_callback = read_callback;
a2dp_sbc_encoder_cb.enqueue_callback = enqueue_callback;
a2dp_sbc_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
a2dp_sbc_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
a2dp_sbc_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
a2dp_sbc_encoder_cb.timestamp = 0;
#if defined(MTK_VND_A2DP_PKT_LEN) && (MTK_VND_A2DP_PKT_LEN == TRUE)
/** M: Firmware inform host to adjust a2dp packet length as 2-DH5/3-DH5 by RSSI.
** Then host will modify a2dp pacekt length of SBC codec after receive vendor event. @{ */
a2dp_sbc_set_3mbps(a2dp_sbc_encoder_cb.peer_supports_3mbps);
/** @} */
#endif
// NOTE: Ignore the restart_input / restart_output flags - this initization
// happens when the connection is (re)started.
bool restart_input = false;
bool restart_output = false;
bool config_updated = false;
a2dp_sbc_encoder_update(a2dp_sbc_encoder_cb.peer_mtu, a2dp_codec_config,
&restart_input, &restart_output, &config_updated);
}
bool A2dpCodecConfigSbc::updateEncoderUserConfig(
const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params, bool* p_restart_input,
bool* p_restart_output, bool* p_config_updated) {
a2dp_sbc_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
a2dp_sbc_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
a2dp_sbc_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
a2dp_sbc_encoder_cb.timestamp = 0;
if (a2dp_sbc_encoder_cb.peer_mtu == 0) {
LOG_ERROR(LOG_TAG,
"%s: Cannot update the codec encoder for %s: "
"invalid peer MTU",
__func__, name().c_str());
return false;
}
a2dp_sbc_encoder_update(a2dp_sbc_encoder_cb.peer_mtu, this, p_restart_input,
p_restart_output, p_config_updated);
return true;
}
// Update the A2DP SBC encoder.
// |peer_mtu| is the peer MTU.
// |a2dp_codec_config| is the A2DP codec to use for the update.
static void a2dp_sbc_encoder_update(uint16_t peer_mtu,
A2dpCodecConfig* a2dp_codec_config,
bool* p_restart_input,
bool* p_restart_output,
bool* p_config_updated) {
SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
uint8_t codec_info[AVDT_CODEC_SIZE];
uint16_t s16SamplingFreq;
int16_t s16BitPool = 0;
int16_t s16BitRate;
int16_t s16FrameLen;
uint8_t protect = 0;
int min_bitpool;
int max_bitpool;
*p_restart_input = false;
*p_restart_output = false;
*p_config_updated = false;
if (!a2dp_codec_config->copyOutOtaCodecConfig(codec_info)) {
LOG_ERROR(LOG_TAG,
"%s: Cannot update the codec encoder for %s: "
"invalid codec config",
__func__, a2dp_codec_config->name().c_str());
return;
}
const uint8_t* p_codec_info = codec_info;
min_bitpool = A2DP_GetMinBitpoolSbc(p_codec_info);
max_bitpool = A2DP_GetMaxBitpoolSbc(p_codec_info);
// The feeding parameters
tA2DP_FEEDING_PARAMS* p_feeding_params = &a2dp_sbc_encoder_cb.feeding_params;
p_feeding_params->sample_rate = A2DP_GetTrackSampleRateSbc(p_codec_info);
p_feeding_params->bits_per_sample =
a2dp_codec_config->getAudioBitsPerSample();
p_feeding_params->channel_count = A2DP_GetTrackChannelCountSbc(p_codec_info);
LOG_DEBUG(LOG_TAG, "%s: sample_rate=%u bits_per_sample=%u channel_count=%u",
__func__, p_feeding_params->sample_rate,
p_feeding_params->bits_per_sample, p_feeding_params->channel_count);
// The codec parameters
p_encoder_params->s16ChannelMode = A2DP_GetChannelModeCodeSbc(p_codec_info);
p_encoder_params->s16NumOfSubBands =
A2DP_GetNumberOfSubbandsSbc(p_codec_info);
p_encoder_params->s16NumOfBlocks = A2DP_GetNumberOfBlocksSbc(p_codec_info);
p_encoder_params->s16AllocationMethod =
A2DP_GetAllocationMethodCodeSbc(p_codec_info);
p_encoder_params->s16SamplingFreq =
A2DP_GetSamplingFrequencyCodeSbc(p_codec_info);
p_encoder_params->s16NumOfChannels =
A2DP_GetTrackChannelCountSbc(p_codec_info);
// Reset invalid parameters
if (!p_encoder_params->s16NumOfSubBands) {
LOG_WARN(LOG_TAG, "%s: SubBands are set to 0, resetting to max (%d)",
__func__, SBC_MAX_NUM_OF_SUBBANDS);
p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
}
if (!p_encoder_params->s16NumOfBlocks) {
LOG_WARN(LOG_TAG, "%s: Blocks are set to 0, resetting to max (%d)",
__func__, SBC_MAX_NUM_OF_BLOCKS);
p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
}
if (!p_encoder_params->s16NumOfChannels) {
LOG_WARN(LOG_TAG, "%s: Channels are set to 0, resetting to max (%d)",
__func__, SBC_MAX_NUM_OF_CHANNELS);
p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
}
uint16_t mtu_size = A2DP_SBC_BUFFER_SIZE - A2DP_SBC_OFFSET - sizeof(BT_HDR);
if (mtu_size < peer_mtu) {
a2dp_sbc_encoder_cb.TxAaMtuSize = mtu_size;
} else {
a2dp_sbc_encoder_cb.TxAaMtuSize = peer_mtu;
}
if (p_encoder_params->s16SamplingFreq == SBC_sf16000)
s16SamplingFreq = 16000;
else if (p_encoder_params->s16SamplingFreq == SBC_sf32000)
s16SamplingFreq = 32000;
else if (p_encoder_params->s16SamplingFreq == SBC_sf44100)
s16SamplingFreq = 44100;
else
s16SamplingFreq = 48000;
// Set the initial target bit rate
p_encoder_params->u16BitRate = a2dp_sbc_source_rate();
LOG_DEBUG(LOG_TAG, "%s: MTU=%d, peer_mtu=%d min_bitpool=%d max_bitpool=%d",
__func__, a2dp_sbc_encoder_cb.TxAaMtuSize, peer_mtu, min_bitpool,
max_bitpool);
LOG_DEBUG(LOG_TAG,
"%s: ChannelMode=%d, NumOfSubBands=%d, NumOfBlocks=%d, "
"AllocationMethod=%d, BitRate=%d, SamplingFreq=%d BitPool=%d",
__func__, p_encoder_params->s16ChannelMode,
p_encoder_params->s16NumOfSubBands,
p_encoder_params->s16NumOfBlocks,
p_encoder_params->s16AllocationMethod, p_encoder_params->u16BitRate,
s16SamplingFreq, p_encoder_params->s16BitPool);
do {
if ((p_encoder_params->s16ChannelMode == SBC_JOINT_STEREO) ||
(p_encoder_params->s16ChannelMode == SBC_STEREO)) {
s16BitPool = (int16_t)((p_encoder_params->u16BitRate *
p_encoder_params->s16NumOfSubBands * 1000 /
s16SamplingFreq) -
((32 + (4 * p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfChannels) +
((p_encoder_params->s16ChannelMode - 2) *
p_encoder_params->s16NumOfSubBands)) /
p_encoder_params->s16NumOfBlocks));
s16FrameLen = 4 +
(4 * p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfChannels) /
8 +
(((p_encoder_params->s16ChannelMode - 2) *
p_encoder_params->s16NumOfSubBands) +
(p_encoder_params->s16NumOfBlocks * s16BitPool)) /
8;
s16BitRate = (8 * s16FrameLen * s16SamplingFreq) /
(p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfBlocks * 1000);
if (s16BitRate > p_encoder_params->u16BitRate) s16BitPool--;
if (p_encoder_params->s16NumOfSubBands == 8)
s16BitPool = (s16BitPool > 255) ? 255 : s16BitPool;
else
s16BitPool = (s16BitPool > 128) ? 128 : s16BitPool;
} else {
s16BitPool =
(int16_t)(((p_encoder_params->s16NumOfSubBands *
p_encoder_params->u16BitRate * 1000) /
(s16SamplingFreq * p_encoder_params->s16NumOfChannels)) -
(((32 / p_encoder_params->s16NumOfChannels) +
(4 * p_encoder_params->s16NumOfSubBands)) /
p_encoder_params->s16NumOfBlocks));
p_encoder_params->s16BitPool =
(s16BitPool > (16 * p_encoder_params->s16NumOfSubBands))
? (16 * p_encoder_params->s16NumOfSubBands)
: s16BitPool;
}
if (s16BitPool < 0) s16BitPool = 0;
LOG_DEBUG(LOG_TAG, "%s: bitpool candidate: %d (%d kbps)", __func__,
s16BitPool, p_encoder_params->u16BitRate);
if (s16BitPool > max_bitpool) {
LOG_DEBUG(LOG_TAG, "%s: computed bitpool too large (%d)", __func__,
s16BitPool);
/* Decrease bitrate */
p_encoder_params->u16BitRate -= A2DP_SBC_BITRATE_STEP;
/* Record that we have decreased the bitrate */
protect |= 1;
} else if (s16BitPool < min_bitpool) {
LOG_WARN(LOG_TAG, "%s: computed bitpool too small (%d)", __func__,
s16BitPool);
/* Increase bitrate */
uint16_t previous_u16BitRate = p_encoder_params->u16BitRate;
p_encoder_params->u16BitRate += A2DP_SBC_BITRATE_STEP;
/* Record that we have increased the bitrate */
protect |= 2;
/* Check over-flow */
if (p_encoder_params->u16BitRate < previous_u16BitRate) protect |= 3;
} else {
break;
}
/* In case we have already increased and decreased the bitrate, just stop */
if (protect == 3) {
LOG_ERROR(LOG_TAG, "%s: could not find bitpool in range", __func__);
break;
}
} while (true);
/* Finally update the bitpool in the encoder structure */
p_encoder_params->s16BitPool = s16BitPool;
LOG_DEBUG(LOG_TAG, "%s: final bit rate %d, final bit pool %d", __func__,
p_encoder_params->u16BitRate, p_encoder_params->s16BitPool);
/* Reset entirely the SBC encoder */
SBC_Encoder_Init(&a2dp_sbc_encoder_cb.sbc_encoder_params);
a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
}
void a2dp_sbc_encoder_cleanup(void) {
memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
}
void a2dp_sbc_feeding_reset(void) {
/* By default, just clear the entire state */
memset(&a2dp_sbc_encoder_cb.feeding_state, 0,
sizeof(a2dp_sbc_encoder_cb.feeding_state));
a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick =
(a2dp_sbc_encoder_cb.feeding_params.sample_rate *
a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8 *
a2dp_sbc_encoder_cb.feeding_params.channel_count *
A2DP_SBC_ENCODER_INTERVAL_MS) /
1000;
LOG_DEBUG(LOG_TAG, "%s: PCM bytes per tick %u", __func__,
a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick);
}
void a2dp_sbc_feeding_flush(void) {
a2dp_sbc_encoder_cb.feeding_state.counter = 0;
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
}
period_ms_t a2dp_sbc_get_encoder_interval_ms(void) {
return A2DP_SBC_ENCODER_INTERVAL_MS;
}
void a2dp_sbc_send_frames(uint64_t timestamp_us) {
uint8_t nb_frame = 0;
uint8_t nb_iterations = 0;
a2dp_sbc_get_num_frame_iteration(&nb_iterations, &nb_frame, timestamp_us);
LOG_VERBOSE(LOG_TAG, "%s: Sending %d frames per iteration, %d iterations",
__func__, nb_frame, nb_iterations);
if (nb_frame == 0) return;
for (uint8_t counter = 0; counter < nb_iterations; counter++) {
// Transcode frame and enqueue
a2dp_sbc_encode_frames(nb_frame);
}
}
// Obtains the number of frames to send and number of iterations
// to be used. |num_of_iterations| and |num_of_frames| parameters
// are used as output param for returning the respective values.
static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
uint8_t* num_of_frames,
uint64_t timestamp_us) {
uint8_t nof = 0;
uint8_t noi = 1;
uint32_t projected_nof = 0;
uint32_t pcm_bytes_per_frame =
a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfSubBands *
a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfBlocks *
a2dp_sbc_encoder_cb.feeding_params.channel_count *
a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
LOG_VERBOSE(LOG_TAG, "%s: pcm_bytes_per_frame %u", __func__,
pcm_bytes_per_frame);
uint32_t us_this_tick = A2DP_SBC_ENCODER_INTERVAL_MS * 1000;
uint64_t now_us = timestamp_us;
if (a2dp_sbc_encoder_cb.feeding_state.last_frame_us != 0)
us_this_tick = (now_us - a2dp_sbc_encoder_cb.feeding_state.last_frame_us);
a2dp_sbc_encoder_cb.feeding_state.last_frame_us = now_us;
a2dp_sbc_encoder_cb.feeding_state.counter +=
a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick * us_this_tick /
(A2DP_SBC_ENCODER_INTERVAL_MS * 1000);
/* Calculate the number of frames pending for this media tick */
projected_nof =
a2dp_sbc_encoder_cb.feeding_state.counter / pcm_bytes_per_frame;
// Update the stats
a2dp_sbc_encoder_cb.stats.media_read_total_expected_frames += projected_nof;
if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
LOG_WARN(LOG_TAG, "%s: limiting frames to be sent from %d to %d", __func__,
projected_nof, MAX_PCM_FRAME_NUM_PER_TICK);
// Update the stats
size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
}
LOG_VERBOSE(LOG_TAG, "%s: frames for available PCM data %u", __func__,
projected_nof);
if (a2dp_sbc_encoder_cb.is_peer_edr) {
#if defined(MTK_VND_A2DP_PKT_LEN) && (MTK_VND_A2DP_PKT_LEN == TRUE)
/** M: Firmware inform host to adjust a2dp packet length as 2-DH5/3-DH5 by RSSI.
** Then host will modify a2dp pacekt length of SBC codec after receive vendor event. @{ */
if (!a2dp_sbc_encoder_cb.tx_sbc_frames
|| (a2dp_sbc_get_3mbps() != a2dp_sbc_encoder_cb.peer_supports_3mbps)) {
a2dp_sbc_encoder_cb.TxAaMtuSize = a2dp_sbc_encoder_cb.peer_mtu;
a2dp_sbc_encoder_cb.peer_supports_3mbps = a2dp_sbc_get_3mbps();
/** @} */
#else
if (!a2dp_sbc_encoder_cb.tx_sbc_frames) {
#endif
LOG_ERROR(LOG_TAG, "%s: tx_sbc_frames not updated, update from here",
__func__);
a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
}
nof = a2dp_sbc_encoder_cb.tx_sbc_frames;
if (!nof) {
LOG_ERROR(LOG_TAG,
"%s: number of frames not updated, set calculated values",
__func__);
nof = projected_nof;
noi = 1;
} else {
if (nof < projected_nof) {
noi = projected_nof / nof; // number of iterations would vary
if (noi > A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK) {
LOG_ERROR(LOG_TAG, "%s: Audio Congestion (iterations:%d > max (%d))",
__func__, noi, A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK);
noi = A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK;
a2dp_sbc_encoder_cb.feeding_state.counter =
noi * nof * pcm_bytes_per_frame;
}
projected_nof = nof;
} else {
noi = 1; // number of iterations is 1
LOG_VERBOSE(LOG_TAG, "%s: reducing frames for available PCM data",
__func__);
nof = projected_nof;
}
}
} else {
// For BR cases nof will be same as the value retrieved at projected_nof
LOG_VERBOSE(LOG_TAG, "%s: headset BR, number of frames %u", __func__, nof);
if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
LOG_ERROR(LOG_TAG, "%s: Audio Congestion (frames: %d > max (%d))",
__func__, projected_nof, MAX_PCM_FRAME_NUM_PER_TICK);
// Update the stats
size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
a2dp_sbc_encoder_cb.feeding_state.counter =
noi * projected_nof * pcm_bytes_per_frame;
}
nof = projected_nof;
}
a2dp_sbc_encoder_cb.feeding_state.counter -= noi * nof * pcm_bytes_per_frame;
LOG_VERBOSE(LOG_TAG, "%s: effective num of frames %u, iterations %u",
__func__, nof, noi);
*num_of_frames = nof;
*num_of_iterations = noi;
}
static void a2dp_sbc_encode_frames(uint8_t nb_frame) {
SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
uint8_t remain_nb_frame = nb_frame;
uint16_t blocm_x_subband =
p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
uint8_t last_frame_len = 0;
while (nb_frame) {
BT_HDR* p_buf = (BT_HDR*)osi_malloc(A2DP_SBC_BUFFER_SIZE);
p_buf->offset = A2DP_SBC_OFFSET;
p_buf->len = 0;
p_buf->layer_specific = 0;
a2dp_sbc_encoder_cb.stats.media_read_total_expected_packets++;
do {
/* Fill allocated buffer with 0 */
memset(a2dp_sbc_encoder_cb.pcmBuffer, 0,
blocm_x_subband * p_encoder_params->s16NumOfChannels);
//
// Read the PCM data and encode it. If necessary, upsample the data.
//
if (a2dp_sbc_read_feeding()) {
uint8_t* output = (uint8_t*)(p_buf + 1) + p_buf->offset + p_buf->len;
int16_t* input = a2dp_sbc_encoder_cb.pcmBuffer;
uint16_t output_len = SBC_Encode(p_encoder_params, input, output);
last_frame_len = output_len;
/* Update SBC frame length */
p_buf->len += output_len;
nb_frame--;
p_buf->layer_specific++;
} else {
LOG_WARN(LOG_TAG, "%s: underflow %d, %d", __func__, nb_frame,
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
a2dp_sbc_encoder_cb.feeding_state.counter +=
nb_frame * p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfBlocks *
a2dp_sbc_encoder_cb.feeding_params.channel_count *
a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
/* no more pcm to read */
nb_frame = 0;
}
} while (
((p_buf->len + last_frame_len) < a2dp_sbc_encoder_cb.TxAaMtuSize) &&
(p_buf->layer_specific < 0x0F) && nb_frame);
if (p_buf->len) {
/*
* Timestamp of the media packet header represent the TS of the
* first SBC frame, i.e the timestamp before including this frame.
*/
*((uint32_t*)(p_buf + 1)) = a2dp_sbc_encoder_cb.timestamp;
a2dp_sbc_encoder_cb.timestamp += p_buf->layer_specific * blocm_x_subband;
uint8_t done_nb_frame = remain_nb_frame - nb_frame;
remain_nb_frame = nb_frame;
if (!a2dp_sbc_encoder_cb.enqueue_callback(p_buf, done_nb_frame)) return;
} else {
a2dp_sbc_encoder_cb.stats.media_read_total_dropped_packets++;
osi_free(p_buf);
}
}
}
static bool a2dp_sbc_read_feeding(void) {
SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
uint16_t blocm_x_subband =
p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
uint32_t read_size;
uint32_t sbc_sampling = 48000;
uint32_t src_samples;
uint16_t bytes_needed = blocm_x_subband * p_encoder_params->s16NumOfChannels *
a2dp_sbc_encoder_cb.feeding_params.bits_per_sample /
8;
static uint16_t up_sampled_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
SBC_MAX_NUM_OF_CHANNELS *
SBC_MAX_NUM_OF_SUBBANDS * 2];
static uint16_t read_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
SBC_MAX_NUM_OF_CHANNELS *
SBC_MAX_NUM_OF_SUBBANDS];
uint32_t src_size_used;
uint32_t dst_size_used;
bool fract_needed;
int32_t fract_max;
int32_t fract_threshold;
uint32_t nb_byte_read;
/* Get the SBC sampling rate */
switch (p_encoder_params->s16SamplingFreq) {
case SBC_sf48000:
sbc_sampling = 48000;
break;
case SBC_sf44100:
sbc_sampling = 44100;
break;
case SBC_sf32000:
sbc_sampling = 32000;
break;
case SBC_sf16000:
sbc_sampling = 16000;
break;
}
a2dp_sbc_encoder_cb.stats.media_read_total_expected_reads_count++;
if (sbc_sampling == a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
read_size =
bytes_needed - a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue;
a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
nb_byte_read = a2dp_sbc_encoder_cb.read_callback(
((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer) +
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
read_size);
a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes +=
nb_byte_read;
if (nb_byte_read != read_size) {
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += nb_byte_read;
return false;
}
a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
return true;
}
/*
* Some Feeding PCM frequencies require to split the number of sample
* to read.
* E.g 128 / 6 = 21.3333 => read 22 and 21 and 21 => max = 2; threshold = 0
*/
fract_needed = false; /* Default */
switch (a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
case 32000:
case 8000:
fract_needed = true;
fract_max = 2; /* 0, 1 and 2 */
fract_threshold = 0; /* Add one for the first */
break;
case 16000:
fract_needed = true;
fract_max = 2; /* 0, 1 and 2 */
fract_threshold = 1; /* Add one for the first two frames*/
break;
}
/* Compute number of sample to read from source */
src_samples = blocm_x_subband;
src_samples *= a2dp_sbc_encoder_cb.feeding_params.sample_rate;
src_samples /= sbc_sampling;
/* The previous division may have a remainder not null */
if (fract_needed) {
if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter <= fract_threshold) {
src_samples++; /* for every read before threshold add one sample */
}
/* do nothing if counter >= threshold */
a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter++; /* one more read */
if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter > fract_max) {
a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter = 0;
}
}
/* Compute number of bytes to read from source */
read_size = src_samples;
read_size *= a2dp_sbc_encoder_cb.feeding_params.channel_count;
read_size *= (a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8);
a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
/* Read Data from UIPC channel */
nb_byte_read =
a2dp_sbc_encoder_cb.read_callback((uint8_t*)read_buffer, read_size);
a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes += nb_byte_read;
if (nb_byte_read < read_size) {
if (nb_byte_read == 0) return false;
/* Fill the unfilled part of the read buffer with silence (0) */
memset(((uint8_t*)read_buffer) + nb_byte_read, 0, read_size - nb_byte_read);
nb_byte_read = read_size;
}
a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
/* Initialize PCM up-sampling engine */
a2dp_sbc_init_up_sample(a2dp_sbc_encoder_cb.feeding_params.sample_rate,
sbc_sampling,
a2dp_sbc_encoder_cb.feeding_params.bits_per_sample,
a2dp_sbc_encoder_cb.feeding_params.channel_count);
/*
* Re-sample the read buffer.
* The output PCM buffer will be stereo, 16 bit per sample.
*/
dst_size_used = a2dp_sbc_up_sample(
(uint8_t*)read_buffer,
(uint8_t*)up_sampled_buffer +
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
nb_byte_read, sizeof(up_sampled_buffer) -
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
&src_size_used);
/* update the residue */
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += dst_size_used;
/* only copy the pcm sample when we have up-sampled enough PCM */
if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue < bytes_needed)
return false;
/* Copy the output pcm samples in SBC encoding buffer */
memcpy((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer, (uint8_t*)up_sampled_buffer,
bytes_needed);
/* update the residue */
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue -= bytes_needed;
if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue != 0) {
memcpy((uint8_t*)up_sampled_buffer,
(uint8_t*)up_sampled_buffer + bytes_needed,
a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
}
return true;
}
static uint8_t calculate_max_frames_per_packet(void) {
uint16_t effective_mtu_size = a2dp_sbc_encoder_cb.TxAaMtuSize;
SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
uint16_t result = 0;
uint32_t frame_len;
LOG_VERBOSE(LOG_TAG, "%s: original AVDTP MTU size: %d", __func__,
a2dp_sbc_encoder_cb.TxAaMtuSize);
if (a2dp_sbc_encoder_cb.is_peer_edr &&
!a2dp_sbc_encoder_cb.peer_supports_3mbps) {
// This condition would be satisfied only if the remote device is
// EDR and supports only 2 Mbps, but the effective AVDTP MTU size
// exceeds the 2DH5 packet size.
LOG_VERBOSE(LOG_TAG,
"%s: The remote device is EDR but does not support 3 Mbps",
__func__);
if (effective_mtu_size > MAX_2MBPS_AVDTP_MTU) {
LOG_WARN(LOG_TAG, "%s: Restricting AVDTP MTU size to %d", __func__,
MAX_2MBPS_AVDTP_MTU);
effective_mtu_size = MAX_2MBPS_AVDTP_MTU;
a2dp_sbc_encoder_cb.TxAaMtuSize = effective_mtu_size;
}
}
if (!p_encoder_params->s16NumOfSubBands) {
LOG_ERROR(LOG_TAG, "%s: SubBands are set to 0, resetting to %d", __func__,
SBC_MAX_NUM_OF_SUBBANDS);
p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
}
if (!p_encoder_params->s16NumOfBlocks) {
LOG_ERROR(LOG_TAG, "%s: Blocks are set to 0, resetting to %d", __func__,
SBC_MAX_NUM_OF_BLOCKS);
p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
}
if (!p_encoder_params->s16NumOfChannels) {
LOG_ERROR(LOG_TAG, "%s: Channels are set to 0, resetting to %d", __func__,
SBC_MAX_NUM_OF_CHANNELS);
p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
}
frame_len = a2dp_sbc_frame_length();
LOG_VERBOSE(LOG_TAG, "%s: Effective Tx MTU to be considered: %d", __func__,
effective_mtu_size);
switch (p_encoder_params->s16SamplingFreq) {
case SBC_sf44100:
if (frame_len == 0) {
LOG_ERROR(LOG_TAG,
"%s: Calculating frame length, resetting it to default %d",
__func__, A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1);
frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1;
}
result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
LOG_VERBOSE(LOG_TAG, "%s: Max number of SBC frames: %d", __func__,
result);
break;
case SBC_sf48000:
if (frame_len == 0) {
LOG_ERROR(LOG_TAG,
"%s: Calculating frame length, resetting it to default %d",
__func__, A2DP_SBC_MAX_HQ_FRAME_SIZE_48);
frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_48;
}
result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
LOG_VERBOSE(LOG_TAG, "%s: Max number of SBC frames: %d", __func__,
result);
break;
default:
LOG_ERROR(LOG_TAG, "%s: Max number of SBC frames: %d", __func__, result);
break;
}
return result;
}
static uint16_t a2dp_sbc_source_rate(void) {
uint16_t rate = A2DP_SBC_DEFAULT_BITRATE;
/* restrict bitrate if a2dp link is non-edr */
if (!a2dp_sbc_encoder_cb.is_peer_edr) {
rate = A2DP_SBC_NON_EDR_MAX_RATE;
LOG_VERBOSE(LOG_TAG, "%s: non-edr a2dp sink detected, restrict rate to %d",
__func__, rate);
}
return rate;
}
static uint32_t a2dp_sbc_frame_length(void) {
SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
uint32_t frame_len = 0;
LOG_VERBOSE(LOG_TAG,
"%s: channel mode: %d, sub-band: %d, number of block: %d, "
"bitpool: %d, sampling frequency: %d, num channels: %d",
__func__, p_encoder_params->s16ChannelMode,
p_encoder_params->s16NumOfSubBands,
p_encoder_params->s16NumOfBlocks, p_encoder_params->s16BitPool,
p_encoder_params->s16SamplingFreq,
p_encoder_params->s16NumOfChannels);
switch (p_encoder_params->s16ChannelMode) {
case SBC_MONO:
/* FALLTHROUGH */
case SBC_DUAL:
frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfChannels) /
CHAR_BIT) +
((uint32_t)(p_encoder_params->s16NumOfBlocks *
p_encoder_params->s16NumOfChannels *
p_encoder_params->s16BitPool) /
CHAR_BIT);
break;
case SBC_STEREO:
frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfChannels) /
CHAR_BIT) +
((uint32_t)(p_encoder_params->s16NumOfBlocks *
p_encoder_params->s16BitPool) /
CHAR_BIT);
break;
case SBC_JOINT_STEREO:
frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
p_encoder_params->s16NumOfSubBands *
p_encoder_params->s16NumOfChannels) /
CHAR_BIT) +
((uint32_t)(p_encoder_params->s16NumOfSubBands +
(p_encoder_params->s16NumOfBlocks *
p_encoder_params->s16BitPool)) /
CHAR_BIT);
break;
default:
LOG_VERBOSE(LOG_TAG, "%s: Invalid channel number: %d", __func__,
p_encoder_params->s16ChannelMode);
break;
}
LOG_VERBOSE(LOG_TAG, "%s: calculated frame length: %d", __func__, frame_len);
return frame_len;
}
period_ms_t A2dpCodecConfigSbc::encoderIntervalMs() const {
return a2dp_sbc_get_encoder_interval_ms();
}
void A2dpCodecConfigSbc::debug_codec_dump(int fd) {
a2dp_sbc_encoder_stats_t* stats = &a2dp_sbc_encoder_cb.stats;
A2dpCodecConfig::debug_codec_dump(fd);
dprintf(fd,
" Packet counts (expected/dropped) : %zu / "
"%zu\n",
stats->media_read_total_expected_packets,
stats->media_read_total_dropped_packets);
dprintf(fd,
" PCM read counts (expected/actual) : %zu / "
"%zu\n",
stats->media_read_total_expected_reads_count,
stats->media_read_total_actual_reads_count);
dprintf(fd,
" PCM read bytes (expected/actual) : %zu / "
"%zu\n",
stats->media_read_total_expected_read_bytes,
stats->media_read_total_actual_read_bytes);
dprintf(fd,
" Frames counts (expected/dropped) : %zu / "
"%zu\n",
stats->media_read_total_expected_frames,
stats->media_read_total_dropped_frames);
}