message.hpp

#pragma once
 
#include <atomic>
#include <concepts>
#include <cstddef>
#include <cstdint>
#include <tuple>
#include <type_traits>
#include <utility>
 
#include "crc.hpp"
#include "libxr_cb.hpp"
#include "libxr_def.hpp"
#include "libxr_mem.hpp"
#include "libxr_time.hpp"
#include "libxr_type.hpp"
#include "lock_queue.hpp"
#include "lockfree_list.hpp"
#include "lockfree_queue.hpp"
#include "mutex.hpp"
#include "queue.hpp"
#include "rbt.hpp"
#include "semaphore.hpp"
#include "thread.hpp"
 
namespace LibXR
{
template <typename Data>
concept TopicPayload =
    !std::is_reference_v<Data> && !std::is_const_v<Data> && !std::is_volatile_v<Data> &&
    std::is_object_v<Data> && std::is_default_constructible_v<Data> &&
    std::is_copy_assignable_v<Data> && std::is_trivially_destructible_v<Data>;
 
template <typename Data>
constexpr void CheckTopicPayload()
{
  static_assert(TopicPayload<Data>,
                "LibXR::Topic typed payload must be a non-cv/ref object type that is "
                "default-constructible, copy-assignable, and trivially destructible.");
}
 
class Topic
{
  enum class LockState : uint32_t
  {
    UNLOCKED = 0,
    LOCKED = 1,
    USE_MUTEX = UINT32_MAX,
  };
 
 public:
  struct Block
  {
    std::atomic<LockState> busy;  
    LockFreeList subers;          
    uint32_t max_length;          
    uint32_t crc32;  
    Mutex* mutex;    
    RawData data;    
    MicrosecondTimestamp timestamp;  
    bool cache;         
    bool check_length;  
  };
 
#ifndef __DOXYGEN__
#pragma pack(push, 1)
  struct PackedDataHeader
  {
    uint8_t prefix;  
    uint8_t data_len_raw[3];  
    uint32_t
        topic_name_crc32;  
    uint8_t timestamp_us_raw[8];  
    uint8_t pack_header_crc8;     
 
    void SetDataLen(uint32_t len);
 
    uint32_t GetDataLen() const;
 
    void SetTimestamp(MicrosecondTimestamp timestamp);
 
    MicrosecondTimestamp GetTimestamp() const;
  };
#pragma pack(pop)
 
  template <typename Data>
  class PackedData;
 
  static constexpr uint8_t PACKET_PREFIX = 0x5A;
  static constexpr size_t PACK_BASE_SIZE = sizeof(PackedDataHeader) + sizeof(uint8_t);
  static_assert(sizeof(PackedDataHeader) == 17);
  static_assert(offsetof(PackedDataHeader, prefix) == 0);
  static_assert(offsetof(PackedDataHeader, data_len_raw) == 1);
  static_assert(offsetof(PackedDataHeader, topic_name_crc32) == 4);
  static_assert(offsetof(PackedDataHeader, timestamp_us_raw) == 8);
  static_assert(offsetof(PackedDataHeader, pack_header_crc8) == 16);
#endif
 
  typedef RBTree<uint32_t>::Node<Block>* TopicHandle;
 
  template <typename Data>
  struct MessageView
  {
    static_assert(TopicPayload<Data>);
 
    MicrosecondTimestamp timestamp;  
    Data& data;                      
  };
 
  template <typename Data>
  struct Message
  {
    static_assert(TopicPayload<Data>);
 
    MicrosecondTimestamp timestamp;  
    Data data;                       
  };
 
  static void Lock(TopicHandle topic);
 
  static void Unlock(TopicHandle topic);
 
  static void LockFromCallback(TopicHandle topic);
 
  static void UnlockFromCallback(TopicHandle topic);
 
  class Domain
  {
   public:
    Domain(const char* name);
 
    RBTree<uint32_t>::Node<RBTree<uint32_t>>* node_;
  };
 
