/****************************************************************************** * * 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 #include #include #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" /* 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; // 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 (!a2dp_sbc_encoder_cb.tx_sbc_frames) { 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); }