DataObjectLockFree.hpp

Go to the documentation of this file.

/***************************************************************************
 tag: Peter Soetens  Mon Jan 19 14:11:26 CET 2004  DataObjectLockFree.hpp

 DataObjectLockFree.hpp -  description
 -------------------
 begin                : Mon January 19 2004
 copyright            : (C) 2004 Peter Soetens
 email                : peter.soetens@mech.kuleuven.ac.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                                *
 *                                                                         *
 ***************************************************************************/

#pragma once

#include <youbot_driver/generic/dataobjectlockfree/target.hpp>
#include <youbot_driver/generic/dataobjectlockfree/os/oro_arch.h>

namespace youbot
{

template<class T>
  class DataObjectLockFree
  {
  public:
    typedef T DataType;

    const unsigned int MAX_THREADS; // = 2
  private:
    const unsigned int BUF_LEN; // = MAX_THREADS+2

    struct DataBuf
    {
      DataBuf() :
          data(), counter(), next()
      {
        oro_atomic_set(&counter, 0);
      }
      DataType data;
      mutable oro_atomic_t counter;
      DataBuf* next;
    };

    typedef DataBuf* volatile VolPtrType;
    typedef DataBuf ValueType;
    typedef DataBuf* PtrType;

    VolPtrType read_ptr;
    VolPtrType write_ptr;

    DataBuf* data;
  public:

    DataObjectLockFree(const T& initial_value = T(), unsigned int max_threads = 2) :
        MAX_THREADS(max_threads), BUF_LEN(max_threads + 2), read_ptr(0), write_ptr(0)
    {
      data = new DataBuf[BUF_LEN];
      read_ptr = &data[0];
      write_ptr = &data[1];
      data_sample(initial_value);
    }

    ~DataObjectLockFree()
    {
      delete[] data;
    }

    virtual DataType Get() const
    {
      DataType cache;
      Get(cache);
      return cache;
    }

    virtual void Get(DataType& pull) const
    {
      PtrType reading;
      // loop to combine Read/Modify of counter
      // This avoids a race condition where read_ptr
      // could become write_ptr ( then we would read corrupted data).
      do
      {
        reading = read_ptr;            // copy buffer location
        oro_atomic_inc(&reading->counter); // lock buffer, no more writes
        // XXX smp_mb
        if (reading != read_ptr)     // if read_ptr changed,
          oro_atomic_dec(&reading->counter); // better to start over.
        else
          break;
      } while (true);
      // from here on we are sure that 'reading'
      // is a valid buffer to read from.
      pull = reading->data;               // takes some time
      // XXX smp_mb
      oro_atomic_dec(&reading->counter);       // release buffer
    }

    virtual void Set(const DataType& push)
    {
      // writeout in any case
      write_ptr->data = push;
      PtrType wrote_ptr = write_ptr;
      // if next field is occupied (by read_ptr or counter),
      // go to next and check again...
      while (oro_atomic_read( &write_ptr->next->counter ) != 0 || write_ptr->next == read_ptr)
      {
        write_ptr = write_ptr->next;
        if (write_ptr == wrote_ptr)
          return; // nothing found, to many readers !
      }

      // we will be able to move, so replace read_ptr
      read_ptr = wrote_ptr;
      write_ptr = write_ptr->next; // we checked this in the while loop
    }

    virtual void data_sample(const DataType& sample)
    {
      // prepare the buffer.
      for (unsigned int i = 0; i < BUF_LEN - 1; ++i)
      {
        data[i].data = sample;
        data[i].next = &data[i + 1];
      }
      data[BUF_LEN - 1].data = sample;
      data[BUF_LEN - 1].next = &data[0];
    }
  };
}