Skip to content

Commit

Permalink
pw_bluetooth: Implement rx packet reassembly
Browse files Browse the repository at this point in the history 
Add support for SDU fragments in IsoInboundPacketAssembler.

Bug: http://b/311639690
Test: pw presubmit --step gn_chre_googletest_nanopb_sapphire_build
Change-Id: I6f903a765dfd2b2448c1451c498e37359f318ad0
Reviewed-on: https://pigweed-review.googlesource.com/c/pigweed/pigweed/+/253432
Commit-Queue: Josh Conner <[email protected]>
Docs-Not-Needed: Josh Conner <[email protected]>
Reviewed-by: Jason Graffius <[email protected]>
Lint: Lint 🤖 <[email protected]>
  • Loading branch information
@josh-conner
josh-conner authored and CQ Bot Account committed Dec 19, 2024
1 parent 89395f2 commit 2db41fe
Show file tree
Hide file tree
Showing 5 changed files with 394 additions and 19 deletions.
135 changes: 118 additions & 17 deletions pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,24 +23,125 @@
namespace bt::iso {

void IsoInboundPacketAssembler::ProcessNext(pw::span<const std::byte> packet) {
const uint8_t* data = reinterpret_cast<const uint8_t*>(packet.data());
auto header_view =
pw::bluetooth::emboss::MakeIsoDataFrameHeaderView(data, packet.size());

switch (header_view.pb_flag().Read()) {
case pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU:
BT_ASSERT(complete_packet_handler_);
complete_packet_handler_(packet);
break;
case pw::bluetooth::emboss::IsoDataPbFlag::FIRST_FRAGMENT:
case pw::bluetooth::emboss::IsoDataPbFlag::INTERMEDIATE_FRAGMENT:
case pw::bluetooth::emboss::IsoDataPbFlag::LAST_FRAGMENT:
// TODO(b/311639690): Add support for fragmented SDUs
bt_log(ERROR,
"iso",
"ISO packet fragmentation not supported yet - dropping packet");
break;
auto packet_view = pw::bluetooth::emboss::MakeIsoDataFramePacketView(
packet.data(), packet.size());

// This should have been checked by the caller.
BT_ASSERT(packet_view.Ok());

pw::bluetooth::emboss::IsoDataPbFlag pb_flag =
packet_view.header().pb_flag().Read();
if ((pb_flag == pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU) ||
(pb_flag == pw::bluetooth::emboss::IsoDataPbFlag::FIRST_FRAGMENT)) {
// If this is the start of an SDU, we shouldn't have anything in the buffer
if (!assembly_buffer_.empty()) {
bt_log(ERROR, "iso", "Incomplete ISO packet received - discarding");
assembly_buffer_.clear();
}
}

if (pb_flag == pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU) {
BT_ASSERT(complete_packet_handler_);
complete_packet_handler_(packet);
return;
}

// When we encounter the first fragment of an SDU, we just copy everything
// into the temporary buffer.
if (pb_flag == pw::bluetooth::emboss::IsoDataPbFlag::FIRST_FRAGMENT) {
// Make sure our buffer has sufficient space to hold the entire assembled
// ISO SDU frame
size_t assembled_frame_size = packet_view.sdu_fragment_offset().Read() +
packet_view.iso_sdu_length().Read();
if (assembled_frame_size > assembly_buffer_.capacity()) {
assembly_buffer_.reserve(assembled_frame_size);
}

assembly_buffer_.resize(packet.size());
std::copy(packet.begin(), packet.end(), assembly_buffer_.begin());
return;
}

BT_ASSERT((pb_flag ==
pw::bluetooth::emboss::IsoDataPbFlag::INTERMEDIATE_FRAGMENT) ||
(pb_flag == pw::bluetooth::emboss::IsoDataPbFlag::LAST_FRAGMENT));
if (!AppendFragment(packet)) {
return;
}

if (pb_flag == pw::bluetooth::emboss::IsoDataPbFlag::LAST_FRAGMENT) {
pw::span<const std::byte> assembly_buffer_span(assembly_buffer_.data(),
assembly_buffer_.size());
complete_packet_handler_(assembly_buffer_span);
}
}

bool IsoInboundPacketAssembler::AppendFragment(
pw::span<const std::byte> packet) {
// Make sure we have previously received fragments
if (assembly_buffer_.empty()) {
bt_log(
ERROR, "iso", "Out-of-order ISO packet fragment received - discarding");
return false;
}

auto assembly_buffer_view = pw::bluetooth::emboss::MakeIsoDataFramePacketView(
assembly_buffer_.data(), assembly_buffer_.size());
BT_DEBUG_ASSERT(assembly_buffer_view.Ok());

auto fragment_view = pw::bluetooth::emboss::MakeIsoDataFramePacketView(
packet.data(), packet.size());
BT_DEBUG_ASSERT(fragment_view.Ok());

// A failure here would indicate that packets are being incorrectly routed to
// the appropriate stream (since we should be using the connection handle to
// figure out where to send the packet).
BT_ASSERT(assembly_buffer_view.header().connection_handle().Read() ==
fragment_view.header().connection_handle().Read());

size_t total_sdu_bytes_received =
assembly_buffer_view.sdu_fragment_size().Read() +
fragment_view.sdu_fragment_size().Read();
size_t complete_sdu_length = assembly_buffer_view.iso_sdu_length().Read();

// Verify that the total amount of SDU data received does not exceed that
// specified in the header from the FIRST_FRAGMENT.
if (total_sdu_bytes_received > complete_sdu_length) {
bt_log(ERROR,
"iso",
"Invalid data fragments received, exceed total SDU length - "
"discarding");
assembly_buffer_.clear();
return false;
}

bool is_last_fragment = fragment_view.header().pb_flag().Read() ==
pw::bluetooth::emboss::IsoDataPbFlag::LAST_FRAGMENT;
if (is_last_fragment && (total_sdu_bytes_received < complete_sdu_length)) {
bt_log(ERROR,
"iso",
"Insufficient data fragments received (%zu bytes received, expected "
"%zu) - discarding",
total_sdu_bytes_received,
complete_sdu_length);
assembly_buffer_.clear();
return false;
}

// Append the SDU data
assembly_buffer_.insert(
assembly_buffer_.end(),
fragment_view.iso_sdu_fragment().BackingStorage().begin(),
fragment_view.iso_sdu_fragment().BackingStorage().end());

// Update the header fields so they are as consistent as possible
assembly_buffer_view.header().data_total_length().Write(
assembly_buffer_.size() - assembly_buffer_view.hdr_size().Read());
if (is_last_fragment) {
assembly_buffer_view.header().pb_flag().Write(
pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU);
}
return true;
}

} // namespace bt::iso
191 changes: 189 additions & 2 deletions pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,13 +23,21 @@

namespace bt::iso {

constexpr uint16_t kDefaultConnectionHandle = 111;
constexpr uint16_t kDefaultPacketSequenceNumber = 456;

class IsoInboundPacketAssemblerTest : public ::testing::Test {
public:
IsoInboundPacketAssembler* assembler() { return &assembler_; }
std::queue<std::vector<std::byte>>* outgoing_packets() {
return &outgoing_packets_;
}

protected:
bool TestFragmentedSdu(
const std::vector<size_t>& fragment_sizes,
std::optional<size_t> complete_sdu_size = std::nullopt);

private:
void HandleCompletePacket(const pw::span<const std::byte>& packet) {
std::vector<std::byte> new_packet(packet.size());
Expand All @@ -54,10 +62,10 @@ TEST_F(IsoInboundPacketAssemblerTest, CompleteSdu) {
std::unique_ptr<std::vector<uint8_t>> sdu_data =
testing::GenDataBlob(sdu_fragment_size, /*starting_value=*/42);
DynamicByteBuffer incoming_packet = testing::IsoDataPacket(
/*connection_handle=*/123,
kDefaultConnectionHandle,
pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU,
/*time_stamp=*/std::nullopt,
/*packet_sequence_number=*/456,
kDefaultPacketSequenceNumber,
/*iso_sdu_length=*/sdu_fragment_size,
pw::bluetooth::emboss::IsoDataPacketStatus::VALID_DATA,
*sdu_data);
Expand All @@ -74,4 +82,183 @@ TEST_F(IsoInboundPacketAssemblerTest, CompleteSdu) {
}
}

bool IsoInboundPacketAssemblerTest::TestFragmentedSdu(
const std::vector<size_t>& fragment_sizes,
std::optional<size_t> complete_sdu_size) {
size_t initial_frames_received = outgoing_packets()->size();

// By default the total SDU size will be the sum of all fragment sizes
if (!complete_sdu_size.has_value()) {
complete_sdu_size = 0;
for (size_t fragment_size : fragment_sizes) {
(*complete_sdu_size) += fragment_size;
}
}

std::unique_ptr<std::vector<uint8_t>> sdu_data =
testing::GenDataBlob(*complete_sdu_size, /*starting_value=*/76);
std::vector<DynamicByteBuffer> iso_data_fragment_packets =
testing::IsoDataFragments(
kDefaultConnectionHandle,
/*time_stamp=*/std::nullopt,
kDefaultPacketSequenceNumber,
pw::bluetooth::emboss::IsoDataPacketStatus::VALID_DATA,
*sdu_data,
fragment_sizes);
for (size_t frames_sent = 0; frames_sent < fragment_sizes.size();
frames_sent++) {
// We should not receive any packets until all of the fragments have been
// sent
if (initial_frames_received != outgoing_packets()->size()) {
return false;
}
pw::span<const std::byte> frame_as_span =
iso_data_fragment_packets[frames_sent].subspan();
assembler()->ProcessNext(frame_as_span);
}
if (outgoing_packets()->size() != (initial_frames_received + 1)) {
return false;
}

// The output should look the same as if we had constructed a non-fragmented
// packet ourselves. Verify that.
DynamicByteBuffer expected_output = testing::IsoDataPacket(
kDefaultConnectionHandle,
pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU,
/*time_stamp=*/std::nullopt,
kDefaultPacketSequenceNumber,
*complete_sdu_size,
pw::bluetooth::emboss::IsoDataPacketStatus::VALID_DATA,
*sdu_data);
pw::span<const std::byte> expected_output_as_span = expected_output.subspan();
return std::equal(expected_output_as_span.begin(),
expected_output_as_span.end(),
outgoing_packets()->back().begin(),
outgoing_packets()->back().end());
}

// FIRST_FRAGMENT + LAST_FRAGMENT
TEST_F(IsoInboundPacketAssemblerTest, TwoSduFragments) {
std::vector<size_t> fragment_sizes = {100, 125};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes));
}

TEST_F(IsoInboundPacketAssemblerTest, OneIntermediateSduFragment) {
std::vector<size_t> fragment_sizes = {100, 125, 150};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes));
}

