subscriber.hpp

#pragma once
 
#include "message.hpp"
 
namespace LibXR
{
enum class Topic::SuberType : uint8_t
{
  SYNC,      
  ASYNC,     
  QUEUE,     
  CALLBACK,  
};
 
struct Topic::SuberBlock
{
  SuberType type;  
};
 
struct Topic::SyncBlock : public SuberBlock
{
  // WAIT_CLAIMED keeps a wakeup owned by the timed-out waiter until it consumes the
  // semaphore post; otherwise a new Wait() could steal that post.
  // WAIT_CLAIMED 表示本次唤醒已经归当前 waiter 所有，直到它消费 semaphore post。
  enum WaitState : uint32_t
  {
    WAIT_IDLE = 0,
    WAITING = 1,
    WAIT_CLAIMED = 2
  };
 
  RawData buff;                    
  MicrosecondTimestamp timestamp;  
  std::atomic<uint32_t> wait_state = WAIT_IDLE;  
  Semaphore sem;  
};
 
template <typename Data>
class Topic::SyncSubscriber
{
 public:
  SyncSubscriber(const char* name, Data& data, Domain* domain = nullptr)
      : SyncSubscriber(Topic(WaitTopic(name, UINT32_MAX, domain)), data)
  {
  }
 
  SyncSubscriber(Topic topic, Data& data)
  {
    CheckSubscriberDataSize<Data>(topic);
 
    block_ = new LockFreeList::Node<SyncBlock>;
    block_->data_.type = SuberType::SYNC;
    block_->data_.timestamp = MicrosecondTimestamp();
    block_->data_.wait_state.store(SyncBlock::WAIT_IDLE, std::memory_order_relaxed);
    block_->data_.buff = RawData(data);
    topic.block_->data_.subers.Add(*block_);
  }
 
  SyncSubscriber(const SyncSubscriber&) = delete;
  SyncSubscriber& operator=(const SyncSubscriber&) = delete;
 
  SyncSubscriber(SyncSubscriber&& other) noexcept : block_(other.block_)
  {
    other.block_ = nullptr;
  }
 
  SyncSubscriber& operator=(SyncSubscriber&& other) noexcept
  {
    if (this != &other)
    {
      block_ = other.block_;
      other.block_ = nullptr;
    }
    return *this;
  }
 
  ErrorCode Wait(uint32_t timeout = UINT32_MAX)
  {
    ASSERT(block_ != nullptr);
 
    auto& data = block_->data_;
    uint32_t expected = SyncBlock::WAIT_IDLE;
    if (!data.wait_state.compare_exchange_strong(expected, SyncBlock::WAITING,
                                                 std::memory_order_acq_rel,
                                                 std::memory_order_acquire))
    {
      return ErrorCode::BUSY;
    }
 
    auto wait_ans = data.sem.Wait(timeout);
    if (wait_ans == ErrorCode::OK)
    {
      data.wait_state.store(SyncBlock::WAIT_IDLE, std::memory_order_release);
      return ErrorCode::OK;
    }
 
    expected = SyncBlock::WAITING;
    if (data.wait_state.compare_exchange_strong(expected, SyncBlock::WAIT_IDLE,
                                                std::memory_order_acq_rel,
                                                std::memory_order_acquire))
    {
      return wait_ans;
    }
 
    ASSERT(data.wait_state.load(std::memory_order_acquire) == SyncBlock::WAIT_CLAIMED);
 
    auto finish_wait_ans = data.sem.Wait(UINT32_MAX);
    UNUSED(finish_wait_ans);
    ASSERT(finish_wait_ans == ErrorCode::OK);
    data.wait_state.store(SyncBlock::WAIT_IDLE, std::memory_order_release);
    return ErrorCode::OK;
  }
 
  MicrosecondTimestamp GetTimestamp() const { return block_->data_.timestamp; }
 
  LockFreeList::Node<SyncBlock>* block_;  
};
 
enum class Topic::ASyncSubscriberState : uint32_t
{
  IDLE = 0,
  WAITING = 1,
  DATA_READY = UINT32_MAX
};
 
struct Topic::ASyncBlock : public Topic::SuberBlock
{
  RawData buff;                    
  MicrosecondTimestamp timestamp;  
  std::atomic<ASyncSubscriberState> state =
      ASyncSubscriberState::IDLE;  
};
 
template <typename Data>
class Topic::ASyncSubscriber
{
 public:
  ASyncSubscriber(const char* name, Domain* domain = nullptr)
      : ASyncSubscriber(Topic(WaitTopic(name, UINT32_MAX, domain)))
  {
  }
 
