1 /*
2 * Arm SCP/MCP Software
3 * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
4 *
5 * SPDX-License-Identifier: BSD-3-Clause
6 */
7
8 #include <fwk_assert.h>
9 #include <fwk_macros.h>
10 #include <fwk_ring.h>
11
12 #include <stdbool.h>
13 #include <stddef.h>
14 #include <string.h>
15
fwk_ring_offset(const struct fwk_ring * ring,size_t idx)16 static size_t fwk_ring_offset(const struct fwk_ring *ring, size_t idx)
17 {
18 return (idx % ring->capacity);
19 }
20
fwk_ring_init(struct fwk_ring * ring,char * storage,size_t storage_size)21 void fwk_ring_init(struct fwk_ring *ring, char *storage, size_t storage_size)
22 {
23 fwk_assert(ring != NULL);
24 fwk_assert(storage != NULL);
25 fwk_assert(storage_size > 0);
26
27 *ring = (struct fwk_ring){
28 .storage = storage,
29 .capacity = storage_size,
30 };
31
32 fwk_ring_clear(ring);
33 }
34
fwk_ring_get_capacity(const struct fwk_ring * ring)35 size_t fwk_ring_get_capacity(const struct fwk_ring *ring)
36 {
37 fwk_assert(ring != NULL);
38
39 return ring->capacity;
40 }
41
fwk_ring_get_length(const struct fwk_ring * ring)42 size_t fwk_ring_get_length(const struct fwk_ring *ring)
43 {
44 fwk_assert(ring != NULL);
45
46 if ((ring->tail >= ring->head) && !ring->full) {
47 return (ring->tail - ring->head);
48 } else {
49 return ((ring->capacity - ring->head) + ring->tail);
50 }
51 }
52
fwk_ring_get_free(const struct fwk_ring * ring)53 size_t fwk_ring_get_free(const struct fwk_ring *ring)
54 {
55 fwk_assert(ring != NULL);
56
57 return ring->capacity - fwk_ring_get_length(ring);
58 }
59
fwk_ring_is_full(const struct fwk_ring * ring)60 bool fwk_ring_is_full(const struct fwk_ring *ring)
61 {
62 fwk_assert(ring != NULL);
63
64 return ring->full;
65 }
66
fwk_ring_is_empty(const struct fwk_ring * ring)67 bool fwk_ring_is_empty(const struct fwk_ring *ring)
68 {
69 fwk_assert(ring != NULL);
70
71 return ((ring->head == ring->tail) && !ring->full);
72 }
73
fwk_ring_pop(struct fwk_ring * ring,char * buffer,size_t buffer_size)74 size_t fwk_ring_pop(struct fwk_ring *ring, char *buffer, size_t buffer_size)
75 {
76 fwk_assert(ring != NULL);
77
78 buffer_size = (buffer == NULL) ?
79 FWK_MIN(buffer_size, fwk_ring_get_length(ring)) :
80 fwk_ring_peek(ring, buffer, buffer_size);
81
82 if (buffer_size > 0) {
83 ring->head = fwk_ring_offset(ring, ring->head + buffer_size);
84
85 ring->full = false;
86 }
87
88 return buffer_size;
89 }
90
fwk_ring_peek(const struct fwk_ring * ring,char * buffer,size_t buffer_size)91 size_t fwk_ring_peek(
92 const struct fwk_ring *ring,
93 char *buffer,
94 size_t buffer_size)
95 {
96 fwk_assert(ring != NULL);
97 fwk_assert(buffer != NULL);
98
99 buffer_size = FWK_MIN(buffer_size, fwk_ring_get_length(ring));
100 if (buffer_size == 0) {
101 return buffer_size;
102 }
103
104 if (fwk_ring_offset(ring, ring->head + buffer_size) > ring->head) {
105 (void)memcpy(buffer, ring->storage + ring->head, buffer_size);
106 } else {
107 size_t chunk_size = ring->capacity - ring->head;
108
109 (void)memcpy(buffer, ring->storage + ring->head, chunk_size);
110 (void)memcpy(
111 buffer + chunk_size, ring->storage, buffer_size - chunk_size);
112 }
113
114 return buffer_size;
115 }
116
fwk_ring_push(struct fwk_ring * ring,const char * buffer,size_t buffer_size)117 size_t fwk_ring_push(
118 struct fwk_ring *ring,
119 const char *buffer,
120 size_t buffer_size)
121 {
122 size_t remaining;
123
124 fwk_assert(ring != NULL);
125 fwk_assert(buffer != NULL);
126
127 if (buffer_size == 0) {
128 return buffer_size;
129 }
130
131 if (buffer_size > ring->capacity) {
132 /*
133 * If the user has requested that we write more bytes than the ring
134 * buffer can hold, capture only the trailing bytes.
135 */
136
137 buffer += (buffer_size - ring->capacity);
138 buffer_size = ring->capacity;
139 }
140
141 remaining = fwk_ring_get_free(ring);
142
143 if (fwk_ring_offset(ring, ring->tail + buffer_size) > ring->tail) {
144 (void)memcpy(ring->storage + ring->tail, buffer, buffer_size);
145 } else {
146 size_t chunk_size = ring->capacity - ring->tail;
147
148 (void)memcpy(ring->storage + ring->tail, buffer, chunk_size);
149 (void)memcpy(
150 ring->storage, buffer + chunk_size, buffer_size - chunk_size);
151 }
152
153 ring->tail = fwk_ring_offset(ring, ring->tail + buffer_size);
154
155 if (buffer_size >= remaining) {
156 ring->head = ring->tail;
157 ring->full = true;
158 }
159
160 /*
161 * Note that if the user tried to write more than the ring buffer could hold
162 * then the length returned will be the amount of data actually written,
163 * which is the capacity of the ring buffer.
164 */
165
166 return buffer_size;
167 }
168
fwk_ring_clear(struct fwk_ring * ring)169 void fwk_ring_clear(struct fwk_ring *ring)
170 {
171 fwk_assert(ring != NULL);
172
173 ring->head = 0;
174 ring->tail = 0;
175
176 ring->full = false;
177 }
178