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>

6 files changed, 322 insertions, 160 deletions

diff --git a/chip/host/uart.c b/chip/host/uart.c
index 3721ea9c78..afde763747 100644
--- a/chip/host/uart.c
+++ b/chip/host/uart.c
@@ -25,12 +25,8 @@ static pthread_t input_thread;
 
 #define INPUT_BUFFER_SIZE 16
 static int char_available;
-static char cached_char_buf[INPUT_BUFFER_SIZE];
-static struct queue cached_char = {
-	.buf_bytes  = INPUT_BUFFER_SIZE,
-	.unit_bytes = sizeof(char),
-	.buf        = cached_char_buf,
-};
+
+QUEUE_CONFIG(cached_char, INPUT_BUFFER_SIZE, char);
 
 #define CONSOLE_CAPTURE_SIZE 2048
 static char capture_buf[CONSOLE_CAPTURE_SIZE];
@@ -149,7 +145,7 @@ void uart_inject_char(char *s, int sz)
 
 	for (i = 0; i < sz; i += INPUT_BUFFER_SIZE - 1) {
 		num_char = MIN(INPUT_BUFFER_SIZE - 1, sz - i);
-		if (!queue_has_space(&cached_char, num_char))
+		if (queue_space(&cached_char) < num_char)
 			return;
 		queue_add_units(&cached_char, s + i, num_char);
 		char_available = num_char;
@@ -173,7 +169,7 @@ void *uart_monitor_stdin(void *d)
 	while (1) {
 		tcsetattr(0, TCSANOW, &new_settings);
 		rv = read(0, buf, INPUT_BUFFER_SIZE);
-		if (queue_has_space(&cached_char, rv)) {
+		if (queue_space(&cached_char) >= rv) {
 			queue_add_units(&cached_char, buf, rv);
 			char_available = rv;
 		}
diff --git a/common/keyboard_8042.c b/common/keyboard_8042.c
index 9a87f5257f..217bf52339 100644
--- a/common/keyboard_8042.c
+++ b/common/keyboard_8042.c
@@ -68,12 +68,7 @@ enum scancode_set_list {
  */
 static struct mutex to_host_mutex;
 
-static uint8_t to_host_buffer[16];
-static struct queue to_host = {
-	.buf_bytes  = sizeof(to_host_buffer),
-	.unit_bytes = sizeof(uint8_t),
-	.buf        = to_host_buffer,
-};
+QUEUE_CONFIG(to_host, 16, uint8_t);
 
 /* Queue command/data from the host */
 enum {
@@ -96,12 +91,7 @@ struct host_byte {
  *
  * Hence, 5 (actually 4 plus one spare) is large enough, but use 8 for safety.
  */
-static uint8_t from_host_buffer[8 * sizeof(struct host_byte)];
-static struct queue from_host = {
-	.buf_bytes  = sizeof(from_host_buffer),
-	.unit_bytes = sizeof(struct host_byte),
-	.buf        = from_host_buffer,
-};
+QUEUE_CONFIG(from_host, 8, struct host_byte);
 
 static int i8042_irq_enabled;
 
@@ -235,6 +225,11 @@ struct kblog_t {
 	 *
 	 * k = to-host queue head pointer before byte dequeued
 	 * K = byte actually sent to host via LPC
+	 *
+	 * The to-host head and tail pointers are logged pre-wrapping to the
+	 * queue size.  This means that they continually increment as units
+	 * are dequeued and enqueued respectively.  Since only the bottom
+	 * byte of the value is logged they will wrap every 256 units.
 	 */
 	uint8_t type;
 	uint8_t byte;
@@ -263,7 +258,7 @@ void keyboard_host_write(int data, int is_cmd)
 
 	h.type = is_cmd ? HOST_COMMAND : HOST_DATA;
 	h.byte = data;
-	queue_add_units(&from_host, &h, 1);
+	queue_add_unit(&from_host, &h);
 	task_wake(TASK_ID_KEYPROTO);
 }
 
@@ -300,8 +295,8 @@ static void i8042_send_to_host(int len, const uint8_t *bytes)
 
 	/* Enqueue output data if there's space */
 	mutex_lock(&to_host_mutex);
-	if (queue_has_space(&to_host, len)) {
-		kblog_put('t', to_host.tail);
+	if (queue_space(&to_host) >= len) {
+		kblog_put('t', to_host.state->tail);
 		queue_add_units(&to_host, bytes, len);
 	}
 	mutex_unlock(&to_host_mutex);
@@ -433,7 +428,7 @@ static void reset_rate_and_delay(void)
 void keyboard_clear_buffer(void)
 {
 	mutex_lock(&to_host_mutex);
-	queue_reset(&to_host);
+	queue_init(&to_host);
 	mutex_unlock(&to_host_mutex);
 	lpc_keyboard_clear_buffer();
 }
@@ -962,7 +957,7 @@ void keyboard_protocol_task(void)
 			}
 
 			/* Get a char from buffer. */
-			kblog_put('k', to_host.head);
+			kblog_put('k', to_host.state->head);
 			queue_remove_unit(&to_host, &chr);
 			kblog_put('K', chr);
 
@@ -1186,17 +1181,24 @@ static int command_8042_internal(int argc, char **argv)
 	ccprintf("controller_ram_address=0x%02x\n", controller_ram_address);
 	ccprintf("A20_status=%d\n", A20_status);
 
-	ccprintf("from_host.buf[]={");
-	for (i = from_host.head; i != from_host.tail;
-			i = (i + 1) % from_host.buf_bytes)
-		ccprintf("0x%02x, ", from_host.buf[i]);
+	ccprintf("from_host[]={");
+	for (i = 0; i < queue_count(&from_host); ++i) {
+		struct host_byte entry;
+
+		queue_peek_units(&from_host, &entry, i, 1);
+
+		ccprintf("0x%02x, 0x%02x, ", entry.type, entry.byte);
+	}
 	ccprintf("}\n");
 
-	ccprintf("to_host.buf[]={");
-	for (i = to_host.head;
-			i != to_host.tail;
-			i = (i + 1) % to_host.buf_bytes)
-		ccprintf("0x%02x, ", to_host.buf[i]);
+	ccprintf("to_host[]={");
+	for (i = 0; i < queue_count(&to_host); ++i) {
+		uint8_t entry;
+
+		queue_peek_units(&to_host, &entry, i, 1);
+
+		ccprintf("0x%02x, ", entry);
+	}
 	ccprintf("}\n");
 
 	return EC_SUCCESS;
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;
+}
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.
diff --git a/test/queue.c b/test/queue.c
index c7b4518e02..d5d499377b 100644
--- a/test/queue.c
+++ b/test/queue.c
@@ -12,142 +12,143 @@
 #include "timer.h"
 #include "util.h"
 
-static char buffer6[6];  /* Max 5 items in queue */
-static struct queue test_queue6 = {
-	.buf_bytes  = sizeof(buffer6),
-	.unit_bytes = sizeof(char),
-	.buf        = buffer6,
-};
-
-static char buffer5[5];  /* Max 2 items (2 byte for each) in queue */
-static struct queue test_queue5 = {
-	.buf_bytes  = sizeof(buffer5),
-	.unit_bytes = 2,
-	.buf        = buffer5,
-};
-
-#define LOOP_DEQUE(q, d, n) \
-	do { \
-		int i; \
-		for (i = 0; i < n; ++i) \
-			TEST_ASSERT(queue_remove_unit(&q, d + i)); \
-	} while (0)
-
-static int test_queue6_empty(void)
+QUEUE_CONFIG(test_queue8, 8, char)
+QUEUE_CONFIG(test_queue2, 2, int16_t)
+
+static int test_queue8_empty(void)
 {
 	char dummy = 1;
 
-	queue_reset(&test_queue6);
-	TEST_ASSERT(queue_is_empty(&test_queue6));
-	TEST_ASSERT(!queue_remove_unit(&test_queue6, &dummy));
-	queue_add_units(&test_queue6, &dummy, 1);
-	TEST_ASSERT(!queue_is_empty(&test_queue6));
+	queue_init(&test_queue8);
+	TEST_ASSERT(queue_is_empty(&test_queue8));
+	TEST_ASSERT(!queue_remove_units(&test_queue8, &dummy, 1));
+	TEST_ASSERT(queue_add_units(&test_queue8, &dummy, 1) == 1);
+	TEST_ASSERT(!queue_is_empty(&test_queue8));
 
 	return EC_SUCCESS;
 }
 
-static int test_queue6_reset(void)
+static int test_queue8_init(void)
 {
 	char dummy = 1;
 
-	queue_reset(&test_queue6);
-	queue_add_units(&test_queue6, &dummy, 1);
-	queue_reset(&test_queue6);
-	TEST_ASSERT(queue_is_empty(&test_queue6));
+	queue_init(&test_queue8);
+	TEST_ASSERT(queue_add_units(&test_queue8, &dummy, 1) == 1);
+	queue_init(&test_queue8);
+	TEST_ASSERT(queue_is_empty(&test_queue8));
 
 	return EC_SUCCESS;
 }
 
-static int test_queue6_fifo(void)
+static int test_queue8_fifo(void)
 {
 	char buf1[3] = {1, 2, 3};
 	char buf2[3];
 
-	queue_reset(&test_queue6);
+	queue_init(&test_queue8);
 
-	queue_add_units(&test_queue6, buf1 + 0, 1);
-	queue_add_units(&test_queue6, buf1 + 1, 1);
-	queue_add_units(&test_queue6, buf1 + 2, 1);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1 + 0, 1) == 1);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1 + 1, 1) == 1);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1 + 2, 1) == 1);
 
