message.hpp

#pragma once
 
#include <atomic>
#include <cstdint>
 
#include "crc.hpp"
#include "libxr_cb.hpp"
#include "libxr_def.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
{
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;       
    bool cache;         
    bool check_length;  
  };
#ifndef __DOXYGEN__
 
#pragma pack(push, 1)
  struct PackedDataHeader
  {
    uint8_t prefix;  
    uint32_t
        topic_name_crc32;  
    uint8_t data_len_raw[3];   
    uint8_t pack_header_crc8;  
 
    void SetDataLen(uint32_t len);
 
    uint32_t GetDataLen() const;
  };
#pragma pack(pop)
#pragma pack(push, 1)
  template <typename Data>
  class PackedData
  {
   public:
#pragma pack(push, 1)
    struct
    {
      PackedDataHeader header_;     
      uint8_t data_[sizeof(Data)];  
    } raw;
 
    uint8_t crc8_;  
 
#pragma pack(pop)
 
    PackedData &operator=(const Data &data)
    {
      memcpy(raw.data_, &data, sizeof(Data));
      crc8_ = CRC8::Calculate(&raw, sizeof(raw));
      return *this;
    }
 
    operator Data() { return *reinterpret_cast<Data *>(raw.data_); }
 
    Data *operator->() { return reinterpret_cast<Data *>(raw.data_); }
 
    const Data *operator->() const { return reinterpret_cast<const Data *>(raw.data_); }
  };
#pragma pack(pop)
 
  static constexpr size_t PACK_BASE_SIZE = sizeof(PackedData<uint8_t>) - 1;
 
