73 lines
1.7 KiB
C
73 lines
1.7 KiB
C
#include "freertos/queue.h"
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
struct QueueDefinition {
|
|
UBaseType_t len;
|
|
UBaseType_t item_size;
|
|
UBaseType_t head;
|
|
UBaseType_t tail;
|
|
UBaseType_t count;
|
|
uint8_t *storage;
|
|
};
|
|
|
|
QueueHandle_t xQueueCreate(UBaseType_t uxQueueLength, UBaseType_t uxItemSize)
|
|
{
|
|
struct QueueDefinition *q = calloc(1, sizeof(*q));
|
|
if (q == NULL) {
|
|
return NULL;
|
|
}
|
|
q->storage = malloc((size_t)uxQueueLength * uxItemSize);
|
|
if (q->storage == NULL) {
|
|
free(q);
|
|
return NULL;
|
|
}
|
|
q->len = uxQueueLength;
|
|
q->item_size = uxItemSize;
|
|
return q;
|
|
}
|
|
|
|
BaseType_t xQueueSend(QueueHandle_t xQueue, const void *pvItemToQueue, TickType_t xTicksToWait)
|
|
{
|
|
(void)xTicksToWait;
|
|
if (xQueue == NULL || pvItemToQueue == NULL) {
|
|
return pdFALSE;
|
|
}
|
|
if (xQueue->count == xQueue->len) {
|
|
return pdFALSE;
|
|
}
|
|
memcpy(xQueue->storage + (size_t)xQueue->tail * xQueue->item_size,
|
|
pvItemToQueue, xQueue->item_size);
|
|
xQueue->tail = (xQueue->tail + 1u) % xQueue->len;
|
|
xQueue->count++;
|
|
return pdTRUE;
|
|
}
|
|
|
|
BaseType_t xQueueReceive(QueueHandle_t xQueue, void *pvBuffer, TickType_t xTicksToWait)
|
|
{
|
|
(void)xTicksToWait;
|
|
if (xQueue == NULL || pvBuffer == NULL) {
|
|
return pdFALSE;
|
|
}
|
|
if (xQueue->count == 0u) {
|
|
return pdFALSE;
|
|
}
|
|
memcpy(pvBuffer, xQueue->storage + (size_t)xQueue->head * xQueue->item_size,
|
|
xQueue->item_size);
|
|
xQueue->head = (xQueue->head + 1u) % xQueue->len;
|
|
xQueue->count--;
|
|
return pdTRUE;
|
|
}
|
|
|
|
BaseType_t xQueueReset(QueueHandle_t xQueue)
|
|
{
|
|
if (xQueue == NULL) {
|
|
return pdFALSE;
|
|
}
|
|
xQueue->head = 0;
|
|
xQueue->tail = 0;
|
|
xQueue->count = 0;
|
|
return pdTRUE;
|
|
}
|