From fcd372af5815015bb54f5838e94673ad7a0d63b9 Mon Sep 17 00:00:00 2001
From: Boris Zbarsky <bzbarsky@apple.com>
Date: Thu, 20 Oct 2022 17:17:26 -0400
Subject: [PATCH] Add a private method on SystemPacketBuffer to find where its
 reserved space starts.

Fixes https://github.com/project-chip/connectedhomeip/issues/5372
---
 src/system/SystemPacketBuffer.cpp | 40 ++++++++++++++++++-------------
 src/system/SystemPacketBuffer.h   |  8 +++++++
 2 files changed, 31 insertions(+), 17 deletions(-)

diff --git a/src/system/SystemPacketBuffer.cpp b/src/system/SystemPacketBuffer.cpp
index f47a54278433e2..e80ddd70b746ce 100644
--- a/src/system/SystemPacketBuffer.cpp
+++ b/src/system/SystemPacketBuffer.cpp
@@ -133,9 +133,9 @@ void PacketBufferHandle::InternalRightSize()
     }
 
     // Reallocate only if enough space will be saved.
-    uint8_t * const start   = reinterpret_cast<uint8_t *>(mBuffer) + PacketBuffer::kStructureSize;
-    uint8_t * const payload = reinterpret_cast<uint8_t *>(mBuffer->payload);
-    const uint16_t usedSize = static_cast<uint16_t>(payload - start + mBuffer->len);
+    const uint8_t * const start   = mBuffer->ReserveStart();
+    const uint8_t * const payload = mBuffer->Start();
+    const uint16_t usedSize       = static_cast<uint16_t>(payload - start + mBuffer->len);
     if (usedSize + kRightSizingThreshold > mBuffer->alloc_size)
     {
         return;
@@ -149,14 +149,14 @@ void PacketBufferHandle::InternalRightSize()
         return;
     }
 
-    uint8_t * const newStart = reinterpret_cast<uint8_t *>(newBuffer) + PacketBuffer::kStructureSize;
+    uint8_t * const newStart = newBuffer->ReserveStart();
     newBuffer->next          = nullptr;
     newBuffer->payload       = newStart + (payload - start);
     newBuffer->tot_len       = mBuffer->tot_len;
     newBuffer->len           = mBuffer->len;
     newBuffer->ref           = 1;
     newBuffer->alloc_size    = static_cast<uint16_t>(usedSize);
-    memcpy(reinterpret_cast<uint8_t *>(newBuffer) + PacketBuffer::kStructureSize, start, usedSize);
+    memcpy(newStart, start, usedSize);
 
     PacketBuffer::Free(mBuffer);
     mBuffer = newBuffer;
@@ -193,7 +193,7 @@ void PacketBufferHandle::InternalRightSize()
 
 void PacketBuffer::SetStart(uint8_t * aNewStart)
 {
-    uint8_t * const kStart = reinterpret_cast<uint8_t *>(this) + kStructureSize;
+    uint8_t * const kStart = ReserveStart();
     uint8_t * const kEnd   = kStart + this->AllocSize();
 
     if (aNewStart < kStart)
@@ -236,9 +236,7 @@ void PacketBuffer::SetDataLength(uint16_t aNewLen, PacketBuffer * aChainHead)
 
 uint16_t PacketBuffer::MaxDataLength() const
 {
-    const uint8_t * const kStart = reinterpret_cast<const uint8_t *>(this) + kStructureSize;
-    const ptrdiff_t kDelta       = static_cast<uint8_t *>(this->payload) - kStart;
-    return static_cast<uint16_t>(this->AllocSize() - kDelta);
+    return static_cast<uint16_t>(AllocSize() - ReservedSize());
 }
 
 uint16_t PacketBuffer::AvailableDataLength() const
@@ -248,10 +246,19 @@ uint16_t PacketBuffer::AvailableDataLength() const
 
 uint16_t PacketBuffer::ReservedSize() const
 {
-    // Cast to size_t is safe because this->payload always points to "after"
-    // this.
-    const size_t kDelta = static_cast<size_t>(static_cast<uint8_t *>(this->payload) - reinterpret_cast<const uint8_t *>(this));
-    return static_cast<uint16_t>(kDelta - kStructureSize);
+    // Cast to uint16_t is safe because Start() always points to "after"
+    // ReserveStart().  At least when the payload is stored inline.
+    return static_cast<uint16_t>(Start() - ReserveStart());
+}
+
+uint8_t * PacketBuffer::ReserveStart()
+{
+    return reinterpret_cast<uint8_t *>(this) + kStructureSize;
+}
+
+const uint8_t * PacketBuffer::ReserveStart() const
+{
+    return reinterpret_cast<const uint8_t *>(this) + kStructureSize;
 }
 
 void PacketBuffer::AddToEnd(PacketBufferHandle && aPacketHandle)
@@ -282,7 +289,7 @@ void PacketBuffer::AddToEnd(PacketBufferHandle && aPacketHandle)
 
 void PacketBuffer::CompactHead()
 {
-    uint8_t * const kStart = reinterpret_cast<uint8_t *>(this) + kStructureSize;
+    uint8_t * const kStart = ReserveStart();
 
     if (this->payload != kStart)
     {
@@ -505,7 +512,7 @@ PacketBufferHandle PacketBufferHandle::New(size_t aAvailableSize, uint16_t aRese
         return PacketBufferHandle();
     }
 
-    lPacket->payload = reinterpret_cast<uint8_t *>(lPacket) + PacketBuffer::kStructureSize + aReservedSize;
+    lPacket->payload = lPacket->ReserveStart() + aReservedSize;
     lPacket->len = lPacket->tot_len = 0;
     lPacket->next                   = nullptr;
     lPacket->ref                    = 1;
@@ -669,8 +676,7 @@ PacketBufferHandle PacketBufferHandle::CloneData() const
             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);
+        memcpy(clone->ReserveStart(), original->ReserveStart(), originalDataSize + originalReservedSize);
 
         if (cloneHead.IsNull())
         {
diff --git a/src/system/SystemPacketBuffer.h b/src/system/SystemPacketBuffer.h
index c61cbcd89bf119..2e8b3635b823d0 100644
--- a/src/system/SystemPacketBuffer.h
+++ b/src/system/SystemPacketBuffer.h
@@ -383,6 +383,14 @@ class DLL_EXPORT PacketBuffer : private pbuf
     void Clear();
     void SetDataLength(uint16_t aNewLen, PacketBuffer * aChainHead);
 
+    /**
+     * Get a pointer to the start of the reserved space (which comes before the
+     * payload).  The actual reserved space is the ReservedSize() bytes starting
+     * at this pointer.
+     */
+    uint8_t * ReserveStart();
+    const uint8_t * ReserveStart() const;
+
     friend class PacketBufferHandle;
     friend class ::PacketBufferTest;
 };