BufferRTOS.h

#pragma once
#include "TinyRobotics/utils/Config.h"
#include "TinyRobotics/utils/Buffers.h"
#include "TinyRobotics/utils/LoggerClass.h"
#include "TinyRobotics/utils/AllocatorPSRAM.h"
 
#ifdef ESP32
#  include <freertos/stream_buffer.h>
#  include "freertos/FreeRTOS.h"
#elif defined(__linux__)
#else
#  include "FreeRTOS.h"
#  include "stream_buffer.h"
#endif
 
namespace tinyrobotics {
 
/**
 * @brief Buffer implementation which is using a FreeRTOS StreamBuffer. The
 * default allocator uses psram is available.
 * @ingroup concurrency
 * @author Phil Schatzmann
 *
 * @tparam T
 */
template <typename T, class TAllocator = AllocatorPSRAM<T>>
class BufferRTOS : public BaseBuffer<T> {
 public:
  BufferRTOS(size_t streamBufferSize, size_t xTriggerLevel = 1,
             TickType_t writeMaxWait = portMAX_DELAY,
             TickType_t readMaxWait = portMAX_DELAY)
      : BaseBuffer<T>() {
    readWait = readMaxWait;
    writeWait = writeMaxWait;
    current_size_bytes = (streamBufferSize+1)  * sizeof(T);
    trigger_level = xTriggerLevel;
 
    if (streamBufferSize > 0) {
      setup();
    }
  }
 
  ~BufferRTOS() { end(); }
 
  /// Re-Allocats the memory and the queue
  bool resize(size_t size) {
    bool result = true;
    int req_size_bytes = (size + 1)*sizeof(T);
    if (current_size_bytes != req_size_bytes) {
      end();
      current_size_bytes = req_size_bytes;
      result = setup();
    }
    return result;
  }
 
  void setReadMaxWait(TickType_t ticks) { readWait = ticks; }
 
  void setWriteMaxWait(TickType_t ticks) { writeWait = ticks; }
 
  void setWriteFromISR(bool active) { write_from_isr = active; }
 
  void setReadFromISR(bool active) { read_from_isr = active; }
 
  // reads a single value
  bool read(T &result) override {
    return readArray(&result, 1)==1;
  }
 
  // reads multiple values
  int readArray(T data[], int len) {
    if (read_from_isr) {
      xHigherPriorityTaskWoken = pdFALSE;
      int result = xStreamBufferReceiveFromISR(xStreamBuffer, (void *)data,
                                               sizeof(T) * len,
                                               &xHigherPriorityTaskWoken);
#ifdef ESP32X
      portYIELD_FROM_ISR();
#else
      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
#endif
      return result / sizeof(T);
    } else {
      return xStreamBufferReceive(xStreamBuffer, (void *)data, sizeof(T) * len,
                                  readWait) / sizeof(T);
    }
  }
 
  int writeArray(const T data[], int len) {
    // LOGD("%s: %d", LOG_METHOD, len);
    if (write_from_isr) {
      xHigherPriorityTaskWoken = pdFALSE;
      int result =
          xStreamBufferSendFromISR(xStreamBuffer, (void *)data, sizeof(T) * len,
                                   &xHigherPriorityTaskWoken);
#ifdef ESP32X
      portYIELD_FROM_ISR();
#else
      portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
#endif
      return result / sizeof(T);
    } else {
      return xStreamBufferSend(xStreamBuffer, (void *)data, sizeof(T) * len,
                               writeWait) / sizeof(T);
    }
  }
 
  // peeks the actual entry from the buffer
  bool peek(T &result) override {
    LOGE("peek not implemented");
    return false;
  }
 
  // checks if the buffer is full
  bool isFull() override {
    return xStreamBufferIsFull(xStreamBuffer) == pdTRUE;
  }
 
  bool isEmpty() { return xStreamBufferIsEmpty(xStreamBuffer) == pdTRUE; }
 
  // write add an entry to the buffer
  bool write(T data) override {
    int len = sizeof(T);
    return writeArray(&data, len) == len;
  }
 
  // clears the buffer
  void reset() override { xStreamBufferReset(xStreamBuffer); }
 
  // provides the number of entries that are available to read
  int available() override {
    return xStreamBufferBytesAvailable(xStreamBuffer) / sizeof(T);
  }
 
  // provides the number of entries that are available to write
  int availableForWrite() override {
    return xStreamBufferSpacesAvailable(xStreamBuffer) / sizeof(T);
  }
 
  // returns the address of the start of the physical read buffer
  T *address() override {
    LOGE("address() not implemented");
    return nullptr;
  }
 
  size_t size() { return current_size_bytes / sizeof(T); }
 
  operator bool() { return xStreamBuffer != nullptr && size()>0;}
 
  void end() {
    if (xStreamBuffer != nullptr) vStreamBufferDelete(xStreamBuffer);
    allocator.deallocate((T*)p_data, allocated_size);
    current_size_bytes = 0;
    p_data = nullptr;
    xStreamBuffer = nullptr;
  }
 
 protected:
  TAllocator allocator;
  StreamBufferHandle_t xStreamBuffer = nullptr;
  StaticStreamBuffer_t static_stream_buffer;
  uint8_t *p_data = nullptr;
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;  // Initialised to pdFALSE.
  int readWait = portMAX_DELAY;
  int writeWait = portMAX_DELAY;
  bool read_from_isr = false;
  bool write_from_isr = false;
  size_t current_size_bytes = 0;
  size_t trigger_level = 0;
  size_t allocated_size = 0;
 
  /// The allocation has been postponed to be done here, so that we can e.g. use
  /// psram
  bool setup() {
    if (current_size_bytes == 0) return true;
 
    // allocate data if necessary
    allocated_size = (current_size_bytes + 1) * sizeof(T);
    if (p_data == nullptr) {
      p_data = (uint8_t *)allocator.allocate(allocated_size);
      // check allocation
      if (p_data == nullptr) {
        LOGE("allocate falied for %d bytes", allocated_size);
        return false;
      }
    }
 
    // create stream buffer if necessary
    if (xStreamBuffer == nullptr) {
      xStreamBuffer = xStreamBufferCreateStatic(current_size_bytes, trigger_level,
                                                p_data, &static_stream_buffer);
    }
    if (xStreamBuffer == nullptr) {
      LOGE("xStreamBufferCreateStatic failed");
      return false;
    }
    // make sure that the data is empty
    reset();
    return true;
  }
 
};
 
/// @brief Template alias for RTOS-based synchronized buffer
/// @ingroup concurrency
template <class T, class TAllocator = AllocatorPSRAM<T>>
using SynchronizedBufferRTOS = BufferRTOS<T, TAllocator>;
 
}  // namespace tinyrobotics