1 files changed, 50 insertions, 0 deletions
diff --git a/yocto/esp32mod/msgq.c b/yocto/esp32mod/msgq.c
new file mode 100755
index 0000000..77ece4b
--- /dev/null
+++ b/yocto/esp32mod/msgq.c
@@ -0,0 +1,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)];
+}