bluetooth/hci: Extend packet fragmenter for ISO (7f00e95f) · Commits · e / os / android_packages_modules_Bluetooth

system/hci/include/hci_internals.h

+29 −1

Original line number	Diff line number	Diff line
		@@ -26,5 +26,33 @@
		#define HCI_SCO_PREAMBLE_SIZE 3
		// 1 byte for event code, 1 byte for parameter length (Volume 2, Part E, 5.4.4)
		#define HCI_EVENT_PREAMBLE_SIZE 2

		// ISO
		// 2 bytes for handle, 2 bytes for data length (Volume 2, Part E, 5.4.5)
		#define HCI_ISO_PREAMBLE_SIZE 4

		#define HCI_ISO_BF_FIRST_FRAGMENTED_PACKET (0)
		#define HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET (1)
		#define HCI_ISO_BF_COMPLETE_PACKET (2)
		#define HCI_ISO_BF_LAST_FRAGMENT_PACKET (3)

		#define HCI_ISO_HEADER_TIMESTAMP_SIZE (4)
		#define HCI_ISO_HEADER_ISO_LEN_SIZE (2)
		#define HCI_ISO_HEADER_PACKET_SEQ_SIZE (2)

		#define HCI_ISO_HEADER_LEN_WITHOUT_TS \
		(HCI_ISO_HEADER_ISO_LEN_SIZE + HCI_ISO_HEADER_PACKET_SEQ_SIZE)
		#define HCI_ISO_HEADER_LEN_WITH_TS \
		(HCI_ISO_HEADER_LEN_WITHOUT_TS + HCI_ISO_HEADER_TIMESTAMP_SIZE)

		#define HCI_ISO_SET_CONTINUATION_FLAG(handle) \
		(((handle)&0x4FFF) \| (0x0001 << 12))
		#define HCI_ISO_SET_COMPLETE_FLAG(handle) (((handle)&0x4FFF) \| (0x0002 << 12))
		#define HCI_ISO_SET_END_FRAG_FLAG(handle) (((handle)&0x4FFF) \| (0x0003 << 12))
		#define HCI_ISO_SET_TIMESTAMP_FLAG(handle) (((handle)&0x3FFF) \| (0x0001 << 14))

		#define HCI_ISO_GET_TS_FLAG(handle) (((handle) >> 14) & 0x0001)
		#define HCI_ISO_GET_PACKET_STATUS_FLAGS(iso_sdu_length) \
		(iso_sdu_length & 0xC000)

		#define HCI_ISO_SDU_LENGTH_MASK 0x0FFF

system/hci/src/packet_fragmenter.cc

+233 −8

