1 /*
2 * Copyright (c) 2022 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <string.h>
9 #include <errno.h>
10 #include <zephyr/cache.h>
11 #include <zephyr/sys/spsc_pbuf.h>
12 #include <zephyr/sys/byteorder.h>
13
14 #define LEN_SZ sizeof(uint32_t)
15 /* Amount of data that is left unused to distinguish between empty and full. */
16 #define FREE_SPACE_DISTANCE sizeof(uint32_t)
17
18 #define PADDING_MARK 0xFF
19
20 #define GET_UTILIZATION(flags) \
21 (((flags) >> SPSC_PBUF_UTILIZATION_OFFSET) & BIT_MASK(SPSC_PBUF_UTILIZATION_BITS))
22
23 #define SET_UTILIZATION(flags, val) \
24 ((flags & ~(BIT_MASK(SPSC_PBUF_UTILIZATION_BITS) << \
25 SPSC_PBUF_UTILIZATION_OFFSET)) | \
26 ((val) << SPSC_PBUF_UTILIZATION_OFFSET))
27
28 /*
29 * In order to allow allocation of continuous buffers (in zero copy manner) buffer
30 * is handling wrapping. When it is detected that request space cannot be allocated
31 * at the end of the buffer but it is available at the beginning, a padding must
32 * be added. Padding is marked using 0xFF byte. Packet length is stored on 2 bytes
33 * but padding marker must be byte long as it is possible that only 1 byte padding
34 * is required. In order to distinguish padding marker from length field following
35 * measures are taken: Length is stored in big endian (MSB byte first). Maximum
36 * packet length is limited to 0XFEFF.
37 */
38
39 /* Helpers */
idx_occupied(uint32_t len,uint32_t a,uint32_t b)40 static uint32_t idx_occupied(uint32_t len, uint32_t a, uint32_t b)
41 {
42 /* It is implicitly assumed a and b cannot differ by more then len. */
43 return (b > a) ? (len - (b - a)) : (a - b);
44 }
45
cache_wb(void * data,size_t len,uint32_t flags)46 static inline void cache_wb(void *data, size_t len, uint32_t flags)
47 {
48 if (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
49 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE))) {
50 sys_cache_data_flush_range(data, len);
51 }
52 }
53
cache_inv(void * data,size_t len,uint32_t flags)54 static inline void cache_inv(void *data, size_t len, uint32_t flags)
55 {
56 if (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
57 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE))) {
58 sys_cache_data_invd_range(data, len);
59 }
60 }
61
get_rd_idx_loc(struct spsc_pbuf * pb,uint32_t flags)62 static uint32_t *get_rd_idx_loc(struct spsc_pbuf *pb, uint32_t flags)
63 {
64 return &pb->common.rd_idx;
65 }
66
get_wr_idx_loc(struct spsc_pbuf * pb,uint32_t flags)67 static uint32_t *get_wr_idx_loc(struct spsc_pbuf *pb, uint32_t flags)
68 {
69 if (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
70 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE))) {
71 return &pb->ext.cache.wr_idx;
72 }
73
74 return &pb->ext.nocache.wr_idx;
75 }
76
get_data_loc(struct spsc_pbuf * pb,uint32_t flags)77 static uint8_t *get_data_loc(struct spsc_pbuf *pb, uint32_t flags)
78 {
79 if (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
80 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE))) {
81 return pb->ext.cache.data;
82 }
83
84 return pb->ext.nocache.data;
85 }
86
get_len(size_t blen,uint32_t flags)87 static uint32_t get_len(size_t blen, uint32_t flags)
88 {
89 uint32_t len = blen - sizeof(struct spsc_pbuf_common);
90
91 if (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
92 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE))) {
93 return len - sizeof(struct spsc_pbuf_ext_cache);
94 }
95
96 return len - sizeof(struct spsc_pbuf_ext_nocache);
97 }
98
check_alignment(void * buf,uint32_t flags)99 static bool check_alignment(void *buf, uint32_t flags)
100 {
101 if ((Z_SPSC_PBUF_DCACHE_LINE > 0) && (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
102 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE)))) {
103 return ((uintptr_t)buf & (Z_SPSC_PBUF_DCACHE_LINE - 1)) == 0;
104 }
105
106 return (((uintptr_t)buf & (sizeof(uint32_t) - 1)) == 0) ? true : false;
107 }
108
spsc_pbuf_init(void * buf,size_t blen,uint32_t flags)109 struct spsc_pbuf *spsc_pbuf_init(void *buf, size_t blen, uint32_t flags)
110 {
111 if (!check_alignment(buf, flags)) {
112 __ASSERT(false, "Failed to initialize due to memory misalignment");
113 return NULL;
114 }
115
116 /* blen must be big enough to contain spsc_pbuf struct, byte of data
117 * and message len (2 bytes).
118 */
119 struct spsc_pbuf *pb = buf;
120 uint32_t *wr_idx_loc = get_wr_idx_loc(pb, flags);
121
122 __ASSERT_NO_MSG(blen > (sizeof(*pb) + LEN_SZ));
123
124 pb->common.len = get_len(blen, flags);
125 pb->common.rd_idx = 0;
126 pb->common.flags = flags;
127 *wr_idx_loc = 0;
128
129 __sync_synchronize();
130 cache_wb(&pb->common, sizeof(pb->common), flags);
131 cache_wb(wr_idx_loc, sizeof(*wr_idx_loc), flags);
132
133 return pb;
134 }
135
spsc_pbuf_alloc(struct spsc_pbuf * pb,uint16_t len,char ** buf)136 int spsc_pbuf_alloc(struct spsc_pbuf *pb, uint16_t len, char **buf)
137 {
138 /* Length of the buffer and flags are immutable - avoid reloading. */
139 const uint32_t pblen = pb->common.len;
140 const uint32_t flags = pb->common.flags;
141 uint32_t *rd_idx_loc = get_rd_idx_loc(pb, flags);
142 uint32_t *wr_idx_loc = get_wr_idx_loc(pb, flags);
143 uint8_t *data_loc = get_data_loc(pb, flags);
144
145 uint32_t space = len + LEN_SZ; /* data + length field */
146
147 if (len == 0 || len > SPSC_PBUF_MAX_LEN) {
148 /* Incorrect call. */
149 return -EINVAL;
150 }
151
152 cache_inv(rd_idx_loc, sizeof(*rd_idx_loc), flags);
153 __sync_synchronize();
154
155 uint32_t wr_idx = *wr_idx_loc;
156 uint32_t rd_idx = *rd_idx_loc;
157 int32_t free_space;
158
159 if (wr_idx >= rd_idx) {
160 int32_t remaining = pblen - wr_idx;
161 /* If SPSC_PBUF_MAX_LEN is set as length try to allocate maximum
162 * possible packet till wrap or from the beginning.
163 * If len is bigger than SPSC_PBUF_MAX_LEN then try to allocate
164 * maximum packet length even if that results in adding a padding.
165 */
166 if (len == SPSC_PBUF_MAX_LEN) {
167 /* At least space for 1 byte packet. */
168 space = LEN_SZ + 1;
169 }
170
171 if ((remaining >= space) || (rd_idx <= space)) {
172 /* Packet will fit at the end. Free space depends on
173 * presence of data at the beginning of the buffer since
174 * there must be one word not used to distinguish between
175 * empty and full state.
176 */
177 free_space = remaining - ((rd_idx > 0) ? 0 : FREE_SPACE_DISTANCE);
178 } else {
179 /* Padding must be added. */
180 data_loc[wr_idx] = PADDING_MARK;
181 __sync_synchronize();
182 cache_wb(&data_loc[wr_idx], sizeof(uint8_t), flags);
183
184 wr_idx = 0;
185 *wr_idx_loc = wr_idx;
186
187 /* Obligatory one word empty space. */
188 free_space = rd_idx - FREE_SPACE_DISTANCE;
189 }
190 } else {
191 /* Obligatory one word empty space. */
192 free_space = rd_idx - wr_idx - FREE_SPACE_DISTANCE;
193 }
194
195 len = MIN(len, MAX(free_space - (int32_t)LEN_SZ, 0));
196 *buf = &data_loc[wr_idx + LEN_SZ];
197
198 return len;
199 }
200
spsc_pbuf_commit(struct spsc_pbuf * pb,uint16_t len)201 void spsc_pbuf_commit(struct spsc_pbuf *pb, uint16_t len)
202 {
203 if (len == 0) {
204 return;
205 }
206
207 /* Length of the buffer and flags are immutable - avoid reloading. */
208 const uint32_t pblen = pb->common.len;
209 const uint32_t flags = pb->common.flags;
210 uint32_t *wr_idx_loc = get_wr_idx_loc(pb, flags);
211 uint8_t *data_loc = get_data_loc(pb, flags);
212
213 uint32_t wr_idx = *wr_idx_loc;
214
215 sys_put_be16(len, &data_loc[wr_idx]);
216 __sync_synchronize();
217 cache_wb(&data_loc[wr_idx], len + LEN_SZ, flags);
218
219 wr_idx += len + LEN_SZ;
220 wr_idx = ROUND_UP(wr_idx, sizeof(uint32_t));
221 wr_idx = wr_idx == pblen ? 0 : wr_idx;
222
223 *wr_idx_loc = wr_idx;
224 __sync_synchronize();
225 cache_wb(wr_idx_loc, sizeof(*wr_idx_loc), flags);
226 }
227
spsc_pbuf_write(struct spsc_pbuf * pb,const char * buf,uint16_t len)228 int spsc_pbuf_write(struct spsc_pbuf *pb, const char *buf, uint16_t len)
229 {
230 char *pbuf;
231 int outlen;
232
233 if (len >= SPSC_PBUF_MAX_LEN) {
234 return -EINVAL;
235 }
236
237 outlen = spsc_pbuf_alloc(pb, len, &pbuf);
238 if (outlen != len) {
239 return outlen < 0 ? outlen : -ENOMEM;
240 }
241
242 memcpy(pbuf, buf, len);
243
244 spsc_pbuf_commit(pb, len);
245
246 return len;
247 }
248
spsc_pbuf_claim(struct spsc_pbuf * pb,char ** buf)249 uint16_t spsc_pbuf_claim(struct spsc_pbuf *pb, char **buf)
250 {
251 /* Length of the buffer and flags are immutable - avoid reloading. */
252 const uint32_t pblen = pb->common.len;
253 const uint32_t flags = pb->common.flags;
254 uint32_t *rd_idx_loc = get_rd_idx_loc(pb, flags);
255 uint32_t *wr_idx_loc = get_wr_idx_loc(pb, flags);
256 uint8_t *data_loc = get_data_loc(pb, flags);
257
258 cache_inv(wr_idx_loc, sizeof(*wr_idx_loc), flags);
259 __sync_synchronize();
260
261 uint32_t wr_idx = *wr_idx_loc;
262 uint32_t rd_idx = *rd_idx_loc;
263
264 if (rd_idx == wr_idx) {
265 return 0;
266 }
267
268 uint32_t bytes_stored = idx_occupied(pblen, wr_idx, rd_idx);
269
270 /* Utilization is calculated at claiming to handle cache case when flags
271 * and rd_idx is in the same cache line thus it should be modified only
272 * by the consumer.
273 */
274 if (IS_ENABLED(CONFIG_SPSC_PBUF_UTILIZATION) && (bytes_stored > GET_UTILIZATION(flags))) {
275 __ASSERT_NO_MSG(bytes_stored <= BIT_MASK(SPSC_PBUF_UTILIZATION_BITS));
276 pb->common.flags = SET_UTILIZATION(flags, bytes_stored);
277 __sync_synchronize();
278 cache_wb(&pb->common.flags, sizeof(pb->common.flags), flags);
279 }
280
281 /* Read message len. */
282 uint16_t len;
283
284 cache_inv(&data_loc[rd_idx], LEN_SZ, flags);
285 if (data_loc[rd_idx] == PADDING_MARK) {
286 /* If padding is found we must check if we are interrupted
287 * padding injection procedure which has 2 steps (adding padding,
288 * changing write index). If padding is added but index is not
289 * yet changed, it indicates that there is no data after the
290 * padding (at the beginning of the buffer).
291 */
292 cache_inv(wr_idx_loc, sizeof(*wr_idx_loc), flags);
293 if (rd_idx == *wr_idx_loc) {
294 return 0;
295 }
296
297 *rd_idx_loc = rd_idx = 0;
298 __sync_synchronize();
299 cache_wb(rd_idx_loc, sizeof(*rd_idx_loc), flags);
300 /* After reading padding we may find out that buffer is empty. */
301 if (rd_idx == wr_idx) {
302 return 0;
303 }
304
305 cache_inv(&data_loc[rd_idx], sizeof(len), flags);
306 }
307
308 len = sys_get_be16(&data_loc[rd_idx]);
309
310 (void)bytes_stored;
311 __ASSERT_NO_MSG(bytes_stored >= (len + LEN_SZ));
312
313 cache_inv(&data_loc[rd_idx + LEN_SZ], len, flags);
314 *buf = &data_loc[rd_idx + LEN_SZ];
315
316 return len;
317 }
318
spsc_pbuf_free(struct spsc_pbuf * pb,uint16_t len)319 void spsc_pbuf_free(struct spsc_pbuf *pb, uint16_t len)
320 {
321 /* Length of the buffer and flags are immutable - avoid reloading. */
322 const uint32_t pblen = pb->common.len;
323 const uint32_t flags = pb->common.flags;
324 uint32_t *rd_idx_loc = get_rd_idx_loc(pb, flags);
325 uint32_t *wr_idx_loc = get_wr_idx_loc(pb, flags);
326 uint16_t rd_idx = *rd_idx_loc + len + LEN_SZ;
327 uint8_t *data_loc = get_data_loc(pb, flags);
328
329 rd_idx = ROUND_UP(rd_idx, sizeof(uint32_t));
330 cache_inv(&data_loc[rd_idx], sizeof(uint8_t), flags);
331 /* Handle wrapping or the fact that next packet is a padding. */
332 if (rd_idx == pblen) {
333 rd_idx = 0;
334 } else if (data_loc[rd_idx] == PADDING_MARK) {
335 cache_inv(wr_idx_loc, sizeof(*wr_idx_loc), flags);
336 /* We may hit the case when producer is in the middle of adding
337 * a padding (which happens in 2 steps: writing padding, resetting
338 * write index) and in that case we cannot consume this padding.
339 */
340 if (rd_idx != *wr_idx_loc) {
341 rd_idx = 0;
342 }
343 } else {
344 /* empty */
345 }
346
347 *rd_idx_loc = rd_idx;
348 __sync_synchronize();
349 cache_wb(rd_idx_loc, sizeof(*rd_idx_loc), flags);
350 }
351
spsc_pbuf_read(struct spsc_pbuf * pb,char * buf,uint16_t len)352 int spsc_pbuf_read(struct spsc_pbuf *pb, char *buf, uint16_t len)
353 {
354 char *pkt;
355 uint16_t plen = spsc_pbuf_claim(pb, &pkt);
356
357 if (plen == 0) {
358 return 0;
359 }
360
361 if (buf == NULL) {
362 return plen;
363 }
364
365 if (len < plen) {
366 return -ENOMEM;
367 }
368
369 memcpy(buf, pkt, plen);
370
371 spsc_pbuf_free(pb, plen);
372
373 return plen;
374 }
375
spsc_pbuf_get_utilization(struct spsc_pbuf * pb)376 int spsc_pbuf_get_utilization(struct spsc_pbuf *pb)
377 {
378 if (!IS_ENABLED(CONFIG_SPSC_PBUF_UTILIZATION)) {
379 return -ENOTSUP;
380 }
381
382 cache_inv(&pb->common.flags, sizeof(pb->common.flags), pb->common.flags);
383 __sync_synchronize();
384
385 return GET_UTILIZATION(pb->common.flags);
386 }
387
