eive-obsw/thirdparty/libcsp/src/csp_qfifo.c
2021-03-04 18:29:28 +01:00

150 lines
3.8 KiB
C

/*
Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include <csp/csp.h>
#include <csp/csp_interface.h>
#include <csp/arch/csp_queue.h>
#include "csp_qfifo.h"
static csp_queue_handle_t qfifo[CSP_ROUTE_FIFOS];
#ifdef CSP_USE_QOS
static csp_queue_handle_t qfifo_events;
#endif
int csp_qfifo_init(void) {
int prio;
/* Create router fifos for each priority */
for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
if (qfifo[prio] == NULL) {
qfifo[prio] = csp_queue_create(CSP_FIFO_INPUT, sizeof(csp_qfifo_t));
if (!qfifo[prio])
return CSP_ERR_NOMEM;
}
}
#ifdef CSP_USE_QOS
/* Create QoS fifo notification queue */
qfifo_events = csp_queue_create(CSP_FIFO_INPUT, sizeof(int));
if (!qfifo_events)
return CSP_ERR_NOMEM;
#endif
return CSP_ERR_NONE;
}
int csp_qfifo_read(csp_qfifo_t * input) {
#ifdef CSP_USE_QOS
int prio, found, event;
/* Wait for packet in any queue */
if (csp_queue_dequeue(qfifo_events, &event, FIFO_TIMEOUT) != CSP_QUEUE_OK)
return CSP_ERR_TIMEDOUT;
/* Find packet with highest priority */
found = 0;
for (prio = 0; prio < CSP_ROUTE_FIFOS; prio++) {
if (csp_queue_dequeue(qfifo[prio], input, 0) == CSP_QUEUE_OK) {
found = 1;
break;
}
}
if (!found) {
csp_log_warn("Spurious wakeup: No packet found");
return CSP_ERR_TIMEDOUT;
}
#else
if (csp_queue_dequeue(qfifo[0], input, FIFO_TIMEOUT) != CSP_QUEUE_OK)
return CSP_ERR_TIMEDOUT;
#endif
return CSP_ERR_NONE;
}
void csp_qfifo_write(csp_packet_t * packet, csp_iface_t * interface, CSP_BASE_TYPE * pxTaskWoken) {
int result;
if (packet == NULL) {
if (pxTaskWoken == NULL) { // Only do logging in non-ISR context
csp_log_warn("csp_new packet called with NULL packet");
}
return;
} else if (interface == NULL) {
if (pxTaskWoken == NULL) { // Only do logging in non-ISR context
csp_log_warn("csp_new packet called with NULL interface");
}
if (pxTaskWoken == NULL)
csp_buffer_free(packet);
else
csp_buffer_free_isr(packet);
return;
}
csp_qfifo_t queue_element;
queue_element.interface = interface;
queue_element.packet = packet;
#ifdef CSP_USE_QOS
int fifo = packet->id.pri;
#else
int fifo = 0;
#endif
if (pxTaskWoken == NULL)
result = csp_queue_enqueue(qfifo[fifo], &queue_element, 0);
else
result = csp_queue_enqueue_isr(qfifo[fifo], &queue_element, pxTaskWoken);
#ifdef CSP_USE_QOS
static int event = 0;
if (result == CSP_QUEUE_OK) {
if (pxTaskWoken == NULL)
csp_queue_enqueue(qfifo_events, &event, 0);
else
csp_queue_enqueue_isr(qfifo_events, &event, pxTaskWoken);
}
#endif
if (result != CSP_QUEUE_OK) {
if (pxTaskWoken == NULL) { // Only do logging in non-ISR context
csp_log_warn("ERROR: Routing input FIFO is FULL. Dropping packet.");
}
interface->drop++;
if (pxTaskWoken == NULL)
csp_buffer_free(packet);
else
csp_buffer_free_isr(packet);
}
}
void csp_qfifo_wake_up(void) {
csp_qfifo_t queue_element;
queue_element.interface = NULL;
queue_element.packet = NULL;
csp_queue_enqueue(qfifo[0], &queue_element, 0);
}