-	LOOP_DEQUE(test_queue6, buf2, 3);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 3) == 3);
 	TEST_ASSERT_ARRAY_EQ(buf1, buf2, 3);
 
 	return EC_SUCCESS;
 }
 
-static int test_queue6_multiple_units_add(void)
+static int test_queue8_multiple_units_add(void)
 {
 	char buf1[5] = {1, 2, 3, 4, 5};
 	char buf2[5];
 
-	queue_reset(&test_queue6);
-	TEST_ASSERT(queue_has_space(&test_queue6, 5));
-	queue_add_units(&test_queue6, buf1, 5);
-	LOOP_DEQUE(test_queue6, buf2, 5);
+	queue_init(&test_queue8);
+	TEST_ASSERT(queue_space(&test_queue8) >= 5);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1, 5) == 5);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 5) == 5);
 	TEST_ASSERT_ARRAY_EQ(buf1, buf2, 5);
 
 	return EC_SUCCESS;
 }
 
-static int test_queue6_removal(void)
+static int test_queue8_removal(void)
 {
 	char buf1[5] = {1, 2, 3, 4, 5};
 	char buf2[5];
 
-	queue_reset(&test_queue6);
-	queue_add_units(&test_queue6, buf1, 5);
+	queue_init(&test_queue8);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1, 5) == 5);
 	/* 1, 2, 3, 4, 5 */
