ListLockFree.hpp

/***************************************************************************
  tag: Peter Soetens  Wed Jan 18 14:11:39 CET 2006  ListLockFree.hpp

                        ListLockFree.hpp -  description
                           -------------------
    begin                : Wed January 18 2006
    copyright            : (C) 2006 Peter Soetens
    email                : peter.soetens@mech.kuleuven.be

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU General Public                   *
 *   License as published by the Free Software Foundation;                 *
 *   version 2 of the License.                                             *
 *                                                                         *
 *   As a special exception, you may use this file as part of a free       *
 *   software library without restriction.  Specifically, if other files   *
 *   instantiate templates or use macros or inline functions from this     *
 *   file, or you compile this file and link it with other files to        *
 *   produce an executable, this file does not by itself cause the         *
 *   resulting executable to be covered by the GNU General Public          *
 *   License.  This exception does not however invalidate any other        *
 *   reasons why the executable file might be covered by the GNU General   *
 *   Public License.                                                       *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU General Public             *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/


#ifndef ORO_LIST_LOCK_FREE_HPP
#define ORO_LIST_LOCK_FREE_HPP

#include <vector>
#include "os/oro_atomic.h"
#include "os/CAS.hpp"
#include <boost/intrusive_ptr.hpp>

#ifdef ORO_PRAGMA_INTERFACE
#pragma interface
#endif

namespace RTT
{
    namespace detail {
        struct IntrusiveStorage
        {
            oro_atomic_t ref;
            IntrusiveStorage() {
                oro_atomic_set(&ref,0);
            }
            virtual ~IntrusiveStorage() {
            }
        };
    }
}


void intrusive_ptr_add_ref(RTT::detail::IntrusiveStorage* p );
void intrusive_ptr_release(RTT::detail::IntrusiveStorage* p );

namespace RTT
{
    template< class T>
    class ListLockFree
    {
    public:
        const unsigned int MAX_THREADS;

        typedef T value_t;
    private:
        typedef std::vector<value_t> BufferType;
        typedef typename BufferType::iterator Iterator;
        typedef typename BufferType::const_iterator CIterator;
        struct Item {
            Item()  {
                //ORO_ATOMIC_INIT(count);
                oro_atomic_set(&count,-1);
            }
            mutable oro_atomic_t count;  // refcount
            BufferType data;
        };

        struct StorageImpl : public detail::IntrusiveStorage
        {
            Item* items;
            StorageImpl(size_t alloc) : items( new Item[alloc] ) {
            }
            ~StorageImpl() {
                delete[] items;
            }
            Item& operator[](int i) {
                return items[i];
            }
        };

        typedef boost::intrusive_ptr<StorageImpl> Storage;

        Storage newStorage(size_t alloc, size_t items, bool init = true)
        {
            Storage st( new StorageImpl(alloc) );
            for (unsigned int i=0; i < alloc; ++i) {
                (*st)[i].data.reserve( items ); // pre-allocate
            }
            // bootstrap the first list :
            if (init) {
                active = &(*st)[0];
                oro_atomic_inc( &active->count );
            }

            return st;
        }

        Storage bufs;
        Item* volatile active;
        Item* volatile blankp;

        // each thread has one 'working' buffer, and one 'active' buffer
        // lock. Thus we require to allocate twice as much buffers as threads,
        // for all the locks to succeed in a worst case scenario.
        inline size_t BufNum() const {
            return MAX_THREADS * 2;
        }

        size_t required;
    public:
        ListLockFree(unsigned int lsize, unsigned int threads = ORONUM_OS_MAX_THREADS )
            : MAX_THREADS( threads ), blankp(0), required(lsize)
        {
            const unsigned int BUF_NUM = BufNum();
            bufs = newStorage( BUF_NUM, lsize );
        }

        ~ListLockFree() {
        }

        size_t capacity() const
        {
            size_t res;
            Storage st;
            Item* orig = lockAndGetActive(st);
            res = orig->data.capacity();
            oro_atomic_dec( &orig->count ); // lockAndGetActive
            return res;
        }

        size_t size() const
        {
            size_t res;
            Storage st;
            Item* orig = lockAndGetActive(st);
            res = orig->data.size();
            oro_atomic_dec( &orig->count ); // lockAndGetActive
            return res;
        }

        bool empty() const
        {
            bool res;
            Storage st;
            Item* orig = lockAndGetActive(st);
            res = orig->data.empty();
            oro_atomic_dec( &orig->count ); // lockAndGetActive
            return res;
        }

        void grow(size_t items = 1) {
            required += items;
            if (required > this->capacity()) {
                this->reserve( required*2 );
            }
        }
        void shrink(size_t items = 1) {
            required -= items;
        }

        void reserve(size_t lsize)
        {
            if (lsize <= this->capacity() )
                return;

            const unsigned int BUF_NUM = BufNum();
            Storage res( newStorage(BUF_NUM, lsize, false) );

            // init the future 'active' buffer.
            Item* nextbuf = &(*res)[0];
            oro_atomic_inc( &nextbuf->count );

            // temporary for current active buffer.
            Item* orig = 0;

            // prevent current bufs from deletion.
            // will free upon return.
            Storage save = bufs;
            // active points at old, bufs points at new:
            // first the refcount is added to res, then
            // bufs' pointer is switched to res' pointer,
            // and stored in a temporary. Then the temp
            // is destructed and decrements bufs' old reference.
            bufs = res;
            // from now on, any findEmptyBuf will use the new bufs,
            // unless the algorithm was entered before the switch.
            // then, it will write the result to the old buf.
            // if it detects we updated active, it will find an
            // empty buf in the new buf. If it gets there before
            // our CAS, our CAS will fail and we try to recopy
            // everything. This retry may be unnessessary
            // if the data already is in the new buf, but for this
            // cornercase, we must be sure.

            // copy active into new:
            do {
                if (orig)
                    oro_atomic_dec(&orig->count);
                orig = lockAndGetActive(); // active is guaranteed to point in valid buffer ( save or bufs )
                nextbuf->data.clear();
                Iterator it( orig->data.begin() );
                while ( it != orig->data.end() ) {
                    nextbuf->data.push_back( *it );
                    ++it;
                }
                // see explanation above: active could have changed,
                // and still point in old buffer. we could check this
                // with pointer arithmetics, but this is not a performant
                // method.
            } while ( OS::CAS(&active, orig, nextbuf ) == false);
            // now,
            // active is guaranteed to point into bufs.
            assert( pointsTo( active, bufs ) );

            oro_atomic_dec( &orig->count ); // lockAndGetActive
            oro_atomic_dec( &orig->count ); // ref count
        }

        void clear()
        {
            Storage bufptr;
            Item* orig(0);
            Item* nextbuf(0);
            int items = 0;
            do {
                if (orig) {
                    oro_atomic_dec(&orig->count);
                    oro_atomic_dec(&nextbuf->count);
                }
                orig = lockAndGetActive(bufptr);
                items = orig->data.size();
                nextbuf = findEmptyBuf(bufptr); // find unused Item in bufs
                nextbuf->data.clear();
            } while ( OS::CAS(&active, orig, nextbuf ) == false );
            oro_atomic_dec( &orig->count ); // lockAndGetActive
            oro_atomic_dec( &orig->count ); // ref count
        }

        bool append( value_t item )
        {
            Item* orig=0;
            Storage bufptr;
            Item* usingbuf(0);
            do {
                if (orig) {
                    oro_atomic_dec(&orig->count);
                    oro_atomic_dec(&usingbuf->count);
                }
                orig = lockAndGetActive( bufptr );
                if ( orig->data.size() == orig->data.capacity() ) { // check for full
                    oro_atomic_dec( &orig->count );
                    return false;
                }
                usingbuf = findEmptyBuf( bufptr ); // find unused Item in bufs
                usingbuf->data = orig->data;
                usingbuf->data.push_back( item );
            } while ( OS::CAS(&active, orig, usingbuf ) ==false);
            oro_atomic_dec( &orig->count ); // lockAndGetActive()
            oro_atomic_dec( &orig->count ); // set list free
            return true;
        }

        value_t front() const
        {
            Storage bufptr;
            Item* orig = lockAndGetActive(bufptr);
            value_t ret(orig->data.front());
            oro_atomic_dec( &orig->count ); //lockAndGetActive
            return ret;
        }

        value_t back() const
        {
            Storage bufptr;
            Item* orig = lockAndGetActive(bufptr);
            value_t ret(orig->data.back());
            oro_atomic_dec( &orig->count ); //lockAndGetActive
            return ret;
        }

        size_t append(const std::vector<T>& items)
        {
            Item* usingbuf(0);
            Item* orig=0;
            int towrite  = items.size();
            Storage bufptr;
            do {
                if (orig) {
                    oro_atomic_dec(&orig->count);
                    oro_atomic_dec(&usingbuf->count);
                }

                orig = lockAndGetActive( bufptr );
                int maxwrite = orig->data.capacity() - orig->data.size();
                if ( maxwrite == 0 ) {
                    oro_atomic_dec( &orig->count ); // lockAndGetActive()
                    return 0;
                }
                if ( towrite > maxwrite )
                    towrite = maxwrite;
                usingbuf = findEmptyBuf( bufptr ); // find unused Item in bufs
                usingbuf->data = orig->data;
                usingbuf->data.insert( usingbuf->data.end(), items.begin(), items.begin() + towrite );
            } while ( OS::CAS(&active, orig, usingbuf ) ==false );
            oro_atomic_dec( &orig->count ); // lockAndGetActive()
            oro_atomic_dec( &orig->count ); // set list free
            return towrite;
        }


        bool erase( value_t item )
        {
            Item* orig=0;
            Item* nextbuf(0);
            Storage bufptr;
            do {
                if (orig) {
                    oro_atomic_dec(&orig->count);
                    oro_atomic_dec(&nextbuf->count);
                }
                orig = lockAndGetActive( bufptr ); // find active in bufptr
                // we do this in the loop because bufs can change.
                nextbuf = findEmptyBuf( bufptr ); // find unused Item in same buf.
                Iterator it( orig->data.begin() );
                while (it != orig->data.end() && !( *it == item ) ) {
                    nextbuf->data.push_back( *it );
                    ++it;
                }
                if ( it == orig->data.end() ) {
                    oro_atomic_dec( &orig->count );
                    oro_atomic_dec( &nextbuf->count );
                    return false; // item not found.
                }
                ++it; // skip item.
                while ( it != orig->data.end() ) {
                    nextbuf->data.push_back( *it );
                    ++it;
                }
            } while ( OS::CAS(&active, orig, nextbuf ) ==false );
            oro_atomic_dec( &orig->count ); // lockAndGetActive
            oro_atomic_dec( &orig->count ); // ref count
            return true;
        }

        template<class Function>
        void apply(Function func )
        {
            Storage st;
            Item* orig = lockAndGetActive(st);
            Iterator it( orig->data.begin() );
            while ( it != orig->data.end() ) {
                func( *it );
                ++it;
            }
            oro_atomic_dec( &orig->count ); //lockAndGetActive
        }

        template<class Function>
        void apply_and_blank(Function func, value_t blank )
        {
            Storage st;
            Item* orig = lockAndGetActive(st);
            Item* newp = findEmptyBuf(st);
            Iterator it( orig->data.begin() );
            // first copy the whole list.
            while ( it != orig->data.end() ) {
                newp->data.push_back( *it );
                ++it;
            }
            blankp = newp;
            it = blankp->data.begin();
            // iterate over copy and skip blanks.
            while ( it != blankp->data.end() ) {
                // XXX Race condition: 'it' can be blanked after
                // comparison or even during func.
                value_t a = *it;
                if ( !(a == blank) )
                    func( a );
                ++it;
            }
            blankp = 0;

            oro_atomic_dec( &orig->count ); //lockAndGetActive
            oro_atomic_dec( &newp->count ); //findEmptyBuf
        }

        bool erase_and_blank(value_t item, value_t blank )
        {
            Storage st;
            bool res = this->erase(item);
            Item* orig = lockAndGetBlank(st);
            if (orig) {
                Iterator it( orig->data.begin() );
                // item may still not be present in the blank-list.
                while ( *it != item ) {
                    ++it;
                    if (it == orig->data.end() ) {
                        oro_atomic_dec( &orig->count ); //lockAndGetBlank
                        return res;
                    }
                }
                (*it) = blank;
                oro_atomic_dec( &orig->count ); //lockAndGetBlank
            }
            return res;
        }

        template<class Function>
        value_t find_if( Function func, value_t blank = value_t() )
        {
            Storage st;
            Item* orig = lockAndGetActive(st);
            Iterator it( orig->data.begin() );
            while ( it != orig->data.end() ) {
                if (func( *it ) == true ) {
                    oro_atomic_dec( &orig->count ); //lockAndGetActive
                    return *it;
                }
                ++it;
            }
            oro_atomic_dec( &orig->count ); //lockAndGetActive
            return blank;
        }
    private:
        Item* findEmptyBuf(Storage& bufptr) {
            // These two functions are copy/pasted from BufferLockFree.
            // If MAX_THREADS is large enough, this will always succeed :
            Item* start = &(*bufptr)[0];
            while( true ) {
                if ( oro_atomic_inc_and_test( &start->count ) )
                    break;
                oro_atomic_dec( &start->count );
                ++start;
                if (start == &(*bufptr)[0] + BufNum() )
                    start = &(*bufptr)[0]; // in case of races, rewind
            }
            assert( pointsTo(start, bufptr) );
            start->data.clear();
            return start; // unique pointer across all threads
        }

        Item* lockAndGetActive(Storage& bufptr) const {
            // This is a kind-of smart-pointer implementation
            // We could move it into Item itself and overload operator=
            Item* orig=0;
            do {
                if (orig)
                    oro_atomic_dec( &orig->count );
                bufptr = bufs;
                orig = active;
                // also check that orig points into bufptr.
                if ( pointsTo(orig, bufptr) )
                    oro_atomic_inc( &orig->count );
                else {
                    orig = 0;
                }
                // this synchronisation point is 'aggressive' (a _sufficient_ condition)
                // if active is still equal to orig, the increase of orig->count is
                // surely valid, since no contention (change of active) occured.
            } while ( active != orig );
            assert( pointsTo(orig, bufptr) );
            return orig;
        }

        Item* lockAndGetActive() const {
            // only operates on active's refcount.
            Item* orig=0;
            do {
                if (orig)
                    oro_atomic_dec( &orig->count );
                orig = active;
                oro_atomic_inc( &orig->count );
                // this synchronisation point is 'aggressive' (a _sufficient_ condition)
                // if active is still equal to orig, the increase of orig->count is
                // surely valid, since no contention (change of active) occured.
            } while ( active != orig );
            return orig;
        }

        Item* lockAndGetBlank(Storage& bufptr) const {
            Item* orig=0;
            do {
                if (orig)
                    oro_atomic_dec( &orig->count );
                bufptr = bufs;
                orig = blankp;
                if (orig == 0)
                    return 0; // no blankp.
                // also check that orig points into bufptr.
                if ( pointsTo(orig, bufptr) )
                    oro_atomic_inc( &orig->count );
                else {
                    orig = 0;
                }
                // this synchronisation point is 'aggressive' (a _sufficient_ condition)
                // if active is still equal to orig, the increase of orig->count is
                // surely valid, since no contention (change of active) occured.
            } while ( blankp != orig );
            assert( pointsTo(orig, bufptr) );
            return orig;
        }

        inline bool pointsTo( Item* p, const Storage& bf ) const {
            return p >= &(*bf)[0] && p <= &(*bf)[ BufNum() - 1 ];
        }

    };
}

#endif