diff --git a/util/queue.c b/util/queue.c index 4437035..cdc55d9 100644 --- a/util/queue.c +++ b/util/queue.c @@ -34,6 +34,22 @@ uint8_t queue_PopByte(queue *q) { 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) { io_WriteConsoleASCII("queue_TopByte: accessing an empty queue\n"); diff --git a/util/queue.h b/util/queue.h index 6c25b0e..42fbf84 100644 --- a/util/queue.h +++ b/util/queue.h @@ -24,6 +24,13 @@ void queue_PushByte(queue *q, const uint8_t b); // pops one byte from the front of the queue, returning it uint8_t queue_PopByte(queue *q); +// write Size bytes to the queue, none written if there is not space for all the bytes +void queue_Push(queue *q, const void *buffer, uintptr_t size); + +// pops Size bytes from the queue, none popped if there are no enough data +// returns the number of bytes popped (either Size or 0) +uintptr_t queue_Pop(queue *q, void *buffer, uintptr_t size); + // return the byte at the front of the queue uint8_t queue_FrontByte(queue *q);