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