message.hpp

#pragma once
 
#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
{
 public:
  struct Block
  {
    uint32_t max_length;  
    uint32_t crc32;       
    Mutex mutex;          
    RawData data;         
    bool cache;         
    bool check_length;  
    LockFreeList subers;  
  };
#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)
    {
      data_len_raw[0] = static_cast<uint8_t>(len >> 16);
      data_len_raw[1] = static_cast<uint8_t>(len >> 8);
      data_len_raw[2] = static_cast<uint8_t>(len);
    }
 
    uint32_t GetDataLen() const
    {
      return static_cast<uint32_t>(data_len_raw[0]) << 16 |
             static_cast<uint32_t>(data_len_raw[1]) << 8 |
             static_cast<uint32_t>(data_len_raw[2]);
    }
  };
#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;
 
  class Domain
  {
   public:
    Domain(const char *name)
    {
      if (!domain_)
      {
        if (!domain_)
        {
          domain_ =
              new RBTree<uint32_t>([](const uint32_t &a, const uint32_t &b)
                                   { return static_cast<int>(a) - static_cast<int>(b); });
        }
      }
 
      auto crc32 = CRC32::Calculate(name, strlen(name));
 
      auto domain = domain_->Search<RBTree<uint32_t>>(crc32);
 
      if (domain != nullptr)
      {
        node_ = domain;
        return;
      }
 
      node_ = new LibXR::RBTree<uint32_t>::Node<LibXR::RBTree<uint32_t>>(
          [](const uint32_t &a, const uint32_t &b)
          { return static_cast<int>(a) - static_cast<int>(b); });
 
      domain_->Insert(*node_, crc32);
    }
 
    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)
    {
      return block_->data_.sem.Wait(timeout);
    }
 
    LockFreeList::Node<SyncBlock> *block_;  
  };
 
  typedef struct ASyncBlock : public SuberBlock
  {
    RawData buff;     
    bool data_ready;  
    bool waiting;     
  } 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_.data_ready; }
 
    Data &GetData()
    {
      block_->data_.data_ready = false;
      return *reinterpret_cast<Data *>(block_->data_.buff.addr_);
    }
 
    void StartWaiting() { block_->data_.waiting = true; }
 
    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);
    }
 
    template <typename Data>
    QueuedSubscriber(const char *name, LockQueue<Data> &queue, Domain *domain = nullptr)
    {
      *this = QueuedSubscriber(WaitTopic(name, UINT32_MAX, domain), queue);
    }
 
    template <typename Data>
    QueuedSubscriber(Topic topic, LockQueue<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)
      {
        LockQueue<Data> *queue = reinterpret_cast<LockQueue<Data> *>(arg);
        queue->PushFromCallback(*reinterpret_cast<const Data *>(data.addr_), in_isr);
      };
 
      topic.block_->data_.subers.Add(*block);
    }
  };
 
  using Callback = LibXR::Callback<LibXR::RawData &>;
 
  typedef struct CallbackBlock : public SuberBlock
  {
    Callback cb;  
  } CallbackBlock;
 
  void RegisterCallback(Callback &cb)
  {
    CallbackBlock block;
    block.cb = cb;
    block.type = SuberType::CALLBACK;
    auto node = new (std::align_val_t(LIBXR_CACHE_LINE_SIZE))
        LockFreeList::Node<CallbackBlock>(block);
    block_->data_.subers.Add(*node);
  }
 
  Topic() {}
 
  Topic(const char *name, uint32_t max_length, Domain *domain = nullptr,
        bool cache = false, bool check_length = false)
  {
    if (!def_domain_)
    {
      if (!def_domain_)
      {
        def_domain_ = new Domain("libxr_def_domain");
      }
    }
 
    if (domain == nullptr)
    {
      domain = def_domain_;
    }
 
    auto crc32 = CRC32::Calculate(name, strlen(name));
 
    auto topic = domain->node_->data_.Search<Block>(crc32);
 
    if (topic)
    {
      ASSERT(topic->data_.max_length == max_length);
      ASSERT(topic->data_.check_length == check_length);
 
      block_ = topic;
    }
    else
    {
      block_ = new RBTree<uint32_t>::Node<Block>;
      block_->data_.max_length = max_length;
      block_->data_.crc32 = crc32;
      block_->data_.data.addr_ = nullptr;
      block_->data_.cache = false;
      block_->data_.check_length = check_length;
 
      domain->node_->data_.Insert(*block_, crc32);
    }
 
    if (cache && !block_->data_.cache)
    {
      EnableCache();
    }
  }
 
  template <typename Data>
  static Topic CreateTopic(const char *name, Domain *domain = nullptr, bool cache = false,
                           bool check_length = true)
  {
    return Topic(name, sizeof(Data), domain, cache, check_length);
  }
 
  Topic(TopicHandle topic) : block_(topic) {}
 
  static TopicHandle Find(const char *name, Domain *domain = nullptr)
  {
    if (domain == nullptr)
    {
      domain = def_domain_;
    }
 
    auto crc32 = CRC32::Calculate(name, strlen(name));
 
    return domain->node_->data_.Search<Block>(crc32);
  }
 
  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()
  {
    block_->data_.mutex.Lock();
    if (!block_->data_.cache)
    {
      block_->data_.cache = true;
      block_->data_.data.addr_ = new uint8_t[block_->data_.max_length];
    }
    block_->data_.mutex.Unlock();
  }
 
  template <typename Data>
  void Publish(Data &data)
  {
    Publish(&data, sizeof(Data));
  }
 
  void Publish(void *addr, uint32_t size)
  {
    block_->data_.mutex.Lock();
    if (block_->data_.check_length)
    {
      ASSERT(size == block_->data_.max_length);
    }
    else
    {
      ASSERT(size <= block_->data_.max_length);
    }
 
    if (block_->data_.cache)
    {
      memcpy(block_->data_.data.addr_, addr, size);
      block_->data_.data.size_ = size;
    }
    else
    {
      block_->data_.data.addr_ = addr;
      block_->data_.data.size_ = size;
    }
 
    RawData data = block_->data_.data;
 
    auto foreach_fun = [&](SuberBlock &block)
    {
      switch (block.type)
      {
        case SuberType::SYNC:
        {
          auto sync = reinterpret_cast<SyncBlock *>(&block);
          memcpy(sync->buff.addr_, data.addr_, data.size_);
          sync->sem.Post();
          break;
        }
        case SuberType::ASYNC:
        {
          auto async = reinterpret_cast<ASyncBlock *>(&block);
          if (async->waiting)
          {
            memcpy(async->buff.addr_, data.addr_, data.size_);
            async->data_ready = true;
          }
          break;
        }
        case SuberType::QUEUE:
        {
          auto queue_block = reinterpret_cast<QueueBlock *>(&block);
          queue_block->fun(data, queue_block->queue, false);
          break;
        }
        case SuberType::CALLBACK:
        {
          auto cb_block = reinterpret_cast<CallbackBlock *>(&block);
          cb_block->cb.Run(false, data);
          break;
        }
      }
      return ErrorCode::OK;
    };
 
    block_->data_.subers.Foreach<SuberBlock>(foreach_fun);
 
    block_->data_.mutex.Unlock();
  }
 
  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_);
      block_->data_.mutex.Lock();
      memcpy(data.addr_, block_->data_.data.addr_, block_->data_.data.size_);
      block_->data_.mutex.Unlock();
    }
    else
    {
      Assert::SizeLimitCheck<Mode>(PACK_BASE_SIZE + block_->data_.data.size_, data.size_);
 
      block_->data_.mutex.Lock();
      PackData(block_->data_.crc32, data, block_->data_.data);
      block_->data_.mutex.Unlock();
    }
 
    return ErrorCode::OK;
  }
 
  static void PackData(uint32_t topic_name_crc32, RawData buffer, RawData source)
  {
    PackedData<uint8_t> *pack = reinterpret_cast<PackedData<uint8_t> *>(buffer.addr_);
 
    memcpy(&pack->raw.data_, source.addr_, source.size_);
 
    pack->raw.header_.prefix = 0xa5;
    pack->raw.header_.topic_name_crc32 = topic_name_crc32;
    pack->raw.header_.SetDataLen(source.size_);
    pack->raw.header_.pack_header_crc8 =
        CRC8::Calculate(&pack->raw, sizeof(PackedDataHeader) - sizeof(uint8_t));
    uint8_t *crc8_pack =
        reinterpret_cast<uint8_t *>(reinterpret_cast<uint8_t *>(pack) + PACK_BASE_SIZE +
                                    source.size_ - sizeof(uint8_t));
    *crc8_pack = CRC8::Calculate(pack, PACK_BASE_SIZE - sizeof(uint8_t) + source.size_);
  }
 
  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)
  {
    TopicHandle topic = nullptr;
    do
    {
      topic = Find(name, domain);
      if (topic == nullptr)
      {
        if (timeout <= Thread::GetTime())
        {
          return nullptr;
        }
        Thread::Sleep(1);
      }
    } while (topic == nullptr);
 
    return topic;
  }
 
  operator TopicHandle() { return block_; }
 
  uint32_t GetKey() const
  {
    if (block_)
    {
      return block_->key;
    }
    else
    {
      return 0;
    }
  }
 
  class Server
  {
   public:
    enum class Status : uint8_t
    {
      WAIT_START,    
      WAIT_TOPIC,    
      WAIT_DATA_CRC  
    };
 
    Server(size_t buffer_length)
        : topic_map_([](const uint32_t &a, const uint32_t &b)
                     { return static_cast<int>(a) - static_cast<int>(b); }),
          queue_(1, buffer_length)
    {
      /* Minimum size: header8 + crc32 + length24 + crc8 + data +  crc8 = 10 */
      ASSERT(buffer_length > PACK_BASE_SIZE);
      parse_buff_.size_ = buffer_length;
      parse_buff_.addr_ = new uint8_t[buffer_length];
    }
 
    void Register(TopicHandle topic)
    {
      auto node = new RBTree<uint32_t>::Node<TopicHandle>(topic);
      topic_map_.Insert(*node, topic->key);
    }
 
    size_t ParseData(ConstRawData data)
    {
      size_t count = 0;
 
      queue_.PushBatch(data.addr_, data.size_);
 
      while (true)
      { /* 1. Check prefix */
        if (status_ == Status::WAIT_START)
        {
          /* Check start frame */
          auto queue_size = queue_.Size();
          for (uint32_t i = 0; i < queue_size; i++)
          {
            uint8_t prefix = 0;
            queue_.Peek(&prefix);
            if (prefix == 0xa5)
            {
              status_ = Status::WAIT_TOPIC;
              break;
            }
            queue_.Pop();
          }
          /* Not found */
          if (status_ == Status::WAIT_START)
          {
            return count;
          }
        }
 
        /* 2. Get topic info */
        if (status_ == Status::WAIT_TOPIC)
        {
          /* Check size&crc */
          if (queue_.Size() >= sizeof(PackedDataHeader))
          {
            queue_.PopBatch(parse_buff_.addr_, sizeof(PackedDataHeader));
            if (CRC8::Verify(parse_buff_.addr_, sizeof(PackedDataHeader)))
            {
              auto header = reinterpret_cast<PackedDataHeader *>(parse_buff_.addr_);
              /* Find topic */
              auto node = topic_map_.Search<TopicHandle>(header->topic_name_crc32);
              if (node)
              {
                data_len_ = header->GetDataLen();
                current_topic_ = *node;
                if (data_len_ + PACK_BASE_SIZE >= queue_.length_)
                {
                  status_ = Status::WAIT_START;
                  continue;
                }
                status_ = Status::WAIT_DATA_CRC;
              }
              else
              {
                status_ = Status::WAIT_START;
                continue;
              }
            }
            else
            {
              status_ = Status::WAIT_START;
              continue;
            }
          }
          else
          {
            return count;
          }
        }
 
        /* 3. Get data */
        if (status_ == Status::WAIT_DATA_CRC)
        {
          /* Check size&crc */
          if (queue_.Size() >= data_len_ + sizeof(uint8_t))
          {
            uint8_t *data =
                reinterpret_cast<uint8_t *>(parse_buff_.addr_) + sizeof(PackedDataHeader);
            queue_.PopBatch(data, data_len_ + sizeof(uint8_t));
            if (CRC8::Verify(parse_buff_.addr_,
                             data_len_ + sizeof(PackedDataHeader) + sizeof(uint8_t)))
            {
              status_ = Status::WAIT_START;
              auto data = reinterpret_cast<uint8_t *>(parse_buff_.addr_) +
                          sizeof(PackedDataHeader);
              if (data_len_ > current_topic_->data_.max_length)
              {
                data_len_ = current_topic_->data_.max_length;
              }
              Topic(current_topic_).Publish(data, data_len_);
 
              count++;
 
              continue;
            }
            else
            {
              status_ = Status::WAIT_START;
              continue;
            }
          }
          else
          {
            return count;
          }
        }
      }
      return count;
    }
 
   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