yocto/esp32mod/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
43
44
45
46
47
48
49
50

#include <linux/module.h>

#include "msgq.h"

#define IDX_MASK(IDX, SIZE)     ((IDX) & ((SIZE) - 1))

int msgq_create(MSGQueue *msgq, uint16_t size) {
    unsigned char **array;

    array = kmalloc(size * sizeof(unsigned char *), GFP_KERNEL);
    if (array == NULL) return -ENOMEM;

    msgq->idx_r = 0;
    msgq->idx_w = 0;
    msgq->size = size;
    msgq->array = array;
    spin_lock_init(&msgq->lock);
    init_waitqueue_head(&msgq->wq);

    return 0;
}

void msgq_destroy(MSGQueue *msgq) {
    kfree(msgq->array);
    msgq->idx_r = 0;
    msgq->idx_w = 0;
    msgq->size = 0;
    msgq->array = NULL;
}

uint16_t msgq_len(MSGQueue *msgq) {
    return (uint16_t)(msgq->idx_w - msgq->idx_r);
}

uint16_t msgq_size(MSGQueue *msgq) {
    return msgq->size;
}

int msgq_push(MSGQueue *msgq, unsigned char *buffer) {
    if ((uint16_t)(msgq->idx_w - msgq->idx_r) == msgq->size) return -EAGAIN;

    msgq->array[IDX_MASK(msgq->idx_w++, msgq->size)] = buffer;
    return 0;
}

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)];
}