PX4 Firmware
PX4 Autopilot Software http://px4.io
ringbuffer.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file ringbuffer.cpp
36  *
37  * A flexible ringbuffer class.
38  */
39 
40 #include "ringbuffer.h"
41 #include <string.h>
42 
43 namespace ringbuffer
44 {
45 
46 RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
47  _num_items(num_items),
48  _item_size(item_size),
49  _buf(new char[(_num_items + 1) * item_size]),
50  _head(_num_items),
51  _tail(_num_items)
52 {}
53 
55 {
56  delete[] _buf;
57 }
58 
59 unsigned
60 RingBuffer::_next(unsigned index)
61 {
62  return (0 == index) ? _num_items : (index - 1);
63 }
64 
65 bool
67 {
68  return _tail == _head;
69 }
70 
71 bool
73 {
74  return _next(_head) == _tail;
75 }
76 
77 unsigned
79 {
80  return (_buf != nullptr) ? _num_items : 0;
81 }
82 
83 void
85 {
86  while (!empty()) {
87  get(nullptr);
88  }
89 }
90 
91 bool
92 RingBuffer::put(const void *val, size_t val_size)
93 {
94  unsigned next = _next(_head);
95 
96  if (next != _tail) {
97  if ((val_size == 0) || (val_size > _item_size)) {
98  val_size = _item_size;
99  }
100 
101  memcpy(&_buf[_head * _item_size], val, val_size);
102  _head = next;
103  return true;
104 
105  } else {
106  return false;
107  }
108 }
109 
110 bool
111 RingBuffer::put(int8_t val)
112 {
113  return put(&val, sizeof(val));
114 }
115 
116 bool
117 RingBuffer::put(uint8_t val)
118 {
119  return put(&val, sizeof(val));
120 }
121 
122 bool
123 RingBuffer::put(int16_t val)
124 {
125  return put(&val, sizeof(val));
126 }
127 
128 bool
129 RingBuffer::put(uint16_t val)
130 {
131  return put(&val, sizeof(val));
132 }
133 
134 bool
135 RingBuffer::put(int32_t val)
136 {
137  return put(&val, sizeof(val));
138 }
139 
140 bool
141 RingBuffer::put(uint32_t val)
142 {
143  return put(&val, sizeof(val));
144 }
145 
146 bool
147 RingBuffer::put(int64_t val)
148 {
149  return put(&val, sizeof(val));
150 }
151 
152 bool
153 RingBuffer::put(uint64_t val)
154 {
155  return put(&val, sizeof(val));
156 }
157 
158 bool
159 RingBuffer::put(float val)
160 {
161  return put(&val, sizeof(val));
162 }
163 
164 bool
165 RingBuffer::put(double val)
166 {
167  return put(&val, sizeof(val));
168 }
169 
170 bool
171 RingBuffer::force(const void *val, size_t val_size)
172 {
173  bool overwrote = false;
174 
175  for (;;) {
176  if (put(val, val_size)) {
177  break;
178  }
179 
180  get(nullptr);
181  overwrote = true;
182  }
183 
184  return overwrote;
185 }
186 
187 bool
188 RingBuffer::force(int8_t val)
189 {
190  return force(&val, sizeof(val));
191 }
192 
193 bool
194 RingBuffer::force(uint8_t val)
195 {
196  return force(&val, sizeof(val));
197 }
198 
199 bool
200 RingBuffer::force(int16_t val)
201 {
202  return force(&val, sizeof(val));
203 }
204 
205 bool
206 RingBuffer::force(uint16_t val)
207 {
208  return force(&val, sizeof(val));
209 }
210 
211 bool
212 RingBuffer::force(int32_t val)
213 {
214  return force(&val, sizeof(val));
215 }
216 
217 bool
218 RingBuffer::force(uint32_t val)
219 {
220  return force(&val, sizeof(val));
221 }
222 
223 bool
224 RingBuffer::force(int64_t val)
225 {
226  return force(&val, sizeof(val));
227 }
228 
229 bool
230 RingBuffer::force(uint64_t val)
231 {
232  return force(&val, sizeof(val));
233 }
234 
235 bool
237 {
238  return force(&val, sizeof(val));
239 }
240 
241 bool
242 RingBuffer::force(double val)
243 {
244  return force(&val, sizeof(val));
245 }
246 
247 // FIXME - clang crashes on this get() call
248 #ifdef __PX4_QURT
249 #define __PX4_SBCAP my_sync_bool_compare_and_swap
250 static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c)
251 {
252  if (*a == b) {
253  *a = c;
254  return true;
255  }
256 
257  return false;
258 }
259 
260 #else
261 #define __PX4_SBCAP __sync_bool_compare_and_swap
262 #endif
263 bool
264 RingBuffer::get(void *val, size_t val_size)
265 {
266  if (_tail != _head) {
267  unsigned candidate;
268  unsigned next;
269 
270  if ((val_size == 0) || (val_size > _item_size)) {
271  val_size = _item_size;
272  }
273 
274  do {
275  /* decide which element we think we're going to read */
276  candidate = _tail;
277 
278  /* and what the corresponding next index will be */
279  next = _next(candidate);
280 
281  /* go ahead and read from this index */
282  if (val != nullptr) {
283  memcpy(val, &_buf[candidate * _item_size], val_size);
284  }
285 
286  /* if the tail pointer didn't change, we got our item */
287  } while (!__PX4_SBCAP(&_tail, candidate, next));
288 
289  return true;
290 
291  } else {
292  return false;
293  }
294 }
295 
296 bool
297 RingBuffer::get(int8_t &val)
298 {
299  return get(&val, sizeof(val));
300 }
301 
302 bool
303 RingBuffer::get(uint8_t &val)
304 {
305  return get(&val, sizeof(val));
306 }
307 
308 bool
309 RingBuffer::get(int16_t &val)
310 {
311  return get(&val, sizeof(val));
312 }
313 
314 bool
315 RingBuffer::get(uint16_t &val)
316 {
317  return get(&val, sizeof(val));
318 }
319 
320 bool
321 RingBuffer::get(int32_t &val)
322 {
323  return get(&val, sizeof(val));
324 }
325 
326 bool
327 RingBuffer::get(uint32_t &val)
328 {
329  return get(&val, sizeof(val));
330 }
331 
332 bool
333 RingBuffer::get(int64_t &val)
334 {
335  return get(&val, sizeof(val));
336 }
337 
338 bool
339 RingBuffer::get(uint64_t &val)
340 {
341  return get(&val, sizeof(val));
342 }
343 
344 bool
345 RingBuffer::get(float &val)
346 {
347  return get(&val, sizeof(val));
348 }
349 
350 bool
351 RingBuffer::get(double &val)
352 {
353  return get(&val, sizeof(val));
354 }
355 
356 unsigned
358 {
359  unsigned tail, head;
360 
361  /*
362  * Make a copy of the head/tail pointers in a fashion that
363  * may err on the side of under-estimating the free space
364  * in the buffer in the case that the buffer is being updated
365  * asynchronously with our check.
366  * If the head pointer changes (reducing space) while copying,
367  * re-try the copy.
368  */
369  do {
370  head = _head;
371  tail = _tail;
372  } while (head != _head);
373 
374  return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
375 }
376 
377 unsigned
379 {
380  /*
381  * Note that due to the conservative nature of space(), this may
382  * over-estimate the number of items in the buffer.
383  */
384  return _num_items - space();
385 }
386 
387 bool
388 RingBuffer::resize(unsigned new_size)
389 {
390  char *old_buffer;
391  char *new_buffer = new char [(new_size + 1) * _item_size];
392 
393  if (new_buffer == nullptr) {
394  return false;
395  }
396 
397  old_buffer = _buf;
398  _buf = new_buffer;
399  _num_items = new_size;
400  _head = new_size;
401  _tail = new_size;
402  delete[] old_buffer;
403  return true;
404 }
405 
406 void
408 {
409  printf("%s %u/%lu (%u/%u @ %p)\n",
410  name,
411  _num_items,
412  (unsigned long)_num_items * _item_size,
413  _head,
414  _tail,
415  _buf);
416 }
417 
418 } // namespace ringbuffer
unsigned space(void)
Definition: ringbuffer.cpp:357
volatile unsigned _tail
removal point in _item_size units
Definition: ringbuffer.h:170
void print_info(const char *name)
Definition: ringbuffer.cpp:407
bool get(void *val, size_t val_size=0)
Get an item from the buffer.
bool put(const void *val, size_t val_size=0)
Put an item into the buffer.
bool resize(unsigned new_size)
Definition: ringbuffer.cpp:388
volatile unsigned _head
insertion point in _item_size units
Definition: ringbuffer.h:169
#define __PX4_SBCAP
Definition: ringbuffer.cpp:261
unsigned count(void)
Definition: ringbuffer.cpp:378
unsigned _next(unsigned index)
Definition: ringbuffer.cpp:60
const char * name
Definition: tests_main.c:58
bool force(const void *val, size_t val_size=0)
Force an item into the buffer, discarding an older item if there is not space.
const size_t _item_size
Definition: ringbuffer.h:167