diff --git a/openrtx/include/queue.h b/openrtx/include/queue.h
index 25b29ac5..89f9157f 100644
--- a/openrtx/include/queue.h
+++ b/openrtx/include/queue.h
@@ -1,3 +1,26 @@
+/***************************************************************************
+ * Copyright (C) 2020 by Federico Amedeo Izzo IU2NUO, *
+ * Niccolò Izzo IU2KIN *
+ * Frederik Saraci IU2NRO *
+ * Silvano Seva IU2KWO *
+ * *
+ * This program is free software; you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation; either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program; if not, see *
+ ***************************************************************************/
+
+#ifndef QUEUE_H
+#define QUEUE_H
+
#include
#include
#include
@@ -17,10 +40,24 @@ typedef struct queue_t
uint32_t buffer[MSG_QTY];
} queue_t;
+/**
+ *
+ */
void queue_init(queue_t *q);
+/**
+ *
+ */
void queue_terminate(queue_t *q);
+/**
+ *
+ */
bool queue_pend(queue_t *q, uint32_t *msg, bool blocking);
+/**
+ *
+ */
bool queue_post(queue_t *q, uint32_t msg);
+
+#endif
diff --git a/openrtx/src/queue.c b/openrtx/src/queue.c
index 9f199c46..411d8404 100644
--- a/openrtx/src/queue.c
+++ b/openrtx/src/queue.c
@@ -1,34 +1,38 @@
#include
#include "queue.h"
-void queue_init(queue_t *q) {
- if(q == NULL)
- return;
+void queue_init(queue_t *q)
+{
+ if(q == NULL) return;
pthread_mutex_init(&q->mutex, NULL);
pthread_cond_init(&q->not_empty, NULL);
- q->read_pos = 0;
+ q->read_pos = 0;
q->write_pos = 0;
- q->msg_num = 0;
+ q->msg_num = 0;
}
-void queue_terminate(queue_t *q) {
- if(q == NULL)
- return;
+void queue_terminate(queue_t *q)
+{
+ if(q == NULL) return;
pthread_mutex_destroy(&q->mutex);
pthread_cond_destroy(&q->not_empty);
}
-bool queue_pend(queue_t *q, uint32_t *msg, bool blocking) {
- if((q == NULL) || (msg == NULL))
- return false;
+bool queue_pend(queue_t *q, uint32_t *msg, bool blocking)
+{
+ if((q == NULL) || (msg == NULL)) return false;
+
pthread_mutex_lock(&q->mutex);
+
// The queue is empty
if(q->msg_num == 0)
{
if(blocking)
{
while(q->msg_num == 0)
+ {
pthread_cond_wait(&q->not_empty, &q->mutex);
+ }
}
else
{
@@ -36,26 +40,36 @@ bool queue_pend(queue_t *q, uint32_t *msg, bool blocking) {
return false;
}
}
+
*msg = q->buffer[q->read_pos];
+
// Wrap around pointer to make a circular buffer
q->read_pos = (q->read_pos + 1) % MSG_QTY;
q->msg_num -= 1;
pthread_mutex_unlock(&q->mutex);
+
return true;
}
-bool queue_post(queue_t *q, uint32_t msg) {
- if(q == NULL)
- return false;
+bool queue_post(queue_t *q, uint32_t msg)
+{
+ if(q == NULL) return false;
+
pthread_mutex_lock(&q->mutex);
+
if(q->msg_num < MSG_QTY)
{
q->buffer[q->write_pos] = msg;
+
// Wrap around pointer to make a circular buffer
q->write_pos = (q->write_pos + 1) % MSG_QTY;
+
// Signal that the queue is not empty
if(q->msg_num == 0)
+ {
pthread_cond_signal(&q->not_empty);
+ }
+
q->msg_num += 1;
}
else
@@ -63,6 +77,7 @@ bool queue_post(queue_t *q, uint32_t msg) {
pthread_mutex_unlock(&q->mutex);
return false;
}
+
pthread_mutex_unlock(&q->mutex);
return true;
}