floss: Support recording for various SCO packet (b1f23f12) · Commits · e / os / android_packages_modules_Bluetooth

system/stack/btm/btm_sco.cc

+17 −47

Original line number	Diff line number	Diff line
		@@ -33,7 +33,6 @@
		#include "device/include/controller.h"
		#include "embdrv/sbc/decoder/include/oi_codec_sbc.h"
		#include "embdrv/sbc/decoder/include/oi_status.h"
		#include "hfp_msbc_decoder.h"
		#include "hfp_msbc_encoder.h"
		#include "osi/include/allocator.h"
		#include "osi/include/log.h"
		@@ -111,8 +110,6 @@ static const uint8_t btm_msbc_zero_packet[] = {
		0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77,
		0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c};

		static const uint8_t btm_msbc_zero_frames[BTM_MSBC_CODE_SIZE] = {0};

		/* Second octet of H2 header is composed by 4 bits fixed 0x8 and 4 bits
		* sequence number 0000, 0011, 1100, 1111. */
		static const uint8_t btm_h2_header_frames_count[] = {0x08, 0x38, 0xc8, 0xf8};
		@@ -210,15 +207,6 @@ static tSCO_CONN* btm_get_active_sco() {
		return nullptr;
		}

		static bool verify_h2_header_seq_num(const uint8_t num) {
		for (int i = 0; i < 4; i++) {
		if (num == btm_h2_header_frames_count[i]) {
		return true;
		}
		}
		return false;
		}

		/*******************************************************************************
		*
		* Function btm_route_sco_data
		@@ -261,40 +249,20 @@ void btm_route_sco_data(BT_HDR* p_msg) {
		}

		const uint8_t* decoded = nullptr;
		size_t written = 0;
		size_t written = 0, rc = 0;
		if (active_sco->is_wbs()) {
		/* TODO(b/235901463): Support packet size != BTM_MSBC_PKT_LEN */
		if (data_len != BTM_MSBC_PKT_LEN) {
		LOG_ERROR("Received invalid mSBC packet with invalid length:%hhu",
		data_len);
		osi_free(p_msg);
		return;
		}
		rc = bluetooth::audio::sco::wbs::enqueue_packet(payload, data_len);
		if (rc != data_len) LOG_DEBUG("Failed to enqueue packet");

		uint8_t h2_header;
		STREAM_TO_UINT8(h2_header, payload);
		if (h2_header != BTM_MSBC_H2_HEADER_0) {
		LOG_ERROR("Received invalid mSBC packet with invalid h2 header:%x",
		h2_header);
		osi_free(p_msg);
		return;
		}

		uint8_t seq_num;
		STREAM_TO_UINT8(seq_num, payload);
		if (!verify_h2_header_seq_num(seq_num)) {
		LOG_ERROR("Received invalid mSBC packet with invalid sequence number :%x",
		seq_num);
		osi_free(p_msg);
		return;
		while (rc) {
		rc = bluetooth::audio::sco::wbs::decode(&decoded);
		if (rc == 0) {
		LOG_DEBUG("Failed to decode frames");
		break;
		}

		if (!hfp_msbc_decoder_decode_packet(p_msg, &decoded)) {
		LOG_ERROR("Decode mSBC packet failed");
		decoded = btm_msbc_zero_frames;
		written += bluetooth::audio::sco::write(decoded, rc);
		}

		written = bluetooth::audio::sco::write(decoded, BTM_MSBC_CODE_SIZE);
		} else {
		written = bluetooth::audio::sco::write(payload, data_len);
		}
		@@ -910,6 +878,7 @@ void btm_sco_connected(const RawAddress& bda, uint16_t hci_handle,
		uint16_t xx;
		bool spt = false;
		tBTM_CHG_ESCO_PARAMS parms = {};
		int codec;

		for (xx = 0; xx < BTM_MAX_SCO_LINKS; xx++, p++) {
		if (((p->state == SCO_ST_CONNECTING) \|\| (p->state == SCO_ST_LISTENING) \|\|
		@@ -944,10 +913,10 @@ void btm_sco_connected(const RawAddress& bda, uint16_t hci_handle,

		(*p->p_conn_cb)(xx);

		codec = hfp_hal_interface::esco_coding_to_codec(
		p->esco.setup.transmit_coding_format.coding_format);
		hfp_hal_interface::notify_sco_connection_change(
		bda, /is_connected=/true,
		hfp_hal_interface::esco_coding_to_codec(
		p->esco.setup.transmit_coding_format.coding_format));
		bda, /is_connected=/true, codec);

		/* In-band (non-offload) data path */
		if (p->is_inband()) {
		@@ -955,8 +924,9 @@ void btm_sco_connected(const RawAddress& bda, uint16_t hci_handle,
		btm_pcm_buf_read_offset = 0;
		btm_pcm_buf_write_offset = 0;
		btm_msbc_num_out_frames = 0;
		hfp_msbc_decoder_init();
		hfp_msbc_encoder_init();
		bluetooth::audio::sco::wbs::init(
		hfp_hal_interface::get_packet_size(codec));
		}

		std::fill(std::begin(btm_pcm_buf), std::end(btm_pcm_buf), 0);
		@@ -1187,8 +1157,8 @@ void btm_sco_on_disconnected(uint16_t hci_handle, tHCI_REASON reason) {

		if (p_sco->is_inband()) {
		if (p_sco->is_wbs()) {
		hfp_msbc_decoder_cleanup();
		hfp_msbc_encoder_cleanup();
		bluetooth::audio::sco::wbs::cleanup();
		}

		bluetooth::audio::sco::cleanup();

system/stack/btm/btm_sco.h

+35 −0

Original line number	Diff line number	Diff line
		@@ -44,6 +44,41 @@ size_t read(uint8_t* p_buf, uint32_t len);
		size_t write(const uint8_t* buf, uint32_t len);
		} // namespace bluetooth::audio::sco

		/* SCO-over-HCI audio HFP WBS related definitions */
		namespace bluetooth::audio::sco::wbs {

		/* Initialize struct used for storing WBS related information.
		* Args:
		* pkt_size - Length of the SCO packet. It is determined based on the BT-USB
		* adapter's capability and alt mode setting. The value should be queried
		* from HAL interface. It will be used to determine the size of the SCO
		* packet buffer.
		*/
		void init(size_t pkt_size);

		/* Clean up when the SCO connection is done */
		void cleanup();

		/* Try to enqueue a packet to a buffer.
		* Args:
		* data - Pointer to received packet data bytes.
		* pkt_size - Length of input packet. Passing packet with inconsistent size
		* from the pkt_size set in init() will trigger a reset of the buffer.
		* Returns:
		* The length of enqueued bytes. 0 if failed.
		*/
		size_t enqueue_packet(const uint8_t* data, size_t pkt_size);

		/* Try to decode mSBC frames from the packets in the buffer.
		* Args:
		* output - Pointer to the decoded PCM bytes caller can read from.
		* Returns:
		* The length of decoded bytes. 0 if failed.
		*/
		size_t decode(const uint8_t** output);

		} // namespace bluetooth::audio::sco::wbs

		/* Define the structures needed by sco */
		typedef enum : uint16_t {
		SCO_ST_UNUSED = 0,

system/stack/btm/btm_sco_hci.cc

+219 −0

Original line number	Diff line number	Diff line
		@@ -21,6 +21,8 @@

		#include <memory>

		#include "hfp_msbc_decoder.h"
		#include "osi/include/allocator.h"
		#include "osi/include/log.h"
		#include "stack/btm/btm_sco.h"
		#include "udrv/include/uipc.h"
		@@ -30,6 +32,11 @@
		// TODO(b/198260375): Make SCO data owner group configurable.
		#define SCO_HOST_DATA_GROUP "bluetooth-audio"

		#define BTM_MSBC_H2_HEADER_0 0x01
		#define BTM_MSBC_CODE_SIZE 240
		#define BTM_MSBC_PKT_LEN 60
		#define BTM_MSBC_SYNC_WORD 0xAD

		namespace {

		std::unique_ptr<tUIPC_STATE> sco_uipc = nullptr;
		@@ -97,6 +104,218 @@ size_t write(const uint8_t* p_buf, uint32_t len) {
		return UIPC_Send(*sco_uipc, UIPC_CH_ID_AV_AUDIO, 0, p_buf, len) ? len : 0;
		}

		namespace wbs {

		/* Second octet of H2 header is composed by 4 bits fixed 0x8 and 4 bits
		* sequence number 0000, 0011, 1100, 1111. */
		static const uint8_t btm_h2_header_frames_count[] = {0x08, 0x38, 0xc8, 0xf8};

		/* Supported SCO packet sizes for mSBC. The wideband speech mSBC frame parsing
		* code ties to limited packet size values. Specifically list them out
		* to check against when setting packet size. The first entry is the default
		* value as a fallback. */
		constexpr size_t btm_wbs_supported_pkt_size[] = {BTM_MSBC_PKT_LEN, 72, 0};
		/* Buffer size should be set to least common multiple of SCO packet size and
		* BTM_MSBC_PKT_LEN for optimizing buffer copy. */
		constexpr size_t btm_wbs_msbc_buffer_size[] = {BTM_MSBC_PKT_LEN, 360, 0};

		static const uint8_t btm_msbc_zero_frames[BTM_MSBC_CODE_SIZE] = {0};

		/* Define the structure that contains mSBC data */
		typedef struct {
		size_t packet_size; /* SCO mSBC packet size supported by lower layer */
		bool check_alignment; /* True to wait for mSBC packet to align */
		size_t buf_size; /* The size of the buffer, determined by the packet_size. */

		uint8_t* msbc_decode_buf; /* Buffer to store mSBC packets to decode */
		size_t decode_buf_wo; /* Write offset of the decode buffer */
		size_t decode_buf_ro; /* Read offset of the decode buffer */

		static size_t get_supported_packet_size(size_t pkt_size,
		size_t* buffer_size) {
		int i;
		for (i = 0; btm_wbs_supported_pkt_size[i] != 0 &&
		btm_wbs_supported_pkt_size[i] != pkt_size;
		i++)
		;
		/* In case of unsupported value, error log and fallback to
		* BTM_MSBC_PKT_LEN(60). */
		if (btm_wbs_supported_pkt_size[i] == 0) {
		LOG_WARN("Unsupported packet size %lu", (unsigned long)pkt_size);
		i = 0;
		}

		if (buffer_size) {
		*buffer_size = btm_wbs_msbc_buffer_size[i];
		}
		return btm_wbs_supported_pkt_size[i];
		}

		bool verify_h2_header_seq_num(const uint8_t num) {
		for (int i = 0; i < 4; i++) {
		if (num == btm_h2_header_frames_count[i]) {
		return true;
		}
		}
		return false;
		}

		public:
		void init(size_t pkt_size) {
		decode_buf_wo = 0;
		decode_buf_ro = 0;

		pkt_size = get_supported_packet_size(pkt_size, &buf_size);
		if (pkt_size != BTM_MSBC_PKT_LEN) check_alignment = true;
		if (pkt_size == packet_size) return;
		packet_size = pkt_size;

		if (msbc_decode_buf) osi_free(msbc_decode_buf);
		msbc_decode_buf = (uint8_t*)osi_calloc(buf_size);
		}

		void deinit() {
		if (msbc_decode_buf) osi_free(msbc_decode_buf);
		}

		size_t decodable() { return decode_buf_wo - decode_buf_ro; }

		size_t write(const uint8_t* input, size_t len) {
		if (len > buf_size - decode_buf_wo) {
		return 0;
		}

		std::copy(input, input + len, msbc_decode_buf + decode_buf_wo);
		decode_buf_wo += len;
		return len;
		}

		const uint8_t* find_msbc_pkt_head() {
		size_t rp = 0;
		while (decode_buf_wo - decode_buf_ro - rp >= BTM_MSBC_PKT_LEN) {
		if ((msbc_decode_buf[decode_buf_ro + rp] != BTM_MSBC_H2_HEADER_0) \|\|
		(!verify_h2_header_seq_num(
		msbc_decode_buf[decode_buf_ro + rp + 1])) \|\|
		(msbc_decode_buf[decode_buf_ro + rp + 2] != BTM_MSBC_SYNC_WORD)) {
		rp++;
		continue;
		}
		return &msbc_decode_buf[decode_buf_ro + rp];
		}

		return nullptr;
		}

		void mark_pkt_decoded() {
		if (decode_buf_ro + BTM_MSBC_PKT_LEN > decode_buf_wo) {
		LOG_ERROR("Trying to mark read offset beyond write offset.");
		return;
		}

		decode_buf_ro += BTM_MSBC_PKT_LEN;
		if (decode_buf_ro == decode_buf_wo) {
		decode_buf_ro = 0;
		decode_buf_wo = 0;
		}
		}

		} tBTM_MSBC_INFO;

		static tBTM_MSBC_INFO* msbc_info = nullptr;

		void init(size_t pkt_size) {
		hfp_msbc_decoder_init();

		if (msbc_info) {
		LOG_WARN("Re-initiating mSBC buffer that is active or not cleaned");
		msbc_info->deinit();
		osi_free(msbc_info);
		}

		msbc_info = (tBTM_MSBC_INFO)osi_calloc(sizeof(msbc_info));
		msbc_info->init(pkt_size);
		}

		void cleanup() {
		hfp_msbc_decoder_cleanup();

		if (msbc_info == nullptr) return;

		msbc_info->deinit();
		osi_free(msbc_info);
		msbc_info = nullptr;
		}

		size_t enqueue_packet(const uint8_t* data, size_t pkt_size) {
		if (msbc_info == nullptr) {
		LOG_WARN("mSBC buffer uninitialized or cleaned");
		return 0;
		}

		if (pkt_size != msbc_info->packet_size) {
		LOG_WARN(
		"Ignoring the coming packet with size %lu that is inconsistent with "
		"the HAL reported packet size %lu",
		(unsigned long)pkt_size, (unsigned long)msbc_info->packet_size);
		return 0;
		}

		if (msbc_info->check_alignment) {
		if (data[0] != BTM_MSBC_H2_HEADER_0 \|\| data[2] != BTM_MSBC_SYNC_WORD) {
		LOG_DEBUG("Waiting for valid mSBC frame head");
		return 0;
		}
		msbc_info->check_alignment = false;
		}

		if (msbc_info->write(data, pkt_size) != pkt_size) {
		LOG_DEBUG("Fail to write packet with size %lu to buffer",
		(unsigned long)pkt_size);
		return 0;
		}

		return pkt_size;
		}

		size_t decode(const uint8_t** out_data) {
		const uint8_t* frame_head = nullptr;

		if (msbc_info == nullptr) {
		LOG_WARN("mSBC buffer uninitialized or cleaned");
		return 0;
		}

		if (msbc_info->decodable() < BTM_MSBC_PKT_LEN) {
		LOG_DEBUG("No complete mSBC packet to decode");
		return 0;
		}

		frame_head = msbc_info->find_msbc_pkt_head();
		if (frame_head == nullptr) {
		LOG_DEBUG("No valid mSBC packet to decode %lu, %lu",
		(unsigned long)msbc_info->decode_buf_ro,
		(unsigned long)msbc_info->decode_buf_wo);
		/* Done with parsing the raw bytes just read. If mSBC frame head not found,
		* we shall handle it as packet loss. */
		goto packet_loss;
		}

		if (!hfp_msbc_decoder_decode_packet(frame_head, out_data)) {
		LOG_DEBUG("Decoding mSBC packet failed");
		goto packet_loss;
		}

		msbc_info->mark_pkt_decoded();
		return BTM_MSBC_CODE_SIZE;

		packet_loss:
		*out_data = btm_msbc_zero_frames;
		msbc_info->mark_pkt_decoded();
		return BTM_MSBC_CODE_SIZE;
		}

		} // namespace wbs

		} // namespace sco
		} // namespace audio
		} // namespace bluetooth

system/stack/btm/hfp_msbc_decoder.cc

+8 −12

Original line number	Diff line number	Diff line
		@@ -25,7 +25,8 @@
		#include "embdrv/sbc/decoder/include/oi_codec_sbc.h"
		#include "embdrv/sbc/decoder/include/oi_status.h"
		#include "osi/include/log.h"
		#include "stack/include/bt_hdr.h"

		#define HFP_MSBC_PKT_LEN 60

		typedef struct {
		OI_CODEC_SBC_DECODER_CONTEXT decoder_context;
		@@ -59,19 +60,14 @@ void hfp_msbc_decoder_cleanup(void) {
		memset(&hfp_msbc_decoder, 0, sizeof(hfp_msbc_decoder));
		}

		bool hfp_msbc_decoder_decode_packet(BT_HDR* p_buf, const uint8_t** out_buf) {
		bool hfp_msbc_decoder_decode_packet(const uint8_t* i_buf,
		const uint8_t** o_buf) {
		const OI_BYTE* oi_data;
		uint32_t oi_size, out_avail;
		int16_t* out_ptr;

		// TODO(b/232463744): Query the HFP HAL for the packet size.
		if (p_buf->len != 63) {
		LOG_ERROR("%s: Invalid packet", __func__);
		return false;
		}

		oi_data = p_buf->data + p_buf->offset;
		oi_size = p_buf->len;
		oi_data = i_buf;
		oi_size = HFP_MSBC_PKT_LEN;
		out_avail = sizeof(hfp_msbc_decoder.decode_buf);
		out_ptr = hfp_msbc_decoder.decode_buf;

		@@ -79,11 +75,11 @@ bool hfp_msbc_decoder_decode_packet(BT_HDR* p_buf, const uint8_t** out_buf) {
		OI_CODEC_SBC_DecodeFrame(&hfp_msbc_decoder.decoder_context, &oi_data,
		&oi_size, out_ptr, &out_avail);
		if (!OI_SUCCESS(status) \|\| out_avail != 240 \|\| oi_size != 0) {
		LOG_ERROR("%s: Decoding failure: %d, %lu, %lu", __func__, status,
		LOG_ERROR("Decoding failure: %d, %lu, %lu", status,
		(unsigned long)out_avail, (unsigned long)oi_size);
		return false;
		}

		out_buf = (const uint8_t)&hfp_msbc_decoder.decode_buf;
		o_buf = (const uint8_t)&hfp_msbc_decoder.decode_buf;
		return true;
		}

system/stack/include/hfp_msbc_decoder.h

+3 −4

Original line number	Diff line number	Diff line
		@@ -23,15 +23,14 @@

		#include <stdint.h>

		#include "stack/include/bt_hdr.h"

		// Initialize the HFP MSBC decoder.
		bool hfp_msbc_decoder_init(void);

		// Cleanup the HFP MSBC decoder.
		void hfp_msbc_decoder_cleanup(void);

		// Decodes \|p_buf\|. \|o_buf\| will be assigned to the decoded frames if available.
		bool hfp_msbc_decoder_decode_packet(BT_HDR* p_buf, const uint8_t** o_buf);
		// Decodes \|i_buf\|. \|o_buf\| will be assigned to the decoded frames if available.
		bool hfp_msbc_decoder_decode_packet(const uint8_t* i_buf,
		const uint8_t** o_buf);

		#endif // HFP_MSBC_DECODER_H