#endif
 
  typedef RBTree<uint32_t>::Node<Block> *TopicHandle;
 
  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
  {
    SYNC,      
    ASYNC,     
    QUEUE,     
    CALLBACK,  
  };
 
  struct SuberBlock
  {
    SuberType type;  
  };
 
  struct SyncBlock : public SuberBlock
  {
    RawData buff;   
    Semaphore sem;  
  };
 
  template <typename Data>
  class SyncSubscriber
  {
   public:
    SyncSubscriber(const char *name, Data &data, Domain *domain = nullptr)
    {
      *this = SyncSubscriber<Data>(WaitTopic(name, UINT32_MAX, domain), data);
    }
 
    SyncSubscriber(Topic topic, Data &data)
    {
      if (topic.block_->data_.check_length)
      {
        ASSERT(topic.block_->data_.max_length == sizeof(Data));
      }
      else
      {
        ASSERT(topic.block_->data_.max_length <= sizeof(Data));
      }
 
      block_ = new LockFreeList::Node<SyncBlock>;
      block_->data_.type = SuberType::SYNC;
      block_->data_.buff = RawData(data);
      topic.block_->data_.subers.Add(*block_);
    }
 
    ErrorCode Wait(uint32_t timeout = UINT32_MAX)
    {
      // TODO: Reset sem
      return block_->data_.sem.Wait(timeout);
    }
 
    LockFreeList::Node<SyncBlock> *block_;  
  };
 
  enum class ASyncSubscriberState : uint32_t
  {
    IDLE = 0,
    WAITING = 1,
    DATA_READY = UINT32_MAX
  };
 
  typedef struct ASyncBlock : public SuberBlock
  {
    RawData buff;                             
    std::atomic<ASyncSubscriberState> state;  
  } ASyncBlock;
 
  template <typename Data>
  class ASyncSubscriber
  {
   public:
    ASyncSubscriber(const char *name, Domain *domain = nullptr)
    {
      *this = ASyncSubscriber(WaitTopic(name, UINT32_MAX, domain));
    }
 
    ASyncSubscriber(Topic topic)
    {
      if (topic.block_->data_.check_length)
      {
        ASSERT(topic.block_->data_.max_length == sizeof(Data));
      }
      else
      {
        ASSERT(topic.block_->data_.max_length <= sizeof(Data));
      }
 
      block_ = new LockFreeList::Node<ASyncBlock>;
      block_->data_.type = SuberType::ASYNC;
      block_->data_.buff = *(new Data);
      topic.block_->data_.subers.Add(*block_);
    }
 
    bool Available()
    {
      return block_->data_.state.load(std::memory_order_acquire) ==
             ASyncSubscriberState::DATA_READY;
    }
 
    Data &GetData()
    {
      block_->data_.state.store(ASyncSubscriberState::IDLE, std::memory_order_release);
      return *reinterpret_cast<Data *>(block_->data_.buff.addr_);
    }
 
    void StartWaiting()
    {
      block_->data_.state.store(ASyncSubscriberState::WAITING, std::memory_order_release);
    }
 
    LockFreeList::Node<ASyncBlock> *block_;  
  };
 
  typedef struct QueueBlock : public SuberBlock
  {
    void *queue;  
    void (*fun)(RawData &, void *,
                bool);  
  } QueueBlock;
 
  class QueuedSubscriber
  {
   public:
    template <typename Data, uint32_t Length>
    QueuedSubscriber(const char *name, LockFreeQueue<Data> &queue,
                     Domain *domain = nullptr)
    {
      *this = QueuedSubscriber(WaitTopic(name, UINT32_MAX, domain), queue);
    }
 
    template <typename Data>
    QueuedSubscriber(Topic topic, LockFreeQueue<Data> &queue)
    {
      if (topic.block_->data_.check_length)
      {
        ASSERT(topic.block_->data_.max_length == sizeof(Data));
      }
      else
      {
        ASSERT(topic.block_->data_.max_length <= sizeof(Data));
      }
 
      auto block = new LockFreeList::Node<QueueBlock>;
      block->data_.type = SuberType::QUEUE;
      block->data_.queue = &queue;
      block->data_.fun = [](RawData &data, void *arg, bool in_isr)
      {
        UNUSED(in_isr);
        LockFreeQueue<Data>* queue = reinterpret_cast<LockFreeQueue<Data>*>(arg);
        queue->Push(*reinterpret_cast<Data*>(data.addr_));
      };
 
      topic.block_->data_.subers.Add(*block);
    }
  };
 
  using Callback = LibXR::Callback<LibXR::RawData &>;
 
  typedef struct CallbackBlock : public SuberBlock
  {
    Callback cb;  
  } CallbackBlock;
 
  void RegisterCallback(Callback &cb);
 
  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 = true)
  {
    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 cache = false, bool check_length = true)
  {
    auto topic = Find(name, domain);
    if (topic == nullptr)
    {
      topic = CreateTopic<Data>(name, domain, cache, check_length).block_;
    }
    return topic;
  }
 
  void EnableCache();
 
  template <typename Data>
  void Publish(Data &data)
  {
    Publish(&data, sizeof(Data));
  }
 
  void Publish(void *addr, uint32_t size);
 
  template <typename Data>
  void PublishFromCallback(Data &data, bool in_isr)
  {
    PublishFromCallback(&data, sizeof(Data), in_isr);
  }
 
  void PublishFromCallback(void *addr, uint32_t size, bool in_isr);
 
  template <SizeLimitMode Mode = SizeLimitMode::MORE>
  ErrorCode DumpData(RawData data, bool pack = false)
  {
    if (block_->data_.data.addr_ == nullptr)
    {
      return ErrorCode::EMPTY;
    }
 
    if (!pack)
    {
      Assert::SizeLimitCheck<Mode>(block_->data_.data.size_, data.size_);
      Lock(block_);
      memcpy(data.addr_, block_->data_.data.addr_, block_->data_.data.size_);
      Unlock(block_);
    }
    else
    {
      Assert::SizeLimitCheck<Mode>(PACK_BASE_SIZE + block_->data_.data.size_, data.size_);
 
      Lock(block_);
      PackData(block_->data_.crc32, data, block_->data_.data);
      Unlock(block_);
    }
 
    return ErrorCode::OK;
  }
 
  static void PackData(uint32_t topic_name_crc32, RawData buffer, RawData source);
 
  template <typename Data>
  ErrorCode DumpData(PackedData<Data> &data)
  {
    if (block_->data_.data.addr_ == nullptr)
    {
      return ErrorCode::EMPTY;
    }
 
    ASSERT(sizeof(Data) == block_->data_.data.size_);
 
    return DumpData<SizeLimitMode::NONE>(RawData(data), true);
  }
 
  template <typename Data>
  ErrorCode DumpData(Data &data)
  {
    if (block_->data_.data.addr_ == nullptr)
    {
      return ErrorCode::EMPTY;
    }
 
    ASSERT(sizeof(Data) == block_->data_.data.size_);
 
    return DumpData<SizeLimitMode::NONE>(data, false);
  }
 
  static TopicHandle WaitTopic(const char *name, uint32_t timeout = UINT32_MAX,
                               Domain *domain = nullptr);
 
  operator TopicHandle() { return block_; }
 
  uint32_t GetKey() const;
 
  class Server
  {
   public:
    enum class Status : uint8_t
    {
      WAIT_START,    
      WAIT_TOPIC,    
      WAIT_DATA_CRC  
    };
 
    Server(size_t buffer_length);
 
    void Register(TopicHandle topic);
 
    size_t ParseData(ConstRawData data);
 
   private:
    Status status_ =
        Status::WAIT_START;  
    uint32_t data_len_ = 0;  
    RBTree<uint32_t> topic_map_;           
    BaseQueue queue_;                      
    RawData parse_buff_;                   
    TopicHandle current_topic_ = nullptr;  
  };
 
 private:
  TopicHandle block_ = nullptr;
 
  static inline RBTree<uint32_t> *domain_ = nullptr;
 
  static inline Domain *def_domain_ = nullptr;
};
}  // namespace LibXR