1 /*
2 * Copyright (c) 2022 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/ztest.h>
8 #include <zephyr/ztress.h>
9 #include <zephyr/sys/spsc_pbuf.h>
10 #include <zephyr/random/random.h>
11
12 #define HDR_LEN sizeof(uint32_t)
13 #define TLEN(len) ROUND_UP(HDR_LEN + len, sizeof(uint32_t))
14 #define STRESS_TIMEOUT_MS ((CONFIG_SYS_CLOCK_TICKS_PER_SEC < 10000) ? 1000 : 15000)
15
16 /* The buffer size itself would be 199 bytes.
17 * 212 - sizeof(struct spsc_pbuf) - 1 = 199.
18 * -1 because internal rd/wr_idx is reserved to mean the buffer is empty.
19 */
use_cache(uint32_t flags)20 static bool use_cache(uint32_t flags)
21 {
22 return IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_ALWAYS) ||
23 (IS_ENABLED(CONFIG_SPSC_PBUF_CACHE_FLAG) && (flags & SPSC_PBUF_CACHE));
24 }
25
test_spsc_pbuf_flags(uint32_t flags)26 static void test_spsc_pbuf_flags(uint32_t flags)
27 {
28 static uint8_t memory_area[216] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
29 static uint8_t rbuf[198];
30 static uint8_t message[20] = {'a'};
31 struct spsc_pbuf *ib;
32 int rlen;
33 int wlen;
34 size_t capacity = (use_cache(flags) ?
35 (sizeof(memory_area) - offsetof(struct spsc_pbuf, ext.cache.data)) :
36 (sizeof(memory_area) - offsetof(struct spsc_pbuf, ext.nocache.data))) -
37 sizeof(uint32_t);
38
39 memset(memory_area, 0, sizeof(memory_area));
40 ib = spsc_pbuf_init(memory_area, sizeof(memory_area), flags);
41 zassert_equal_ptr(ib, memory_area, NULL);
42 zassert_equal(spsc_pbuf_capacity(ib), capacity);
43
44 /* Try writing invalid value. */
45 rlen = spsc_pbuf_write(ib, rbuf, 0);
46 zassert_equal(rlen, -EINVAL);
47 rlen = spsc_pbuf_write(ib, rbuf, SPSC_PBUF_MAX_LEN);
48 zassert_equal(rlen, -EINVAL);
49
50 /* Try to write more than buffer can store. */
51 rlen = spsc_pbuf_write(ib, rbuf, sizeof(rbuf));
52 zassert_equal(rlen, -ENOMEM);
53
54 /* Read empty buffer. */
55 rlen = spsc_pbuf_read(ib, rbuf, sizeof(rbuf));
56 zassert_equal(rlen, 0);
57
58 /* Single write and read. */
59 wlen = spsc_pbuf_write(ib, message, sizeof(message));
60 zassert_equal(wlen, sizeof(message));
61
62 rlen = spsc_pbuf_read(ib, rbuf, sizeof(rbuf));
63 zassert_equal(rlen, sizeof(message));
64
65 ib = spsc_pbuf_init(memory_area, sizeof(memory_area), flags);
66 zassert_equal_ptr(ib, memory_area, NULL);
67
68 int repeat = capacity / (sizeof(message) + sizeof(uint32_t));
69
70 for (int i = 0; i < repeat; i++) {
71 wlen = spsc_pbuf_write(ib, message, sizeof(message));
72 zassert_equal(wlen, sizeof(message));
73 }
74
75 wlen = spsc_pbuf_write(ib, message, sizeof(message));
76 zassert_equal(wlen, -ENOMEM);
77
78 /* Test reading with buf == NULL, should return len of the next message to read. */
79 rlen = spsc_pbuf_read(ib, NULL, 0);
80 zassert_equal(rlen, sizeof(message));
81
82 /* Read with len == 0 and correct buf pointer. */
83 rlen = spsc_pbuf_read(ib, rbuf, 0);
84 zassert_equal(rlen, -ENOMEM);
85
86 /* Read whole data from the buffer. */
87 for (size_t i = 0; i < repeat; i++) {
88 wlen = spsc_pbuf_read(ib, rbuf, sizeof(rbuf));
89 zassert_equal(wlen, sizeof(message));
90 }
91
92 /* Buffer is empty */
93 rlen = spsc_pbuf_read(ib, NULL, 0);
94 zassert_equal(rlen, 0);
95
96 /* Write message that would be wrapped around. */
97 wlen = spsc_pbuf_write(ib, message, sizeof(message));
98 zassert_equal(wlen, sizeof(message));
99
100 /* Read wrapped message. */
101 rlen = spsc_pbuf_read(ib, rbuf, sizeof(rbuf));
102 zassert_equal(rlen, sizeof(message));
103 zassert_equal(message[0], 'a');
104 }
105
ZTEST(test_spsc_pbuf,test_spsc_pbuf_ut)106 ZTEST(test_spsc_pbuf, test_spsc_pbuf_ut)
107 {
108 test_spsc_pbuf_flags(0);
109 }
110
ZTEST(test_spsc_pbuf,test_spsc_pbuf_ut_cache)111 ZTEST(test_spsc_pbuf, test_spsc_pbuf_ut_cache)
112 {
113 test_spsc_pbuf_flags(SPSC_PBUF_CACHE);
114 }
115
check_buffer(char * buf,uint16_t len,char exp)116 static int check_buffer(char *buf, uint16_t len, char exp)
117 {
118 for (uint16_t i = 0; i < len; i++) {
119 if (buf[i] != exp) {
120 return -EINVAL;
121 }
122 }
123
124 return 0;
125 }
126
packet_write(struct spsc_pbuf * pb,uint16_t len,uint16_t outlen,uint8_t id,int exp_rv,int line)127 static void packet_write(struct spsc_pbuf *pb,
128 uint16_t len,
129 uint16_t outlen,
130 uint8_t id,
131 int exp_rv,
132 int line)
133 {
134 int rv;
135 char *buf;
136
137 rv = spsc_pbuf_alloc(pb, len, &buf);
138 zassert_equal(rv, exp_rv, "%d: Unexpected rv:%d (exp:%d)", line, rv, exp_rv);
139 if (rv < 0) {
140 return;
141 }
142 zassert_equal((uintptr_t)buf % sizeof(uint32_t), 0, "%d: Expected aligned buffer", line);
143 zassert_true(rv >= outlen, "%d: Unexpected rv (bigger than %d)", line, rv, outlen);
144
145 for (uint16_t i = 0; i < outlen; i++) {
146 buf[i] = id + i;
147 }
148
149 if (outlen > 0) {
150 spsc_pbuf_commit(pb, outlen);
151 }
152 }
153 #define PACKET_WRITE(_pb, _len, _outlen, _id, _exp_rv) \
154 packet_write(_pb, _len, _outlen, _id, _exp_rv, __LINE__)
155
packet_consume(struct spsc_pbuf * pb,uint16_t exp_rv,uint8_t exp_id,int line)156 static void packet_consume(struct spsc_pbuf *pb,
157 uint16_t exp_rv,
158 uint8_t exp_id,
159 int line)
160 {
161 uint16_t rv;
162 char *buf;
163
164 rv = spsc_pbuf_claim(pb, &buf);
165 zassert_equal(rv, exp_rv, "%d: Unexpected rv:%d (exp:%d)", line, rv, exp_rv);
166 if (rv == 0) {
167 return;
168 }
169
170 for (int i = 0; i < rv; i++) {
171 zassert_equal(buf[i], exp_id + i, "%d: Unexpected value %d (exp:%d) at %d",
172 line, buf[i], exp_id + i, i);
173 }
174
175 spsc_pbuf_free(pb, rv);
176 }
177
178 #define PACKET_CONSUME(_pb, _exp_rv, _exp_id) packet_consume(_pb, _exp_rv, _exp_id, __LINE__)
179
ZTEST(test_spsc_pbuf,test_0cpy)180 ZTEST(test_spsc_pbuf, test_0cpy)
181 {
182 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
183 struct spsc_pbuf *pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
184 uint32_t capacity = spsc_pbuf_capacity(pb);
185 uint16_t len1;
186 uint16_t len2;
187
188 /* Writing 0 length returns error. */
189 PACKET_WRITE(pb, 0, 0, 0, -EINVAL);
190 spsc_pbuf_commit(pb, 0);
191
192 PACKET_WRITE(pb, SPSC_PBUF_MAX_LEN, 0, 0, capacity - sizeof(uint32_t));
193
194 len1 = capacity - 8 - 2 * sizeof(uint32_t);
195 PACKET_WRITE(pb, len1, len1, 0, len1);
196
197 /* Remaining space. */
198 len2 = capacity - TLEN(len1) - HDR_LEN;
199 /* Request exceeding capacity*/
200 PACKET_WRITE(pb, len2 + 1, 0, 1, len2);
201
202 PACKET_WRITE(pb, len2, len2, 1, len2);
203
204 /* Consume packets. */
205 PACKET_CONSUME(pb, len1, 0);
206 PACKET_CONSUME(pb, len2, 1);
207
208 /* No more packets. */
209 PACKET_CONSUME(pb, 0, 0);
210 }
211
ZTEST(test_spsc_pbuf,test_0cpy_smaller)212 ZTEST(test_spsc_pbuf, test_0cpy_smaller)
213 {
214 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
215 struct spsc_pbuf *pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
216 uint32_t capacity = spsc_pbuf_capacity(pb);
217 uint16_t len1;
218 uint16_t len2;
219
220 len1 = capacity - 10 - sizeof(uint16_t);
221 PACKET_WRITE(pb, len1, len1 - 5, 0, len1);
222
223 len2 = 10 - sizeof(uint16_t) - 1;
224 PACKET_WRITE(pb, len2, len2, 1, len2);
225
226 /* Consume packets. */
227 PACKET_CONSUME(pb, len1 - 5, 0);
228 PACKET_CONSUME(pb, len2, 1);
229 PACKET_CONSUME(pb, 0, 0);
230 }
231
ZTEST(test_spsc_pbuf,test_0cpy_discard)232 ZTEST(test_spsc_pbuf, test_0cpy_discard)
233 {
234 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
235 struct spsc_pbuf *pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
236 uint32_t capacity = spsc_pbuf_capacity(pb);
237 int len1, len2;
238
239 len1 = 14;
240 PACKET_WRITE(pb, len1, len1, 0, len1);
241
242 len2 = capacity - TLEN(len1) - 10;
243 PACKET_WRITE(pb, len2, len2, 1, len2);
244
245 /* Consume first packet */
246 PACKET_CONSUME(pb, len1, 0);
247
248 /* Consume next packet. At this point buffer shall be completely empty. */
249 PACKET_CONSUME(pb, len2, 1);
250
251 /* Allocate but then discard by committing 0 length. Alloc will add padding. */
252 PACKET_WRITE(pb, len1, 0, 0, len1);
253
254 /* No packet in the buffer. */
255 PACKET_CONSUME(pb, 0, 0);
256
257 /* Buffer is empty except for the padding added by alloc. */
258 len2 = len1 + len2 - sizeof(uint16_t);
259 PACKET_WRITE(pb, len2, 0, 0, len2);
260 }
261
ZTEST(test_spsc_pbuf,test_0cpy_corner1)262 ZTEST(test_spsc_pbuf, test_0cpy_corner1)
263 {
264 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
265 struct spsc_pbuf *pb;
266 uint32_t capacity;
267 char *buf;
268 uint16_t len;
269 uint16_t len1;
270 uint16_t len2;
271
272 pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
273 capacity = spsc_pbuf_capacity(pb);
274
275 /* Commit 5 byte packet. */
276 len1 = 5;
277 PACKET_WRITE(pb, len1, len1, 0, len1);
278
279 /* Attempt to allocate packet till the end of the buffer. */
280 len2 = capacity;
281 len2 = spsc_pbuf_alloc(pb, len2, &buf);
282
283 uint16_t exp_len2 = capacity - TLEN(len1) - HDR_LEN;
284
285 zassert_equal(len2, exp_len2, "got %d, exp: %d", len2, exp_len2);
286
287 len = spsc_pbuf_claim(pb, &buf);
288 zassert_equal(len1, len);
289 spsc_pbuf_free(pb, len);
290
291 spsc_pbuf_commit(pb, len2);
292
293 len = spsc_pbuf_claim(pb, &buf);
294 zassert_equal(len2, len);
295 spsc_pbuf_free(pb, len);
296 }
297
ZTEST(test_spsc_pbuf,test_0cpy_corner2)298 ZTEST(test_spsc_pbuf, test_0cpy_corner2)
299 {
300 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
301 struct spsc_pbuf *pb;
302 uint32_t capacity;
303 uint16_t len1;
304 uint16_t len2;
305
306 pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
307 capacity = spsc_pbuf_capacity(pb);
308
309 /* Commit 16 byte packet. */
310 len1 = 16;
311 PACKET_WRITE(pb, len1, len1, 0, len1);
312
313 /* Attempt to allocate packet that will leave 5 bytes at the end. */
314 len2 = capacity - TLEN(len1) - HDR_LEN - 5;
315 PACKET_WRITE(pb, len2, len2, 1, len2);
316
317 /* Free first packet. */
318 PACKET_CONSUME(pb, len1, 0);
319
320 /* Allocate something that does not fit at the end. */
321 len1 = 8;
322 PACKET_WRITE(pb, len1, len1, 2, len1);
323
324 /* There should be no place in the buffer now, only length field would fill.*/
325 PACKET_WRITE(pb, 1, 0, 2, 0);
326
327 /* Free second packet. */
328 PACKET_CONSUME(pb, len2, 1);
329
330 /* Get longest available. As now there is only one packet at the beginning
331 * that should be remaining space decremented by length fields.
332 */
333 uint16_t exp_len = capacity - TLEN(len1) - HDR_LEN;
334
335 PACKET_WRITE(pb, capacity, 0, 2, exp_len);
336 }
337
ZTEST(test_spsc_pbuf,test_largest_alloc)338 ZTEST(test_spsc_pbuf, test_largest_alloc)
339 {
340 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
341 struct spsc_pbuf *pb;
342 uint32_t capacity;
343 uint16_t len1;
344 uint16_t len2;
345
346 pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
347 capacity = spsc_pbuf_capacity(pb);
348
349 len1 = 15;
350 PACKET_WRITE(pb, len1, len1, 0, len1);
351 PACKET_CONSUME(pb, len1, 0);
352
353 len2 = capacity - TLEN(len1) - TLEN(10);
354 PACKET_WRITE(pb, len2, len2, 1, len2);
355
356 PACKET_WRITE(pb, SPSC_PBUF_MAX_LEN, 0, 1, 12);
357
358 PACKET_WRITE(pb, SPSC_PBUF_MAX_LEN - 1, 0, 1, 12);
359
360 pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
361 capacity = spsc_pbuf_capacity(pb);
362
363 len1 = 15;
364 PACKET_WRITE(pb, len1, len1, 0, len1);
365 PACKET_CONSUME(pb, len1, 0);
366
367 len2 = capacity - TLEN(len1) - TLEN(12);
368 PACKET_WRITE(pb, len2, len2, 1, len2);
369
370 PACKET_WRITE(pb, SPSC_PBUF_MAX_LEN - 1, 0, 1, 12);
371 }
372
ZTEST(test_spsc_pbuf,test_utilization)373 ZTEST(test_spsc_pbuf, test_utilization)
374 {
375 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
376 struct spsc_pbuf *pb;
377 uint32_t capacity;
378 uint16_t len1, len2, len3;
379 int u;
380
381 pb = spsc_pbuf_init(buffer, sizeof(buffer), 0);
382
383 if (!IS_ENABLED(CONFIG_SPSC_PBUF_UTILIZATION)) {
384 zassert_equal(spsc_pbuf_get_utilization(pb), -ENOTSUP);
385 return;
386 }
387 capacity = spsc_pbuf_capacity(pb);
388
389 len1 = 10;
390 PACKET_WRITE(pb, len1, len1, 0, len1);
391 u = spsc_pbuf_get_utilization(pb);
392 zassert_equal(u, 0);
393
394 PACKET_CONSUME(pb, len1, 0);
395 u = spsc_pbuf_get_utilization(pb);
396 zassert_equal(u, TLEN(len1));
397
398 len2 = 11;
399 PACKET_WRITE(pb, len2, len2, 1, len2);
400 PACKET_CONSUME(pb, len2, 1);
401 u = spsc_pbuf_get_utilization(pb);
402 zassert_equal(u, TLEN(len2));
403
404 len3 = capacity - TLEN(len1) - TLEN(len2);
405 PACKET_WRITE(pb, SPSC_PBUF_MAX_LEN, len3, 2, len3);
406 PACKET_CONSUME(pb, len3, 2);
407
408 u = spsc_pbuf_get_utilization(pb);
409 int exp_u = TLEN(len3);
410
411 zassert_equal(u, exp_u);
412 }
413
414 struct stress_data {
415 struct spsc_pbuf *pbuf;
416 uint32_t capacity;
417 uint32_t write_cnt;
418 uint32_t read_cnt;
419 uint32_t wr_err;
420 };
421
stress_read(void * user_data,uint32_t cnt,bool last,int prio)422 bool stress_read(void *user_data, uint32_t cnt, bool last, int prio)
423 {
424 struct stress_data *ctx = (struct stress_data *)user_data;
425 char buf[128];
426 int len;
427 int rpt = (sys_rand8_get() & 3) + 1;
428
429 for (int i = 0; i < rpt; i++) {
430 len = spsc_pbuf_read(ctx->pbuf, buf, (uint16_t)sizeof(buf));
431 if (len == 0) {
432 return true;
433 }
434
435 if (len < 0) {
436 zassert_true(false, "Unexpected error: %d, cnt:%d", len, ctx->read_cnt);
437 }
438
439 zassert_ok(check_buffer(buf, len, ctx->read_cnt));
440 ctx->read_cnt++;
441 }
442
443 return true;
444 }
445
stress_write(void * user_data,uint32_t cnt,bool last,int prio)446 bool stress_write(void *user_data, uint32_t cnt, bool last, int prio)
447 {
448 struct stress_data *ctx = (struct stress_data *)user_data;
449 char buf[128];
450 uint16_t len = 1 + (sys_rand16_get() % (ctx->capacity / 4));
451 int rpt = (sys_rand8_get() & 1) + 1;
452
453 zassert_true(len < sizeof(buf), "len:%d %d", len, ctx->capacity);
454
455 for (int i = 0; i < rpt; i++) {
456 memset(buf, (uint8_t)ctx->write_cnt, len);
457 if (spsc_pbuf_write(ctx->pbuf, buf, len) == len) {
458 ctx->write_cnt++;
459 } else {
460 ctx->wr_err++;
461 }
462 }
463
464 return true;
465 }
466
ZTEST(test_spsc_pbuf,test_stress)467 ZTEST(test_spsc_pbuf, test_stress)
468 {
469 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
470 static struct stress_data ctx = {};
471 uint32_t repeat = 0;
472
473 ctx.pbuf = spsc_pbuf_init(buffer, sizeof(buffer), 0);
474 ctx.capacity = spsc_pbuf_capacity(ctx.pbuf);
475
476 ztress_set_timeout(K_MSEC(STRESS_TIMEOUT_MS));
477 TC_PRINT("Reading from an interrupt, writing from a thread\n");
478 ZTRESS_EXECUTE(ZTRESS_TIMER(stress_read, &ctx, repeat, Z_TIMEOUT_TICKS(4)),
479 ZTRESS_THREAD(stress_write, &ctx, repeat, 2000, Z_TIMEOUT_TICKS(4)));
480 TC_PRINT("Writes:%d failures: %d\n", ctx.write_cnt, ctx.wr_err);
481
482 TC_PRINT("Writing from an interrupt, reading from a thread\n");
483 ZTRESS_EXECUTE(ZTRESS_TIMER(stress_write, &ctx, repeat, Z_TIMEOUT_TICKS(4)),
484 ZTRESS_THREAD(stress_read, &ctx, repeat, 1000, Z_TIMEOUT_TICKS(4)));
485 TC_PRINT("Writes:%d failures: %d\n", ctx.write_cnt, ctx.wr_err);
486 }
487
stress_claim_free(void * user_data,uint32_t cnt,bool last,int prio)488 bool stress_claim_free(void *user_data, uint32_t cnt, bool last, int prio)
489 {
490 struct stress_data *ctx = (struct stress_data *)user_data;
491 char *buf;
492 uint16_t len;
493 int rpt = sys_rand8_get() % 0x3;
494
495 for (int i = 0; i < rpt; i++) {
496 len = spsc_pbuf_claim(ctx->pbuf, &buf);
497
498 if (len == 0) {
499 return true;
500 }
501
502 zassert_ok(check_buffer(buf, len, ctx->read_cnt));
503
504 spsc_pbuf_free(ctx->pbuf, len);
505
506 ctx->read_cnt++;
507 }
508
509 return true;
510 }
511
stress_alloc_commit(void * user_data,uint32_t cnt,bool last,int prio)512 bool stress_alloc_commit(void *user_data, uint32_t cnt, bool last, int prio)
513 {
514 struct stress_data *ctx = (struct stress_data *)user_data;
515 uint16_t rnd = sys_rand16_get();
516 uint16_t len = 1 + (rnd % (ctx->capacity / 4));
517 int rpt = rnd % 0x3;
518 char *buf;
519 int err;
520
521 for (int i = 0; i < rpt; i++) {
522 err = spsc_pbuf_alloc(ctx->pbuf, len, &buf);
523 zassert_true(err >= 0);
524 if (err != len) {
525 return true;
526 }
527
528 memset(buf, (uint8_t)ctx->write_cnt, len);
529
530 spsc_pbuf_commit(ctx->pbuf, len);
531 ctx->write_cnt++;
532 }
533
534 return true;
535 }
536
ZTEST(test_spsc_pbuf,test_stress_0cpy)537 ZTEST(test_spsc_pbuf, test_stress_0cpy)
538 {
539 static uint8_t buffer[128] __aligned(MAX(Z_SPSC_PBUF_DCACHE_LINE, 4));
540 static struct stress_data ctx;
541 uint32_t repeat = 0;
542
543 ctx.write_cnt = 0;
544 ctx.read_cnt = 0;
545 ctx.pbuf = spsc_pbuf_init(buffer, sizeof(buffer), 0);
546 ctx.capacity = spsc_pbuf_capacity(ctx.pbuf);
547
548 ztress_set_timeout(K_MSEC(STRESS_TIMEOUT_MS));
549 ZTRESS_EXECUTE(ZTRESS_THREAD(stress_claim_free, &ctx, repeat, 0, Z_TIMEOUT_TICKS(4)),
550 ZTRESS_THREAD(stress_alloc_commit, &ctx, repeat, 1000, Z_TIMEOUT_TICKS(4)));
551
552 ZTRESS_EXECUTE(ZTRESS_THREAD(stress_alloc_commit, &ctx, repeat, 0, Z_TIMEOUT_TICKS(4)),
553 ZTRESS_THREAD(stress_claim_free, &ctx, repeat, 1000, Z_TIMEOUT_TICKS(4)));
554 }
555
556 ZTEST_SUITE(test_spsc_pbuf, NULL, NULL, NULL, NULL, NULL);
557