TEST_F(IsoInboundPacketAssemblerTest, MultipleIntermediateSduFragments) {
std::vector<size_t> fragment_sizes = {100, 125, 250, 500, 25};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes));
}

TEST_F(IsoInboundPacketAssemblerTest, MultipleTinySduFragments) {
std::vector<size_t> fragment_sizes = {1, 1, 1, 1, 1, 1, 1, 1};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes));
}

// LAST_FRAGMENT takes us over the total SDU size -- packet should be discarded
TEST_F(IsoInboundPacketAssemblerTest, LastFragmentSduTooLong) {
std::vector<size_t> fragment_sizes = {100, 125, 250, 500, 26};
ASSERT_FALSE(TestFragmentedSdu(fragment_sizes, 1000));
EXPECT_EQ(outgoing_packets()->size(), 0u);
}

// INTERMEDIATE_FRAGMENT takes us over the total SDU size -- packet should be
// discarded
TEST_F(IsoInboundPacketAssemblerTest, IntermediateFragmentSduTooLong) {
std::vector<size_t> fragment_sizes = {100, 125, 250, 526, 100};
ASSERT_FALSE(TestFragmentedSdu(fragment_sizes, 1000));
EXPECT_EQ(outgoing_packets()->size(), 0u);
}

TEST_F(IsoInboundPacketAssemblerTest,
NextSduReceivedBeforePreviousOneComplete) {
constexpr uint16_t kCompleteSduSize = 375;
std::vector<size_t> fragment_sizes = {125, 125, 125};

std::unique_ptr<std::vector<uint8_t>> sdu_data =
testing::GenDataBlob(kCompleteSduSize, /*starting_value=*/202);
std::vector<DynamicByteBuffer> iso_data_fragment_packets =
testing::IsoDataFragments(
kDefaultConnectionHandle,
/*time_stamp=*/std::nullopt,
kDefaultPacketSequenceNumber,
pw::bluetooth::emboss::IsoDataPacketStatus::VALID_DATA,
*sdu_data,
fragment_sizes);
// Send all but the last frame
for (size_t frames_sent = 0; frames_sent < (fragment_sizes.size() - 1);
frames_sent++) {
// We should not receive any packets until all of the fragments have been
// sent
ASSERT_EQ(outgoing_packets()->size(), 0u);

pw::span<const std::byte> frame_as_span =
iso_data_fragment_packets[frames_sent].subspan();
assembler()->ProcessNext(frame_as_span);
}
ASSERT_EQ(outgoing_packets()->size(), 0u);

// We should be able to send follow-up SDUs and the partially-received
// one will have been dropped.
std::vector<size_t> fragment_sizes_2 = {100, 125, 250, 500, 25};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_2));
std::vector<size_t> fragment_sizes_3 = {443};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_3));
}