-	LOOP_DEQUE(test_queue6, buf2, 3);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 3) == 3);
 	TEST_ASSERT_ARRAY_EQ(buf1, buf2, 3);
 	/* 4, 5 */
-	queue_add_units(&test_queue6, buf1, 2);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1, 2) == 2);
 	/* 4, 5, 1, 2 */
-	TEST_ASSERT(queue_has_space(&test_queue6, 1));
-	TEST_ASSERT(!queue_has_space(&test_queue6, 2));
-	LOOP_DEQUE(test_queue6, buf2, 1);
+	TEST_ASSERT(queue_space(&test_queue8) == 4);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 1) == 1);
 	TEST_ASSERT(buf2[0] == 4);
 	/* 5, 1, 2 */
-	queue_add_units(&test_queue6, buf1 + 2, 2);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1 + 2, 2) == 2);
 	/* 5, 1, 2, 3, 4 */
-	TEST_ASSERT(!queue_has_space(&test_queue6, 1));
-	LOOP_DEQUE(test_queue6, buf2, 1);
+	TEST_ASSERT(queue_space(&test_queue8) == 3);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1 + 2, 3) == 3);
+	/* 5, 1, 2, 3, 4, 3, 4, 5 */
+	TEST_ASSERT(queue_space(&test_queue8) == 0);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 1) == 1);
 	TEST_ASSERT(buf2[0] == 5);
-	LOOP_DEQUE(test_queue6, buf2, 4);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 4) == 4);
 	TEST_ASSERT_ARRAY_EQ(buf1, buf2, 4);
-	TEST_ASSERT(queue_is_empty(&test_queue6));
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 3) == 3);
+	TEST_ASSERT_ARRAY_EQ(buf1 + 2, buf2, 3);
+	TEST_ASSERT(queue_is_empty(&test_queue8));
 	/* Empty */
