event_queue.c

#include <stddef.h>

#include "event_queue.h"

// Toggle the MSB every time an index wraps.
// This tells pi and ci apart if they are equal (empty vs full).
#define WRAP_CHECK(var) do {            \
  if (((var) & QUEUE_SIZE_MASK) == 0) { \
    (var) = (~((var) & 0x80) & 0x80);   \
  }                                     \
} while (0);

void event_queue_init(event_queue_t *eq) {
  eq->pi = 0;
  eq->ci = 0;

  mutex_init(&eq->mutex);
  cond_init(&eq->cond);
}

void event_queue_produce(event_queue_t *eq, event_t v) {
  mutex_lock(&eq->mutex);

  // Add to queue, overwriting any previous element if it is full
  eq->values[eq->pi++ & QUEUE_SIZE_MASK] = v;
  WRAP_CHECK(eq->pi);

  // Advance consumer index if necessary
  if ((eq->pi & QUEUE_SIZE_MASK) == (eq->ci & QUEUE_SIZE_MASK)) {
    eq->ci++;
    WRAP_CHECK(eq->ci);
  }

  // Wake up a single consumer
  cond_signal(&eq->cond);

  mutex_unlock(&eq->mutex);
}

event_t event_queue_consume(event_queue_t *eq) {
  event_t e;

  mutex_lock(&eq->mutex);

  // Wait until there is an element to consume
  while (eq->ci == eq->pi) {
    cond_wait(&eq->cond, &eq->mutex);
  }

  e = eq->values[eq->ci++ & QUEUE_SIZE_MASK];
  WRAP_CHECK(eq->ci);

  mutex_unlock(&eq->mutex);

  return e;
};

event_queue.h

#ifndef _EVENT_QUEUE_H
#define _EVENT_QUEUE_H

#include <mutex.h>
#include <cond.h>

#include "event.h"

#define QUEUE_SIZE 16
#define QUEUE_SIZE_MASK ((QUEUE_SIZE)-1)

typedef struct event_queue_s event_queue_t;

struct event_queue_s {
  event_t values[QUEUE_SIZE];
  volatile uint8_t pi;
  volatile uint8_t ci;

  mutex_t mutex;
  cond_t cond;
};

void event_queue_init(event_queue_t *);

void event_queue_produce(event_queue_t *, event_t v);

event_t event_queue_consume(event_queue_t *);

#endif