TEST_F(IsoInboundPacketAssemblerTest, UnexpectedIntermediateFragmentReceived) {
std::unique_ptr<std::vector<uint8_t>> sdu_data =
testing::GenDataBlob(/*size=*/100, /*starting_value=*/99);
DynamicByteBuffer incoming_packet = testing::IsoDataPacket(
kDefaultConnectionHandle,
pw::bluetooth::emboss::IsoDataPbFlag::INTERMEDIATE_FRAGMENT,
/*time_stamp=*/std::nullopt,
/*packet_sequence_number=*/std::nullopt,
/*iso_sdu_length=*/std::nullopt,
/*packet_status_flag=*/std::nullopt,
*sdu_data);
ASSERT_EQ(outgoing_packets()->size(), 0u);
pw::span<const std::byte> frame_as_span = incoming_packet.subspan();
assembler()->ProcessNext(frame_as_span);

// Nothing passed through
ASSERT_EQ(outgoing_packets()->size(), 0u);

// We should be able to send follow-up SDUs and the incomplete fragment will
// have been dropped.
std::vector<size_t> fragment_sizes_2 = {100, 125, 250, 500, 25};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_2));
std::vector<size_t> fragment_sizes_3 = {443};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_3));
}

TEST_F(IsoInboundPacketAssemblerTest, UnexpectedLastFragmentReceived) {
std::unique_ptr<std::vector<uint8_t>> sdu_data =
testing::GenDataBlob(/*size=*/100, /*starting_value=*/99);
DynamicByteBuffer incoming_packet = testing::IsoDataPacket(
kDefaultConnectionHandle,
pw::bluetooth::emboss::IsoDataPbFlag::LAST_FRAGMENT,
/*time_stamp=*/std::nullopt,
/*packet_sequence_number=*/std::nullopt,
/*iso_sdu_length=*/std::nullopt,
/*packet_status_flag=*/std::nullopt,
*sdu_data);
ASSERT_EQ(outgoing_packets()->size(), 0u);
pw::span<const std::byte> frame_as_span = incoming_packet.subspan();
assembler()->ProcessNext(frame_as_span);

// Nothing passed through
ASSERT_EQ(outgoing_packets()->size(), 0u);

// We should be able to send follow-up SDUs and the incomplete fragment will
// have been dropped.
std::vector<size_t> fragment_sizes_2 = {100, 125, 250, 500, 25};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_2));
std::vector<size_t> fragment_sizes_3 = {443};
ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_3));
}

} // namespace bt::iso
24 changes: 24 additions & 0 deletions pw_bluetooth_sapphire/host/iso/iso_stream.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,6 +333,30 @@ void IsoStreamImpl::SetupDataPath(
}

void IsoStreamImpl::ReceiveInboundPacket(pw::span<const std::byte> packet) {
size_t packet_size = packet.size();
auto packet_view = pw::bluetooth::emboss::MakeIsoDataFramePacketView(
packet.data(), packet.size());
if (!packet_view.Ok()) {
bt_log(ERROR,
"iso",
"Incoming ISO frame failed consistency checks - ignoring");
return;
}

size_t data_total_length = packet_view.header().data_total_length().Read();
size_t header_size =
pw::bluetooth::emboss::IsoDataFrameHeaderView::SizeInBytes();
size_t packet_actual_size = data_total_length + header_size;

// This condition should have been caught by Emboss
BT_ASSERT_MSG(packet_size >= packet_actual_size,
"Packet too short to hold data specified in header");

// Truncate any extra data at end of packet
if (packet_size > packet_actual_size) {
packet = packet.subspan(0, packet_actual_size);
}

inbound_assembler_.ProcessNext(packet);
}

Expand Down
Loading

0 comments on commit 2db41fe

Please sign in to comment.