  ASyncSubscriber(Topic topic)
  {
    CheckSubscriberDataSize<Data>(topic);
 
    block_ = new LockFreeList::Node<ASyncBlock>;
    block_->data_.type = SuberType::ASYNC;
    block_->data_.timestamp = MicrosecondTimestamp();
    block_->data_.buff = NewSubscriberBuffer<Data>();
    topic.block_->data_.subers.Add(*block_);
  }
 
  ASyncSubscriber(const ASyncSubscriber&) = delete;
  ASyncSubscriber& operator=(const ASyncSubscriber&) = delete;
 
  ASyncSubscriber(ASyncSubscriber&& other) noexcept : block_(other.block_)
  {
    other.block_ = nullptr;
  }
 
  ASyncSubscriber& operator=(ASyncSubscriber&& other) noexcept
  {
    if (this != &other)
    {
      block_ = other.block_;
      other.block_ = nullptr;
    }
    return *this;
  }
 
  bool Available()
  {
    return block_->data_.state.load(std::memory_order_acquire) ==
           ASyncSubscriberState::DATA_READY;
  }
 
  Data& GetData()
  {
    if (block_->data_.state.load(std::memory_order_acquire) ==
        ASyncSubscriberState::DATA_READY)
    {
      block_->data_.state.store(ASyncSubscriberState::IDLE, std::memory_order_release);
    }
    return *reinterpret_cast<Data*>(block_->data_.buff.addr_);
  }
 
  MicrosecondTimestamp GetTimestamp() const { return block_->data_.timestamp; }
 
  void StartWaiting()
  {
    if (block_->data_.state.load(std::memory_order_acquire) ==
        ASyncSubscriberState::IDLE)
    {
      block_->data_.state.store(ASyncSubscriberState::WAITING,
                                std::memory_order_release);
    }
  }
 
  LockFreeList::Node<ASyncBlock>* block_;  
};
 
struct Topic::QueueBlock : public Topic::SuberBlock
{
  void* queue;  
  void (*fun)(MicrosecondTimestamp, RawData,
              QueueBlock&);  
};
 
class Topic::QueuedSubscriber
{
 public:
  template <typename Data>
  QueuedSubscriber(const char* name, LockFreeQueue<Data>& queue, Domain* domain = nullptr)
      : QueuedSubscriber(Topic(WaitTopic(name, UINT32_MAX, domain)), queue)
  {
  }
 
  template <typename Data>
  QueuedSubscriber(const char* name, LockFreeQueue<Message<Data>>& queue,
                   Domain* domain = nullptr)
      : QueuedSubscriber(Topic(WaitTopic(name, UINT32_MAX, domain)), queue)
  {
  }
 
  template <typename Data>
  QueuedSubscriber(Topic topic, LockFreeQueue<Data>& queue)
  {
    CheckSubscriberDataSize<Data>(topic);
 
    block_ = new LockFreeList::Node<QueueBlock>;
    block_->data_.type = SuberType::QUEUE;
    block_->data_.queue = &queue;
    block_->data_.fun = [](MicrosecondTimestamp, RawData data, QueueBlock& block)
    {
      LockFreeQueue<Data>* queue = reinterpret_cast<LockFreeQueue<Data>*>(block.queue);
      (void)queue->Push(*reinterpret_cast<Data*>(data.addr_));
    };
 
    topic.block_->data_.subers.Add(*block_);
  }
 
  template <typename Data>
  QueuedSubscriber(Topic topic, LockFreeQueue<Message<Data>>& queue)
  {
    CheckSubscriberDataSize<Data>(topic);
 
    block_ = new LockFreeList::Node<QueueBlock>;
    block_->data_.type = SuberType::QUEUE;
    block_->data_.queue = &queue;
    block_->data_.fun =
        [](MicrosecondTimestamp timestamp, RawData data, QueueBlock& block)
    {
      LockFreeQueue<Message<Data>>* queue =
          reinterpret_cast<LockFreeQueue<Message<Data>>*>(block.queue);
      (void)queue->Push(Message<Data>{timestamp, *reinterpret_cast<Data*>(data.addr_)});
    };
 
    topic.block_->data_.subers.Add(*block_);
  }
 
  QueuedSubscriber(const QueuedSubscriber&) = delete;
  QueuedSubscriber& operator=(const QueuedSubscriber&) = delete;
 
  QueuedSubscriber(QueuedSubscriber&& other) noexcept : block_(other.block_)
  {
    other.block_ = nullptr;
  }
 
  QueuedSubscriber& operator=(QueuedSubscriber&& other) noexcept
  {
    if (this != &other)
    {
      block_ = other.block_;
      other.block_ = nullptr;
    }
    return *this;
  }
 
 private:
  LockFreeList::Node<QueueBlock>* block_ = nullptr;
};
 
class Topic::Callback
{
  template <typename Function>
  struct FunctionTraits;
 
