SPSCQueue.hpp

// Copyright (C) 2021 by the IntelliStream team (https://github.com/intellistream)
 
#pragma once
 
#include <atomic>
#include <cassert>
#include <cstddef>
#include <memory> // std::allocator
#include <new>    // std::hardware_destructive_interference_size
#include <stdexcept>
#include <type_traits> // std::enable_if, std::is_*_constructible
#include <thread>
#include <mutex>
#include <iostream>
#include <condition_variable>
#include <pthread.h>
#include <sys/time.h>
using namespace std::literals::chrono_literals;
using namespace std;
 
namespace INTELLI {
template<typename T, typename Allocator = std::allocator<T>>
class SPSCQueue {
 
#if defined(__cpp_if_constexpr) && defined(__cpp_lib_void_t)
  template<typename Alloc2, typename = void>
  struct has_allocate_at_least : std::false_type {
  };
 
  template<typename Alloc2>
  struct has_allocate_at_least<
      Alloc2, std::void_t<typename Alloc2::value_type,
                          decltype(std::declval<Alloc2 &>().allocate_at_least(
                              size_t{}))>> : std::true_type {
  };
#endif
 
 public:
  pthread_cond_t cond;
  pthread_mutex_t mutex;
  explicit SPSCQueue(const size_t capacity,
                     const Allocator &allocator = Allocator())
      : capacity_(capacity), allocator_(allocator) {
 
    // The queue needs at least one element
    if (capacity_ < 1) {
      capacity_ = 1;
    }
    capacity_++; // Needs one slack element
    // Prevent overflowing size_t
    if (capacity_ > SIZE_MAX - 2 * kPadding) {
      capacity_ = SIZE_MAX - 2 * kPadding;
    }
 
#if defined(__cpp_if_constexpr) && defined(__cpp_lib_void_t)
    if constexpr (has_allocate_at_least<Allocator>::value) {
      auto res = allocator_.allocate_at_least(capacity_ + 2 * kPadding);
      slots_ = res.ptr;
      capacity_ = res.count - 2 * kPadding;
    } else {
      slots_ = std::allocator_traits<Allocator>::allocate(
          allocator_, capacity_ + 2 * kPadding);
    }
#else
    slots_ = std::allocator_traits<Allocator>::allocate(
        allocator_, capacity_ + 2 * kPadding);
#endif
 
    static_assert(alignof(SPSCQueue<T>) == kCacheLineSize, "");
    static_assert(sizeof(SPSCQueue<T>) >= 3 * kCacheLineSize, "");
    assert(reinterpret_cast<char *>(&readIdx_) -
        reinterpret_cast<char *>(&writeIdx_) >=
        static_cast<std::ptrdiff_t>(kCacheLineSize));
  }
 
  ~SPSCQueue() {
    while (front()) {
      pop();
    }
    std::allocator_traits<Allocator>::deallocate(allocator_, slots_,
                                                 capacity_ + 2 * kPadding);
  }
 
  // non-copyable and non-movable
  SPSCQueue(const SPSCQueue &) = delete;
 
  SPSCQueue &operator=(const SPSCQueue &) = delete;
  std::mutex g_mutex;
  condition_variable g_con;
 
  void wakeUpSink(void) {
    //std::unique_lock<std::mutex> lock(g_mutex);
 
    g_con.notify_one();
 
 
    //lock.unlock();
  }
  void waitForSource(void) {  //  printf("enter sleep\r\n");
    std::unique_lock<std::mutex> lock(g_mutex);
    g_con.wait(lock);
 
    // printf("end sleep\r\n");
    // pthread_mutex_lock(&mutex);
 
    // pthread_mutex_unlock(&mutex);
    //
 
 
  }
  template<typename... Args>
  void emplace(Args &&...args)
  noexcept(
  std::is_nothrow_constructible<T, Args && ...>::value) {
    static_assert(std::is_constructible<T, Args &&...>::value,
                  "T must be constructible with Args&&...");
    auto const writeIdx = writeIdx_.load(std::memory_order_relaxed);
    auto nextWriteIdx = writeIdx + 1;
    if (nextWriteIdx == capacity_) {
      nextWriteIdx = 0;
    }
    while (nextWriteIdx == readIdxCache_) {
      readIdxCache_ = readIdx_.load(std::memory_order_acquire);
    }
    new(&slots_[writeIdx + kPadding]) T(std::forward<Args>(args)...);
    writeIdx_.store(nextWriteIdx, std::memory_order_release);
  }
 
