136 lines
2.7 KiB
C
136 lines
2.7 KiB
C
#ifndef QUEUE_H
|
|
#define QUEUE_H
|
|
|
|
/* FIFO queue. Functions like queue_get handle disabling/restoring
|
|
interrupts automatically; prefixed versions like _queue_get do
|
|
not. */
|
|
|
|
/* This is kind of a mess, because it's been turned to inline mush in
|
|
order to improve speed. */
|
|
|
|
#include <stdint.h>
|
|
#include <util/atomic.h>
|
|
|
|
#define __QUEUE_NEXT(q, x) (((q->x+1) == q->len) ? 0 : (q->x+1))
|
|
|
|
struct queue {
|
|
volatile int len;
|
|
volatile uint8_t *data;
|
|
volatile int start;
|
|
volatile int end;
|
|
};
|
|
|
|
/* Initialize the queue "q"
|
|
Pass the data buffer and maximum length to use */
|
|
static inline void queue_init(struct queue *q, uint8_t *data, int len)
|
|
{
|
|
q->len = len;
|
|
q->data = data;
|
|
q->start = 0;
|
|
q->end = 0;
|
|
}
|
|
|
|
/* Macro form of initializer */
|
|
#define QUEUE_INITIALIZER(thedata, thelen) { .data = thedata, .len = thelen }
|
|
|
|
/* Get a byte from queue. Returns -1 if queue empty */
|
|
static inline int _queue_get(struct queue *q)
|
|
{
|
|
int ret = -1;
|
|
if (q->start != q->end) {
|
|
ret = q->data[q->start];
|
|
q->start = __QUEUE_NEXT(q, start);
|
|
}
|
|
return ret;
|
|
}
|
|
static inline int queue_get(struct queue *q)
|
|
{
|
|
int ret;
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
|
ret = _queue_get(q);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/* Put a byte in queue. Returns -1 if queue full, 0 otherwise */
|
|
static inline int _queue_put(struct queue *q, uint8_t v)
|
|
{
|
|
int next = __QUEUE_NEXT(q, end);
|
|
if (next != q->start) {
|
|
q->data[q->end] = v;
|
|
q->end = next;
|
|
return 0;
|
|
}
|
|
return -1;
|
|
}
|
|
static inline int queue_put(struct queue *q, uint8_t v)
|
|
{
|
|
int ret;
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
|
ret = _queue_put(q, v);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/* Returns 1 if the operation would succeed, 0 otherwise */
|
|
static inline int _queue_can_get(struct queue *q)
|
|
{
|
|
return (q->start != q->end) ? 1 : 0;
|
|
}
|
|
static inline int _queue_can_put(struct queue *q)
|
|
{
|
|
return (__QUEUE_NEXT(q, end) != q->start) ? 1 : 0;
|
|
|
|
}
|
|
static inline int queue_can_get(struct queue *q)
|
|
{
|
|
int ret;
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
|
ret = _queue_can_get(q);
|
|
}
|
|
return ret;
|
|
}
|
|
static inline int queue_can_put(struct queue *q)
|
|
{
|
|
int ret;
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
|
ret = _queue_can_put(q);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/* Returns number of bytes in the queue */
|
|
static inline int _queue_get_len(struct queue *q)
|
|
{
|
|
int len = q->end - q->start;
|
|
if (len < 0)
|
|
len += q->len;
|
|
return len;
|
|
}
|
|
|
|
static inline int queue_get_len(struct queue *q)
|
|
{
|
|
int ret;
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
|
ret = _queue_get_len(q);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/* Returns number of bytes FREE in the queue */
|
|
static inline int _queue_get_free(struct queue *q)
|
|
{
|
|
/* If q->len is 64, we can only fit 63 bytes */
|
|
return q->len - _queue_get_len(q) - 1;
|
|
}
|
|
static inline int queue_get_free(struct queue *q)
|
|
{
|
|
int ret;
|
|
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
|
ret = _queue_get_free(q);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
#endif
|