Line data Source code
1 : #pragma once
2 :
3 : #include <cstdint>
4 : #include <cstring>
5 :
6 : #include <Settings/NamespaceSettings.hpp>
7 :
8 : #include <MIDI_Interfaces/BLEMIDI/BLEAPI.hpp>
9 :
10 : BEGIN_CS_NAMESPACE
11 :
12 : template <class T>
13 : struct NonatomicBLERingBufSize {
14 : T value;
15 : /// Alignment for size, and read/write pointers to avoid false sharing.
16 : constexpr static size_t alignment = alignof(T);
17 37 : T load_acquire() const { return value; }
18 10 : void add_release(T t) { value += t; }
19 19 : void sub_release(T t) { value -= t; }
20 : };
21 :
22 : /// Circular FIFO buffer for buffering BLE packet data. It supports both
23 : /// complete BLE packets and packets split over multiple chunks. Full packets
24 : /// that are added to the FIFO might be split up over multiple chunks.
25 : /// @tparam Capacity
26 : /// Buffer size (bytes). Note that the actual maximum data size may be
27 : /// up to 6 bytes less because of data structure overhead.
28 : /// @tparam SizeT
29 : /// The type to use for the size of tbe buffer. Should be atomic if this
30 : /// buffer is to be used as a SPSC queue between two threads.
31 : /// See @ref NonatomicBLERingBufSize for an example.
32 : template <uint_fast16_t Capacity,
33 : class SizeT = NonatomicBLERingBufSize<uint_fast16_t>>
34 : class BLERingBuf {
35 : private:
36 : struct Header {
37 : uint16_t size : 14;
38 : uint8_t type : 2;
39 : Header() = default;
40 80 : Header(uint16_t size, BLEDataType type)
41 80 : : size {size}, type {static_cast<uint8_t>(type)} {}
42 52 : BLEDataType getType() const { return static_cast<BLEDataType>(type); }
43 : };
44 : constexpr static uint_fast16_t header_size = sizeof(Header);
45 : static_assert(header_size == 2, "");
46 :
47 : constexpr static uint_fast16_t capacity = Capacity;
48 : unsigned char buffer[capacity];
49 : alignas(SizeT::alignment) uint_fast16_t read_p = 0;
50 : alignas(SizeT::alignment) uint_fast16_t write_p = header_size;
51 : alignas(SizeT::alignment) SizeT size {header_size};
52 :
53 198 : constexpr static uint_fast16_t ceil_h(uint_fast16_t i) {
54 198 : return ((i + header_size - 1) / header_size) * header_size;
55 : }
56 :
57 : public:
58 34 : BLERingBuf() {
59 34 : Header header {0, BLEDataType::None};
60 : static_assert(capacity % header_size == 0, "");
61 34 : std::memcpy(buffer, &header, header_size);
62 34 : }
63 :
64 : /// Copy the given data into the buffer. May be split up into two chunks,
65 : /// in which case the type will be set to @ref BLEDataType::Continuation
66 : /// for the second chunk.
67 : /// @retval false The buffer is full, nothing added to the buffer.
68 27 : bool push(BLEDataView data, BLEDataType type = BLEDataType::Packet) {
69 27 : if (type == BLEDataType::None)
70 0 : return false;
71 27 : uint_fast16_t loc_size = size.load_acquire();
72 27 : uint_fast16_t add_size = 0;
73 27 : assert(loc_size <= capacity);
74 27 : assert(write_p < capacity);
75 27 : assert(write_p % header_size == 0);
76 : // We need to write at least one header
77 27 : uint_fast16_t expected_req_size = loc_size + data.length + header_size;
78 27 : if (expected_req_size > capacity)
79 2 : return false; // not enough space
80 : // Contiguous size remaining (excluding header)
81 25 : uint_fast16_t contig_size = capacity - write_p - header_size;
82 25 : assert(contig_size < capacity);
83 : // We may need to write a second header
84 25 : uint_fast16_t worst_case_req_size = expected_req_size + header_size;
85 25 : if (data.length > contig_size && worst_case_req_size > capacity)
86 0 : return false; // not enough space
87 : // Write the first header for the packet
88 25 : uint16_t size_1 = std::min<uint_fast16_t>(contig_size, data.length);
89 25 : Header header {size_1, type};
90 25 : std::memcpy(buffer + write_p, &header, sizeof(header));
91 25 : write_p += header_size;
92 25 : add_size += header_size;
93 : // Write first data
94 25 : if (size_1 > 0) // avoid memcpy with nullptr
95 24 : std::memcpy(buffer + write_p, data.data, size_1);
96 25 : write_p += ceil_h(size_1);
97 25 : add_size += ceil_h(size_1);
98 25 : if (write_p == capacity)
99 3 : write_p = 0;
100 : // Now write the remainder at the beginning of the circular buffer
101 25 : uint16_t size_2 = data.length - size_1;
102 25 : if (size_2 > 0) {
103 : // Write the continuation header
104 2 : Header header {size_2, BLEDataType::Continuation};
105 2 : std::memcpy(buffer + write_p, &header, sizeof(header));
106 2 : write_p += header_size;
107 2 : add_size += header_size;
108 : // Write the remainder of the data
109 2 : std::memcpy(buffer + write_p, data.data + size_1, size_2);
110 2 : write_p += ceil_h(size_2);
111 2 : add_size += ceil_h(size_2);
112 : }
113 25 : size.add_release(add_size);
114 25 : return true;
115 : }
116 :
117 : /// Get a view to the next chunk of data. The view remains valid until the
118 : /// next call to @ref pop.
119 : /// @retval BLEDataType::None
120 : /// No data available.
121 : /// @retval BLEDataType::Packet
122 : /// The @p data output parameter points to the first chunk of a
123 : /// packet.
124 : /// @retval BLEDataType::Continuation
125 : /// The @p data output parameter points to a chunk of continuation
126 : /// data of the same packet.
127 52 : BLEDataType pop(BLEDataView &data) {
128 52 : uint_fast16_t loc_size = size.load_acquire();
129 52 : assert(loc_size >= header_size);
130 52 : assert(read_p < capacity);
131 52 : assert(read_p % header_size == 0);
132 : // Read the old header
133 52 : Header old_header;
134 52 : std::memcpy(&old_header, buffer + read_p, sizeof(old_header));
135 : // If the previous chunk is the only thing left in the buffer
136 52 : if (loc_size - header_size == ceil_h(old_header.size)) {
137 : // If there is still actual data left in the buffer
138 25 : if (old_header.getType() != BLEDataType::None) {
139 : // Free the old data, preserving space for a header
140 19 : read_p += ceil_h(old_header.size);
141 19 : size.sub_release(ceil_h(old_header.size));
142 : // Write an empty header
143 19 : Header header {0, BLEDataType::None};
144 19 : std::memcpy(buffer + read_p, &header, sizeof(header));
145 : }
146 25 : data = {nullptr, 0};
147 25 : return BLEDataType::None;
148 : } else {
149 : // Otherwise, discard the old data and header
150 27 : read_p += header_size + ceil_h(old_header.size);
151 27 : size.sub_release(header_size + ceil_h(old_header.size));
152 27 : if (read_p == capacity)
153 3 : read_p = 0;
154 : // Read the next header
155 27 : Header header;
156 27 : std::memcpy(&header, buffer + read_p, sizeof(header));
157 27 : data = {buffer + read_p + header_size, header.size};
158 27 : return header.getType();
159 : }
160 : }
161 : };
162 :
163 : END_CS_NAMESPACE
|
