Support chained buffers in PacketBufferHandle::CloneData() (#5355)

* Support chained buffers in PacketBufferHandle::CloneData()

#### Problem

It's sometimes useful to clone an entire packet buffer chain (e.g. #5309).

(If cloning only a single buffer is necessary,
 `NewWithData(buffer->Start(), buffer->DataLength(), 0, buffer->ReservedSize())`
 will do the job.)

#### Summary of Changes

Support chained buffers in `PacketBufferHandle::CloneData()`.

Fixes #5348 - Support CloneData with chained buffers in PacketBufferHandle

* Restyled by clang-format

* review fix

Co-authored-by: Restyled.io <commits@restyled.io>

Author: kpschoedel

src/system/SystemPacketBuffer.cpp

@@ -605,14 +605,34 @@ PacketBufferHandle PacketBufferHandle::PopHead()
     return PacketBufferHandle(head);
 }
 
-PacketBufferHandle PacketBufferHandle::CloneData(uint16_t aAdditionalSize, uint16_t aReservedSize)
+PacketBufferHandle PacketBufferHandle::CloneData()
 {
-    if (!mBuffer->Next().IsNull())
+    PacketBufferHandle cloneHead;
+
+    for (PacketBuffer * original = mBuffer; original != nullptr; original = static_cast<PacketBuffer *>(original->next))
     {
-        // We do not clone an entire chain.
-        return PacketBufferHandle();
+        uint16_t originalDataSize     = original->MaxDataLength();
+        uint16_t originalReservedSize = original->ReservedSize();
+        PacketBufferHandle clone      = PacketBufferHandle::New(originalDataSize, originalReservedSize);
+        if (clone.IsNull())
+        {
+            return PacketBufferHandle();
+        }
+        clone.mBuffer->tot_len = clone.mBuffer->len = original->len;
+        memcpy(reinterpret_cast<uint8_t *>(clone.mBuffer) + PacketBuffer::kStructureSize,
+               reinterpret_cast<uint8_t *>(original) + PacketBuffer::kStructureSize, originalDataSize + originalReservedSize);
+
+        if (cloneHead.IsNull())
+        {
+            cloneHead = std::move(clone);
+        }
+        else
+        {
+            cloneHead->AddToEnd(std::move(clone));
+        }
     }
-    return NewWithData(mBuffer->Start(), mBuffer->DataLength(), aAdditionalSize, aReservedSize);
+
+    return cloneHead;
 }
 
 } // namespace System

src/system/SystemPacketBuffer.h

@@ -598,16 +598,11 @@ class DLL_EXPORT PacketBufferHandle
                                           uint16_t aReservedSize = PacketBuffer::kDefaultHeaderReserve);
 
     /**
-     * Creates a copy of the data in this packet.
+     * Creates a copy of a packet buffer (or chain).
      *
-     * Does NOT support chained buffers.
-     *
-     *  @param[in]  aAdditionalSize Size of additional application data space after the initial contents.
-     *  @param[in]  aReservedSize   Number of octets to reserve for protocol headers.
-     *
-     * @returns empty handle on allocation failure.
+     * @returns empty handle on allocation failure. Otherwise, the returned buffer has the same sizes and contents as the original.
      */
