51 lines
1.2 KiB
C
51 lines
1.2 KiB
C
#include "icc_queue.h"
|
|
#include <string.h>
|
|
|
|
void iccq_create(ICCQueue * q, volatile uint8_t * p, uint32_t length, uint32_t elemSize) {
|
|
q->elements = p;
|
|
q->length = length;
|
|
q->elemSize = elemSize;
|
|
q->readIdx = 0;
|
|
q->writeIdx = 0;
|
|
memset(q->elements, 0, length * elemSize);
|
|
|
|
return q;
|
|
}
|
|
|
|
void iccq_clear(ICCQueue * q) {
|
|
q->readIdx = 0;
|
|
q->writeIdx = 0;
|
|
memset(q->elements, 0, q->length * q->elemSize);
|
|
}
|
|
|
|
uint32_t iccq_avail(const ICCQueue * q) {
|
|
return ((q->writeIdx < q->readIdx) ? (q->writeIdx + q->length) : (q->writeIdx)) - q->readIdx;
|
|
}
|
|
|
|
#define MQ_NEXT(size,current) (((current)+1)%(size))
|
|
|
|
bool iccq_push(ICCQueue * q, const void * src) {
|
|
if (MQ_NEXT(q->length, q->writeIdx) == q->readIdx) { // cannot push, queue is full
|
|
return false;
|
|
}
|
|
|
|
// can push
|
|
memcpy(q->elements + q->writeIdx * q->elemSize, src, q->elemSize);
|
|
|
|
// advance write pointer
|
|
q->writeIdx = MQ_NEXT(q->length, q->writeIdx);
|
|
|
|
return true;
|
|
}
|
|
|
|
void iccq_top(ICCQueue * q, void * dest) {
|
|
memcpy(dest, q->elements + q->readIdx, q->elemSize);
|
|
}
|
|
|
|
void iccq_pop(ICCQueue * q) {
|
|
if (iccq_avail(q) > 0) { // if there's something to pop
|
|
q->readIdx = MQ_NEXT(q->length, q->readIdx);
|
|
}
|
|
}
|
|
|