diff --git a/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler.cc b/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler.cc index 2689b7cdc..7b6793a41 100644 --- a/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler.cc +++ b/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler.cc @@ -23,24 +23,125 @@ namespace bt::iso { void IsoInboundPacketAssembler::ProcessNext(pw::span packet) { - const uint8_t* data = reinterpret_cast(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 assembly_buffer_span(assembly_buffer_.data(), + assembly_buffer_.size()); + complete_packet_handler_(assembly_buffer_span); + } +} + +bool IsoInboundPacketAssembler::AppendFragment( + pw::span 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 diff --git a/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler_test.cc b/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler_test.cc index d3e8a80c5..6be8bd65a 100644 --- a/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler_test.cc +++ b/pw_bluetooth_sapphire/host/iso/iso_inbound_packet_assembler_test.cc @@ -23,6 +23,9 @@ namespace bt::iso { +constexpr uint16_t kDefaultConnectionHandle = 111; +constexpr uint16_t kDefaultPacketSequenceNumber = 456; + class IsoInboundPacketAssemblerTest : public ::testing::Test { public: IsoInboundPacketAssembler* assembler() { return &assembler_; } @@ -30,6 +33,11 @@ class IsoInboundPacketAssemblerTest : public ::testing::Test { return &outgoing_packets_; } + protected: + bool TestFragmentedSdu( + const std::vector& fragment_sizes, + std::optional complete_sdu_size = std::nullopt); + private: void HandleCompletePacket(const pw::span& packet) { std::vector new_packet(packet.size()); @@ -54,10 +62,10 @@ TEST_F(IsoInboundPacketAssemblerTest, CompleteSdu) { std::unique_ptr> 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); @@ -74,4 +82,183 @@ TEST_F(IsoInboundPacketAssemblerTest, CompleteSdu) { } } +bool IsoInboundPacketAssemblerTest::TestFragmentedSdu( + const std::vector& fragment_sizes, + std::optional 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> sdu_data = + testing::GenDataBlob(*complete_sdu_size, /*starting_value=*/76); + std::vector 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 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 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 fragment_sizes = {100, 125}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes)); +} + +TEST_F(IsoInboundPacketAssemblerTest, OneIntermediateSduFragment) { + std::vector fragment_sizes = {100, 125, 150}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes)); +} + +TEST_F(IsoInboundPacketAssemblerTest, MultipleIntermediateSduFragments) { + std::vector fragment_sizes = {100, 125, 250, 500, 25}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes)); +} + +TEST_F(IsoInboundPacketAssemblerTest, MultipleTinySduFragments) { + std::vector 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 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 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 fragment_sizes = {125, 125, 125}; + + std::unique_ptr> sdu_data = + testing::GenDataBlob(kCompleteSduSize, /*starting_value=*/202); + std::vector 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 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 fragment_sizes_2 = {100, 125, 250, 500, 25}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_2)); + std::vector fragment_sizes_3 = {443}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_3)); +} + +TEST_F(IsoInboundPacketAssemblerTest, UnexpectedIntermediateFragmentReceived) { + std::unique_ptr> 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 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 fragment_sizes_2 = {100, 125, 250, 500, 25}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_2)); + std::vector fragment_sizes_3 = {443}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_3)); +} + +TEST_F(IsoInboundPacketAssemblerTest, UnexpectedLastFragmentReceived) { + std::unique_ptr> 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 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 fragment_sizes_2 = {100, 125, 250, 500, 25}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_2)); + std::vector fragment_sizes_3 = {443}; + ASSERT_TRUE(TestFragmentedSdu(fragment_sizes_3)); +} + } // namespace bt::iso diff --git a/pw_bluetooth_sapphire/host/iso/iso_stream.cc b/pw_bluetooth_sapphire/host/iso/iso_stream.cc index f052d36b0..77fdbfe65 100644 --- a/pw_bluetooth_sapphire/host/iso/iso_stream.cc +++ b/pw_bluetooth_sapphire/host/iso/iso_stream.cc @@ -333,6 +333,30 @@ void IsoStreamImpl::SetupDataPath( } void IsoStreamImpl::ReceiveInboundPacket(pw::span 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); } diff --git a/pw_bluetooth_sapphire/host/iso/iso_stream_test.cc b/pw_bluetooth_sapphire/host/iso/iso_stream_test.cc index 54c418f7a..eb2f8331d 100644 --- a/pw_bluetooth_sapphire/host/iso/iso_stream_test.cc +++ b/pw_bluetooth_sapphire/host/iso/iso_stream_test.cc @@ -438,4 +438,60 @@ TEST_F(IsoStreamTest, ReadRequestedAndThenRejected) { ASSERT_EQ(iso_stream()->ReadNextQueuedIncomingPacket(), nullptr); } +// Bad packets will not be passed on +TEST_F(IsoStreamTest, BadPacket) { + EstablishCis(pw::bluetooth::emboss::StatusCode::SUCCESS); + SetupDataPath( + pw::bluetooth::emboss::DataPathDirection::OUTPUT, + /*codec_configuration=*/std::nullopt, + /*cmd_complete_status=*/pw::bluetooth::emboss::StatusCode::SUCCESS, + iso::IsoStream::SetupDataPathError::kSuccess); + const size_t kIsoSduLength = 212; + std::unique_ptr> sdu_data = + testing::GenDataBlob(kIsoSduLength, /*starting_value=*/91); + DynamicByteBuffer packet0 = testing::IsoDataPacket( + /*connection_handle=*/iso_stream()->cis_handle(), + pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU, + /*time_stamp=*/std::nullopt, + /*packet_sequence_number=*/0, + kIsoSduLength, + pw::bluetooth::emboss::IsoDataPacketStatus::VALID_DATA, + *sdu_data); + // Improperly formatted packets are discarded. To test this, we'll remove the + // last byte of SDU data before we send it. + pw::span packet0_as_span = + packet0.subspan(0, packet0.size() - 1); + ASSERT_EQ(iso_stream()->ReadNextQueuedIncomingPacket(), nullptr); + iso_stream()->ReceiveInboundPacket(packet0_as_span); + ASSERT_EQ(complete_incoming_sdus()->size(), 0u); +} + +// Extra data at the end of the frame will be removed +TEST_F(IsoStreamTest, ExcessDataIsTruncated) { + EstablishCis(pw::bluetooth::emboss::StatusCode::SUCCESS); + SetupDataPath( + pw::bluetooth::emboss::DataPathDirection::OUTPUT, + /*codec_configuration=*/std::nullopt, + /*cmd_complete_status=*/pw::bluetooth::emboss::StatusCode::SUCCESS, + iso::IsoStream::SetupDataPathError::kSuccess); + const size_t kIsoSduLength = 212; + std::unique_ptr> sdu_data = + testing::GenDataBlob(kIsoSduLength, /*starting_value=*/91); + DynamicByteBuffer packet0 = testing::IsoDataPacket( + /*connection_handle=*/iso_stream()->cis_handle(), + pw::bluetooth::emboss::IsoDataPbFlag::COMPLETE_SDU, + /*time_stamp=*/std::nullopt, + /*packet_sequence_number=*/0, + kIsoSduLength, + pw::bluetooth::emboss::IsoDataPacketStatus::VALID_DATA, + *sdu_data); + size_t original_packet0_size = packet0.size(); + packet0.expand(packet0.size() + 100); + pw::span packet0_as_span = packet0.subspan(); + ASSERT_EQ(iso_stream()->ReadNextQueuedIncomingPacket(), nullptr); + iso_stream()->ReceiveInboundPacket(packet0_as_span); + ASSERT_EQ(complete_incoming_sdus()->size(), 1u); + EXPECT_EQ(complete_incoming_sdus()->front().size(), original_packet0_size); +} + } // namespace bt::iso diff --git a/pw_bluetooth_sapphire/host/iso/public/pw_bluetooth_sapphire/internal/host/iso/iso_inbound_packet_assembler.h b/pw_bluetooth_sapphire/host/iso/public/pw_bluetooth_sapphire/internal/host/iso/iso_inbound_packet_assembler.h index 6afd3e3f1..11bd5af16 100644 --- a/pw_bluetooth_sapphire/host/iso/public/pw_bluetooth_sapphire/internal/host/iso/iso_inbound_packet_assembler.h +++ b/pw_bluetooth_sapphire/host/iso/public/pw_bluetooth_sapphire/internal/host/iso/iso_inbound_packet_assembler.h @@ -14,6 +14,7 @@ #pragma once +#include "pw_bluetooth_sapphire/internal/host/iso/iso_common.h" #include "pw_function/function.h" #include "pw_span/span.h" @@ -29,8 +30,14 @@ class IsoInboundPacketAssembler { // full SDU, pass it along to the complete packet handler. void ProcessNext(pw::span packet); + // Add a fragment (either an INTERMEDIATE_FRAGMENT or a LAST_FRAGMENT) to + // assembly_buffer_ and update the buffer headers. + bool AppendFragment(pw::span packet); + private: PacketHandler complete_packet_handler_; + + IsoDataPacket assembly_buffer_; }; } // namespace bt::iso