1 /*
2 * Copyright (c) 2021 Intel Corporation
3 * SPDX-License-Identifier: Apache-2.0
4 */
5 #include <zephyr/sys/util.h>
6 #include <zephyr/sys/winstream.h>
7
8 /* This code may be used (e.g. for trace/logging) in very early
9 * environments where the standard library isn't available yet.
10 * Implement a simple memcpy() as an option.
11 */
12 #ifndef CONFIG_WINSTREAM_STDLIB_MEMCOPY
13 # define MEMCPY(dst, src, n) \
14 do { for (int i = 0; i < (n); i++) { (dst)[i] = (src)[i]; } } while (0)
15 #else
16 # include <string.h>
17 # define MEMCPY memcpy
18 #endif
19
20 /* These are just compiler barriers now. Zephyr doesn't currently
21 * have a framework for hardware memory ordering, and all our targets
22 * (arm64 excepted) either promise firm ordering (x86) or are in-order
23 * cores (everything else). But these are marked for future
24 * enhancement.
25 */
26 #define READ_BARRIER() __asm__ volatile("" ::: "memory")
27 #define WRITE_BARRIER() __asm__ volatile("")
28
idx_mod(struct sys_winstream * ws,uint32_t idx)29 static uint32_t idx_mod(struct sys_winstream *ws, uint32_t idx)
30 {
31 return idx >= ws->len ? idx - ws->len : idx;
32 }
33
34 /* Computes modular a - b, assuming a and b are in [0:len) */
idx_sub(struct sys_winstream * ws,uint32_t a,uint32_t b)35 static uint32_t idx_sub(struct sys_winstream *ws, uint32_t a, uint32_t b)
36 {
37 return idx_mod(ws, a + (ws->len - b));
38 }
39
sys_winstream_write(struct sys_winstream * ws,const char * data,uint32_t len)40 void sys_winstream_write(struct sys_winstream *ws,
41 const char *data, uint32_t len)
42 {
43 uint32_t len0 = len, suffix;
44 uint32_t start = ws->start, end = ws->end, seq = ws->seq;
45
46 /* Overflow: if we're truncating then just reset the buffer.
47 * (Max bytes buffered is actually len-1 because start==end is
48 * reserved to mean "empty")
49 */
50 if (len > ws->len - 1) {
51 start = end;
52 len = ws->len - 1;
53 }
54
55 /* Make room in the buffer by advancing start first (note same
56 * len-1 from above)
57 */
58 len = MIN(len, ws->len);
59 if (seq != 0) {
60 uint32_t avail = (ws->len - 1) - idx_sub(ws, end, start);
61
62 if (len > avail) {
63 ws->start = idx_mod(ws, start + (len - avail));
64 WRITE_BARRIER();
65 }
66 }
67
68 /* Had to truncate? */
69 if (len < len0) {
70 ws->start = end;
71 data += len0 - len;
72 }
73
74 suffix = MIN(len, ws->len - end);
75 MEMCPY(&ws->data[end], data, suffix);
76 if (len > suffix) {
77 MEMCPY(&ws->data[0], data + suffix, len - suffix);
78 }
79
80 ws->end = idx_mod(ws, end + len);
81 ws->seq += len0; /* seq represents dropped bytes too! */
82 WRITE_BARRIER();
83 }
84
sys_winstream_read(struct sys_winstream * ws,uint32_t * seq,char * buf,uint32_t buflen)85 uint32_t sys_winstream_read(struct sys_winstream *ws,
86 uint32_t *seq, char *buf, uint32_t buflen)
87 {
88 uint32_t seq0 = *seq, start, end, wseq, len, behind, copy, suffix;
89
90 do {
91 start = ws->start; end = ws->end; wseq = ws->seq;
92 READ_BARRIER();
93
94 /* No change in buffer state or empty initial stream are easy */
95 if (*seq == wseq || start == end) {
96 *seq = wseq;
97 return 0;
98 }
99
100 /* Underflow: we're trying to read from a spot farther
101 * back than start. We dropped some bytes, so cheat
102 * and just drop them all to catch up.
103 */
104 behind = wseq - *seq;
105 if (behind > idx_sub(ws, ws->end, ws->start)) {
106 *seq = wseq;
107 return 0;
108 }
109
110 /* Copy data */
111 copy = idx_sub(ws, ws->end, behind);
112 len = MIN(buflen, behind);
113 suffix = MIN(len, ws->len - copy);
114 MEMCPY(buf, &ws->data[copy], suffix);
115 if (len > suffix) {
116 MEMCPY(buf + suffix, &ws->data[0], len - suffix);
117 }
118 *seq = seq0 + len;
119
120 /* Check vs. the state we initially read and repeat if
121 * needed. This can't loop forever even if the other
122 * side is stuck spamming writes: we'll run out of
123 * buffer space and exit via the underflow condition.
124 */
125 READ_BARRIER();
126 } while (start != ws->start || wseq != ws->seq);
127
128 return len;
129 }
130
