{

// Design: Based on a queue-of-queues. The low-level queues are just

// circular buffers with front and tail indices indicating where the

// next element to dequeue is and where the next element can be enqueued,

// respectively. Each low-level queue is called a "block". Each block

// wastes exactly one element's worth of space to keep the design simple

// (if front == tail then the queue is empty, and can't be full).

// The high-level queue is a circular linked list of blocks; again there

// is a front and tail, but this time they are pointers to the blocks.

// The front block is where the next element to be dequeued is, provided

// the block is not empty. The back block is where elements are to be

// enqueued, provided the block is not full.

// The producer thread owns all the tail indices/pointers. The consumer

// thread owns all the front indices/pointers. Both threads read each

// other's variables, but only the owning thread updates them. E.g. After

// the consumer reads the producer's tail, the tail may change before the

// consumer is done dequeuing an object, but the consumer knows the tail

// will never go backwards, only forwards.

// If there is no room to enqueue an object, an additional block (of

// greater size than the last block) is added. Blocks are never removed.

// Constructs a queue that can hold maxSize elements without further

// allocations. Allocates maxSize + 1, rounded up to the nearest power

// of 2, elements.

explicit ReaderWriterQueue(size_t maxSize = 15)

: largestBlockSize(ceilToPow2(maxSize + 1)) // We need a spare slot to fit maxSize elements in the block

#ifndef NDEBUG

,enqueuing(false)

,dequeuing(false)

#endif

{

assert(maxSize > 0);

auto firstBlockRaw = static_cast<char*>(std::malloc(sizeof(Block) + std::alignment_of<Block>::value - 1));

auto firstBlock = new (align_for<Block>(firstBlockRaw)) Block(largestBlockSize, firstBlockRaw);

firstBlock->next = firstBlock;

frontBlock = firstBlock;

tailBlock = firstBlock;

// Make sure the reader/writer threads will have the initialized memory setup above:

fence(memory_order_sync);

}

// Note: The queue should not be accessed concurrently while it's

// being deleted. It's up to the user to synchronize this.

~ReaderWriterQueue()

{

// Make sure we get the latest version of all variables from other CPUs:

fence(memory_order_sync);

// Destroy any remaining objects in queue and free memory

Block* frontBlock_ = frontBlock;

Block* block = frontBlock_;

do {

Block* nextBlock = block->next;

size_t blockFront = block->front;

size_t blockTail = block->tail;

for (size_t i = blockFront; i != blockTail; i = (i + 1) & block->sizeMask()) {

auto element = reinterpret_cast<T*>(block->data + i * sizeof(T));

element->~T();

(void)element;

}

std::free(block->rawThis);

block = nextBlock;

} while (block != frontBlock_);

}

// Enqueues a copy of element if there is room in the queue.

// Returns true if the element was enqueued, false otherwise.

// Does not allocate memory.

AE_FORCEINLINE bool try_enqueue(T const& element)

{

return inner_enqueue<CannotAlloc>(element);

}

// Enqueues a moved copy of element if there is room in the queue.

// Returns true if the element was enqueued, false otherwise.

// Does not allocate memory.

AE_FORCEINLINE bool try_enqueue(T&& element)

{

return inner_enqueue<CannotAlloc>(std::forward<T>(element));

}

// Enqueues a copy of element on the queue.

// Allocates an additional block of memory if needed.

AE_FORCEINLINE void enqueue(T const& element)

{

inner_enqueue<CanAlloc>(element);

}

// Enqueues a moved copy of element on the queue.

// Allocates an additional block of memory if needed.

AE_FORCEINLINE void enqueue(T&& element)

{

inner_enqueue<CanAlloc>(std::forward<T>(element));

}

// Attempts to dequeue an element; if the queue is empty,

// returns false instead. If the queue has at least one element,

// moves front to result using operator=, then returns true.

template<typename U>

bool try_dequeue(U& result)

{

#ifndef NDEBUG

ReentrantGuard guard(this->dequeuing);

#endif

// High-level pseudocode:

// Remember where the tail block is

// If the front block has an element in it, dequeue it

// Else

// If front block was the tail block when we entered the function, return false

// Else advance to next block and dequeue the item there

// Note that we have to use the value of the tail block from before we check if the front

// block is full or not, in case the front block is empty and then, before we check if the

// tail block is at the front block or not, the producer fills up the front block *and

// moves on*, which would make us skip a filled block. Seems unlikely, but was consistently

// reproducible in practice.

Block* tailBlockAtStart = tailBlock;

fence(memory_order_acquire);

Block* frontBlock_ = frontBlock.load();

size_t blockTail = frontBlock_->tail.load();

size_t blockFront = frontBlock_->front.load();

fence(memory_order_acquire);

if (blockFront != blockTail) {

// Front block not empty, dequeue from here

auto element = reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));

result = std::move(*element);

element->~T();

blockFront = (blockFront + 1) & frontBlock_->sizeMask();

fence(memory_order_release);

frontBlock_->front = blockFront;

}

