Implement a packet queue (incl. tests)

impl/src/layer2/packet_queue.c

@@ -0,0 +1,81 @@
+#include <stdlib.h>
+
+#include "packet_queue.h"
+
+void packet_queue_init(packet_queue_t *q)
+{
+	q->write_idx = 0;
+	q->read_idx = 0;
+}
+
+
+void packet_queue_destroy(packet_queue_t *q)
+{
+	packet_queue_delete(q, packet_queue_get_used_space(q));
+}
+
+
+bool packet_queue_can_add_packet(const packet_queue_t *q)
+{
+	return packet_queue_get_free_space(q) != 0;
+}
+
+
+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) {
+		return false;
+	}
+
+	q->entries[q->write_idx].header   = *header;
+	q->entries[q->write_idx].data     = data;
+	q->entries[q->write_idx].data_len = data_len;
+
+	q->write_idx = next_idx;
+
+	return true;
+}
+
+
+const packet_queue_entry_t* packet_queue_get(const packet_queue_t *q, size_t index)
+{
+	if(index >= packet_queue_get_used_space(q)) {
+		return NULL;
+	}
+
+	size_t internal_index = (q->read_idx + index) % PACKET_QUEUE_SIZE;
+
+	return &q->entries[internal_index];
+}
+
+
+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) {
+			// stop early if there are no entries left to delete
+			break;
+		}
+
+		packet_queue_entry_t *entry = &q->entries[q->read_idx];
+
+		free(entry->data);
+		entry->data_len = 0;
+
+		q->read_idx++;
+	}
+}
+
+
+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;
+}
+
+
+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;
+}

impl/src/layer2/packet_queue.h

@@ -0,0 +1,75 @@
+#ifndef PACKET_QUEUE_H
+#define PACKET_QUEUE_H
+
+#include "packet_structs.h"
+
+#define PACKET_QUEUE_SIZE 16
+
+typedef struct packet_queue_entry_s {
+	layer2_packet_header_t  header;
+	uint8_t                *data;
+	size_t                  data_len;
+} packet_queue_entry_t;
+
+typedef struct packet_queue_s {
+	packet_queue_entry_t entries[PACKET_QUEUE_SIZE];
+
+	size_t write_idx; //!< Write index in \ref entries
+	size_t read_idx;  //!< Read index in \ref entries
+} packet_queue_t;
+
+/*!\brief Initialize a packet queue.
+ * \param q   Pointer to the queue struct to initialize.
+ */
+void packet_queue_init(packet_queue_t *q);
+
+/*!\brief Destroy a packet queue and free all memory.
+ * \param q   Pointer to the queue struct to destroy.
+ */
+void packet_queue_destroy(packet_queue_t *q);
+
+/*!\brief Check if a packet can be added to the queue.
+ * \param q            Pointer to the queue to check.
+ * \returns            True if a packet can be added, false if the queue is full.
+ */
+bool packet_queue_can_add_packet(const packet_queue_t *q);
+
+/*!\brief Add a packet to the queue.
+ * \details
+ * This function takes ownership of the given data block which must be
+ * allocated using malloc(). It is free()d when the packet is removed from the
+ * queue.
+ *
+ * \param q            Pointer to the queue to append to.
+ * \param header       The packet header.
+ * \param data         The packet data.
+ * \param data_len     Length of the given packet data.
+ * \returns            True if the packet was added, false otherwise.
+ */
+bool packet_queue_add(packet_queue_t *q, const layer2_packet_header_t *header, uint8_t *data, size_t data_len);
+
+/*!\brief Get the nth packet in the queue without removing it.
+ *
+ * \param q            Pointer to the queue to access.
+ * \param index        The index of the packet to retrieve.
+ * \returns            A pointer to the entry or NULL if the queue does not have this entry.
+ */
+const packet_queue_entry_t* packet_queue_get(const packet_queue_t *q, size_t index);
+
+/*!\brief Free packets up to the given entry index.
+ *
+ * \param q            Pointer to the queue to access.
+ * \param count        Number of entries to delete (maximum; stops early if no entries are left).
+ */
+void packet_queue_delete(packet_queue_t *q, size_t count);
+
+/*!\brief Get the number of used slots in the packet queue.
+ */
+size_t packet_queue_get_used_space(const packet_queue_t *q);
+
+
+/*!\brief Get the number of free slots in the packet queue.
+ */
+size_t packet_queue_get_free_space(const packet_queue_t *q);
+
+#endif // PACKET_QUEUE_H

impl/test/CMakeLists.txt

@@ -145,3 +145,16 @@ add_executable(
 target_link_libraries(
 	test_ham64
 )
+
+#------------------------------------
+
+add_executable(
+	test_packet_queue
+	../src/layer2/packet_queue.c
+	../src/layer2/packet_queue.h
+	test_packet_queue.c
+)
+
+target_link_libraries(
+	test_packet_queue
+)

impl/test/test_packet_queue.c

@@ -0,0 +1,69 @@
+#include <assert.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <layer2/packet_queue.h>
+
+int main(void)
+{
+	packet_queue_t queue;
+
+	layer2_packet_header_t header = {0};
+	uint8_t *data;
+	size_t data_len;
+
+	// initialize
+	packet_queue_init(&queue);
+
+	// add a first packet
+	header.tx_seq_nr = 0;
+	data_len = 2;
+	data = malloc(data_len);
+	strncpy((char*)data, "A", data_len);
+
+	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);
+
+	// add more packets until the queue is / should be full
+	for(size_t i = 0; i < PACKET_QUEUE_SIZE-2; i++) {
+		char s[2];
+		s[0] = 'A' + i + 1;
+		s[1] = 0;
+
+		header.tx_seq_nr = i+1;
+		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);
+	}
+
+	// adding another packet should fail now
+	assert(!packet_queue_add(&queue, &header, data, data_len));
+
+	// delete the first five packets
+	packet_queue_delete(&queue, 5);
+
+	assert(packet_queue_get_free_space(&queue) == 5);
+
+	// now the sequence number of the first packet should be 5
+	const packet_queue_entry_t *entry = packet_queue_get(&queue, 0);
+
+	assert(entry->header.tx_seq_nr == 5);
+
+	// try to access the 10th and 11th 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);
+
+	packet_queue_destroy(&queue);
+
+	printf("Test completed successfully!\n");
+}