  template <typename Return, typename... Args>
  struct FunctionTraits<Return (*)(Args...)>
  {
    using ReturnType = Return;
    static constexpr size_t ARITY = sizeof...(Args);
 
    template <size_t Index>
    using Arg = std::tuple_element_t<Index, std::tuple<Args...>>;
  };
 
  template <typename T>
  struct MessageViewTraits
  {
    static constexpr bool VALUE = false;
  };
 
  template <typename Data>
  struct MessageViewTraits<MessageView<Data>>
  {
    static constexpr bool VALUE = true;
    using DataType = Data;
  };
 
  template <typename T>
  using RemoveCVRef = std::remove_cv_t<std::remove_reference_t<T>>;
 
  template <typename T>
  static constexpr bool IS_RAW_DATA =
      std::same_as<RemoveCVRef<T>, RawData> || std::same_as<RemoveCVRef<T>, ConstRawData>;
 
  template <typename T>
  static constexpr bool IS_MESSAGE_VIEW = MessageViewTraits<RemoveCVRef<T>>::VALUE;
 
  template <typename T>
  static constexpr bool IS_TYPED_DATA =
      !IS_RAW_DATA<T> && !IS_MESSAGE_VIEW<T> && TopicPayload<RemoveCVRef<T>>;
 
  template <typename T>
  static constexpr bool IS_CALLBACK_PAYLOAD =
      IS_RAW_DATA<T> || IS_MESSAGE_VIEW<T> || IS_TYPED_DATA<T>;
 
  struct BlockHeader
  {
    using RunFun = void (*)(const BlockHeader*, bool, MicrosecondTimestamp, RawData&);
 
    RunFun run = nullptr;
    uint32_t payload_size = 0;
  };
 
  template <typename PayloadArg>
  static consteval uint32_t PayloadSize()
  {
    if constexpr (IS_RAW_DATA<PayloadArg>)
    {
      return 0;
    }
    else if constexpr (IS_MESSAGE_VIEW<PayloadArg>)
    {
      using View = MessageViewTraits<RemoveCVRef<PayloadArg>>;
      return sizeof(typename View::DataType);
    }
    else
    {
      static_assert(IS_TYPED_DATA<PayloadArg>,
                    "LibXR::Topic::Callback payload must be RawData, ConstRawData, "
                    "Topic::MessageView<T>, T, T&, or const T&.");
      return sizeof(RemoveCVRef<PayloadArg>);
    }
  }
 
  template <typename Function, typename BoundArg, typename PayloadArg>
  static void InvokePayload(Function fun, BoundArg& arg, bool in_isr,
                            MicrosecondTimestamp timestamp, RawData& data)
  {
    if constexpr (std::same_as<RemoveCVRef<PayloadArg>, RawData>)
    {
      fun(in_isr, arg, data);
    }
    else if constexpr (std::same_as<RemoveCVRef<PayloadArg>, ConstRawData>)
    {
      ConstRawData const_data(data);
      fun(in_isr, arg, const_data);
    }
    else if constexpr (IS_MESSAGE_VIEW<PayloadArg>)
    {
      using View = MessageViewTraits<RemoveCVRef<PayloadArg>>;
      using Data = typename View::DataType;
      MessageView<Data> message{timestamp, *reinterpret_cast<Data*>(data.addr_)};
      fun(in_isr, arg, message);
    }
    else
    {
      using Data = RemoveCVRef<PayloadArg>;
      fun(in_isr, arg, *reinterpret_cast<Data*>(data.addr_));
    }
  }
 
  template <typename Function, typename BoundArg, typename PayloadArg>
  struct PayloadOnlyBlock : public BlockHeader
  {
    PayloadOnlyBlock(Function fun, BoundArg&& arg)
        : BlockHeader{&Run, PayloadSize<PayloadArg>()}, fun_(fun), arg_(std::move(arg))
    {
    }
 
    static void Run(const BlockHeader* header, bool in_isr,
                    MicrosecondTimestamp timestamp, RawData& data)
    {
      auto* block = static_cast<const PayloadOnlyBlock*>(header);
      InvokePayload<Function, BoundArg, PayloadArg>(
          block->fun_, const_cast<BoundArg&>(block->arg_), in_isr, timestamp, data);
    }
 
    Function fun_;
    BoundArg arg_;
  };
 
  template <typename Function, typename BoundArg, typename PayloadArg>
  struct TimestampPayloadBlock : public BlockHeader
  {
    TimestampPayloadBlock(Function fun, BoundArg&& arg)
        : BlockHeader{&Run, PayloadSize<PayloadArg>()}, fun_(fun), arg_(std::move(arg))
    {
    }
 
