Partially implemented BitStream class - only write() so far.

libuavcan/src/marshalling/bit_stream.cpp

@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <sstream>
+#include <uavcan/internal/marshalling/bit_stream.hpp>
+
+namespace uavcan
+{
+
+int BitStream::write(const uint8_t* bytes, const int bitlen)
+{
+    // Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_)
+    uint8_t tmp[MAX_BYTES_PER_RW + 1];
+
+    // Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write()
+    const int bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
+    assert(MAX_BYTES_PER_RW >= bytelen);
+    tmp[0] = tmp[bytelen - 1] = 0;
+
+    copyBitArray(bytes, 0, bitlen, tmp, bit_offset_ % 8);
+
+    const int new_bit_offset = bit_offset_ + bitlen;
+
+    // Bitcopy algorithm resets skipped bits in the first byte. Restore them back.
+    tmp[0] |= byte_cache_;
+
+    // (new_bit_offset % 8 == 0) means that this write was perfectly aligned.
+    byte_cache_ = (new_bit_offset % 8) ? tmp[bytelen - 1] : 0;
+
+    /*
+     * Dump the data into the destination buffer.
+     * Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value
+     * within the next write() operation.
+     */
+    const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
+    bit_offset_ = new_bit_offset;
+
+    if (write_res < 0)
+        return write_res;
+    return (write_res == bytelen) ? 0 : -1;
+}
+
+std::string BitStream::toString() const
+{
+    std::ostringstream os;
+    for (int offset = 0; true; offset++)
+    {
+        uint8_t byte = 0;
+        if (1 != buf_.read(offset, &byte, 1))
+            break;
+        for (int i = 7; i >= 0; i--)     // Most significant goes first
+            os << !!(byte & (1 << i));
+        os << " ";
+    }
+    std::string output = os.str();
+    if (output.length())
+        output.erase(output.length() - 1, 1);
+    return output;
+}
+
+}