1 files changed, 249 insertions, 0 deletions
diff --git a/pw_containers/variable_length_entry_queue.c b/pw_containers/variable_length_entry_queue.c
new file mode 100644
index 000000000..7089b6fe3
--- /dev/null
+++ b/pw_containers/variable_length_entry_queue.c
@@ -0,0 +1,249 @@
+// Copyright 2023 The Pigweed Authors
+//
+// 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
+//
+//     https://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 "pw_containers/variable_length_entry_queue.h"
+
+#include <string.h>
+
+#include "pw_assert/check.h"
+#include "pw_varint/varint.h"
+
+// Access the underlying buffer size and capacity (one less than size).
+static uint32_t BufferSize(const uint32_t* queue) { return queue[0]; }
+static uint32_t Capacity(const uint32_t* queue) {
+  return BufferSize(queue) - 1;
+}
+
+// Access the head and tail offsets.
+#define HEAD(queue) queue[1]
+#define TAIL(queue) queue[2]
+
+// Access the data. Strict aliasing rules do not apply to byte pointers.
+static uint8_t* WritableData(uint32_t* queue) { return (uint8_t*)&queue[3]; }
+static const uint8_t* Data(const uint32_t* queue) {
+  return (const uint8_t*)&queue[3];
+}
+
+static uint32_t WrapIndex(pw_VariableLengthEntryQueue_ConstHandle queue,
+                          uint32_t offset) {
+  if (offset >= BufferSize(queue)) {
+    offset -= BufferSize(queue);
+  }
+  return offset;
+}
+
+typedef struct {
+  uint32_t prefix;
+  uint32_t data;
+} EntrySize;
+
+// Returns the size of an entry, including both the prefix length and data size.
+static EntrySize ReadEntrySize(pw_VariableLengthEntryQueue_ConstHandle queue,
+                               uint32_t offset) {
+  EntrySize size = {0, 0};
+
+  bool keep_going;
+  do {
+    PW_DCHECK_UINT_NE(size.prefix, PW_VARINT_MAX_INT32_SIZE_BYTES);
+
+    keep_going = pw_varint_DecodeOneByte32(
+        Data(queue)[offset], size.prefix++, &size.data);
+    offset = WrapIndex(queue, offset + 1);
+  } while (keep_going);
+
+  return size;
+}
+
+static uint32_t EncodePrefix(pw_VariableLengthEntryQueue_ConstHandle queue,
+                             uint8_t prefix[PW_VARINT_MAX_INT32_SIZE_BYTES],
+                             uint32_t data_size_bytes) {
+  const uint32_t prefix_size = (uint32_t)pw_varint_Encode32(
+      data_size_bytes, prefix, PW_VARINT_MAX_INT32_SIZE_BYTES);
+
+  // Check that the ring buffer is capable of holding entries of this size.
+  PW_CHECK_UINT_LE(prefix_size + data_size_bytes,
+                   Capacity(queue),
+                   "Entry is too large for this VariableLengthEntryQueue");
+  return prefix_size;
+}
+
+// Returns the total encoded size of an entry.
+static uint32_t ReadEncodedEntrySize(
+    pw_VariableLengthEntryQueue_ConstHandle queue, uint32_t offset) {
+  const EntrySize entry_size = ReadEntrySize(queue, offset);
+  return entry_size.prefix + entry_size.data;
+}
+
+static uint32_t PopNonEmpty(pw_VariableLengthEntryQueue_Handle queue) {
+  const uint32_t entry_size = ReadEncodedEntrySize(queue, HEAD(queue));
+  HEAD(queue) = WrapIndex(queue, HEAD(queue) + entry_size);
+  return entry_size;
+}
+
+// Copies data to the buffer, wrapping around the end if needed.
+static uint32_t CopyAndWrap(pw_VariableLengthEntryQueue_Handle queue,
+                            uint32_t tail,
+                            const uint8_t* data,
+                            uint32_t size) {
+  // Copy the new data in one or two chunks. The first chunk is copied to the
+  // byte after the tail, the second from the beginning of the buffer. Both may
+  // be zero bytes.
+  uint32_t first_chunk = BufferSize(queue) - tail;
+  if (first_chunk >= size) {
+    first_chunk = size;
+  } else {  // Copy 2nd chunk from the beginning of the buffer (may be 0 bytes).
+    memcpy(WritableData(queue),
+           (const uint8_t*)data + first_chunk,
+           size - first_chunk);
+  }
+  memcpy(&WritableData(queue)[tail], data, first_chunk);
+  return WrapIndex(queue, tail + size);
+}
+
+static void AppendEntryKnownToFit(pw_VariableLengthEntryQueue_Handle queue,
+                                  const uint8_t* prefix,
+                                  uint32_t prefix_size,
+                                  const void* data,
+                                  uint32_t size) {
+  // Calculate the new tail offset. Don't update it until the copy is complete.
+  uint32_t tail = TAIL(queue);
+  tail = CopyAndWrap(queue, tail, prefix, prefix_size);
+  TAIL(queue) = CopyAndWrap(queue, tail, data, size);
+}
+
+static inline uint32_t AvailableBytes(
+    pw_VariableLengthEntryQueue_ConstHandle queue) {
+  uint32_t tail = TAIL(queue);
+  if (tail < HEAD(queue)) {
+    tail += BufferSize(queue);
+  }
+  return Capacity(queue) - (tail - HEAD(queue));
+}
+
+void pw_VariableLengthEntryQueue_Push(pw_VariableLengthEntryQueue_Handle queue,
+                                      const void* data,
+                                      const uint32_t data_size_bytes) {
+  uint8_t prefix[PW_VARINT_MAX_INT32_SIZE_BYTES];
+  uint32_t prefix_size = EncodePrefix(queue, prefix, data_size_bytes);
+
+  PW_CHECK(prefix_size + data_size_bytes <= AvailableBytes(queue),
+           "Insufficient remaining space for entry");
+
+  AppendEntryKnownToFit(queue, prefix, prefix_size, data, data_size_bytes);
+}
+
+void pw_VariableLengthEntryQueue_PushOverwrite(
+    pw_VariableLengthEntryQueue_Handle queue,
+    const void* data,
+    const uint32_t data_size_bytes) {
+  uint8_t prefix[PW_VARINT_MAX_INT32_SIZE_BYTES];
+  uint32_t prefix_size = EncodePrefix(queue, prefix, data_size_bytes);
+
+  uint32_t available_bytes = AvailableBytes(queue);
+  while (data_size_bytes + prefix_size > available_bytes) {
+    available_bytes += PopNonEmpty(queue);
+  }
+
+  AppendEntryKnownToFit(queue, prefix, prefix_size, data, data_size_bytes);
+}
+
+void pw_VariableLengthEntryQueue_Pop(pw_VariableLengthEntryQueue_Handle queue) {
+  PW_CHECK(!pw_VariableLengthEntryQueue_Empty(queue));
+  PopNonEmpty(queue);
+}
+
+void pw_VariableLengthEntryQueue_Iterator_Advance(
+    pw_VariableLengthEntryQueue_Iterator* iterator) {
+  iterator->_pw_offset = WrapIndex(
+      iterator->_pw_queue,
+      iterator->_pw_offset +
+          ReadEncodedEntrySize(iterator->_pw_queue, iterator->_pw_offset));
+}
+
+pw_VariableLengthEntryQueue_Entry pw_VariableLengthEntryQueue_GetEntry(
+    const pw_VariableLengthEntryQueue_Iterator* iterator) {
+  pw_VariableLengthEntryQueue_ConstHandle queue = iterator->_pw_queue;
+
+  pw_VariableLengthEntryQueue_Entry entry;
+  EntrySize size = ReadEntrySize(queue, iterator->_pw_offset);
+  uint32_t offset_1 = WrapIndex(queue, iterator->_pw_offset + size.prefix);
+
+  const uint32_t first_chunk = BufferSize(queue) - offset_1;
+
+  if (size.data <= first_chunk) {
+    entry.size_1 = size.data;
+    entry.size_2 = 0;
+  } else {
+    entry.size_1 = first_chunk;
+    entry.size_2 = size.data - first_chunk;
+  }
+
+  entry.data_1 = Data(queue) + offset_1;
+  entry.data_2 = Data(queue) + WrapIndex(queue, offset_1 + entry.size_1);
+  return entry;
+}
+
+uint32_t pw_VariableLengthEntryQueue_Entry_Copy(
+    const pw_VariableLengthEntryQueue_Entry* entry,
+    void* dest,
+    uint32_t count) {
+  PW_DCHECK(dest != NULL || count == 0u);
+
+  const uint32_t entry_size = entry->size_1 + entry->size_2;
+  const uint32_t to_copy = count < entry_size ? count : entry_size;
+
+  if (to_copy == 0u) {
+    return 0u;
+  }
+
+  memcpy(dest, entry->data_1, entry->size_1);
+
+  const uint32_t remaining = to_copy - entry->size_1;
+  if (remaining != 0u) {
+    memcpy((uint8_t*)dest + entry->size_1, entry->data_2, remaining);
+  }
+
+  return to_copy;
+}
+
+const uint8_t* _pw_VariableLengthEntryQueue_Entry_GetPointerChecked(
+    const pw_VariableLengthEntryQueue_Entry* entry, size_t index) {
+  PW_CHECK_UINT_LT(index, entry->size_1 + entry->size_2);
+  return _pw_VariableLengthEntryQueue_Entry_GetPointer(entry, index);
+}
+
+uint32_t pw_VariableLengthEntryQueue_Size(
+    pw_VariableLengthEntryQueue_ConstHandle queue) {
+  uint32_t entry_count = 0;
+  uint32_t offset = HEAD(queue);
+
+  while (offset != TAIL(queue)) {
+    offset = WrapIndex(queue, offset + ReadEncodedEntrySize(queue, offset));
+    entry_count += 1;
+  }
+  return entry_count;
+}
+
+uint32_t pw_VariableLengthEntryQueue_SizeBytes(
+    pw_VariableLengthEntryQueue_ConstHandle queue) {
+  uint32_t total_entry_size_bytes = 0;
+  uint32_t offset = HEAD(queue);
+
+  while (offset != TAIL(queue)) {
+    const EntrySize size = ReadEntrySize(queue, offset);
+    offset = WrapIndex(queue, offset + size.prefix + size.data);
+    total_entry_size_bytes += size.data;
+  }
+  return total_entry_size_bytes;
+}