path: root/pw_containers/variable_length_entry_queue.c

// 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;
}