linux/Thread.hh

Go to the documentation of this file.

 
#ifndef CRL_MULTISENSE_THREAD_HH
#define CRL_MULTISENSE_THREAD_HH
 
#include <unistd.h>
#include <stdint.h>
#include <pthread.h>
#include <sched.h>
#include <errno.h>
#include <string.h>
#include <linux/futex.h>
#include <sys/syscall.h>
 
#include <vector>
#include <deque>
 
#include "MultiSense/details/utility/Portability.hh"
 
#include <utility/Exception.hh>
 
namespace crl {
namespace multisense {
namespace details {
namespace utility {
 
//
// Forward declarations.
 
class ScopedLock;
 
//
// A simple class to wrap pthread creation and joining
 
class Thread {
public:
 
    static CRL_CONSTEXPR uint32_t FLAGS_DETACH = (1 << 0);
 
    Thread(void    *(*functionP)(void *),
           void    *contextP=NULL,
           uint32_t flags=0,
           int32_t  scheduler=-1,
           int32_t  priority=0) : m_flags(flags) {
 
        pthread_attr_t tattr;
        pthread_attr_init(&tattr);
 
        //
        // -1 means the user wants default scheduling behavior
 
        if (-1 != scheduler) {
            struct sched_param sattr = {0};
 
            //
            // Set our scheduling policy
 
            if (0 != pthread_attr_setschedpolicy(&tattr, scheduler))
                CRL_EXCEPTION("pthread_attr_setschedpolicy(scheduler=%d) failed: %s",
                                   scheduler, strerror(errno));
            //
            // Set our scheduling parameters (just priority)
 
            sattr.sched_priority = priority;
            if (0 != pthread_attr_setschedparam(&tattr, &sattr))
                CRL_EXCEPTION("pthread_attr_setschedparam(pri=%d) failed: %s",
                                   priority, strerror(errno));
            //
            // We must set EXPLICIT_SCHED so the parent's scheduler is not
            // automatically inherited
 
            if (0 != pthread_attr_setinheritsched(&tattr, PTHREAD_EXPLICIT_SCHED))
                CRL_EXCEPTION("pthread_attr_setinheritsched(explicit) failed: %s",
                                   strerror(errno));
        }
 
        //
        // Create detached, if asked to do so
 
        if (FLAGS_DETACH & m_flags &&
            0 != pthread_attr_setdetachstate(&tattr, PTHREAD_CREATE_DETACHED))
            CRL_EXCEPTION("pthread_attr_setdetachstate() failed: %s", strerror(errno));
 
        //
        // Finally, create the thread
 
        if (0 != pthread_create(&m_id, &tattr, functionP, contextP))
            CRL_EXCEPTION("pthread_create() failed: %s", strerror(errno));
    };
 
    ~Thread() {
        if (!(m_flags & FLAGS_DETACH) &&
            0 !=pthread_join(m_id, NULL))
            CRL_DEBUG("pthread_join() failed: %s\n", strerror(errno));
    };
 
private:
 
    uint32_t  m_flags;
    pthread_t m_id;
};
 
//
// A simple mutex class
 
class Mutex {
public:
    friend class ScopedLock;
 
    Mutex() : m_mutex() {
        if (0 != pthread_mutex_init(&m_mutex, NULL))
            CRL_EXCEPTION("pthread_mutex_init() failed: %s",
                          strerror(errno));
    }
 
    ~Mutex() {
        pthread_mutex_destroy(&m_mutex);
    };
 
private:
    pthread_mutex_t m_mutex;
};
 
//
// A simple scoped lock class
 
class ScopedLock
{
public:
 
    ScopedLock(Mutex& mutex) {
        this->lock(&mutex.m_mutex);
    };
 
    ScopedLock(pthread_mutex_t *lockP) {
        this->lock(lockP);
    };
 
    ScopedLock(pthread_mutex_t& lock) {
        this->lock(&lock);
    };
 
    ~ScopedLock() {
        pthread_mutex_unlock(m_lockP);
    };
 
private:
 
    void lock(pthread_mutex_t *lockP) {
        m_lockP = lockP;
        pthread_mutex_lock(m_lockP);
    };
 
    pthread_mutex_t *m_lockP;
};
 
// A futex-based semaphore.
//
// This implementation does not work across processes.
 
class Semaphore {
public:
 
    //
    // Wait for a post (decrement). If thread contention,
    // we may wake up, but be unable to snatch
    // the bait.. hence the while loop.
 
    bool wait() {
        do {
            if (0 == wait_())
                return true;
        } while (1);
    };
 
    //
    // Wait for a post, retrying until timeout
 
    bool timedWait(const double& timeout) {
 
        if (timeout < 0.0)
            CRL_EXCEPTION("invalid timeout: %f", timeout);
 
        struct timespec ts;
        ts.tv_sec  = timeout;
        ts.tv_nsec = (timeout - ts.tv_sec) * 1e9;
 
        do {
            int32_t ret = wait_(&ts);
 
            if (0 == ret)
                return true;
            else if (ETIMEDOUT == ret)
                return false;
 
        } while (1);
    };
 
