1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include "data_queue.h"
7
8 #define DATA_QUEUE_MEMORY_ALIGN sizeof(uint32_t)
9
10 struct data_queue_format {
11 uint32_t header; /* Required by kernel k_queue_append */
12 size_t size;
13 uint32_t data[];
14 };
15
16
data_queue_init(struct data_queue * q,void * mem,size_t bytes)17 void data_queue_init(struct data_queue *q, void *mem, size_t bytes)
18 {
19 k_heap_init(&q->h, mem, bytes);
20 k_queue_init(&q->q);
21 }
22
data_queue_put(struct data_queue * q,const void * data,size_t bytes,k_timeout_t timeout)23 int data_queue_put(struct data_queue *q, const void *data, size_t bytes, k_timeout_t timeout)
24 {
25 struct data_queue_format *buffer = k_heap_aligned_alloc(
26 &q->h,
27 DATA_QUEUE_MEMORY_ALIGN,
28 bytes + sizeof(struct data_queue_format),
29 timeout);
30
31 if (!buffer) {
32 return -ENOMEM;
33 }
34 buffer->size = bytes;
35 memcpy(buffer->data, data, bytes);
36
37 k_queue_append(&q->q, buffer);
38 return 0;
39 }
40
data_queue_get(struct data_queue * q,size_t * size,k_timeout_t timeout)41 void *data_queue_get(struct data_queue *q, size_t *size, k_timeout_t timeout)
42 {
43 struct data_queue_format *buffer = k_queue_get(&q->q, timeout);
44
45 if (!buffer) {
46 return NULL;
47 }
48
49 if (size) {
50 *size = buffer->size;
51 }
52 return buffer->data;
53 }
54
data_queue_release(struct data_queue * q,void * data)55 void data_queue_release(struct data_queue *q, void *data)
56 {
57 struct data_queue_format *buffer = CONTAINER_OF(data, struct data_queue_format, data);
58
59 k_heap_free(&q->h, buffer);
60 }
61
data_queue_is_empty(struct data_queue * q)62 int data_queue_is_empty(struct data_queue *q)
63 {
64 return k_queue_is_empty(&q->q);
65 }
66