105 lines
3.0 KiB
C++
105 lines
3.0 KiB
C++
// This file is part of Necroware's GamePort adapter firmware.
|
|
// Copyright (C) 2021 Necroware
|
|
//
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by // the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <https://www.gnu.org/licenses/>.
|
|
|
|
#pragma once
|
|
|
|
#include <Arduino.h>
|
|
|
|
template <size_t Size>
|
|
struct Buffer {
|
|
static const uint8_t MAX_SIZE{Size};
|
|
uint8_t data[MAX_SIZE]{};
|
|
uint8_t size{};
|
|
};
|
|
|
|
template <size_t S>
|
|
class BufferFillerImpl {
|
|
public:
|
|
using BufferType = Buffer<S>;
|
|
|
|
explicit BufferFillerImpl(BufferType& buffer)
|
|
: m_buffer(buffer) {}
|
|
|
|
~BufferFillerImpl() = default;
|
|
BufferFillerImpl(const BufferFillerImpl&) = delete;
|
|
BufferFillerImpl(BufferFillerImpl&&) = default;
|
|
BufferFillerImpl& operator=(const BufferFillerImpl&) = delete;
|
|
BufferFillerImpl& operator=(BufferFillerImpl&&) = default;
|
|
|
|
template <typename T>
|
|
BufferFillerImpl& push(const T& bits) {
|
|
return push(uint32_t(bits), sizeof(bits) * BITS_PER_BYTE);
|
|
}
|
|
|
|
BufferFillerImpl& push(uint32_t bits, size_t count) {
|
|
while(count) {
|
|
const auto pushed = fillup(bits, count);
|
|
bits >>= pushed;
|
|
count -= pushed;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
BufferFillerImpl& align() {
|
|
if (m_bitsUsed) {
|
|
m_buffer.size++;
|
|
m_bitsUsed = 0u;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
private:
|
|
static constexpr auto BITS_PER_BYTE{8u};
|
|
|
|
BufferType& m_buffer;
|
|
size_t m_bitsUsed{};
|
|
|
|
size_t fillup(byte bits, size_t count) {
|
|
|
|
const auto freeBits = BITS_PER_BYTE - m_bitsUsed;
|
|
if (freeBits == 0) {
|
|
return 0;
|
|
}
|
|
|
|
// If the total count is above the free bits available in the current
|
|
// byte, then push just a chunk to fill up the current byte
|
|
if (freeBits < count) {
|
|
return fillup(bits, freeBits);
|
|
}
|
|
|
|
if (m_bitsUsed == 0u) {
|
|
m_buffer.data[m_buffer.size] = 0u;
|
|
}
|
|
|
|
// Obviously the total count is less, than the amount of free bits
|
|
// available in the current byte, so the following block will simply
|
|
// add the bits to the current byte and return
|
|
const auto mask = uint32_t(1u << count) - 1u;
|
|
m_buffer.data[m_buffer.size] |= byte(bits & mask) << m_bitsUsed;
|
|
m_bitsUsed += count;
|
|
if (m_bitsUsed == BITS_PER_BYTE) {
|
|
m_buffer.size++;
|
|
m_bitsUsed = 0u;
|
|
}
|
|
return count;
|
|
}
|
|
};
|
|
|
|
template <size_t S>
|
|
BufferFillerImpl<S> BufferFiller(Buffer<S>& buffer) {
|
|
return BufferFillerImpl<S>(buffer);
|
|
}
|
|
|