Queue: Add functionality needed by new USART stream driver

Previously there was no way to remove multiple units at a time
from the queue, and the queue was wasting an entry to disambiguate
full from empty.  There was also no way to get the free entry
count from the queue, only the ability to query if it was above
a required amount.  The queue was also storing its constant
compile time configuration as well as its dynamic state in the
same structure.  This wasted RAM on configuration information
that doesn't change.

This refactor fixes these issues, making the queue suitable for
use in the new USART stream driver.

Signed-off-by: Anton Staaf <robotboy@chromium.org>

BRANCH=None
BUG=None
TEST=make buildall -j

Change-Id: I284cee52d8189928dbc4c499f87ab34e14019e5a
Reviewed-on: https://chromium-review.googlesource.com/210533
Reviewed-by: Vic Yang <victoryang@chromium.org>
Reviewed-by: Randall Spangler <rspangler@chromium.org>
Commit-Queue: Anton Staaf <robotboy@chromium.org>
Tested-by: Anton Staaf <robotboy@chromium.org>

1 files changed, 100 insertions, 26 deletions

diff --git a/common/queue.c b/common/queue.c
index 5386cc7c2f..b301b338ea 100644
--- a/common/queue.c
+++ b/common/queue.c
@@ -7,51 +7,125 @@
 #include "queue.h"
 #include "util.h"
 
-void queue_reset(struct queue *queue)
+void queue_init(struct queue const *q)
 {
-	queue->head = queue->tail = 0;
+	ASSERT(POWER_OF_TWO(q->buffer_units));
+
+	q->state->head = 0;
+	q->state->tail = 0;
+}
+
+int queue_is_empty(struct queue const *q)
+{
+	return q->state->head == q->state->tail;
+}
+
+size_t queue_count(struct queue const *q)
+{
+	return q->state->tail - q->state->head;
 }
 
-int queue_is_empty(const struct queue *q)
+size_t queue_space(struct queue const *q)
 {
-	return q->head == q->tail;
+	return q->buffer_units - queue_count(q);
 }
 
-int queue_has_space(const struct queue *q, int unit_count)
+size_t queue_add_unit(struct queue const *q, void const *src)
 {
-	if (q->tail >= q->head)
-		return (q->tail + unit_count * q->unit_bytes) <=
-		       (q->head + q->buf_bytes - 1);
+	size_t tail = q->state->tail & (q->buffer_units - 1);
+
+	if (queue_space(q) == 0)
+		return 0;
+
+	if (q->unit_bytes == 1)
+		q->buffer[tail] = *((uint8_t *) src);
 	else
-		return (q->tail + unit_count * q->unit_bytes) <=
-		       (q->head - 1);
+		memcpy(q->buffer + tail * q->unit_bytes, src, q->unit_bytes);
+
+	q->state->tail += 1;
+
+	return 1;
 }
 
-void queue_add_units(struct queue *q, const void *src, int unit_count)
+size_t queue_add_units(struct queue const *q, void const *src, size_t count)
 {
-	const uint8_t *s = (const uint8_t *)src;
+	size_t transfer = MIN(count, queue_space(q));
+	size_t tail     = q->state->tail & (q->buffer_units - 1);
+	size_t first    = MIN(transfer, q->buffer_units - tail);
 
-	if (!queue_has_space(q, unit_count))
-		return;
+	memcpy(q->buffer + tail * q->unit_bytes,
+	       src,
+	       first * q->unit_bytes);
 
-	for (unit_count *= q->unit_bytes; unit_count; unit_count--) {
-		q->buf[q->tail++] = *(s++);
-		q->tail %= q->buf_bytes;
-	}
+	if (first < transfer)
+		memcpy(q->buffer,
+		       ((uint8_t const *) src) + first * q->unit_bytes,
+		       (transfer - first) * q->unit_bytes);
+
+	q->state->tail += transfer;
+
+	return transfer;
 }
 
-int queue_remove_unit(struct queue *q, void *dest)
+static void queue_read_safe(struct queue const *q,
+			    void *dest,
+			    size_t head,
+			    size_t transfer)
 {
-	int count;
-	uint8_t *d = (uint8_t *)dest;
+	size_t first = MIN(transfer, q->buffer_units - head);
+
+	memcpy(dest,
+	       q->buffer + head * q->unit_bytes,
+	       first * q->unit_bytes);
+
+	if (first < transfer)
+		memcpy(((uint8_t *) dest) + first * q->unit_bytes,
+		       q->buffer,
+		       (transfer - first) * q->unit_bytes);
+}
 
-	if (queue_is_empty(q))
+size_t queue_remove_unit(struct queue const *q, void *dest)
+{
+	size_t head = q->state->head & (q->buffer_units - 1);
+
+	if (queue_count(q) == 0)
 		return 0;
 
-	for (count = q->unit_bytes; count; count--) {
-		*(d++) = q->buf[q->head++];
-		q->head %= q->buf_bytes;
-	}
+	if (q->unit_bytes == 1)
+		*((uint8_t *) dest) = q->buffer[head];
+	else
+		memcpy(dest, q->buffer + head * q->unit_bytes, q->unit_bytes);
+
+	q->state->head += 1;
 
 	return 1;
 }
+
+size_t queue_remove_units(struct queue const *q, void *dest, size_t count)
+{
+	size_t transfer = MIN(count, queue_count(q));
+	size_t head     = q->state->head & (q->buffer_units - 1);
+
+	queue_read_safe(q, dest, head, transfer);
+
+	q->state->head += transfer;
+
+	return transfer;
+}
+
+size_t queue_peek_units(struct queue const *q,
+			void *dest,
+			size_t i,
+			size_t count)
+{
+	size_t available = queue_count(q);
+	size_t transfer  = MIN(count, available - i);
+
+	if (i < available) {
+		size_t head = (q->state->head + i) & (q->buffer_units - 1);
+
+		queue_read_safe(q, dest, head, transfer);
+	}
+
+	return transfer;
+}