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; }