Original line number	Diff line number	Diff line
		@@ -38,7 +38,6 @@

		#define HANDLE_MASK 0x0FFF
		#define START_PACKET_BOUNDARY 2
		#define CONTINUATION_PACKET_BOUNDARY 1
		#define L2CAP_HEADER_PDU_LEN_SIZE 2
		#define L2CAP_HEADER_CID_SIZE 2
		#define L2CAP_HEADER_SIZE (L2CAP_HEADER_PDU_LEN_SIZE + L2CAP_HEADER_CID_SIZE)
		@@ -50,25 +49,39 @@ static const controller_t* controller;
		static const packet_fragmenter_callbacks_t* callbacks;

		static std::unordered_map<uint16_t /* handle /, BT_HDR> partial_packets;
		static std::unordered_map<uint16_t /* handle /, BT_HDR> partial_iso_packets;

		static void init(const packet_fragmenter_callbacks_t* result_callbacks) {
		callbacks = result_callbacks;
		}

		static void cleanup() { partial_packets.clear(); }
		static void cleanup() {
		partial_packets.clear();
		partial_iso_packets.clear();
		}

		static bool check_uint16_overflow(uint16_t a, uint16_t b) {
		return (UINT16_MAX - a) < b;
		}

		static void fragment_and_dispatch_acl(BT_HDR* packet);
		static void fragment_and_dispatch_iso(BT_HDR* packet);

		static void fragment_and_dispatch(BT_HDR* packet) {
		CHECK(packet != NULL);

		uint16_t event = packet->event & MSG_EVT_MASK;
		uint8_t* stream = packet->data + packet->offset;

		// We only fragment ACL packets
		if (event != MSG_STACK_TO_HC_HCI_ACL) {
		if (event == MSG_STACK_TO_HC_HCI_ACL) {
		fragment_and_dispatch_acl(packet);
		} else if (event == MSG_STACK_TO_HC_HCI_ISO) {
		fragment_and_dispatch_iso(packet);
		} else {
		callbacks->fragmented(packet, true);
		return;
		}
		}

		static void fragment_and_dispatch_acl(BT_HDR* packet) {
		uint16_t max_data_size =
		SUB_EVENT(packet->event) == LOCAL_BR_EDR_CONTROLLER_ID
		? controller->get_acl_data_size_classic()
		@@ -77,6 +90,8 @@ static void fragment_and_dispatch(BT_HDR* packet) {
		uint16_t max_packet_size = max_data_size + HCI_ACL_PREAMBLE_SIZE;
		uint16_t remaining_length = packet->len;

		uint8_t* stream = packet->data + packet->offset;

		uint16_t continuation_handle;
		STREAM_TO_UINT16(continuation_handle, stream);
		continuation_handle = APPLY_CONTINUATION_FLAG(continuation_handle);
		@@ -115,8 +130,216 @@ static void fragment_and_dispatch(BT_HDR* packet) {
		callbacks->fragmented(packet, true);
		}

		static bool check_uint16_overflow(uint16_t a, uint16_t b) {
		return (UINT16_MAX - a) < b;
		static void fragment_and_dispatch_iso(BT_HDR* packet) {
		uint8_t* stream = packet->data + packet->offset;
		uint16_t max_data_size = controller->get_iso_data_size();
		uint16_t max_packet_size = max_data_size + HCI_ISO_PREAMBLE_SIZE;
		uint16_t remaining_length = packet->len;

		uint16_t handle;
		STREAM_TO_UINT16(handle, stream);

		if (packet->layer_specific & BT_ISO_HDR_CONTAINS_TS) {
		// First packet might have timestamp
		handle = HCI_ISO_SET_TIMESTAMP_FLAG(handle);
		}

		if (remaining_length <= max_packet_size) {
		stream = packet->data + packet->offset;
		UINT16_TO_STREAM(stream, HCI_ISO_SET_COMPLETE_FLAG(handle));
		} else {
		while (remaining_length > max_packet_size) {
		// Make sure we use the right ISO packet size
		stream = packet->data + packet->offset;
		STREAM_SKIP_UINT16(stream);
		UINT16_TO_STREAM(stream, max_data_size);

		packet->len = max_packet_size;
		callbacks->fragmented(packet, false);

		packet->offset += max_data_size;
		remaining_length -= max_data_size;
		packet->len = remaining_length;

		// Write the ISO header for the next fragment
		stream = packet->data + packet->offset;
		if (remaining_length > max_packet_size) {
		UINT16_TO_STREAM(stream,
		HCI_ISO_SET_CONTINUATION_FLAG(handle & HANDLE_MASK));
		} else {
		UINT16_TO_STREAM(stream,
		HCI_ISO_SET_END_FRAG_FLAG(handle & HANDLE_MASK));
		}
		UINT16_TO_STREAM(stream, remaining_length - HCI_ISO_PREAMBLE_SIZE);
		}
		}
		callbacks->fragmented(packet, true);
		}

		static void reassemble_and_dispatch_iso(UNUSED_ATTR BT_HDR* packet) {
		uint8_t* stream = packet->data;
		uint16_t handle;
		uint16_t iso_length;
		uint8_t iso_hdr_len = HCI_ISO_HEADER_LEN_WITHOUT_TS;
		BT_HDR* partial_packet;
		uint16_t iso_full_len;

		STREAM_TO_UINT16(handle, stream);
		STREAM_TO_UINT16(iso_length, stream);
		// last 2 bits is RFU
		iso_length = iso_length & 0x3FFF;

		CHECK(iso_length == packet->len - HCI_ISO_PREAMBLE_SIZE);

		uint8_t boundary_flag = GET_BOUNDARY_FLAG(handle);
		uint8_t ts_flag = HCI_ISO_GET_TS_FLAG(handle);
		handle = handle & HANDLE_MASK;

		auto map_iter = partial_iso_packets.find(handle);

		switch (boundary_flag) {
		case HCI_ISO_BF_COMPLETE_PACKET:
		case HCI_ISO_BF_FIRST_FRAGMENTED_PACKET:
		uint16_t iso_sdu_length;
		uint8_t packet_status_flags;

		if (map_iter != partial_iso_packets.end()) {
		LOG_WARN(
		"%s found unfinished packet for the iso handle with start packet. "
		"Dropping old.",
		__func__);
		BT_HDR* hdl = map_iter->second;
		partial_iso_packets.erase(map_iter);
		buffer_allocator->free(hdl);
		}

		if (ts_flag) {
		/* Skip timestamp u32 */
		STREAM_SKIP_UINT32(stream);
		packet->layer_specific \|= BT_ISO_HDR_CONTAINS_TS;
		iso_hdr_len = HCI_ISO_HEADER_LEN_WITH_TS;
		}

		if (iso_length < iso_hdr_len) {
		LOG_WARN("%s ISO packet too small (%d < %d). Dropping it.", __func__,
		packet->len, iso_hdr_len);
		buffer_allocator->free(packet);
		return;
		}

		/* Skip packet_seq. */
		STREAM_SKIP_UINT16(stream);
		STREAM_TO_UINT16(iso_sdu_length, stream);

		/* Silently ignore empty report if there's no 'lost data' flag set. */
		if (iso_sdu_length == 0) {
		buffer_allocator->free(packet);
		return;
		}

		packet_status_flags = HCI_ISO_GET_PACKET_STATUS_FLAGS(iso_sdu_length);
		iso_sdu_length = iso_sdu_length & HCI_ISO_SDU_LENGTH_MASK;

		if (packet_status_flags)
		LOG_ERROR("%s packet status flags: 0x%02x", __func__,
		packet_status_flags);

		iso_full_len = iso_sdu_length + iso_hdr_len + HCI_ISO_PREAMBLE_SIZE;
		if ((iso_full_len + sizeof(BT_HDR)) > BT_DEFAULT_BUFFER_SIZE) {
		LOG_ERROR("%s Dropping ISO packet with invalid length (%d).", __func__,
		iso_sdu_length);
		buffer_allocator->free(packet);
		return;
		}

		if ((boundary_flag == HCI_ISO_BF_COMPLETE_PACKET) &&
		(iso_full_len != packet->len)) {
		LOG_ERROR("%s corrupted ISO frame", __func__);
		return;
		}

		partial_packet =
		(BT_HDR*)buffer_allocator->alloc(iso_full_len + sizeof(BT_HDR));
		if (!partial_packet) {
		LOG_ERROR("%s cannot allocate partial packet", __func__);
		buffer_allocator->free(packet);
		return;
		}

		partial_packet->event = packet->event;
		partial_packet->len = iso_full_len;
		partial_packet->layer_specific = packet->layer_specific;

		memcpy(partial_packet->data, packet->data, packet->len);

		// Update the ISO data size to indicate the full expected length
		stream = partial_packet->data;
		STREAM_SKIP_UINT16(stream); // skip the ISO handle
		UINT16_TO_STREAM(stream, iso_full_len - HCI_ISO_PREAMBLE_SIZE);

		if (boundary_flag == HCI_ISO_BF_FIRST_FRAGMENTED_PACKET) {
		partial_packet->offset = packet->len;
		partial_iso_packets[handle] = partial_packet;
		} else {
		packet->layer_specific \|= BT_ISO_HDR_OFFSET_POINTS_DATA;
		partial_packet->offset = iso_hdr_len + HCI_ISO_PREAMBLE_SIZE;
		callbacks->reassembled(partial_packet);
		}

		buffer_allocator->free(packet);
		break;

		case HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET:
		// pass-through
		case HCI_ISO_BF_LAST_FRAGMENT_PACKET:
		if (map_iter == partial_iso_packets.end()) {
		LOG_WARN("%s got continuation for unknown packet. Dropping it.",
		__func__);
		buffer_allocator->free(packet);
		return;
		}

		partial_packet = map_iter->second;
		if (partial_packet->len <
		(partial_packet->offset + packet->len - HCI_ISO_PREAMBLE_SIZE)) {
		LOG_ERROR(
		"%s got packet which would exceed expected length of %d. "
		"dropping full packet",
		__func__, partial_packet->len);
		buffer_allocator->free(packet);
		partial_iso_packets.erase(map_iter);
		buffer_allocator->free(partial_packet);
		return;
		}

		memcpy(partial_packet->data + partial_packet->offset,
		packet->data + HCI_ISO_PREAMBLE_SIZE,
		packet->len - HCI_ISO_PREAMBLE_SIZE);

		if (boundary_flag == HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET) {
		partial_packet->offset += packet->len - HCI_ISO_PREAMBLE_SIZE;
		buffer_allocator->free(packet);
		return;
		}

		partial_packet->layer_specific \|= BT_ISO_HDR_OFFSET_POINTS_DATA;
		partial_packet->offset = HCI_ISO_PREAMBLE_SIZE;
		if (partial_packet->layer_specific & BT_ISO_HDR_CONTAINS_TS)
		partial_packet->offset += HCI_ISO_HEADER_LEN_WITH_TS;
		else
		partial_packet->offset += HCI_ISO_HEADER_LEN_WITHOUT_TS;

		buffer_allocator->free(packet);

		partial_iso_packets.erase(map_iter);
		callbacks->reassembled(partial_packet);

		break;
		default:
		LOG_ERROR("%s Unexpected packet, dropping full packet", __func__);
		buffer_allocator->free(packet);
		break;
		}
		}

		static void reassemble_and_dispatch(UNUSED_ATTR BT_HDR* packet) {
		@@ -236,6 +459,8 @@ static void reassemble_and_dispatch(UNUSED_ATTR BT_HDR* packet) {
		callbacks->reassembled(partial_packet);
		}
		}
		} else if ((packet->event & MSG_EVT_MASK) == MSG_HC_TO_STACK_HCI_ISO) {
		reassemble_and_dispatch_iso(packet);
		} else {
		callbacks->reassembled(packet);
		}

system/hci/test/packet_fragmenter_test.cc

+375 −34

File changed.

Preview size limit exceeded, changes collapsed.

system/stack/include/bt_types.h

+8 −0

Original line number	Diff line number	Diff line
		@@ -215,6 +215,10 @@
		#define BT_EVT_BTIF 0xA000
		#define BT_EVT_CONTEXT_SWITCH_EVT (0x0001 \| BT_EVT_BTIF)

		/* ISO Layer specific */
		#define BT_ISO_HDR_CONTAINS_TS (0x0001)
		#define BT_ISO_HDR_OFFSET_POINTS_DATA (0x0002)

		/* Define the header of each buffer used in the Bluetooth stack.
		*/
		typedef struct {
		@@ -418,6 +422,10 @@ typedef struct {
		do { \
		(p) += 2; \
		} while (0)
		#define STREAM_SKIP_UINT32(p) \
		do { \
		(p) += 4; \
		} while (0)

		/*******************************************************************************
		* Macros to get and put bytes to and from a field (Little Endian format).