    static void Run(const BlockHeader* header, bool in_isr,
                    MicrosecondTimestamp timestamp, RawData& data)
    {
      auto* block = static_cast<const TimestampPayloadBlock*>(header);
      if constexpr (std::same_as<RemoveCVRef<PayloadArg>, RawData>)
      {
        block->fun_(in_isr, block->arg_, timestamp, data);
      }
      else if constexpr (std::same_as<RemoveCVRef<PayloadArg>, ConstRawData>)
      {
        ConstRawData const_data(data);
        block->fun_(in_isr, block->arg_, timestamp, const_data);
      }
      else if constexpr (IS_MESSAGE_VIEW<PayloadArg>)
      {
        using View = MessageViewTraits<RemoveCVRef<PayloadArg>>;
        using Data = typename View::DataType;
        MessageView<Data> message{timestamp, *reinterpret_cast<Data*>(data.addr_)};
        block->fun_(in_isr, block->arg_, timestamp, message);
      }
      else
      {
        using Data = RemoveCVRef<PayloadArg>;
        block->fun_(in_isr, block->arg_, timestamp, *reinterpret_cast<Data*>(data.addr_));
      }
    }
 
    Function fun_;
    BoundArg arg_;
  };
 
  template <typename Function, typename BoundArg, size_t Arity>
  struct Factory
  {
    static_assert(Arity == 3 || Arity == 4,
                  "LibXR::Topic::Callback function must be void(bool, Arg, Payload) "
                  "or void(bool, Arg, MicrosecondTimestamp, Payload).");
  };
 
  template <typename Function, typename BoundArg>
  struct Factory<Function, BoundArg, 3>
  {
    using Traits = FunctionTraits<Function>;
    using PayloadArg = typename Traits::template Arg<2>;
 
    static BlockHeader* Create(Function fun, BoundArg&& arg)
    {
      static_assert(std::same_as<typename Traits::ReturnType, void>);
      static_assert(std::same_as<typename Traits::template Arg<0>, bool>);
      static_assert(std::same_as<typename Traits::template Arg<1>, BoundArg>);
      static_assert(IS_CALLBACK_PAYLOAD<PayloadArg>);
      return new PayloadOnlyBlock<Function, BoundArg, PayloadArg>(fun, std::move(arg));
    }
  };
 
  template <typename Function, typename BoundArg>
  struct Factory<Function, BoundArg, 4>
  {
    using Traits = FunctionTraits<Function>;
    using TimestampArg = typename Traits::template Arg<2>;
    using PayloadArg = typename Traits::template Arg<3>;
 
    static BlockHeader* Create(Function fun, BoundArg&& arg)
    {
      static_assert(std::same_as<typename Traits::ReturnType, void>);
      static_assert(std::same_as<typename Traits::template Arg<0>, bool>);
      static_assert(std::same_as<typename Traits::template Arg<1>, BoundArg>);
      static_assert(std::same_as<RemoveCVRef<TimestampArg>, MicrosecondTimestamp>);
      static_assert(IS_CALLBACK_PAYLOAD<PayloadArg>);
      return new TimestampPayloadBlock<Function, BoundArg, PayloadArg>(fun,
                                                                       std::move(arg));
    }
  };
 
  static void EmptyRun(const BlockHeader*, bool, MicrosecondTimestamp, RawData&) {}
 
  inline static BlockHeader empty_block_{&EmptyRun, 0};
 
 public:
  Callback() = default;
  Callback(const Callback&) = default;
  Callback& operator=(const Callback&) = default;
 
  template <typename BoundArg, typename Callable>
  [[nodiscard]] static Callback Create(Callable fun, BoundArg arg)
  {
    using Function = decltype(+std::declval<Callable>());
    using Traits = FunctionTraits<Function>;
    static_assert(Traits::ARITY == 3 || Traits::ARITY == 4,
                  "LibXR::Topic::Callback::Create expects a capture-free callable "
                  "with bool and bound-argument parameters.");
    return Callback(
        Factory<Function, BoundArg, Traits::ARITY>::Create(+fun, std::move(arg)));
  }
 
  void Run(bool in_isr, MicrosecondTimestamp timestamp, RawData& data) const
  {
    block_->run(block_, in_isr, timestamp, data);
  }
 
  uint32_t PayloadSize() const { return block_->payload_size; }
 
 private:
  explicit Callback(BlockHeader* block) : block_(block ? block : &empty_block_) {}
 
  BlockHeader* block_ = &empty_block_;
};
 
struct Topic::CallbackBlock : public Topic::SuberBlock
{
  explicit CallbackBlock(Callback& callback) : cb(callback)
  {
    type = SuberType::CALLBACK;
  }
 
  Callback cb;  
};
}  // namespace LibXR