-- uniprim.cpp
#include<uniprim.h>

bool CAS(void *data, uint32_t cmp1, uint32_t rep1)
{
	bool ret1 = false;
	_asm
	{
		mov esi, data;
		mov eax, cmp1;
		mov ecx, rep1;
		lock cmpxchg [esi], ecx;
		jz suc;
		mov ret1, 0
		jmp end;
		suc:
		mov ret1, 1;
		end:
	}
	return ret1;
}

bool CAS2(void *data, uint32_t cmp1, uint32_t cmp2, uint32_t rep1, uint32_t rep2)
{
	bool ret1 = false;
	_asm
	{
		push ebx;
		mov esi, data;
		mov eax, cmp1;
		mov edx, cmp2;
		mov ebx, rep1;
		mov ecx, rep2;
		lock cmpxchg8b [esi];
		jz suc;
		mov ret1, 0
		jmp end;
		suc:
		mov ret1, 1;
		end:
		pop ebx;
	}
	return ret1;
}

-- uniprim.h
#pragma once

#include<stdint.h>

// Universal primitives
bool CAS(void *ptr, uint32_t cmp, uint32_t rep);
bool CAS2(void *ptr, uint32_t cmp1, uint32_t cmp2, uint32_t rep1, uint32_t rep2);

-- queue.h
#pragma once

#include<uniprim.h>

// This is thread safe
template<typename T, T N> class Queue
{
public:
	Queue() : m_pHead(0), m_pTail(0), m_cPushes(0), m_cPops(0) { }

	// Practically copied out of game programming gems 6
	void Add(T data)
	{
		node<T> *pNode = new node<T>;
		pNode->m_pNext = NULL;
		pNode->m_data = data;

		uint32_t cPushes;
		uint32_t cPops;
		node<T> *pTail;

		while (true)
		{
			cPushes = m_cPushes;
			cPops = m_cPops;
			pTail = (node<T>*)m_pTail;
			if (cPushes != m_cPushes)
				continue;

			if (!pTail)
				if (CAS2(&m_pTail, NULL, cPushes, (uint32_t)pNode, cPushes +1))
					break;
			else if (CAS(&pTail->m_pNext, NULL, (uint32_t)pNode))
					break;
			else
				CAS2(&m_pTail, (uint32_t)pTail, cPushes, (uint32_t)pTail->m_pNext, cPushes + 1);
		}

		CAS2(&m_pHead, NULL, cPops, (uint32_t)pNode, cPops + 1);
		CAS2(&m_pTail, (uint32_t)pTail, cPushes, (uint32_t)pNode, cPushes + 1);
	}

	T Remove(void)
	{
		node<T> *pHead;
		while (true)
		{
			uint32_t cPops = m_cPops;
			uint32_t cPushes = m_cPushes;
			pHead = (node<T>*)m_pHead;
			if (cPops != m_cPops)
				continue;
			node<T> *pNext = pHead->m_pNext;

			if (pHead == m_pTail)
				CAS2(&m_pTail, (uint32_t)pHead, cPushes, (uint32_t)pNext, cPushes + 1);

			if (CAS2(&m_pHead, (uint32_t)pHead, cPops, (uint32_t)pNext, cPops + 1))
			{
				T data = pHead->m_data;
				delete pHead;
				return data;
			}
		}
		return (T)N;
	}

private:
	template<class T> struct node
	{
		T m_data;
		node<T> *m_pNext;
	};

	volatile node<T> *m_pHead;
	volatile unsigned int m_cPops;
	volatile node<T> *m_pTail;
	volatile unsigned int m_cPushes;
};