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>

2 files changed, 109 insertions, 21 deletions

diff --git a/include/queue.h b/include/queue.h
index 2c967ee4a8..9be0f31bdc 100644
--- a/include/queue.h
+++ b/include/queue.h
@@ -4,37 +4,122 @@
  *
  * Queue data structure.
  */
+#ifndef INCLUDE_QUEUE_H
+#define INCLUDE_QUEUE_H
 
 #include "common.h"
 
-/* Generic queue container.
- *
- *   head: next to dequeqe
- *   tail: next to enqueue
- *
- *   Empty:
- *     head == tail
- *   Full:
- *     ((tail + 1) % buf_bytes) == head
+#include <stddef.h>
+#include <stdint.h>
+
+/* Generic queue container. */
+
+/*
+ * RAM state for a queue.
+ */
+struct queue_state {
+	/*
+	 * The queue head and tail pointers are not wrapped until they are
+	 * needed to access the queue buffer.  This has a number of advantages,
+	 * the queue doesn't have to waste an entry to disambiguate full and
+	 * empty for one.  It also provides a convenient total enqueue/dequeue
+	 * log (one that does wrap at the limit of a size_t however).
+	 *
+	 * Empty:
+	 *     head == tail
+	 *
+	 * Full:
+	 *     head - tail == buffer_units
+	 */
+	size_t head; /* head: next to dequeue */
+	size_t tail; /* tail: next to enqueue */
+};
+
+/*
+ * Queue configuration stored in flash.
  */
 struct queue {
-	int head, tail;
-	int buf_bytes;   /* size of buffer (in byte) */
-	int unit_bytes;  /* size of unit (in byte) */
-	uint8_t *buf;
+	struct queue_state volatile *state;
+
+	size_t  buffer_units; /* size of buffer (in units) */
+	size_t  unit_bytes;   /* size of unit   (in byte) */
+	uint8_t *buffer;
 };
 
-/* Reset the queue to empty state. */
-void queue_reset(struct queue *queue);
+/*
+ * Convenience macro for construction of a Queue along with its backing buffer
+ * and state structure.
+ */
+#define QUEUE_CONFIG(NAME, SIZE, TYPE)					\
+	static TYPE CONCAT2(NAME, _buffer)[SIZE];			\
+									\
+	static struct queue_state CONCAT2(NAME, _state);		\
+	struct queue const NAME =					\
+	{								\
+		.state        = &CONCAT2(NAME, _state),			\
+		.buffer_units = SIZE,					\
+		.unit_bytes   = sizeof(TYPE),				\
+		.buffer       = (uint8_t *) CONCAT2(NAME, _buffer),	\
+	};
+
+/* Initialize the queue to empty state. */
+void queue_init(struct queue const *q);
 
 /* Return TRUE if the queue is empty. */
-int queue_is_empty(const struct queue *q);
+int queue_is_empty(struct queue const *q);
+
+/* Return the number of units stored in the queue. */
+size_t queue_count(struct queue const *q);
+
+/* Return the number of units worth of free space the queue has. */
+size_t queue_space(struct queue const *q);
 
-/* Return TRUE if the queue has at least one unit space. */
-int queue_has_space(const struct queue *q, int unit_count);
+/* Add one unit to queue. */
+size_t queue_add_unit(struct queue const *q, void const *src);
 
-/* Add multiple units into queue. */
-void queue_add_units(struct queue *q, const void *src, int unit_count);
+/* Add multiple units to queue. */
+size_t queue_add_units(struct queue const *q, void const *src, size_t count);
 
 /* Remove one unit from the begin of the queue. */
-int queue_remove_unit(struct queue *q, void *dest);
+size_t queue_remove_unit(struct queue const *q, void *dest);
+
+/* Remove multiple units from the begin of the queue. */
+size_t queue_remove_units(struct queue const *q, void *dest, size_t count);
+
+/* Peek (return but don't remove) the count elements starting with the i'th. */
+size_t queue_peek_units(struct queue const *q,
+			void *dest,
+			size_t i,
+			size_t count);
+
+/*
+ * These macros will statically select the queue functions based on the number
+ * of units that are to be added or removed if they can.  The single unit add
+ * and remove functions are much faster than calling the equivalent generic
+ * version with a count of one.
+ */
+#define QUEUE_ADD_UNITS(q, src, count)					\
+	({								\
+		size_t result;						\
+									\
+		if (count == 1)						\
+			result = queue_add_unit(q, src);		\
+		else							\
+			result = queue_add_units(q, src, count);	\
+									\
+		result;							\
+	})
+
+#define QUEUE_REMOVE_UNITS(q, dest, count)				\
+	({								\
+		size_t result;						\
+									\
+		if (count == 1)						\
+			result = queue_remove_unit(q, dest);		\
+		else							\
+			result = queue_remove_units(q, dest, count);	\
+									\
+		result;							\
+	})
+
+#endif /* INCLUDE_QUEUE_H */
diff --git a/include/util.h b/include/util.h
index e952123e37..c012470a47 100644
--- a/include/util.h
+++ b/include/util.h
@@ -56,6 +56,9 @@
 #define NULL ((void *)0)
 #endif
 
+/* True of x is a power of two */
+#define POWER_OF_TWO(x) (x && !(x & (x - 1)))
+
 /**
  * macros for integer division with various rounding variants
  * default integer division rounds down.