Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
framed_ring_buffer.c
Go to the documentation of this file.
1/*
2 * General purpose circular buffer
3 *
4 * Copyright (C) 2021 Fabien-B <fabien-b@github.com>
5 *
6 * This file is part of paparazzi. See LICENCE file.
7 */
8
10#include <string.h>
11
12/*
13 * One producer (put) and one consumer (get) can run in different threads: each only writes its own
14 * offset, with release semantics after the data, and reads the other one with acquire semantics.
15 */
16#define FRB_LOAD(_x) __atomic_load_n(&(_x), __ATOMIC_ACQUIRE)
17#define FRB_STORE(_x, _v) __atomic_store_n(&(_x), (_v), __ATOMIC_RELEASE)
18
19
20void framed_ring_buffer_init(struct framed_ring_buffer *cb, uint8_t *buffer, size_t len)
21{
22 cb->_buf = buffer;
23 cb->_buf_len = len;
24 cb->read_offset = 0;
25 cb->write_offset = 0;
26}
27
28
29int framed_ring_buffer_get(struct framed_ring_buffer *cb, uint8_t *buf, size_t len)
30{
31 // buffer empty
32 if (cb->read_offset == FRB_LOAD(cb->write_offset)) { return CIR_ERROR_NO_MSG; }
33 // LEN| MSG...| LEN | MSG...
34 uint16_t msg_len_p = cb->_buf[cb->read_offset] | (cb->_buf[(cb->read_offset + 1) % cb->_buf_len] << 8);
35 // output buffer too small
36 if (len < msg_len_p) { return CIR_ERROR_BUFFER_TOO_SMALL; }
37
38 size_t end_offset = cb->read_offset + msg_len_p + 2;
39 if (end_offset >= cb->_buf_len) {
40 end_offset -= cb->_buf_len;
41 }
42 uint8_t *start = cb->_buf + ((cb->read_offset + 2) % cb->_buf_len);
43
44 // No wrapping can be read in one go
45 if (end_offset >= ((cb->read_offset + 2) % cb->_buf_len)) {
46 memcpy(buf, start, msg_len_p);
47 } else {
48 size_t len1 = cb->_buf_len - (cb->read_offset + 2);
49 size_t len2 = msg_len_p - len1;
50 memcpy(buf, start, len1);
51 memcpy(buf + len1, cb->_buf, len2);
52 }
53
54 int nb_bytes = msg_len_p;
55 FRB_STORE(cb->read_offset, end_offset);
56 return nb_bytes;
57}
58
59int framed_ring_buffer_put(struct framed_ring_buffer *cb, const uint8_t *buf, size_t len)
60{
61 size_t available = framed_ring_buffer_available(cb);
62
63 if (len > available) {
65 }
66
67 size_t end_offset = cb->write_offset + len + 2;
68 if (end_offset >= cb->_buf_len) {
69 end_offset -= cb->_buf_len;
70 }
71
72
73 // Write the length and wrap around if needed
74 cb->_buf[cb->write_offset] = len & 0xFF;
75 cb->_buf[(cb->write_offset + 1) % cb->_buf_len] = (len >> 8) & 0xFF;
76
77 // Write the date no wrapping needed
78 if (end_offset > cb->write_offset) {
79 memcpy(cb->_buf + cb->write_offset + 2, buf, len);
80 }
81 // Wrapping data partially
82 else if ((cb->write_offset + 2) < cb->_buf_len) {
83 size_t len1 = cb->_buf_len - (cb->write_offset + 2);
84 size_t len2 = len - len1;
85 memcpy(cb->_buf + cb->write_offset + 2, buf, len1);
86 memcpy(cb->_buf, buf + len1, len2);
87 }
88 // Only data is wrapped (starting at the beginning or 1 byte in for wrapped length)
89 else {
90 memcpy(&cb->_buf[(cb->write_offset + 2) % cb->_buf_len], buf, len);
91 }
92
93 FRB_STORE(cb->write_offset, end_offset);
94 return 0;
95}
96
98 // buffer empty
99 if (cb->read_offset == cb->write_offset) { return CIR_ERROR_NO_MSG; }
100
101 size_t record_head_offset = cb->read_offset;
102 size_t end_offset = cb->read_offset;
103
104 while(end_offset != cb->write_offset) {
105 record_head_offset = end_offset;
106 uint16_t msg_len_p = cb->_buf[record_head_offset] | (cb->_buf[(record_head_offset + 1) % cb->_buf_len] << 8);
107 end_offset = record_head_offset + msg_len_p + 2;
108 if (end_offset >= cb->_buf_len) {
109 end_offset -= cb->_buf_len;
110 }
111 }
112
113 cb->write_offset = record_head_offset;
114 return 0;
115}
116
118 // write_offset == read_offset is considered an empty buffer. => 3= 2 (lenght) + 1 (margin)
119 int available = 0;
120 size_t read_offset = FRB_LOAD(cb->read_offset);
121 size_t write_offset = FRB_LOAD(cb->write_offset);
122 if (read_offset > write_offset) {
123 available = (int)read_offset - (int)write_offset - 3;
124 } else {
125 available = cb->_buf_len - ((int)write_offset - (int)read_offset) - 3;
126 }
127
128 return available > 0 ? (size_t)available : 0;
129}
130
132 cb->read_offset = 0;
133 cb->write_offset = 0;
134}
int framed_ring_buffer_drop_last(struct framed_ring_buffer *cb)
Drop last inserted record.
#define FRB_LOAD(_x)
int framed_ring_buffer_put(struct framed_ring_buffer *cb, const uint8_t *buf, size_t len)
Copy buf in the circular buffer.
void framed_ring_buffer_clear(struct framed_ring_buffer *cb)
Clear buffer.
size_t framed_ring_buffer_available(struct framed_ring_buffer *cb)
Get the available sapce for the next buffer.
void framed_ring_buffer_init(struct framed_ring_buffer *cb, uint8_t *buffer, size_t len)
initialize a circular buffer.
#define FRB_STORE(_x, _v)
int framed_ring_buffer_get(struct framed_ring_buffer *cb, uint8_t *buf, size_t len)
copy the next buffer available in cb to buf.
@ CIR_ERROR_NO_MSG
circular buffer is empty
@ CIR_ERROR_NO_SPACE_AVAILABLE
no space available in the circular buffer
@ CIR_ERROR_BUFFER_TOO_SMALL
destination buffer is too small
This is a general purpose circular buffer for storing variable lenght buffers in a FIFO order.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.