added queue mock

src/common/typedefs.h

@@ -20,6 +20,11 @@ typedef uint64_t absolute_time_t;
 typedef unsigned int uint;
 typedef struct
 {
-    uint8_t _dummy;
+    uint8_t *data;
+    unsigned int element_size;
+    unsigned int element_count;
+    unsigned int head;
+    unsigned int tail;
+    unsigned int level;
 } queue_t;
 #endif

src/slate/slate.c

@@ -22,6 +22,14 @@ void free_slate(slate_t *slate)
 {
     // Free the filesys buffer if it was allocated - note free(NULL) is a no-op
     // as per C specification, so this is safe even if allocation failed.
+    if (slate != NULL)
+    {
+        queue_free(&slate->payload_command_data);
+        queue_free(&slate->tx_queue);
+        queue_free(&slate->rx_queue);
+        queue_free(&slate->rpi_uart_queue);
+    }
+
     free(slate->filesys_buffer);
     slate->filesys_buffer = NULL;
 }

src/test_mocks/pico/util/queue.h

@@ -2,35 +2,80 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
 
+#include "logger.h"
 #include "typedefs.h"
 
-// Mock queue functions that are no-ops in tests
 static inline void queue_init(queue_t *q, unsigned int element_size,
                               unsigned int element_count)
 {
+    q->element_size = element_size;
+    q->element_count = element_count;
+    q->head = 0;
+    q->tail = 0;
+    q->level = 0;
+    ASSERT(element_size > 0 && element_count > 0 &&
+           element_size * element_count < UINT32_MAX);
+    q->data = malloc(element_size * element_count);
+    ASSERT(q->data != NULL);
 }
+
 static inline bool queue_try_add(queue_t *q, void *data)
 {
+    if (q->level >= q->element_count)
+        return false;
+
+    ASSERT(q->element_count > 0 && q->data != NULL);
+    memcpy(q->data + q->tail * q->element_size, data, q->element_size);
+    q->tail = (q->tail + 1) % q->element_count;
+    q->level++;
     return true;
 }
+
 static inline bool queue_try_remove(queue_t *q, void *data)
 {
-    return false;
+    if (q->level == 0)
+        return false;
+
+    ASSERT(q->element_count > 0 && q->data != NULL);
+    memcpy(data, q->data + q->head * q->element_size, q->element_size);
+    q->head = (q->head + 1) % q->element_count;
+    q->level--;
+    return true;
 }
+
 static inline bool queue_try_peek(queue_t *q, void *data)
 {
-    return false;
+    if (q->level == 0)
+        return false;
+
+    ASSERT(q->element_count > 0 && q->data != NULL);
+    memcpy(data, q->data + q->head * q->element_size, q->element_size);
+    return true;
 }
+
 static inline bool queue_is_empty(queue_t *q)
 {
-    return true;
+    return q->level == 0;
 }
+
 static inline bool queue_is_full(queue_t *q)
 {
-    return false;
+    return q->level == q->element_count;
 }
+
 static inline unsigned int queue_get_level(queue_t *q)
 {
-    return 0;
+    return q->level;
+}
+
+static inline void queue_free(queue_t *q)
+{
+    free(q->data);
+    q->data = NULL;
+    q->level = 0;
+    q->head = 0;
+    q->tail = 0;
 }