73 lines
1.6 KiB
C
73 lines
1.6 KiB
C
|
|
||
|
|
#include "queue.h"
|
||
|
|
|
||
|
|
#include <stdio.h>
|
||
|
|
|
||
|
|
|
||
|
|
void queue_InitBuffered(queue *q, void *buffer, uintptr_t size) {
|
||
|
|
q->data = q->begin = q->end = buffer;
|
||
|
|
q->size = size;
|
||
|
|
q->count = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void queue_PushByte(queue *q, const uint8_t b) {
|
||
|
|
if (q->count == q->size) { // no more space
|
||
|
|
fprintf(stderr, "queue_PushByte: full[%llu bytes], discarding byte 0x%x\n", q->size, b);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
q->count++;
|
||
|
|
*((uint8_t *)(q->end++)) = b;
|
||
|
|
if (q->end == q->data + q->size)
|
||
|
|
q->end = q->data; // out of the buffer: wrap around
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t queue_PopByte(queue *q) {
|
||
|
|
if (q->count == 0) {
|
||
|
|
fprintf(stderr, "queue_PopByte: poping an empty queue\n");
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
q->count--;
|
||
|
|
uint8_t data = *((uint8_t *)(q->begin++));
|
||
|
|
if (q->begin == q->data + q->size)
|
||
|
|
q->begin = q->data; // wrap around
|
||
|
|
return data;
|
||
|
|
}
|
||
|
|
|
||
|
|
void queue_Push(queue *q, const void *buffer, uintptr_t size) {
|
||
|
|
// TODO Optimize queue_Push and queue_Pop
|
||
|
|
if (queue_Space(q) < size)
|
||
|
|
return;
|
||
|
|
for (const uint8_t *i = buffer; i < (const uint8_t *)buffer + size; i++)
|
||
|
|
queue_PushByte(q, *i);
|
||
|
|
}
|
||
|
|
|
||
|
|
uintptr_t queue_Pop(queue *q, void *buffer, uintptr_t size) {
|
||
|
|
if (queue_Size(q) < size)
|
||
|
|
return 0;
|
||
|
|
for (uint8_t *i = buffer; i < (uint8_t *)buffer + size; i++)
|
||
|
|
*i = queue_PopByte(q);
|
||
|
|
return size;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t queue_FrontByte(queue *q) {
|
||
|
|
if (q->count == 0) {
|
||
|
|
fprintf(stderr, "queue_TopByte: accessing an empty queue\n");
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
return *((uint8_t *)q->begin);
|
||
|
|
}
|
||
|
|
|
||
|
|
bool queue_Empty(queue *q) {
|
||
|
|
return q->count == 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
uintptr_t queue_Size(queue *q) {
|
||
|
|
return q->count;
|
||
|
|
}
|
||
|
|
|
||
|
|
uintptr_t queue_Space(queue *q) {
|
||
|
|
return q->size - q->count;
|
||
|
|
}
|