2 files changed, 88 insertions(+), 5 deletions(-)

M include/queue.h
M src/queue.c

@@ 1,25 1,78 @@
 #include <stdint.h>
 #include <stdbool.h>
+#include <stdatomic.h>
 
 #ifndef QUEUE_H
 #define QUEUE_H
 
 typedef struct {
-  uint16_t curr_read_ptr;
-  uint16_t curr_write_ptr;
-
   uint16_t element_size;
-
   uint16_t length;
-  uint16_t space;
+
+  atomic_uint_least16_t curr_read_ptr;
+  atomic_uint_least16_t curr_write_ptr;
+  atomic_uint_least16_t space;
 
   uint8_t elements[0];
 } queue_t;
 
+/**
+ * @brief Allocate needed memory for given element size and total length of the fifo.
+ * @param[in] element_size The size of one element.
+ * @param[in] length The maximum number of elements
+ * @return The malloced memory, not initialized. Call @see queue_init on this.
+ */
 queue_t* queue_malloc(uint16_t element_size, uint16_t length);
+
+/**
+ * @brief Initialize queue, given allocated memory
+ * @details To allocate  the memory, @see queue_malloc might be used.
+ * @param[in,out] queue The queue to initialize
+ * @param[in] element_size The size of one element.
+ * @param[in] length The maximum number of elements.
+ */
 void queue_init(queue_t* queue, uint16_t element_size, uint16_t length);
+
+/**
+ * @brief Enqueue the given element to the queue.
+ * @param[in,out] queue The queue to enqueue the element to.
+ * @param[out] element The element to enqueue, its size should be element_size used in @see queue_init. If it isn't, wrong part of memory might be read.
+ * @return Whether the element was enqueued. If false, queue is full.
+ */
 bool queue_enqueue(queue_t* queue, void* element);
+
+/**
+ * @brief Dequeue the first element.
+ * @details The element at beginning of queue is returned, and the pointer is incremented
+ * so that the element is no longer valid and there is space for a new one. This implementation
+ * is not thread safe! The element might get overwritten.
+ * @param[in,out] queue The queue to dequeue from.
+ * @return The element.
+ */
 void* queue_dequeue(queue_t* queue);
+
+/**
+ * @brief Dequeue the first element.
+ * @details The element at beginning of queue is returned, and the pointer is incremented.
+ * @param[in,out] queue The queue to dequeue from.
+ * @param[out] element Space for the element to write it to.
+ * @return If there was an element to read.
+ */
+bool queue_dequeue_safely(queue_t* queue, void* element);
+
+/**
+ * @brief Show the first element.
+ * @details The element at beginning of queue is returned,
+ * @param[in,out] queue The queue to peek to.
+ * @return The element.
+ */
 void* queue_peek(queue_t* queue);
 
+/**
+ * @brief Get number of elements in the queue
+ * @param[in,out] queue The queue.
+ * @return Number of elements in the queue.
+ */
+uint16_t queue_count(queue_t* queue);
+
 #endif // QUEUE_H

@@ 40,6 40,32 @@ void *queue_dequeue(queue_t *queue) {
   return element;
 }
 
+bool queue_dequeue_safely(queue_t *queue, void *element) {
+  uint8_t* queue_element = (uint8_t*)queue_peek(queue);
+
+  if (queue_element == NULL) {
+    return false;
+  }
+
+  uint8_t* target_element = (uint8_t*)element;
+
+  // First copy
+  for (uint16_t i = 0; i < queue->element_size; i++) {
+    *(target_element + i) = *(queue_element + i);
+  }
+
+  // Then "commit"
+  if (element != NULL) {
+    queue->curr_read_ptr += queue->element_size;
+    queue->curr_read_ptr %= queue->length * queue->element_size;
+    queue->space++;
+    // From now on, the element might be rewritten in queue,
+    // but is has already been copied.
+  }
+
+  return true;
+}
+
 void *queue_peek(queue_t *queue) {
   if (queue->space == queue->length) {
     return NULL;


@@ 47,3 73,7 @@ void *queue_peek(queue_t *queue) {
 
   return &queue->elements[queue->curr_read_ptr];
 }
+
+uint16_t queue_count(queue_t *queue) {
+  return queue->length - queue->space;
+}