1 /* ring_buffer.c: Simple ring buffer API */
2 
3 /*
4  * Copyright (c) 2015 Intel Corporation
5  *
6  * SPDX-License-Identifier: Apache-2.0
7  */
8 
9 #include <zephyr/sys/ring_buffer.h>
10 #include <string.h>
11 
ring_buf_put_claim(struct ring_buf * buf,uint8_t ** data,uint32_t size)12 uint32_t ring_buf_put_claim(struct ring_buf *buf, uint8_t **data, uint32_t size)
13 {
14 	uint32_t free_space, wrap_size;
15 	int32_t base;
16 
17 	base = buf->put_base;
18 	wrap_size = buf->put_head - base;
19 	if (unlikely(wrap_size >= buf->size)) {
20 		/* put_base is not yet adjusted */
21 		wrap_size -= buf->size;
22 		base += buf->size;
23 	}
24 	wrap_size = buf->size - wrap_size;
25 
26 	free_space = ring_buf_space_get(buf);
27 	size = MIN(size, free_space);
28 	size = MIN(size, wrap_size);
29 
30 	*data = &buf->buffer[buf->put_head - base];
31 	buf->put_head += size;
32 
33 	return size;
34 }
35 
ring_buf_put_finish(struct ring_buf * buf,uint32_t size)36 int ring_buf_put_finish(struct ring_buf *buf, uint32_t size)
37 {
38 	uint32_t finish_space, wrap_size;
39 
40 	finish_space = buf->put_head - buf->put_tail;
41 	if (unlikely(size > finish_space)) {
42 		return -EINVAL;
43 	}
44 
45 	buf->put_tail += size;
46 	buf->put_head = buf->put_tail;
47 
48 	wrap_size = buf->put_tail - buf->put_base;
49 	if (unlikely(wrap_size >= buf->size)) {
50 		/* we wrapped: adjust put_base */
51 		buf->put_base += buf->size;
52 	}
53 
54 	return 0;
55 }
56 
ring_buf_put(struct ring_buf * buf,const uint8_t * data,uint32_t size)57 uint32_t ring_buf_put(struct ring_buf *buf, const uint8_t *data, uint32_t size)
58 {
59 	uint8_t *dst;
60 	uint32_t partial_size;
61 	uint32_t total_size = 0U;
62 	int err;
63 
64 	do {
65 		partial_size = ring_buf_put_claim(buf, &dst, size);
66 		memcpy(dst, data, partial_size);
67 		total_size += partial_size;
68 		size -= partial_size;
69 		data += partial_size;
70 	} while (size && partial_size);
71 
72 	err = ring_buf_put_finish(buf, total_size);
73 	__ASSERT_NO_MSG(err == 0);
74 	ARG_UNUSED(err);
75 
76 	return total_size;
77 }
78 
ring_buf_get_claim(struct ring_buf * buf,uint8_t ** data,uint32_t size)79 uint32_t ring_buf_get_claim(struct ring_buf *buf, uint8_t **data, uint32_t size)
80 {
81 	uint32_t available_size, wrap_size;
82 	int32_t base;
83 
84 	base = buf->get_base;
85 	wrap_size = buf->get_head - base;
86 	if (unlikely(wrap_size >= buf->size)) {
87 		/* get_base is not yet adjusted */
88 		wrap_size -= buf->size;
89 		base += buf->size;
90 	}
91 	wrap_size = buf->size - wrap_size;
92 
93 	available_size = ring_buf_size_get(buf);
94 	size = MIN(size, available_size);
95 	size = MIN(size, wrap_size);
96 
97 	*data = &buf->buffer[buf->get_head - base];
98 	buf->get_head += size;
99 
100 	return size;
101 }
102 
ring_buf_get_finish(struct ring_buf * buf,uint32_t size)103 int ring_buf_get_finish(struct ring_buf *buf, uint32_t size)
104 {
105 	uint32_t finish_space, wrap_size;
106 
107 	finish_space = buf->get_head - buf->get_tail;
108 	if (unlikely(size > finish_space)) {
109 		return -EINVAL;
110 	}
111 
112 	buf->get_tail += size;
113 	buf->get_head = buf->get_tail;
114 
115 	wrap_size = buf->get_tail - buf->get_base;
116 	if (unlikely(wrap_size >= buf->size)) {
117 		/* we wrapped: adjust get_base */
118 		buf->get_base += buf->size;
119 	}
120 
121 	return 0;
122 }
123 
ring_buf_get(struct ring_buf * buf,uint8_t * data,uint32_t size)124 uint32_t ring_buf_get(struct ring_buf *buf, uint8_t *data, uint32_t size)
125 {
126 	uint8_t *src;
127 	uint32_t partial_size;
128 	uint32_t total_size = 0U;
129 	int err;
130 
131 	do {
132 		partial_size = ring_buf_get_claim(buf, &src, size);
133 		if (data) {
134 			memcpy(data, src, partial_size);
135 			data += partial_size;
136 		}
137 		total_size += partial_size;
138 		size -= partial_size;
139 	} while (size && partial_size);
140 
141 	err = ring_buf_get_finish(buf, total_size);
142 	__ASSERT_NO_MSG(err == 0);
143 	ARG_UNUSED(err);
144 
145 	return total_size;
146 }
147 
ring_buf_peek(struct ring_buf * buf,uint8_t * data,uint32_t size)148 uint32_t ring_buf_peek(struct ring_buf *buf, uint8_t *data, uint32_t size)
149 {
150 	uint8_t *src;
151 	uint32_t partial_size;
152 	uint32_t total_size = 0U;
153 	int err;
154 
155 	size = MIN(size, ring_buf_size_get(buf));
156 
157 	do {
158 		partial_size = ring_buf_get_claim(buf, &src, size);
159 		__ASSERT_NO_MSG(data != NULL);
160 		memcpy(data, src, partial_size);
161 		data += partial_size;
162 		total_size += partial_size;
163 		size -= partial_size;
164 	} while (size && partial_size);
165 
166 	/* effectively unclaim total_size bytes */
167 	err = ring_buf_get_finish(buf, 0);
168 	__ASSERT_NO_MSG(err == 0);
169 	ARG_UNUSED(err);
170 
171 	return total_size;
172 }
173 
174 /**
175  * Internal data structure for a buffer header.
176  *
177  * We want all of this to fit in a single uint32_t. Every item stored in the
178  * ring buffer will be one of these headers plus any extra data supplied
179  */
180 struct ring_element {
181 	uint32_t  type   :16; /**< Application-specific */
182 	uint32_t  length :8;  /**< length in 32-bit chunks */
183 	uint32_t  value  :8;  /**< Room for small integral values */
184 };
185 
ring_buf_item_put(struct ring_buf * buf,uint16_t type,uint8_t value,uint32_t * data32,uint8_t size32)186 int ring_buf_item_put(struct ring_buf *buf, uint16_t type, uint8_t value,
187 		      uint32_t *data32, uint8_t size32)
188 {
189 	uint8_t *dst, *data = (uint8_t *)data32;
190 	struct ring_element *header;
191 	uint32_t space, size, partial_size, total_size;
192 	int err;
193 
194 	space = ring_buf_space_get(buf);
195 	size = size32 * 4;
196 	if (size + sizeof(struct ring_element) > space) {
197 		return -EMSGSIZE;
198 	}
199 
200 	err = ring_buf_put_claim(buf, &dst, sizeof(struct ring_element));
201 	__ASSERT_NO_MSG(err == sizeof(struct ring_element));
202 
203 	header = (struct ring_element *)dst;
204 	header->type = type;
205 	header->length = size32;
206 	header->value = value;
207 	total_size = sizeof(struct ring_element);
208 
209 	do {
210 		partial_size = ring_buf_put_claim(buf, &dst, size);
211 		memcpy(dst, data, partial_size);
212 		size -= partial_size;
213 		total_size += partial_size;
214 		data += partial_size;
215 	} while (size && partial_size);
216 	__ASSERT_NO_MSG(size == 0);
217 
218 	err = ring_buf_put_finish(buf, total_size);
219 	__ASSERT_NO_MSG(err == 0);
220 	ARG_UNUSED(err);
221 
222 	return 0;
223 }
224 
ring_buf_item_get(struct ring_buf * buf,uint16_t * type,uint8_t * value,uint32_t * data32,uint8_t * size32)225 int ring_buf_item_get(struct ring_buf *buf, uint16_t *type, uint8_t *value,
226 		      uint32_t *data32, uint8_t *size32)
227 {
228 	uint8_t *src, *data = (uint8_t *)data32;
229 	struct ring_element *header;
230 	uint32_t size, partial_size, total_size;
231 	int err;
232 
233 	if (ring_buf_is_empty(buf)) {
234 		return -EAGAIN;
235 	}
236 
237 	err = ring_buf_get_claim(buf, &src, sizeof(struct ring_element));
238 	__ASSERT_NO_MSG(err == sizeof(struct ring_element));
239 
240 	header = (struct ring_element *)src;
241 
242 	if (data && (header->length > *size32)) {
243 		*size32 = header->length;
244 		ring_buf_get_finish(buf, 0);
245 		return -EMSGSIZE;
246 	}
247 
248 	*size32 = header->length;
249 	*type = header->type;
250 	*value = header->value;
251 	total_size = sizeof(struct ring_element);
252 
253 	size = *size32 * 4;
254 
255 	do {
256 		partial_size = ring_buf_get_claim(buf, &src, size);
257 		if (data) {
258 			memcpy(data, src, partial_size);
259 			data += partial_size;
260 		}
261 		total_size += partial_size;
262 		size -= partial_size;
263 	} while (size && partial_size);
264 
265 	err = ring_buf_get_finish(buf, total_size);
266 	__ASSERT_NO_MSG(err == 0);
267 	ARG_UNUSED(err);
268 
269 	return 0;
270 }
271