-	queue_add_units(&test_queue6, buf1, 5);
-	LOOP_DEQUE(test_queue6, buf2, 5);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1, 5) == 5);
+	TEST_ASSERT(queue_remove_units(&test_queue8, buf2, 5) == 5);
 	TEST_ASSERT_ARRAY_EQ(buf1, buf2, 5);
 
 	return EC_SUCCESS;
 }
 
-static int test_queue5_odd_even(void)
+static int test_queue8_peek(void)
+{
+	char buf1[5] = {1, 2, 3, 4, 5};
+	char buf2[5];
+
+	queue_init(&test_queue8);
+	TEST_ASSERT(queue_add_units(&test_queue8, buf1, 5) == 5);
+	/* 1, 2, 3, 4, 5 */
+	TEST_ASSERT(queue_count(&test_queue8) == 5);
+	TEST_ASSERT(queue_space(&test_queue8) == 3);
+	TEST_ASSERT(queue_peek_units(&test_queue8, buf2, 2, 3) == 3);
+	TEST_ASSERT_ARRAY_EQ(buf1 + 2, buf2, 3);
+	TEST_ASSERT(queue_count(&test_queue8) == 5);
+	TEST_ASSERT(queue_space(&test_queue8) == 3);
+
+	return EC_SUCCESS;
+}
+
+static int test_queue2_odd_even(void)
 {
 	uint16_t buf1[3] = {1, 2, 3};
 	uint16_t buf2[3];
 
-	queue_reset(&test_queue5);
-	queue_add_units(&test_queue5, buf1, 1);
+	queue_init(&test_queue2);
+	TEST_ASSERT(queue_add_units(&test_queue2, buf1, 1) == 1);
 	/* 1 */
-	TEST_ASSERT(!queue_has_space(&test_queue5, 2));
-	TEST_ASSERT(queue_has_space(&test_queue5, 1));
-	queue_add_units(&test_queue5, buf1 + 1, 1);
+	TEST_ASSERT(queue_space(&test_queue2) == 1);
+	TEST_ASSERT(queue_add_units(&test_queue2, buf1 + 1, 1) == 1);
 	/* 1, 2 */
-	TEST_ASSERT(!queue_has_space(&test_queue5, 1));
-	LOOP_DEQUE(test_queue5, buf2, 2);
+	TEST_ASSERT(queue_space(&test_queue2) == 0);
+	TEST_ASSERT(queue_remove_units(&test_queue2, buf2, 2) == 2);
 	TEST_ASSERT_ARRAY_EQ(buf1, buf2, 2);
-	TEST_ASSERT(queue_is_empty(&test_queue5));
+	TEST_ASSERT(queue_is_empty(&test_queue2));
 	/* Empty */
-	TEST_ASSERT(!queue_has_space(&test_queue5, 3));
-	TEST_ASSERT(queue_has_space(&test_queue5, 2));
-	TEST_ASSERT(queue_has_space(&test_queue5, 1));
-	queue_add_units(&test_queue5, buf1 + 2, 1);
+	TEST_ASSERT(queue_space(&test_queue2) == 2);
+	TEST_ASSERT(queue_add_units(&test_queue2, buf1 + 2, 1) == 1);
 	/* 3 */
-	LOOP_DEQUE(test_queue5, buf2, 1);
+	TEST_ASSERT(queue_remove_units(&test_queue2, buf2, 1) == 1);
 	TEST_ASSERT(buf2[0] == 3);
-	TEST_ASSERT(queue_is_empty(&test_queue5));
+	TEST_ASSERT(queue_is_empty(&test_queue2));
 
 	return EC_SUCCESS;
 }
@@ -156,12 +157,13 @@ void run_test(void)
 {
 	test_reset();
 
-	RUN_TEST(test_queue6_empty);
-	RUN_TEST(test_queue6_reset);
-	RUN_TEST(test_queue6_fifo);
-	RUN_TEST(test_queue6_multiple_units_add);
-	RUN_TEST(test_queue6_removal);
-	RUN_TEST(test_queue5_odd_even);
+	RUN_TEST(test_queue8_empty);
+	RUN_TEST(test_queue8_init);
+	RUN_TEST(test_queue8_fifo);
+	RUN_TEST(test_queue8_multiple_units_add);
+	RUN_TEST(test_queue8_removal);
+	RUN_TEST(test_queue8_peek);
+	RUN_TEST(test_queue2_odd_even);
 
 	test_print_result();
 }