  template<typename... Args>
  bool try_emplace(Args &&...args)
  noexcept(
  std::is_nothrow_constructible<T, Args && ...>::value) {
    static_assert(std::is_constructible<T, Args &&...>::value,
                  "T must be constructible with Args&&...");
    auto const writeIdx = writeIdx_.load(std::memory_order_relaxed);
    auto nextWriteIdx = writeIdx + 1;
    if (nextWriteIdx == capacity_) {
      nextWriteIdx = 0;
    }
    if (nextWriteIdx == readIdxCache_) {
      readIdxCache_ = readIdx_.load(std::memory_order_acquire);
      if (nextWriteIdx == readIdxCache_) {
        return false;
      }
    }
    new(&slots_[writeIdx + kPadding]) T(std::forward<Args>(args)...);
    writeIdx_.store(nextWriteIdx, std::memory_order_release);
    return true;
  }
 
  void push(const T &v)
  noexcept(std::is_nothrow_copy_constructible<T>::value) {
    static_assert(std::is_copy_constructible<T>::value,
                  "T must be copy constructible");
    emplace(v);
    // g_con.notify_all();
  }
 
  template<typename P, typename = typename std::enable_if<
      std::is_constructible<T, P &&>::value>::type>
  void push(P &&v)
  noexcept(std::is_nothrow_constructible<T, P &&>::value) {
    emplace(std::forward<P>(v));
  }
 
  bool
  try_push(const T &v)
  noexcept(std::is_nothrow_copy_constructible<T>::value) {
    static_assert(std::is_copy_constructible<T>::value,
                  "T must be copy constructible");
    return try_emplace(v);
  }
 
  template<typename P, typename = typename std::enable_if<
      std::is_constructible<T, P &&>::value>::type>
  bool try_push(P &&v)
  noexcept(std::is_nothrow_constructible<T, P &&>::value) {
    return try_emplace(std::forward<P>(v));
  }
 
  T *front()
  noexcept {
    auto const readIdx = readIdx_.load(std::memory_order_relaxed);
    if (readIdx == writeIdxCache_) {
      writeIdxCache_ = writeIdx_.load(std::memory_order_acquire);
      if (writeIdxCache_ == readIdx) {
        return nullptr;
      }
    }
    return &slots_[readIdx + kPadding];
  }
 
  void pop()
  noexcept {
    static_assert(std::is_nothrow_destructible<T>::value,
                  "T must be nothrow destructible");
    auto const readIdx = readIdx_.load(std::memory_order_relaxed);
    assert(writeIdx_.load(std::memory_order_acquire) != readIdx);
    slots_[readIdx + kPadding].~T();
    auto nextReadIdx = readIdx + 1;
    if (nextReadIdx == capacity_) {
      nextReadIdx = 0;
    }
    readIdx_.store(nextReadIdx, std::memory_order_release);
  }
 
  size_t size() const
  noexcept {
    std::ptrdiff_t diff = writeIdx_.load(std::memory_order_acquire) -
        readIdx_.load(std::memory_order_acquire);
    if (diff < 0) {
      diff += capacity_;
    }
    return static_cast<size_t>(diff);
  }
 
  bool empty() const
  noexcept {
 
    return size() == 0;
 
  }
 
  size_t capacity() const
  noexcept { return capacity_ - 1; }
 
 private:
#ifdef __cpp_lib_hardware_interference_size
  static constexpr size_t kCacheLineSize =
          std::hardware_destructive_interference_size;
#else
  static constexpr size_t
      kCacheLineSize = 64;
#endif
 
  // Padding to avoid false sharing between slots_ and adjacent allocations
  static constexpr size_t
      kPadding = (kCacheLineSize - 1) / sizeof(T) + 1;
 
 private:
  size_t capacity_;
  T *slots_;
#if defined(__has_cpp_attribute) && __has_cpp_attribute(no_unique_address)
  Allocator allocator_ [[no_unique_address]];
#else
  Allocator allocator_;
#endif
 
  // Align to cache line size in order to avoid false sharing
  // readIdxCache_ and writeIdxCache_ is used to reduce the amount of cache
  // coherency traffic
  alignas(kCacheLineSize)
  std::atomic<size_t> writeIdx_ = {0};
  alignas(kCacheLineSize)
  size_t readIdxCache_ = 0;
  alignas(kCacheLineSize)
  std::atomic<size_t> readIdx_ = {0};
  alignas(kCacheLineSize)
  size_t writeIdxCache_ = 0;
 
  // Padding to avoid adjacent allocations to share cache line with
  // writeIdxCache_
  char padding_[kCacheLineSize - sizeof(writeIdxCache_)];
};
} // namespace rigtorp