The FifoEmbed Library

Listing 3

#include "fifo.h"

/* Initialization */
static inline
void pktq_init (FIFO_T *q, FIFO_CELL_T *base, int size) {
  volatile FIFO_CELL_T *ptr;
  int n;

  FIFO_INIT (q, base, size);
  for (ptr = q->head.l, n = size; n--; ptr++) *ptr = NULL;
}

/* Producers */
static inline
FIFO_CELL_T *pktq_ins_valid (FIFO_T *q, FIFO_SIZE_T n) {
  FIFO_SIZE_T avail = FIFO_AVAIL (q);

  if (n != 0 && ((n < 0 && n >= avail - q->size + 2)
                 || n <= avail))
  {
    volatile FIFO_CELL_T *ptr = q->tail.l + n - 1;
    ptr = CIRC_ARR_WRAP (ptr, q->base, q->end, q->size);
    if (*ptr == NULL) return (FIFO_CELL_T *) ptr;
  }

  return NULL;
}

static inline void pktq_ins_at (FIFO_T *q, int n,
  FIFO_CELL_T pkt, volatile FIFO_CELL_T *ptr)
{
  *ptr = pkt;
  if (n > 0)
    q->tail.w = CIRC_ARR_ADV_PTR (ptr, q->base, q->end);
}

static inline
void pktq_ins_oo (FIFO_T *q, int n, FIFO_CELL_T pkt) {
  int len = FIFO_USED (q);
  volatile FIFO_CELL_T *ptr = q->tail.l + n - 1;

  ptr = CIRC_ARR_WRAP (ptr, q->base, q->end, q->size);
  pktq_ins_at (q, n, pkt, ptr);
}

/* Consumers */
STATIC_INLINE void pktq_drop (FIFO_T *q, FIFO_SIZE_T n) {
  FIFO_SIZE_T total;
  volatile FIFO_CELL_T *ptr;

  for (ptr = q->head.l, total = 0; n;
       ptr = CIRC_ARR_ADV_PTR (ptr, q->base, q->end), total++)
    if (ptr == q->tail.r) break;
    else if (*ptr) { *ptr = NULL; n--; }
  FIFO_DROP (q, total);
}

static inline FIFO_CELL_T pktq_read (FIFO_T *q) {
  FIFO_CELL_T result = NULL;
  FIFO_SIZE_T total;
  volatile FIFO_CELL_T *ptr;

  for (ptr = q->head.l, total = 0; ptr != q->tail.r;
       ptr = CIRC_ARR_ADV_PTR (ptr, q->base, q->end), total++)
    if (*ptr) {
      result = *ptr; *ptr = NULL; total++;
      break;
    }

  FIFO_DROP (q, total);
  return result;
}