gqueue.c

#include <gqueue.h>

void queue_enqueue(volatile void *q, const void *elt)
{
	volatile struct generic_queue *gq = q;
	if (gq->len == 0) {
		gq->head = gq->memory;
		gq->tail = gq->memory;
	} else {
		if (gq->tail == gq->memory + (gq->max_capacity - 1) * gq->item_size)
			/* FIXME: Should something be done about
			 * queue length? Substact one, maybe?*/
			gq->tail = gq->memory;
		else
			gq->tail = (uint8_t *)gq->tail + gq->item_size;
	}

	memcpy((void*)gq->tail, elt, gq->item_size);
	gq->len++;
}

void queue_dequeue(volatile void *q, void *elt)
{
	volatile struct generic_queue *gq = q;
	memcpy(elt, (void*)gq->head, gq->item_size);

	if (gq->head == gq->memory + (gq->max_capacity - 1) * gq->item_size)
		/* FIXME: Should something be done about
		 * queue length? Substact one, maybe?*/
		gq->head = gq->memory;
	else
		if (gq->len > 1)
			gq->head = (uint8_t *)gq->head + gq->item_size;
	gq->len--;
}