From 9845514a46e4c588cbb6fa0839c63b28f92da36f Mon Sep 17 00:00:00 2001
From: Simon Ruderich <simon@ruderich.org>
Date: Tue, 23 Jul 2024 08:41:25 +0200
Subject: [PATCH] layer2/packet_queue: permit using full queue size

Previously, only PACKET_QUEUE_SIZE - 1 elements could be stored.
---
 impl/src/layer2/packet_queue.c | 18 +++++++++---------
 impl/src/layer2/packet_queue.h |  1 +
 impl/test/test_packet_queue.c  | 18 +++++++++---------
 3 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/impl/src/layer2/packet_queue.c b/impl/src/layer2/packet_queue.c
index 3e3eb72..ce01374 100644
--- a/impl/src/layer2/packet_queue.c
+++ b/impl/src/layer2/packet_queue.c
@@ -6,6 +6,7 @@ void packet_queue_init(packet_queue_t *q)
 {
 	q->write_idx = 0;
 	q->read_idx = 0;
+	q->count = 0;
 }
 
 
@@ -23,9 +24,7 @@ bool packet_queue_can_add_packet(const packet_queue_t *q)
 
 bool packet_queue_add(packet_queue_t *q, const layer2_packet_header_t *header, uint8_t *data, size_t data_len)
 {
-	size_t next_idx = (q->write_idx + 1) % PACKET_QUEUE_SIZE;
-
-	if(next_idx == q->read_idx) {
+	if (!packet_queue_can_add_packet(q)) {
 		return false;
 	}
 
@@ -33,7 +32,8 @@ bool packet_queue_add(packet_queue_t *q, const layer2_packet_header_t *header, u
 	q->entries[q->write_idx].data     = data;
 	q->entries[q->write_idx].data_len = data_len;
 
-	q->write_idx = next_idx;
+	q->write_idx = (q->write_idx + 1) % PACKET_QUEUE_SIZE;
+	q->count++;
 
 	return true;
 }
@@ -54,7 +54,7 @@ const packet_queue_entry_t* packet_queue_get(const packet_queue_t *q, size_t ind
 void packet_queue_delete(packet_queue_t *q, size_t count)
 {
 	for(size_t i = 0; i < count; i++) {
-		if(q->read_idx == q->write_idx) {
+		if(packet_queue_get_used_space(q) == 0) {
 			// stop early if there are no entries left to delete
 			break;
 		}
@@ -68,19 +68,19 @@ void packet_queue_delete(packet_queue_t *q, size_t count)
 
 		entry->data_len = 0;
 
-		q->read_idx++;
-		q->read_idx %= PACKET_QUEUE_SIZE;
+		q->read_idx = (q->read_idx + 1) % PACKET_QUEUE_SIZE;
+		q->count--;
 	}
 }
 
 
 size_t packet_queue_get_free_space(const packet_queue_t *q)
 {
-	return (q->read_idx + PACKET_QUEUE_SIZE - 1 - q->write_idx) % PACKET_QUEUE_SIZE;
+	return PACKET_QUEUE_SIZE - q->count;
 }
 
 
 size_t packet_queue_get_used_space(const packet_queue_t *q)
 {
-	return (q->write_idx + PACKET_QUEUE_SIZE - q->read_idx) % PACKET_QUEUE_SIZE;
+	return q->count;
 }
diff --git a/impl/src/layer2/packet_queue.h b/impl/src/layer2/packet_queue.h
index 27e8b50..cd1bfd0 100644
--- a/impl/src/layer2/packet_queue.h
+++ b/impl/src/layer2/packet_queue.h
@@ -16,6 +16,7 @@ typedef struct packet_queue_s {
 
 	size_t write_idx; //!< Write index in \ref entries
 	size_t read_idx;  //!< Read index in \ref entries
+	size_t count; //!< Currently used space in queue
 } packet_queue_t;
 
 /*!\brief Initialize a packet queue.
diff --git a/impl/test/test_packet_queue.c b/impl/test/test_packet_queue.c
index 3b63d1d..9fb13c6 100644
--- a/impl/test/test_packet_queue.c
+++ b/impl/test/test_packet_queue.c
@@ -26,23 +26,23 @@ int main(void)
 	assert(packet_queue_add(&queue, &header, data, data_len));
 
 	assert(packet_queue_get_used_space(&queue) == 1);
-	assert(packet_queue_get_free_space(&queue) == PACKET_QUEUE_SIZE-2);
+	assert(packet_queue_get_free_space(&queue) == PACKET_QUEUE_SIZE-1);
 
 	// add more packets until the queue is / should be full
-	for(size_t i = 0; i < PACKET_QUEUE_SIZE-2; i++) {
+	for(size_t i = 1; i < PACKET_QUEUE_SIZE; i++) {
 		char s[2];
-		s[0] = 'A' + i + 1;
+		s[0] = 'A' + i;
 		s[1] = 0;
 
-		header.tx_seq_nr = i+1;
+		header.tx_seq_nr = i;
 		data_len = 2;
 		data = malloc(data_len);
 		strncpy((char*)data, s, data_len);
 
 		assert(packet_queue_add(&queue, &header, data, data_len));
 
-		assert(packet_queue_get_used_space(&queue) == i+2);
-		assert(packet_queue_get_free_space(&queue) == PACKET_QUEUE_SIZE-3-i);
+		assert(packet_queue_get_used_space(&queue) == i+1);
+		assert(packet_queue_get_free_space(&queue) == PACKET_QUEUE_SIZE-1-i);
 	}
 
 	// adding another packet should fail now
@@ -58,10 +58,10 @@ int main(void)
 
 	assert(entry->header.tx_seq_nr == 5);
 
-	// try to access the 10th and 11th packet, which are the last existing and
+	// try to access the 11th and 12th packet, which are the last existing and
 	// the first nonexisting packets
-	assert(packet_queue_get(&queue, 9) != NULL);
-	assert(packet_queue_get(&queue, 10) == NULL);
+	assert(packet_queue_get(&queue, 10) != NULL);
+	assert(packet_queue_get(&queue, 11) == NULL);
 
 	packet_queue_destroy(&queue);