recipes-bsp/esp32spid/src/msgq.c


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
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);
}