-    PacketBufferHandle CloneData(uint16_t aAdditionalSize = 0, uint16_t aReservedSize = PacketBuffer::kDefaultHeaderReserve);
+    PacketBufferHandle CloneData();
 
     /**
      * Perform an implementation-defined check on the validity of a PacketBufferHandle.

src/system/tests/TestSystemPacketBuffer.cpp

@@ -67,6 +67,14 @@ using ::chip::System::pbuf;
 #define TO_LWIP_PBUF(x) (reinterpret_cast<struct pbuf *>(reinterpret_cast<void *>(x)))
 #define OF_LWIP_PBUF(x) (reinterpret_cast<PacketBuffer *>(reinterpret_cast<void *>(x)))
 
+namespace {
+void ScrambleData(uint8_t * start, uint16_t length)
+{
+    for (uint16_t i = 0; i < length; ++i)
+        ++start[i];
+}
+} // namespace
+
 /*
  * An instance of this class created for the test suite.
  * It is a friend class of `PacketBuffer` and `PacketBufferHandle` because some tests
@@ -120,6 +128,7 @@ class PacketBufferTest
     static void CheckHandleHold(nlTestSuite * inSuite, void * inContext);
     static void CheckHandleAdvance(nlTestSuite * inSuite, void * inContext);
     static void CheckHandleRightSize(nlTestSuite * inSuite, void * inContext);
+    static void CheckHandleCloneData(nlTestSuite * inSuite, void * inContext);
     static void CheckPacketBufferWriter(nlTestSuite * inSuite, void * inContext);
     static void CheckBuildFreeList(nlTestSuite * inSuite, void * inContext);
 
@@ -1707,6 +1716,82 @@ void PacketBufferTest::CheckHandleRightSize(nlTestSuite * inSuite, void * inCont
 #endif // CHIP_SYSTEM_PACKETBUFFER_HAS_RIGHT_SIZE
 }
 
+void PacketBufferTest::CheckHandleCloneData(nlTestSuite * inSuite, void * inContext)
+{
+    struct TestContext * const theContext = static_cast<struct TestContext *>(inContext);
+    PacketBufferTest * const test         = theContext->test;
+    NL_TEST_ASSERT(inSuite, test->mContext == theContext);
+
+    uint8_t lPayload[2 * PacketBuffer::kMaxSizeWithoutReserve];
+    for (size_t i = 0; i < sizeof(lPayload); ++i)
+    {
+        lPayload[i] = static_cast<uint8_t>(random());
+    }
+
+    for (auto & config_1 : test->configurations)
+    {
+        for (auto & config_2 : test->configurations)
+        {
+            if (&config_1 == &config_2)
+            {
+                continue;
+            }
+
+            test->PrepareTestBuffer(&config_1);
+            test->PrepareTestBuffer(&config_2);
+
+            const uint8_t * payload_1 = lPayload;
+            memcpy(config_1.handle->Start(), payload_1, config_1.handle->MaxDataLength());
+            config_1.handle->SetDataLength(config_1.handle->MaxDataLength());
+
+            const uint8_t * payload_2 = lPayload + config_1.handle->MaxDataLength();
+            memcpy(config_2.handle->Start(), payload_2, config_2.handle->MaxDataLength());
+            config_2.handle->SetDataLength(config_2.handle->MaxDataLength());
+
+            // Clone single buffer.
+            PacketBufferHandle clone_1 = config_1.handle.CloneData();
+            NL_TEST_ASSERT(inSuite, !clone_1.IsNull());
+            NL_TEST_ASSERT(inSuite, clone_1->DataLength() == config_1.handle->DataLength());
+            NL_TEST_ASSERT(inSuite, memcmp(clone_1->Start(), payload_1, clone_1->DataLength()) == 0);
+            if (clone_1->DataLength())
+            {
+                // Verify that modifying the clone does not affect the original.
+                ScrambleData(clone_1->Start(), clone_1->DataLength());
+                NL_TEST_ASSERT(inSuite, memcmp(clone_1->Start(), payload_1, clone_1->DataLength()) != 0);
+                NL_TEST_ASSERT(inSuite, memcmp(config_1.handle->Start(), payload_1, config_1.handle->DataLength()) == 0);
+            }
+
+            // Clone buffer chain.
+            config_1.handle->AddToEnd(config_2.handle.Retain());
+            NL_TEST_ASSERT(inSuite, config_1.handle->HasChainedBuffer());
+            clone_1                         = config_1.handle.CloneData();
+            PacketBufferHandle clone_1_next = clone_1->Next();
+            NL_TEST_ASSERT(inSuite, !clone_1.IsNull());
+            NL_TEST_ASSERT(inSuite, clone_1->HasChainedBuffer());
+            NL_TEST_ASSERT(inSuite, clone_1->DataLength() == config_1.handle->DataLength());
+            NL_TEST_ASSERT(inSuite, clone_1->TotalLength() == config_1.handle->TotalLength());
+            NL_TEST_ASSERT(inSuite, clone_1_next->DataLength() == config_2.handle->DataLength());
+            NL_TEST_ASSERT(inSuite, memcmp(clone_1->Start(), payload_1, clone_1->DataLength()) == 0);
+            NL_TEST_ASSERT(inSuite, memcmp(clone_1_next->Start(), payload_2, clone_1_next->DataLength()) == 0);
+            if (clone_1->DataLength())
+            {
+                ScrambleData(clone_1->Start(), clone_1->DataLength());
+                NL_TEST_ASSERT(inSuite, memcmp(clone_1->Start(), payload_1, clone_1->DataLength()) != 0);
+                NL_TEST_ASSERT(inSuite, memcmp(config_1.handle->Start(), payload_1, config_1.handle->DataLength()) == 0);
+            }
+            if (clone_1_next->DataLength())
+            {
+                ScrambleData(clone_1_next->Start(), clone_1_next->DataLength());
+                NL_TEST_ASSERT(inSuite, memcmp(clone_1_next->Start(), payload_2, clone_1_next->DataLength()) != 0);
+                NL_TEST_ASSERT(inSuite, memcmp(config_2.handle->Start(), payload_2, config_2.handle->DataLength()) == 0);
+            }
+
+            config_1.handle = nullptr;
+            config_2.handle = nullptr;
+        }
+    }
+}
+
 void PacketBufferTest::CheckPacketBufferWriter(nlTestSuite * inSuite, void * inContext)
 {
     struct TestContext * const theContext = static_cast<struct TestContext *>(inContext);
@@ -1773,6 +1858,7 @@ const nlTest sTests[] =
     NL_TEST_DEF("PacketBuffer::HandleHold",             PacketBufferTest::CheckHandleHold),
     NL_TEST_DEF("PacketBuffer::HandleAdvance",          PacketBufferTest::CheckHandleAdvance),
     NL_TEST_DEF("PacketBuffer::HandleRightSize",        PacketBufferTest::CheckHandleRightSize),
+    NL_TEST_DEF("PacketBuffer::HandleCloneData",        PacketBufferTest::CheckHandleCloneData),
     NL_TEST_DEF("PacketBuffer::PacketBufferWriter",     PacketBufferTest::CheckPacketBufferWriter),
 
     NL_TEST_SENTINEL()