else if (frontBlock_ != tailBlockAtStart) {

// Front block is empty but there's another block ahead, advance to it

Block* nextBlock = frontBlock_->next;

// Don't need an acquire fence here since next can only ever be set on the tailBlock,

// and we're not the tailBlock, and we did an acquire earlier after reading tailBlock which

// ensures next is up-to-date on this CPU in case we recently were at tailBlock.

size_t nextBlockFront = nextBlock->front.load();

size_t nextBlockTail = nextBlock->tail;

fence(memory_order_acquire);

// Since the tailBlock is only ever advanced after being written to,

// we know there's for sure an element to dequeue on it

assert(nextBlockFront != nextBlockTail);

AE_UNUSED(nextBlockTail);

// We're done with this block, let the producer use it if it needs

fence(memory_order_release); // Expose possibly pending changes to frontBlock->front from last dequeue

frontBlock = frontBlock_ = nextBlock;

compiler_fence(memory_order_release); // Not strictly needed

auto element = reinterpret_cast<T*>(frontBlock_->data + nextBlockFront * sizeof(T));

result = std::move(*element);

element->~T();

nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask();

fence(memory_order_release);

frontBlock_->front = nextBlockFront;

}

else {

// No elements in current block and no other block to advance to

return false;

}

return true;

}

// Returns a pointer to the front element in the queue (the one that

// would be removed next by a call to `try_dequeue` or `pop`). If the

// queue appears empty at the time the method is called, nullptr is

// returned instead.

// Must be called only from the consumer thread.

T* peek()

{

#ifndef NDEBUG

ReentrantGuard guard(this->dequeuing);

#endif

// See try_dequeue() for reasoning

Block* tailBlockAtStart = tailBlock;

fence(memory_order_acquire);

Block* frontBlock_ = frontBlock.load();

size_t blockTail = frontBlock_->tail.load();

size_t blockFront = frontBlock_->front.load();

fence(memory_order_acquire);

if (blockFront != blockTail) {

return reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));

}

else if (frontBlock_ != tailBlockAtStart) {

Block* nextBlock = frontBlock_->next;

size_t nextBlockFront = nextBlock->front.load();

fence(memory_order_acquire);

assert(nextBlockFront != nextBlock->tail);

return reinterpret_cast<T*>(nextBlock->data + nextBlockFront * sizeof(T));

}

return nullptr;

}

// Removes the front element from the queue, if any, without returning it.

// Returns true on success, or false if the queue appeared empty at the time

// `pop` was called.

bool pop()