  enum class SuberType : uint8_t;
  struct SuberBlock;
  struct SyncBlock;
  template <typename Data>
  class SyncSubscriber;
  enum class ASyncSubscriberState : uint32_t;
  struct ASyncBlock;
  template <typename Data>
  class ASyncSubscriber;
  struct QueueBlock;
  class QueuedSubscriber;
  class Callback;
  struct CallbackBlock;
 
  void RegisterCallback(Callback& cb);
 
  static MicrosecondTimestamp NowTimestamp();
 
  Topic();
 
  Topic(const char* name, uint32_t max_length, Domain* domain = nullptr,
        bool multi_publisher = false, bool cache = false, bool check_length = false);
 
  template <typename Data>
  static Topic CreateTopic(const char* name, Domain* domain = nullptr,
                           bool multi_publisher = false, bool cache = false,
                           bool check_length = false)
  {
    CheckTopicPayload<Data>();
    return Topic(name, sizeof(Data), domain, multi_publisher, cache, check_length);
  }
 
  Topic(TopicHandle topic);
 
  static TopicHandle Find(const char* name, Domain* domain = nullptr);
 
  template <typename Data>
  static TopicHandle FindOrCreate(const char* name, Domain* domain = nullptr,
                                  bool multi_publisher = false, bool cache = false,
                                  bool check_length = false)
  {
    CheckTopicPayload<Data>();
    auto topic = Find(name, domain);
    if (topic == nullptr)
    {
      topic =
          CreateTopic<Data>(name, domain, multi_publisher, cache, check_length).block_;
    }
    return topic;
  }
 
  void EnableCache();
 
  template <typename Data>
  void Publish(Data& data)
  {
    CheckTopicPayload<Data>();
    Publish(static_cast<void*>(&data), static_cast<uint32_t>(sizeof(Data)));
  }
 
  template <typename Data>
  void Publish(Data& data, MicrosecondTimestamp timestamp)
  {
    CheckTopicPayload<Data>();
    Publish(static_cast<void*>(&data), static_cast<uint32_t>(sizeof(Data)), timestamp);
  }
 
  void Publish(void* addr, uint32_t size);
 
  void Publish(void* addr, uint32_t size, MicrosecondTimestamp timestamp);
 
  template <typename Data>
  void PublishFromCallback(Data& data, bool in_isr)
  {
    CheckTopicPayload<Data>();
    PublishFromCallback(static_cast<void*>(&data), static_cast<uint32_t>(sizeof(Data)),
                        in_isr);
  }
 
  template <typename Data>
  void PublishFromCallback(Data& data, MicrosecondTimestamp timestamp, bool in_isr)
  {
    CheckTopicPayload<Data>();
    PublishFromCallback(static_cast<void*>(&data), static_cast<uint32_t>(sizeof(Data)),
                        timestamp, in_isr);
  }
 
  void PublishFromCallback(void* addr, uint32_t size, bool in_isr);
  void PublishFromCallback(void* addr, uint32_t size, MicrosecondTimestamp timestamp,
                           bool in_isr);
 
  static void PackData(uint32_t topic_name_crc32, RawData buffer,
                       MicrosecondTimestamp timestamp, ConstRawData data);
 
  template <typename Data>
  ErrorCode DumpData(PackedData<Data>& data);
 
  template <SizeLimitMode Mode = SizeLimitMode::MORE>
  ErrorCode DumpData(RawData data)
  {
    MicrosecondTimestamp timestamp;
    return DumpPayload<Mode>(data, timestamp);
  }
 
  template <SizeLimitMode Mode = SizeLimitMode::MORE>
  ErrorCode DumpData(RawData data, MicrosecondTimestamp& timestamp)
  {
    return DumpPayload<Mode>(data, timestamp);
  }
 
  template <typename Data>
    requires(!std::same_as<std::remove_cv_t<Data>, RawData>)
  ErrorCode DumpData(Data& data)
  {
    MicrosecondTimestamp timestamp;
    return DumpData(data, timestamp);
  }
 
