data_lockfree.hpp

Go to the documentation of this file.

#ifndef ROSRTT_DATA_LOCK_FREE_HPP
#define ROSRTT_DATA_LOCK_FREE_HPP

#include "oro_arch.h"

namespace hpcl_rtt
{
template<class T>
class DataObjectLockFree
{
public:
  typedef T DataType;
  typedef typename boost::shared_ptr<DataObjectLockFree<T> > shared_ptr;

  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];
  }
}; 

}
#endif