{

#ifndef NDEBUG

ReentrantGuard guard(this->dequeuing);

#endif

// See try_dequeue() for reasoning

Block* tailBlockAtStart = tailBlock;

fence(memory_order_acquire);

Block* frontBlock_ = frontBlock.load();

size_t blockTail = frontBlock_->tail.load();

size_t blockFront = frontBlock_->front.load();

fence(memory_order_acquire);

if (blockFront != blockTail) {

// Front block not empty, pop

auto element = reinterpret_cast<T*>(frontBlock_->data + blockFront * sizeof(T));

element->~T();

blockFront = (blockFront + 1) & frontBlock_->sizeMask();

fence(memory_order_release);

frontBlock_->front = blockFront;

}

else if (frontBlock_ != tailBlockAtStart) {

// Front block is empty but there's another block ahead, advance to it

Block* nextBlock = frontBlock_->next;

size_t nextBlockFront = nextBlock->front.load();

size_t nextBlockTail = nextBlock->tail;

fence(memory_order_acquire);

assert(nextBlockFront != nextBlockTail);

AE_UNUSED(nextBlockTail);

fence(memory_order_release);

frontBlock = frontBlock_ = nextBlock;

compiler_fence(memory_order_release);

auto element = reinterpret_cast<T*>(frontBlock_->data + nextBlockFront * sizeof(T));

element->~T();

nextBlockFront = (nextBlockFront + 1) & frontBlock_->sizeMask();

fence(memory_order_release);

frontBlock_->front = nextBlockFront;

}

else {

// No elements in current block and no other block to advance to

return false;

}

return true;

}

// Returns the approximate number of items currently in the queue.

// Safe to call from both the producer and consumer threads.

inline size_t size_approx() const

{

size_t result = 0;

Block* frontBlock_ = frontBlock.load();

Block* block = frontBlock_;

do {

fence(memory_order_acquire);

size_t blockFront = block->front.load();

size_t blockTail = block->tail.load();

result += (blockTail - blockFront) & block->sizeMask();

block = block->next.load();

} while (block != frontBlock_);

return result;

}

enum AllocationMode { CanAlloc, CannotAlloc };

template<AllocationMode canAlloc, typename U>

bool inner_enqueue(U&& element)

{

#ifndef NDEBUG

ReentrantGuard guard(this->enqueuing);

#endif

// High-level pseudocode (assuming we're allowed to alloc a new block):

// If room in tail block, add to tail

// Else check next block

// If next block is not the head block, enqueue on next block

// Else create a new block and enqueue there

// Advance tail to the block we just enqueued to

Block* tailBlock_ = tailBlock.load();

size_t blockFront = tailBlock_->front.load();

size_t blockTail = tailBlock_->tail.load();

fence(memory_order_acquire);

size_t nextBlockTail = (blockTail + 1) & tailBlock_->sizeMask();

if (nextBlockTail != blockFront) {

// This block has room for at least one more element

char* location = tailBlock_->data + blockTail * sizeof(T);

new (location) T(std::forward<U>(element));

fence(memory_order_release);

tailBlock_->tail = nextBlockTail;

}

else if (tailBlock_->next.load() != frontBlock) {

// Note that the reason we can't advance to the frontBlock and start adding new entries there

// is because if we did, then dequeue would stay in that block, eventually reading the new values,

// instead of advancing to the next full block (whose values were enqueued first and so should be

// consumed first).

fence(memory_order_acquire); // Ensure we get latest writes if we got the latest frontBlock

// tailBlock is full, but there's a free block ahead, use it

Block* tailBlockNext = tailBlock_->next.load();

size_t nextBlockFront = tailBlockNext->front.load();

nextBlockTail = tailBlockNext->tail.load();

fence(memory_order_acquire);

// This block must be empty since it's not the head block and we

// go through the blocks in a circle

assert(nextBlockFront == nextBlockTail);

AE_UNUSED(nextBlockFront);

char* location = tailBlockNext->data + nextBlockTail * sizeof(T);

new (location) T(std::forward<U>(element));

tailBlockNext->tail = (nextBlockTail + 1) & tailBlockNext->sizeMask();

fence(memory_order_release);

tailBlock = tailBlockNext;

}

else if (canAlloc == CanAlloc) {

// tailBlock is full and there's no free block ahead; create a new block

largestBlockSize *= 2;

auto newBlockRaw = static_cast<char*>(std::malloc(sizeof(Block) + std::alignment_of<Block>::value - 1));

auto newBlock = new (align_for<Block>(newBlockRaw)) Block(largestBlockSize, newBlockRaw);

new (newBlock->data) T(std::forward<U>(element));

assert(newBlock->front == 0);

newBlock->tail = 1;

newBlock->next = tailBlock_->next.load();

tailBlock_->next = newBlock;

// Might be possible for the dequeue thread to see the new tailBlock->next

// *without* seeing the new tailBlock value, but this is OK since it can't

// advance to the next block until tailBlock is set anyway (because the only

// case where it could try to read the next is if it's already at the tailBlock,

// and it won't advance past tailBlock in any circumstance).

fence(memory_order_release);

tailBlock = newBlock;

}

else if (canAlloc == CannotAlloc) {

// Would have had to allocate a new block to enqueue, but not allowed

return false;

}

else {

assert(false && "Should be unreachable code");

return false;

}

return true;

}

