Queue.hh

Go to the documentation of this file.

#ifndef COSA_QUEUE_HH
#define COSA_QUEUE_HH

#include "Cosa/Types.h"
#include "Cosa/Power.hh"

template <class T, uint8_t NMEMB>
class Queue {
  static_assert(NMEMB && !(NMEMB & (NMEMB - 1)), "NMEMB should be power of 2");
public:
  Queue() :
    m_put(0),
    m_get(0)
  {}

  uint8_t available() const
    __attribute__((always_inline))
  {
    return ((NMEMB + m_put - m_get) & MASK);
  }

  uint8_t room() const
    __attribute__((always_inline))
  {
    return ((NMEMB - m_put + m_get - 1) & MASK);
  }

  bool enqueue(T* data);

  bool enqueue_P(const T* data);

  bool dequeue(T* data);

  void await(T* data);

private:
  static const uint8_t MASK = (NMEMB - 1);
  volatile uint8_t m_put;
  volatile uint8_t m_get;
  T m_buffer[NMEMB];
};

template <class T, uint8_t NMEMB>
bool
Queue<T,NMEMB>::enqueue(T* data)
{
  synchronized {
    uint8_t next = (m_put + 1) & MASK;
    if (UNLIKELY(next == m_get)) return (false);
    m_buffer[next] = *data;
    m_put = next;
  }
  return (true);
}

template <class T, uint8_t NMEMB>
bool
Queue<T,NMEMB>::enqueue_P(const T* data)
{
  synchronized {
    uint8_t next = (m_put + 1) & MASK;
    if (UNLIKELY(next == m_get)) return (false);
    memcpy_P(&m_buffer[next], data, sizeof(T));
    m_put = next;
  }
  return (true);
}

template <class T, uint8_t NMEMB>
bool
Queue<T,NMEMB>::dequeue(T* data)
{
  synchronized {
    if (UNLIKELY(m_get == m_put)) return (false);
    uint8_t next = (m_get + 1) & MASK;
    m_get = next;
    *data = m_buffer[next];
  }
  return (true);
}

template <class T, uint8_t NMEMB>
void
Queue<T,NMEMB>::await(T* data)
{
  while (!dequeue(data)) yield();
}

#endif