One of my favorite real-time system tricks is to use queues, or linked-list
to handle message passing. Then the whole system can be lots of small co-routines, without the overhead of task-switching. Small and tight code really can run fast. Usually the problem you get with this is malloc/free overhead.

A pre-allocated chunk based system solves that. I'll post it to a future blog.

## queue.c
#include 
#include 

#include "include/queue.h"
/*
** Queue.c
*/

struct entry_struct *unqueue(q)
struct queue_struct *q;
{
struct entry_struct *entry;

  if (q==NULL)         return(NULL);
  if (q->head == NULL) return(NULL);
  entry = q->head;
  q->head = entry->next;
  if (q->head == NULL) q->tail = NULL;
  entry->next = NULL;
  entry->prev = NULL;
  return(entry);
}

void queue(q,entry)
struct queue_struct *q; 
struct entry_struct *entry;
{
  if (q==NULL) return;
  if (entry == NULL) return;
  entry->next = NULL;
  entry->prev = NULL;
  if (q->head == NULL){
     q->head = entry;
     q->tail = entry;
    return;
    }
  entry->prev = q->tail;
  q->tail->next = entry;
  q->tail = entry;
 return;
}

struct queue_struct *init_queue(void)
{
struct queue_struct *q;

    q = (struct queue_struct *)malloc(sizeof(struct queue_struct *));
        q->head = NULL;
        q->tail = NULL;
        return(q);
}

## queue.h
/*
** Queue.h
*/

struct entry_struct {
       struct entry_struct *next;
       struct entry_struct *prev;
};

struct queue_struct {
     struct entry_struct  *head;
     struct entry_struct *tail;
};

struct entry_struct *unqueue(struct queue_struct *);
void queue(struct queue_struct *, struct entry_struct *);
struct queue_struct *init_queue(void);