// Disable copying

ReaderWriterQueue(ReaderWriterQueue const&) { }

// Disable assignment

ReaderWriterQueue& operator=(ReaderWriterQueue const&) { }

AE_FORCEINLINE static size_t ceilToPow2(size_t x)

{

// From http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2

--x;

x |= x >> 1;

x |= x >> 2;

x |= x >> 4;

for (size_t i = 1; i < sizeof(size_t); i <<= 1) {

x |= x >> (i << 3);

}

++x;

return x;

}

template<typename U>

static AE_FORCEINLINE char* align_for(char* ptr)

{

const std::size_t alignment = std::alignment_of<U>::value;

return ptr + (alignment - (reinterpret_cast<std::uintptr_t>(ptr) % alignment)) % alignment;

}

#ifndef NDEBUG

struct ReentrantGuard

{

ReentrantGuard(bool& _inSection)

: inSection(_inSection)

{

assert(!inSection);

if (inSection) {

throw std::runtime_error("ReaderWriterQueue does not support enqueuing or dequeuing elements from other elements' ctors and dtors");

}

inSection = true;

}

~ReentrantGuard() { inSection = false; }

private:

ReentrantGuard& operator=(ReentrantGuard const&);

private:

bool& inSection;

};

#endif

struct Block

{

// Avoid false-sharing by putting highly contended variables on their own cache lines

AE_ALIGN(CACHE_LINE_SIZE)

weak_atomic<size_t> front; // (Atomic) Elements are read from here

AE_ALIGN(CACHE_LINE_SIZE)

weak_atomic<size_t> tail; // (Atomic) Elements are enqueued here

AE_ALIGN(CACHE_LINE_SIZE) // next isn't very contended, but we don't want it on the same cache line as tail (which is)

weak_atomic<Block*> next; // (Atomic)

char* data; // Contents (on heap) are aligned to T's alignment

const size_t size;

AE_FORCEINLINE size_t sizeMask() const { return size - 1; }

// size must be a power of two (and greater than 0)

Block(size_t const& _size, char* rawThis)

: front(0), tail(0), next(nullptr), size(_size), rawThis(rawThis)

{

// Allocate enough memory for an array of Ts, aligned

size_t alignment = std::alignment_of<T>::value;

data = rawData = static_cast<char*>(std::malloc(sizeof(T) * size + alignment - 1));

assert(rawData);

auto alignmentOffset = (uintptr_t)rawData % alignment;

if (alignmentOffset != 0) {

data += alignment - alignmentOffset;

}

}

~Block()

{

std::free(rawData);

}

private:

// C4512 - Assignment operator could not be generated

Block& operator=(Block const&);

private:

char* rawData;

public:

char* rawThis;

};

weak_atomic<Block*> frontBlock; // (Atomic) Elements are enqueued to this block

char cachelineFiller[CACHE_LINE_SIZE - sizeof(weak_atomic<Block*>)];

weak_atomic<Block*> tailBlock; // (Atomic) Elements are dequeued from this block

size_t largestBlockSize;

#ifndef NDEBUG

bool enqueuing;

bool dequeuing;

#endif