Separate H4 packet parsing from data retrieval. (7feae21a) · Commits · e / os / android_packages_modules_Bluetooth

system/vendor_libs/test_vendor_lib/Android.bp

+2 −0

Original line number	Original line	Diff line number	Diff line
	@@ -32,6 +32,7 @@ cc_library_static {
	"model/devices/device.cc",		"model/devices/device.cc",
	"model/devices/device_properties.cc",		"model/devices/device_properties.cc",
	"model/devices/h4_packetizer.cc",		"model/devices/h4_packetizer.cc",
			"model/devices/h4_parser.cc",
	"model/devices/hci_protocol.cc",		"model/devices/hci_protocol.cc",
	"model/devices/hci_socket_device.cc",		"model/devices/hci_socket_device.cc",
	"model/devices/keyboard.cc",		"model/devices/keyboard.cc",
	@@ -100,6 +101,7 @@ cc_test_host {
	],		],
	srcs: [		srcs: [
	"test/async_manager_unittest.cc",		"test/async_manager_unittest.cc",
			"test/h4_parser_unittest.cc",
	"test/security_manager_unittest.cc",		"test/security_manager_unittest.cc",
	],		],
	header_libs: [		header_libs: [

system/vendor_libs/test_vendor_lib/model/devices/h4_packetizer.cc

+10 −125

Original line number	Original line	Diff line number	Diff line
	@@ -17,37 +17,15 @@
	#include "h4_packetizer.h"		#include "h4_packetizer.h"

	#include <cerrno>		#include <cerrno>

	#include <dlfcn.h>		#include <dlfcn.h>
	#include <fcntl.h>		#include <fcntl.h>
	#include <sys/uio.h>		#include <sys/uio.h>
	#include <unistd.h>		#include <unistd.h>

	#include "os/log.h"		#include "os/log.h"
			#include "osi/include/osi.h"

	namespace test_vendor_lib {		namespace test_vendor_lib {
	size_t H4Packetizer::HciGetPacketLengthForType(PacketType type,
	const uint8_t* preamble) const {
	static const size_t
	packet_length_offset[static_cast<size_t>(PacketType::ISO) + 1] = {
	0,
	H4Packetizer::COMMAND_LENGTH_OFFSET,
	H4Packetizer::ACL_LENGTH_OFFSET,
	H4Packetizer::SCO_LENGTH_OFFSET,
	H4Packetizer::EVENT_LENGTH_OFFSET,
	H4Packetizer::ISO_LENGTH_OFFSET,
	};

	size_t offset = packet_length_offset[static_cast<size_t>(type)];
	size_t size = preamble[offset];
	if (type == PacketType::ACL) {
	size \|= ((size_t)preamble[offset + 1]) << 8u;
	}
	if (type == PacketType::ISO) {
	size \|= ((size_t)preamble[offset + 1] & 0x0fu) << 8u;
	}
	return size;
	}

	H4Packetizer::H4Packetizer(int fd, PacketReadCallback command_cb,		H4Packetizer::H4Packetizer(int fd, PacketReadCallback command_cb,
	PacketReadCallback event_cb,		PacketReadCallback event_cb,
	@@ -55,18 +33,14 @@ H4Packetizer::H4Packetizer(int fd, PacketReadCallback command_cb,
	PacketReadCallback iso_cb,		PacketReadCallback iso_cb,
	ClientDisconnectCallback disconnect_cb)		ClientDisconnectCallback disconnect_cb)
	: uart_fd_(fd),		: uart_fd_(fd),
	command_cb_(std::move(command_cb)),		h4_parser_(command_cb, event_cb, acl_cb, sco_cb, iso_cb),
	event_cb_(std::move(event_cb)),
	acl_cb_(std::move(acl_cb)),
	sco_cb_(std::move(sco_cb)),
	iso_cb_(std::move(iso_cb)),
	disconnect_cb_(std::move(disconnect_cb)) {}		disconnect_cb_(std::move(disconnect_cb)) {}

	size_t H4Packetizer::Send(uint8_t type, const uint8_t* data, size_t length) {		size_t H4Packetizer::Send(uint8_t type, const uint8_t* data, size_t length) {
	struct iovec iov[] = {{&type, sizeof(type)}, {const_cast<uint8_t*>(data), length}};		struct iovec iov[] = {{&type, sizeof(type)}, {const_cast<uint8_t*>(data), length}};
	ssize_t ret = 0;		ssize_t ret = 0;
	do {		do {
	ret = TEMP_FAILURE_RETRY(writev(uart_fd_, iov, sizeof(iov) / sizeof(iov[0])));		OSI_NO_INTR(ret = writev(uart_fd_, iov, sizeof(iov) / sizeof(iov[0])));
	} while (-1 == ret && EAGAIN == errno);		} while (-1 == ret && EAGAIN == errno);

	if (ret == -1) {		if (ret == -1) {
	@@ -78,59 +52,14 @@ size_t H4Packetizer::Send(uint8_t type, const uint8_t* data, size_t length) {
	return ret;		return ret;
	}		}

	void H4Packetizer::OnPacketReady() {
	switch (hci_packet_type_) {
	case PacketType::COMMAND:
	command_cb_(packet_);
	break;
	case PacketType::ACL:
	acl_cb_(packet_);
	break;
	case PacketType::SCO:
	sco_cb_(packet_);
	break;
	case PacketType::EVENT:
	event_cb_(packet_);
	break;
	case PacketType::ISO:
	iso_cb_(packet_);
	break;
	default:
	LOG_ALWAYS_FATAL("Unimplemented packet type %d",
	static_cast<int>(hci_packet_type_));
	}
	// Get ready for the next type byte.
	hci_packet_type_ = PacketType::UNKNOWN;
	}

	void H4Packetizer::OnDataReady(int fd) {		void H4Packetizer::OnDataReady(int fd) {
	if (disconnected_) return;		if (disconnected_) return;
	ssize_t bytes_to_read = 0;		ssize_t bytes_to_read = h4_parser_.BytesRequested();
	uint8_t* buffer_pointer = nullptr;		std::vector<uint8_t> buffer(bytes_to_read);

	static const size_t preamble_size[static_cast<size_t>(PacketType::ISO) + 1] =
	{
	0,
	H4Packetizer::COMMAND_PREAMBLE_SIZE,
	H4Packetizer::ACL_PREAMBLE_SIZE,
	H4Packetizer::SCO_PREAMBLE_SIZE,
	H4Packetizer::EVENT_PREAMBLE_SIZE,
	H4Packetizer::ISO_PREAMBLE_SIZE,
	};
	switch (state_) {
	case HCI_TYPE:
	bytes_to_read = 1;
	buffer_pointer = &packet_type_;
	break;
	case HCI_PREAMBLE:
	case HCI_PAYLOAD:
	bytes_to_read = packet_.size() - bytes_read_;
	buffer_pointer = packet_.data() + bytes_read_;
	break;
	}

	ssize_t bytes_read =		ssize_t bytes_read;
	TEMP_FAILURE_RETRY(read(fd, buffer_pointer, bytes_to_read));		OSI_NO_INTR(bytes_read = read(fd, buffer.data(), bytes_to_read));
	if (bytes_read == 0) {		if (bytes_read == 0) {
	LOG_INFO("remote disconnected!");		LOG_INFO("remote disconnected!");
	disconnected_ = true;		disconnected_ = true;
	@@ -146,55 +75,11 @@ void H4Packetizer::OnDataReady(int fd) {
	disconnect_cb_();		disconnect_cb_();
	return;		return;
	} else {		} else {
	LOG_ALWAYS_FATAL(		LOG_ALWAYS_FATAL("Read error in %d: %s", h4_parser_.CurrentState(),
	"Read error in %s: %s",
	state_ == HCI_TYPE
	? "HCI_TYPE"
	: state_ == HCI_PREAMBLE ? "HCI_PREAMBLE" : "HCI_PAYLOAD",
	strerror(errno));		strerror(errno));
	}		}
	} else if (bytes_read > bytes_to_read) {
	LOG_ALWAYS_FATAL("More bytes read (%u) than expected (%u)!",
	static_cast<int>(bytes_read),
	static_cast<int>(bytes_to_read));
	}

	switch (state_) {
	case HCI_TYPE:
	hci_packet_type_ = static_cast<PacketType>(packet_type_);
	if (hci_packet_type_ != PacketType::ACL &&
	hci_packet_type_ != PacketType::SCO &&
	hci_packet_type_ != PacketType::COMMAND &&
	hci_packet_type_ != PacketType::EVENT &&
	hci_packet_type_ != PacketType::ISO) {
	LOG_ALWAYS_FATAL("Unimplemented packet type %hhd", packet_type_);
	}
	state_ = HCI_PREAMBLE;
	bytes_read_ = 0;
	packet_.resize(preamble_size[static_cast<size_t>(hci_packet_type_)]);
	break;
	case HCI_PREAMBLE:
	bytes_read_ += bytes_read;
	if (bytes_read_ == packet_.size()) {
	size_t payload_size =
	HciGetPacketLengthForType(hci_packet_type_, packet_.data());
	if (payload_size == 0) {
	OnPacketReady();
	state_ = HCI_TYPE;
	} else {
	packet_.resize(packet_.size() + payload_size);
	state_ = HCI_PAYLOAD;
	}
	}
	break;
	case HCI_PAYLOAD:
	bytes_read_ += bytes_read;
	if (bytes_read_ == packet_.size()) {
	OnPacketReady();
	state_ = HCI_TYPE;
	}
	break;
	}		}
			h4_parser_.Consume(buffer.data(), bytes_read);
	}		}

	} // namespace test_vendor_lib		} // namespace test_vendor_lib

system/vendor_libs/test_vendor_lib/model/devices/h4_packetizer.h

+6 −52

Original line number	Original line	Diff line number	Diff line
	@@ -19,22 +19,15 @@
	#include <functional>		#include <functional>
	#include <vector>		#include <vector>

			#include "h4_parser.h"
	#include "hci_protocol.h"		#include "hci_protocol.h"

	namespace test_vendor_lib {		namespace test_vendor_lib {

	using HciPacketReadyCallback = std::function<void(void)>;		// A socket based H4Packetizer. Call OnDataReady whenever
	using ClientDisconnectCallback = std::function<void()>;		// data can be read from file descriptor fd.
			//
	enum class PacketType : uint8_t {		// This is only supported on unix.
	UNKNOWN = 0,
	COMMAND = 1,
	ACL = 2,
	SCO = 3,
	EVENT = 4,
	ISO = 5,
	};

	class H4Packetizer : public HciProtocol {		class H4Packetizer : public HciProtocol {
	public:		public:
	H4Packetizer(int fd, PacketReadCallback command_cb,		H4Packetizer(int fd, PacketReadCallback command_cb,
	@@ -44,53 +37,14 @@ class H4Packetizer : public HciProtocol {

	size_t Send(uint8_t type, const uint8_t* data, size_t length) override;		size_t Send(uint8_t type, const uint8_t* data, size_t length) override;

	void OnPacketReady();

	void OnDataReady(int fd);		void OnDataReady(int fd);

	// 2 bytes for opcode, 1 byte for parameter length (Volume 2, Part E, 5.4.1)
	static constexpr size_t COMMAND_PREAMBLE_SIZE = 3;
	static constexpr size_t COMMAND_LENGTH_OFFSET = 2;

	// 2 bytes for handle, 2 bytes for data length (Volume 2, Part E, 5.4.2)
	static constexpr size_t ACL_PREAMBLE_SIZE = 4;
	static constexpr size_t ACL_LENGTH_OFFSET = 2;

	// 2 bytes for handle, 1 byte for data length (Volume 2, Part E, 5.4.3)
	static constexpr size_t SCO_PREAMBLE_SIZE = 3;
	static constexpr size_t SCO_LENGTH_OFFSET = 2;

	// 1 byte for event code, 1 byte for parameter length (Volume 2, Part
	// E, 5.4.4)
	static constexpr size_t EVENT_PREAMBLE_SIZE = 2;
	static constexpr size_t EVENT_LENGTH_OFFSET = 1;

	// 2 bytes for handle and flags, 12 bits for length (Volume 2, Part E, 5.4.5)
	static constexpr size_t ISO_PREAMBLE_SIZE = 4;
	static constexpr size_t ISO_LENGTH_OFFSET = 2;

	private:		private:
	int uart_fd_;		int uart_fd_;
			H4Parser h4_parser_;
	PacketReadCallback command_cb_;
	PacketReadCallback event_cb_;
	PacketReadCallback acl_cb_;
	PacketReadCallback sco_cb_;
	PacketReadCallback iso_cb_;

	ClientDisconnectCallback disconnect_cb_;		ClientDisconnectCallback disconnect_cb_;
	bool disconnected_{false};		bool disconnected_{false};

	size_t HciGetPacketLengthForType(PacketType type,
	const uint8_t* preamble) const;

	PacketType hci_packet_type_{PacketType::UNKNOWN};

	enum State { HCI_TYPE, HCI_PREAMBLE, HCI_PAYLOAD };
	State state_{HCI_TYPE};
	uint8_t packet_type_{};
	std::vector<uint8_t> packet_;
	size_t bytes_read_{0};
	};		};

	} // namespace test_vendor_lib		} // namespace test_vendor_lib

system/vendor_libs/test_vendor_lib/model/devices/h4_parser.cc

0 → 100644

+173 −0

Original line number	Original line	Diff line number	Diff line
			//
			// Copyright 20 The Android Open Source Project
			//
			// 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.
			//

			#include "model/devices/h4_parser.h" // for H4Parser, PacketType, H4Pars...

			#include <stddef.h> // for size_t
			#include <cstdint> // for uint8_t, int32_t
			#include <functional> // for function
			#include <utility> // for move
			#include <vector> // for vector

			#include "model/devices/hci_protocol.h" // for PacketReadCallback
			#include "os/log.h" // for LOG_ALWAYS_FATAL, LOG_INFO

			namespace test_vendor_lib {

			void H4Parser::Reset() {
			state_ = HCI_TYPE;
			packet_.clear();
			bytes_wanted_ = 0;
			packet_type_ = 0;
			}

			size_t H4Parser::HciGetPacketLengthForType(PacketType type,
			const uint8_t* preamble) const {
			static const size_t
			packet_length_offset[static_cast<size_t>(PacketType::ISO) + 1] = {
			0,
			H4Parser::COMMAND_LENGTH_OFFSET,
			H4Parser::ACL_LENGTH_OFFSET,
			H4Parser::SCO_LENGTH_OFFSET,
			H4Parser::EVENT_LENGTH_OFFSET,
			H4Parser::ISO_LENGTH_OFFSET,
			};

			size_t offset = packet_length_offset[static_cast<size_t>(type)];
			size_t size = preamble[offset];
			if (type == PacketType::ACL) {
			size \|= ((size_t)preamble[offset + 1]) << 8u;
			}
			if (type == PacketType::ISO) {
			size \|= ((size_t)preamble[offset + 1] & 0x0fu) << 8u;
			}
			return size;
			}

			H4Parser::H4Parser(PacketReadCallback command_cb, PacketReadCallback event_cb,
			PacketReadCallback acl_cb, PacketReadCallback sco_cb,
			PacketReadCallback iso_cb)
			: command_cb_(std::move(command_cb)),
			event_cb_(std::move(event_cb)),
			acl_cb_(std::move(acl_cb)),
			sco_cb_(std::move(sco_cb)),
			iso_cb_(std::move(iso_cb)) {}

			void H4Parser::OnPacketReady() {
			switch (hci_packet_type_) {
			case PacketType::COMMAND:
			command_cb_(packet_);
			break;
			case PacketType::ACL:
			acl_cb_(packet_);
			break;
			case PacketType::SCO:
			sco_cb_(packet_);
			break;
			case PacketType::EVENT:
			event_cb_(packet_);
			break;
			case PacketType::ISO:
			iso_cb_(packet_);
			break;
			default:
			LOG_ALWAYS_FATAL("Unimplemented packet type %d",
			static_cast<int>(hci_packet_type_));
			}
			// Get ready for the next type byte.
			hci_packet_type_ = PacketType::UNKNOWN;
			}

			size_t H4Parser::BytesRequested() {
			switch (state_) {
			case HCI_TYPE:
			return 1;
			case HCI_PREAMBLE:
			case HCI_PAYLOAD:
			return bytes_wanted_;
			}
			}

			bool H4Parser::Consume(uint8_t* buffer, int32_t bytes_read) {
			size_t bytes_to_read = BytesRequested();
			if (bytes_read <= 0) {
			LOG_INFO("remote disconnected, or unhandled error?");
			return false;
			} else if (bytes_read > BytesRequested()) {
			LOG_ALWAYS_FATAL("More bytes read (%u) than expected (%u)!",
			static_cast<int>(bytes_read),
			static_cast<int>(bytes_to_read));
			}

			static const size_t preamble_size[static_cast<size_t>(PacketType::ISO) + 1] =
			{
			0,
			H4Parser::COMMAND_PREAMBLE_SIZE,
			H4Parser::ACL_PREAMBLE_SIZE,
			H4Parser::SCO_PREAMBLE_SIZE,
			H4Parser::EVENT_PREAMBLE_SIZE,
			H4Parser::ISO_PREAMBLE_SIZE,
			};
			switch (state_) {
			case HCI_TYPE:
			// bytes_read >= 1
			packet_type_ = *buffer;
			packet_.clear();
			break;
			case HCI_PREAMBLE:
			case HCI_PAYLOAD:
			packet_.insert(packet_.end(), buffer, buffer + bytes_read);
			bytes_wanted_ -= bytes_read;
			break;
			}

			switch (state_) {
			case HCI_TYPE:
			hci_packet_type_ = static_cast<PacketType>(packet_type_);
			if (hci_packet_type_ != PacketType::ACL &&
			hci_packet_type_ != PacketType::SCO &&
			hci_packet_type_ != PacketType::COMMAND &&
			hci_packet_type_ != PacketType::EVENT &&
			hci_packet_type_ != PacketType::ISO) {
			LOG_ALWAYS_FATAL("Unimplemented packet type %hhd", packet_type_);
			}
			state_ = HCI_PREAMBLE;
			bytes_wanted_ = preamble_size[static_cast<size_t>(hci_packet_type_)];
			break;
			case HCI_PREAMBLE:

			if (bytes_wanted_ == 0) {
			size_t payload_size =
			HciGetPacketLengthForType(hci_packet_type_, packet_.data());
			if (payload_size == 0) {
			OnPacketReady();
			state_ = HCI_TYPE;
			} else {
			bytes_wanted_ = payload_size;
			state_ = HCI_PAYLOAD;
			}
			}
			break;
			case HCI_PAYLOAD:
			if (bytes_wanted_ == 0) {
			OnPacketReady();
			state_ = HCI_TYPE;
			}
			break;
			}
			return true;
			}
			} // namespace test_vendor_lib

system/vendor_libs/test_vendor_lib/model/devices/h4_parser.h

0 → 100644

+122 −0

Original line number	Original line	Diff line number	Diff line
			//
			// Copyright 2021 The Android Open Source Project
			//
			// 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.
			//

			#pragma once

			#include <stddef.h> // for size_t
			#include <cstdint> // for uint8_t, int32_t
			#include <functional> // for function
			#include <ostream> // for operator<<, ostream
			#include <vector> // for vector

			#include "model/devices/hci_protocol.h" // for PacketReadCallback

			namespace test_vendor_lib {

			using HciPacketReadyCallback = std::function<void(void)>;
			using ClientDisconnectCallback = std::function<void()>;

			enum class PacketType : uint8_t {
			UNKNOWN = 0,
			COMMAND = 1,
			ACL = 2,
			SCO = 3,
			EVENT = 4,
			ISO = 5,
			};

			// An H4 Parser can parse H4 Packets and will invoke the proper callback
			// once a packet has been parsed.
			//
			// You use it as follows:
			//
			// H4Parser h4(....);
			// size_t nr_bytes = h4.BytesRequested();
			// std::vector fill_this_vector_with_at_most_nr_bytes(nr_bytes);
			// h4.Consume(fill_this_vector_with_at_most_nr_bytes.data(), nr_bytes.size());
			//
			// The parser will invoke the proper callbacks once a packet has been parsed.
			// The parser keeps internal state and is not thread safe.
			class H4Parser {
			public:
			enum State { HCI_TYPE, HCI_PREAMBLE, HCI_PAYLOAD };

			H4Parser(PacketReadCallback command_cb, PacketReadCallback event_cb,
			PacketReadCallback acl_cb, PacketReadCallback sco_cb,
			PacketReadCallback iso_cb);

			// Consumes the given number of bytes, returns true on success.
			bool Consume(uint8_t* buffer, int32_t bytes);

			// The maximum number of bytes the parser can consume in the current state.
			size_t BytesRequested();

			// Resets the parser to the empty, initial state.
			void Reset();

			State CurrentState() { return state_; };

			private:
			void OnPacketReady();

			// 2 bytes for opcode, 1 byte for parameter length (Volume 2, Part E, 5.4.1)
			static constexpr size_t COMMAND_PREAMBLE_SIZE = 3;
			static constexpr size_t COMMAND_LENGTH_OFFSET = 2;

			// 2 bytes for handle, 2 bytes for data length (Volume 2, Part E, 5.4.2)
			static constexpr size_t ACL_PREAMBLE_SIZE = 4;
			static constexpr size_t ACL_LENGTH_OFFSET = 2;

			// 2 bytes for handle, 1 byte for data length (Volume 2, Part E, 5.4.3)
			static constexpr size_t SCO_PREAMBLE_SIZE = 3;
			static constexpr size_t SCO_LENGTH_OFFSET = 2;

			// 1 byte for event code, 1 byte for parameter length (Volume 2, Part
			// E, 5.4.4)
			static constexpr size_t EVENT_PREAMBLE_SIZE = 2;
			static constexpr size_t EVENT_LENGTH_OFFSET = 1;

			// 2 bytes for handle and flags, 12 bits for length (Volume 2, Part E, 5.4.5)
			static constexpr size_t ISO_PREAMBLE_SIZE = 4;
			static constexpr size_t ISO_LENGTH_OFFSET = 2;

			PacketReadCallback command_cb_;
			PacketReadCallback event_cb_;
			PacketReadCallback acl_cb_;
			PacketReadCallback sco_cb_;
			PacketReadCallback iso_cb_;

			size_t HciGetPacketLengthForType(PacketType type,
			const uint8_t* preamble) const;

			PacketType hci_packet_type_{PacketType::UNKNOWN};

			State state_{HCI_TYPE};
			uint8_t packet_type_{};
			std::vector<uint8_t> packet_;
			size_t bytes_wanted_{0};
			};

			inline std::ostream& operator<<(std::ostream& os,
			H4Parser::State const& state_) {
			os << (state_ == H4Parser::State::HCI_TYPE
			? "HCI_TYPE"
			: state_ == H4Parser::State::HCI_PREAMBLE ? "HCI_PREAMBLE"
			: "HCI_PAYLOAD");
			return os;
			}

			} // namespace test_vendor_lib