even faster condvar

#include <windows.h>
#include <cassert>
#include <exception>
#include <intrin.h>

#if !defined(NDEBUG)
    #define ONDEBUG(expr) (expr)
#else
    #define ONDEBUG(expr) ((void)0)
#endif

inline
DWORD FastGetCurrentThreadId()
{
#if defined(_M_IX86)
    return __readfsdword( 0x24 );
#elif defined(_M_AMD64)
    return *(DWORD *)(__readgsqword( 0x30 ) + 0x48);
#endif
}

class Exception : public std::exception
{
};

class ResourceException : Exception
{
};

template <typename TYPE>
inline
TYPE AutoThrowResourceException( TYPE t )
{
    if( !t )
        throw ResourceException();

    return t;
}

class XHANDLE
{
public:
         XHANDLE( HANDLE h = NULL );
         XHANDLE();
         ~XHANDLE();
    void CloseHandle();

public:
    HANDLE h;
};

inline
XHANDLE::XHANDLE( HANDLE h )
{
    this->h = h;
}

inline
XHANDLE::~XHANDLE()
{
    BOOL fClosed;

    if( h && h != INVALID_HANDLE_VALUE )
        fClosed = ::CloseHandle( h ),
        assert(fClosed);
}

inline
void XHANDLE::CloseHandle()
{
    ::CloseHandle( h );
    h = NULL;
}


class CondVar
{
public:
         CondVar();
         ~CondVar();
    void Enter();
    void Wait();
    void Release();
    void ReleaseAll();
    void Leave();

private:
    LONGLONG volatile m_llOwnersAndWaiters;
    DWORD    volatile m_dwRecursionCount;
    DWORD    volatile m_dwOwningThreadId;
    XHANDLE           m_xhEvtEnter,
                      m_xhSemRelease;

private:
    static
    DWORD Owners( LONGLONG llOwnersAndWaiters )
    {
        return (DWORD)llOwnersAndWaiters;
    }

    static
    DWORD Waiters( LONGLONG llOwnersAndWaiters )
    {
        return (DWORD)((DWORDLONG)llOwnersAndWaiters >> 32);
    }
};

CondVar::CondVar() :
    m_xhEvtEnter(   AutoThrowResourceException( ::CreateEvent( NULL, FALSE, FALSE, NULL ) ) ),
    m_xhSemRelease( AutoThrowResourceException( ::CreateSemaphore( NULL, 0, 0x7FFFFFFF, NULL ) ) )
{
    m_llOwnersAndWaiters = 0;
    m_dwRecursionCount   = 0;
    m_dwOwningThreadId   = 0;
}

CondVar::~CondVar()
{
}

void CondVar::Enter()
{
    if( m_dwOwningThreadId == FastGetCurrentThreadId() )
    {
        m_dwRecursionCount++;
        return;
    }

    LONGLONG llOwnersAndWaiters = ::InterlockedIncrement64( &m_llOwnersAndWaiters  );

    assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ));

    if( (Owners( llOwnersAndWaiters ) - Waiters( llOwnersAndWaiters )) > 1 )
        for( ; ::WaitForSingleObject( m_xhEvtEnter.h, INFINITE ) != WAIT_OBJECT_0; assert(false) );

    ONDEBUG(llOwnersAndWaiters = m_llOwnersAndWaiters);
    assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) && m_dwOwningThreadId == 0);

    m_dwOwningThreadId = FastGetCurrentThreadId();
}

void CondVar::Wait()
{
    LONGLONG llOwnersAndWaiters;
    DWORD    dwSavedRecusionCount;

    ONDEBUG(llOwnersAndWaiters = m_llOwnersAndWaiters);
    assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) && m_dwOwningThreadId == FastGetCurrentThreadId());

    m_dwOwningThreadId   = 0;
    dwSavedRecusionCount = m_dwRecursionCount;
    m_dwRecursionCount   = 0;
    llOwnersAndWaiters   = ::InterlockedAdd64( &m_llOwnersAndWaiters, 0x100000000 );

    if( Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) )
        for( ; !::SetEvent( m_xhEvtEnter.h ); assert(false) );

    HANDLE ahWait[2] = { m_xhEvtEnter.h, m_xhSemRelease.h };

    for( ; ::WaitForMultipleObjects( 2, ahWait, TRUE, INFINITE ) != WAIT_OBJECT_0; assert(false) );

    ONDEBUG(llOwnersAndWaiters = m_llOwnersAndWaiters);
    assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) && m_dwOwningThreadId == 0);

    m_dwOwningThreadId = FastGetCurrentThreadId();
    m_dwRecursionCount = dwSavedRecusionCount;
}

