diff options
author | Uros Majstorovic <majstor@majstor.org> | 2021-08-27 02:01:29 +0200 |
---|---|---|
committer | Uros Majstorovic <majstor@majstor.org> | 2021-08-27 02:01:29 +0200 |
commit | 6c4998209307cd84982bf056fafa3a3135667bb2 (patch) | |
tree | 3801722505109db67f8e99ffff153ed070465d9a /recipes-bsp/esp32spid/src/msgq.c | |
parent | 33dec797ef8d0d556723bba7c2bd46c4b59f5bea (diff) |
added esp32 spi daemon
Diffstat (limited to 'recipes-bsp/esp32spid/src/msgq.c')
-rw-r--r-- | recipes-bsp/esp32spid/src/msgq.c | 42 |
1 files changed, 42 insertions, 0 deletions
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 <stdlib.h> +#include <pthread.h> + +#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); +} |