  template <typename Data>
    requires(!std::same_as<std::remove_cv_t<Data>, RawData>)
  ErrorCode DumpData(Data& data, MicrosecondTimestamp& timestamp)
  {
    CheckTopicPayload<Data>();
    return DumpPayload<SizeLimitMode::LESS>(RawData(data), timestamp);
  }
 
  MicrosecondTimestamp GetTimestamp() const;
 
  static TopicHandle WaitTopic(const char* name, uint32_t timeout = UINT32_MAX,
                               Domain* domain = nullptr);
 
  operator TopicHandle() { return block_; }
 
  uint32_t GetKey() const;
 
  class Server;
 
 private:
  TopicHandle block_ = nullptr;
 
  static inline RBTree<uint32_t>* domain_ = nullptr;
 
  static inline Domain* def_domain_ = nullptr;
 
  static void EnsureDomainRegistry();
  static Domain* EnsureDefaultDomain();
 
  template <typename Data>
  static void CheckSubscriberDataSize(Topic topic)
  {
    CheckTopicPayload<Data>();
    if (topic.block_->data_.check_length)
    {
      ASSERT(topic.block_->data_.max_length == sizeof(Data));
    }
    else
    {
      ASSERT(topic.block_->data_.max_length <= sizeof(Data));
    }
  }
 
  template <typename Data>
  static RawData NewSubscriberBuffer()
  {
    CheckTopicPayload<Data>();
    auto* data = new Data;
    return RawData(*data);
  }
 
  template <SizeLimitMode Mode = SizeLimitMode::MORE>
  ErrorCode DumpPayload(RawData buffer, MicrosecondTimestamp& timestamp)
  {
    if (block_->data_.data.addr_ == nullptr)
    {
      return ErrorCode::EMPTY;
    }
 
    const size_t payload_size = block_->data_.data.size_;
    size_t copy_size = payload_size;
 
    if constexpr (Mode == SizeLimitMode::EQUAL)
    {
      if (payload_size != buffer.size_)
      {
        return ErrorCode::SIZE_ERR;
      }
    }
    else if constexpr (Mode == SizeLimitMode::LESS)
    {
      if (payload_size < buffer.size_)
      {
        return ErrorCode::SIZE_ERR;
      }
      copy_size = buffer.size_;
    }
    else if constexpr (Mode == SizeLimitMode::MORE)
    {
      if (payload_size > buffer.size_)
      {
        return ErrorCode::SIZE_ERR;
      }
    }
 
    Lock(block_);
    timestamp = block_->data_.timestamp;
    LibXR::Memory::FastCopy(buffer.addr_, block_->data_.data.addr_, copy_size);
    Unlock(block_);
 
    return ErrorCode::OK;
  }
 
  template <SizeLimitMode Mode = SizeLimitMode::MORE>
  ErrorCode DumpPacket(RawData buffer)
  {
    if (block_->data_.data.addr_ == nullptr)
    {
      return ErrorCode::EMPTY;
    }
 
    Assert::SizeLimitCheck<Mode>(PACK_BASE_SIZE + block_->data_.data.size_, buffer.size_);
    Lock(block_);
    PackData(block_->data_.crc32, buffer, block_->data_.timestamp, block_->data_.data);
    Unlock(block_);
 
    return ErrorCode::OK;
  }
 
  void PublishRaw(void* addr, uint32_t size, MicrosecondTimestamp timestamp,
                  bool from_callback, bool in_isr);
 
  static void CheckPublishSize(TopicHandle topic, uint32_t size);
  static RawData StorePublishedData(TopicHandle topic, void* addr, uint32_t size,
                                    MicrosecondTimestamp timestamp);
  static void DispatchSubscriber(SuberBlock& block, MicrosecondTimestamp timestamp,
                                 RawData data, bool from_callback, bool in_isr);
  static void DispatchSubscribers(TopicHandle topic, MicrosecondTimestamp timestamp,
                                  RawData data, bool from_callback, bool in_isr);
};
}  // namespace LibXR
 
#include "message/packet.hpp"
#include "message/server.hpp"
#include "message/subscriber.hpp"