    //
    // Post to the semaphore (increment.) Here we
    // signal the futex to wake up any waiters.
 
    bool post() {
 
        //
        // Limit the posts, if asked to do so
 
        if (m_maximum > 0 && m_avail >= static_cast<int>(m_maximum))
            return false;
 
        const int32_t nval = __sync_add_and_fetch(&m_avail, 1);
        if (m_waiters > 0)
            syscall(__NR_futex, &m_avail, FUTEX_WAKE, nval, NULL, 0, 0);
 
        return true;
    };
 
    //
    // Decrement the semaphore to zero in one-shot.. may
    // fail with thread contention, returns true if
    // successful
 
    bool clear() {
        int32_t val = m_avail;
        if (val > 0)
            return __sync_bool_compare_and_swap(&m_avail, val, 0);
        return true;
    };
 
    int32_t count    () { return m_avail;   };
    int32_t waiters  () { return m_waiters; };
    bool    decrement() { return wait();    };
    bool    increment() { return post();    };
 
    Semaphore(std::size_t max=0) :
        m_maximum(max),
        m_avail(0),
        m_waiters(0) {};
 
    ~Semaphore() {};
 
private:
 
    //
    // This actually does the synchronized decrement if possible, and goes
    // to sleep on the futex if not.
 
    inline int32_t wait_(const struct timespec *tsP=NULL) {
 
        //
        // Can we decrement the requested amount? If so, return success
 
        const int32_t val = m_avail;
        if (val >= 1 && __sync_bool_compare_and_swap(&m_avail, val, val - 1))
            return 0;
 
        //
        // We must go to sleep until someone increments. Also keep track of
        // how many threads are waiting on this futex.
 
        __sync_fetch_and_add(&m_waiters, 1);
        const int32_t ret = syscall(__NR_futex, &m_avail, FUTEX_WAIT, val, tsP, 0, 0);
        __sync_fetch_and_sub(&m_waiters, 1);
 
        //
        // If we just woke up on the futex, then return EAGAIN, so we
        // can come back in here and attempt a synchronized decrement again.
 
        if (ETIMEDOUT == ret || -1 == ret) // hmmm.. timeouts are returning -1
            return ETIMEDOUT;
        else
            return EAGAIN;
    };
 
    typedef int32_t aligned_int32_t __attribute__((aligned (4))); // unnecessary ?
 
    const std::size_t m_maximum;
    aligned_int32_t   m_avail;
    aligned_int32_t   m_waiters;
};
 
//
// A templatized variable signaler
 
template<class T> class WaitVar {
public:
 
    void post(const T& data) {
        {
            ScopedLock lock(m_lock);
            m_val = data;
        }
        m_sem.post();
    };
 
    bool wait(T& data) {
        m_sem.wait();
        {
            ScopedLock lock(m_lock);
            data = m_val;
        }
        return true;
    };
 
    bool timedWait(T& data,
                   const double& timeout) {
 
        if (false == m_sem.timedWait(timeout))
            return false;
        {
            ScopedLock lock(m_lock);
            data = m_val;
        }
        return true;
    }
 
    //
    // Use a semaphore with max value of 1. The WaitVar will
    // either be in a signaled state, or not.
 
    WaitVar() : m_val(),
                m_lock(),
                m_sem(1) {};
 
private:
 
    T                 m_val;
    Mutex     m_lock;
    Semaphore m_sem;
};
 
//
// A templatized wait queue
 
template <class T> class WaitQueue {
public:
 
    void post(const T& data) {
        bool postSem=true;
        {
            ScopedLock lock(m_lock);
 
            //
            // Limit deque size, if requested
 
            if (m_maximum > 0 &&
                m_maximum == m_queue.size()) {
 
                //
                // If at max entries, we will pop_front the oldest,
                // push_back the newest, and leave the semaphore alone
 
                m_queue.pop_front();
                postSem = false;
            }
 
            m_queue.push_back(data);
        }
        if (postSem)
            m_sem.post();
    };
 
    void kick() {
        m_sem.post();
    };
 
    bool wait(T& data) {
        m_sem.wait();
        {
            ScopedLock lock(m_lock);
 
            if (0 == m_queue.size())
                return false;
            else {
                data = m_queue.front();
                m_queue.pop_front();
                return true;
            }
        }
    }
 
    uint32_t waiters() {
        return m_sem.waiters();
    };
 
    uint32_t size() {
        ScopedLock lock(m_lock);
        return m_queue.size();
    }
 
    void clear() {
        ScopedLock lock(m_lock);
        m_queue.clear();
        while(false == m_sem.clear());
    }
 
    WaitQueue(std::size_t max=0) :
        m_maximum(max) {};
 
private:
 
    const std::size_t m_maximum;
    std::deque<T>     m_queue;
    Mutex             m_lock;
    Semaphore         m_sem;
};
 
}}}} // namespaces
 
#endif /* #ifndef CRL_MULTISENSE_THREAD_HH */