void CondVar::Release()
{
    LONGLONG llOwnersAndWaiters,
             llOwnersAndWaitersPrevOrChanged;

    for( llOwnersAndWaiters = m_llOwnersAndWaiters; Waiters( llOwnersAndWaiters ); llOwnersAndWaiters = llOwnersAndWaitersPrevOrChanged )
    {
        assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) && m_dwOwningThreadId == FastGetCurrentThreadId());

        if( (llOwnersAndWaitersPrevOrChanged = ::InterlockedCompareExchange64( &m_llOwnersAndWaiters, llOwnersAndWaiters - 0x100000000, llOwnersAndWaiters )) == llOwnersAndWaiters )
        {
            for (; !::ReleaseSemaphore( m_xhSemRelease.h, 1, NULL ); assert( false ));
            break;
        }
    }
}

void CondVar::ReleaseAll()
{
    LONGLONG llOwnersAndWaiters,
             llOwnersAndWaitersPrevOrChanged;

    for( llOwnersAndWaiters = m_llOwnersAndWaiters; Waiters( llOwnersAndWaiters ); llOwnersAndWaiters = llOwnersAndWaitersPrevOrChanged )
    {
        assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) && m_dwOwningThreadId == FastGetCurrentThreadId());

        if( (llOwnersAndWaitersPrevOrChanged = ::InterlockedCompareExchange64( &m_llOwnersAndWaiters, llOwnersAndWaiters & 0x0FFFFFFFF, llOwnersAndWaiters )) == llOwnersAndWaiters )
        {
            for (; !::ReleaseSemaphore( m_xhSemRelease.h, (LONG)Waiters( llOwnersAndWaiters ), NULL ); assert( false ));
            break;
        }
    }
}

void CondVar::Leave()
{
    LONGLONG llOwnersAndWaiters;
    LONG     lRecursionCount;

    ONDEBUG(llOwnersAndWaiters = m_llOwnersAndWaiters);
    assert(Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) && m_dwOwningThreadId == FastGetCurrentThreadId());

    if( (lRecursionCount = m_dwRecursionCount) != 0 )
    {
        m_dwRecursionCount = lRecursionCount - 1;
        return;
    }

    m_dwOwningThreadId = 0;

    llOwnersAndWaiters = ::InterlockedDecrement64( &m_llOwnersAndWaiters );

    if( Owners( llOwnersAndWaiters ) > Waiters( llOwnersAndWaiters ) )
        for( ; !::SetEvent( m_xhEvtEnter.h ); assert(false) );
}

/* ---------------------------- unit-test ---------------------------- */

#include <cstdio>
#include <cstdlib>
#include <deque>
#include <map>

DWORD WINAPI ThreadFunc( LPVOID lpvThreadParam );

using namespace std;

struct ThreadResult
{
    DWORD dwThreadId;
    DWORD dwCounter;
};

CondVar             cv;
deque<ThreadResult> dqtr;
bool volatile       fStop = false;

int main()
{
    int const              NTHREADS = 16;
    HANDLE                 ahThreads[NTHREADS];
    int                    i;
    std::map<DWORD, DWORD> mTRs;

    for( i = 0; i < NTHREADS; i++ )
        ahThreads[i] = CreateThread( NULL, 0, ThreadFunc, NULL, 0, NULL );

    for( i = 0; i < 10000; i++ )
    {
        ThreadResult tr;

        ::cv.Enter();

        if( ::dqtr.empty() )
            ::cv.Wait();

        tr = ::dqtr.front();
        ::dqtr.pop_front();
        ::cv.Leave();

        printf( "Thread: %08X - Number: %d\n", (unsigned)tr.dwThreadId, (unsigned)tr.dwCounter );

        if( mTRs.find( tr.dwThreadId ) == mTRs.end() )
            mTRs[tr.dwThreadId] = tr.dwCounter;
        else
            assert((mTRs[tr.dwThreadId] + 1) == tr.dwCounter),
            mTRs[tr.dwThreadId] = tr.dwCounter;
    }

    for( ::fStop = true, i = 0; i < NTHREADS; i++ )
        WaitForSingleObject( ahThreads[i], INFINITE );

    return 0;
}

DWORD WINAPI ThreadFunc( LPVOID lpvThreadParam )
{
    ThreadResult tr;

    tr.dwThreadId = FastGetCurrentThreadId();
    tr.dwCounter  = 0;

    for( ; !::fStop; tr.dwCounter++ )
    {
        ::cv.Enter();
        ::dqtr.push_back( tr );
        ::cv.Release();
        ::cv.Leave();
    }

    return 0;
}