From 6c4998209307cd84982bf056fafa3a3135667bb2 Mon Sep 17 00:00:00 2001 From: Uros Majstorovic Date: Fri, 27 Aug 2021 02:01:29 +0200 Subject: added esp32 spi daemon --- recipes-bsp/esp32spid/src/msgq.c | 42 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 recipes-bsp/esp32spid/src/msgq.c (limited to 'recipes-bsp/esp32spid/src/msgq.c') diff --git a/recipes-bsp/esp32spid/src/msgq.c b/recipes-bsp/esp32spid/src/msgq.c new file mode 100644 index 0000000..ff9f59e --- /dev/null +++ b/recipes-bsp/esp32spid/src/msgq.c @@ -0,0 +1,42 @@ +#include +#include + +#include "msgq.h" + +#define IDX_MASK(IDX, SIZE) ((IDX) & ((SIZE) - 1)) + +int msgq_init(MSGQueue *msgq, unsigned char **array, uint16_t size) { + int rv; + + msgq->idx_r = 0; + msgq->idx_w = 0; + msgq->size = size; + msgq->array = array; + rv = pthread_mutex_init(&msgq->mutex, NULL); + if (rv) { + return MSGQ_ERR; + } + + rv = pthread_cond_init(&msgq->cond, NULL); + if (rv) { + pthread_mutex_destroy(&msgq->mutex); + return MSGQ_ERR; + } +} + +int msgq_push(MSGQueue *msgq, unsigned char *buffer) { + if ((uint16_t)(msgq->idx_w - msgq->idx_r) == msgq->size) return MSGQ_ERR_FULL; + + msgq->array[IDX_MASK(msgq->idx_w++, msgq->size)] = buffer; + return MSGQ_OK; +} + +unsigned char *msgq_pop(MSGQueue *msgq) { + if (msgq->idx_r == msgq->idx_w) return NULL; + + return msgq->array[IDX_MASK(msgq->idx_r++, msgq->size)]; +} + +uint16_t msgq_len(MSGQueue *msgq) { + return (uint16_t)(msgq->idx_w - msgq->idx_r); +} -- cgit v1.2.3