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

2026-07-16 23:40:34 +08:00 · 2014-02-20 19:03:43 +04:00
parent e5a8302b9d
commit 3cf6a5ff60
4 changed files with 350 additions and 0 deletions
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <stdint.h>
+#include <string>
+#include <uavcan/internal/util.hpp>
+#include <uavcan/internal/transport/transfer_buffer.hpp>
+
+namespace uavcan
+{
+
+extern "C" void bitarray_copy(const unsigned char *src_org, int src_offset, int src_len,
+                                    unsigned char *dst_org, int dst_offset);
+
+class BitStream
+{
+    enum { MAX_BYTES_PER_RW = 16 };
+
+    ITransferBuffer& buf_;
+    int bit_offset_;
+    uint8_t byte_cache_;
+
+    static inline unsigned int bitlenToBytelen(unsigned int bits) { return (bits + 7) / 8; }
+
+    static inline void copyBitArray(const uint8_t* src_org, int src_offset, int src_len,
+                                          uint8_t* dst_org, int dst_offset)
+    {
+        bitarray_copy(reinterpret_cast<const unsigned char*>(src_org), src_offset, src_len,
+                      reinterpret_cast<unsigned char*>(dst_org), dst_offset);
+    }
+
+public:
+    BitStream(ITransferBuffer& buf)
+    : buf_(buf)
+    , bit_offset_(0)
+    , byte_cache_(0)
+    {
+        StaticAssert<sizeof(uint8_t) == 1>::check();
+    }
+
+    int write(const uint8_t* bytes, const int bitlen);
+
+    std::string toString() const;
+};
+
+}
@@ -0,0 +1,119 @@
+/*
+ * Fast bit array copy algorithm.
+ * Source: http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays
+ */
+
+#include <limits.h>
+#include <string.h>
+#include <stddef.h>
+
+#define PREPARE_FIRST_COPY()                                      \
+    do {                                                          \
+    if (src_len >= (CHAR_BIT - dst_offset_modulo)) {              \
+        *dst     &= reverse_mask[dst_offset_modulo];              \
+        src_len -= CHAR_BIT - dst_offset_modulo;                  \
+    } else {                                                      \
+        *dst     &= reverse_mask[dst_offset_modulo]               \
+              | reverse_mask_xor[dst_offset_modulo + src_len + 1];\
+         c       &= reverse_mask[dst_offset_modulo + src_len    ];\
+        src_len = 0;                                              \
+    } } while (0)
+
+
+void
+bitarray_copy(const unsigned char *src_org, int src_offset, int src_len,
+                    unsigned char *dst_org, int dst_offset)
+{
+    static const unsigned char reverse_mask[] =
+        { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
+    static const unsigned char reverse_mask_xor[] =
+        { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
+
+    if (src_len > 0) {
+        const unsigned char *src;
+              unsigned char *dst;
+        int                  src_offset_modulo,
+                             dst_offset_modulo;
+
+        src = src_org + (src_offset / CHAR_BIT);
+        dst = dst_org + (dst_offset / CHAR_BIT);
+
+        src_offset_modulo = src_offset % CHAR_BIT;
+        dst_offset_modulo = dst_offset % CHAR_BIT;
+
+        if (src_offset_modulo == dst_offset_modulo) {
+            int              byte_len;
+            int              src_len_modulo;
+            if (src_offset_modulo) {
+                unsigned char   c;
+
+                c = reverse_mask_xor[dst_offset_modulo]     & *src++;
+
+                PREPARE_FIRST_COPY();
+                *dst++ |= c;
+            }
+
+            byte_len = src_len / CHAR_BIT;
+            src_len_modulo = src_len % CHAR_BIT;
+
+            if (byte_len) {
+                memcpy(dst, src, byte_len);
+                src += byte_len;
+                dst += byte_len;
+            }
+            if (src_len_modulo) {
+                *dst     &= reverse_mask_xor[src_len_modulo];
+                *dst |= reverse_mask[src_len_modulo]     & *src;
+            }
+        } else {
+            int             bit_diff_ls,
+                            bit_diff_rs;
+            int             byte_len;
+            int             src_len_modulo;
+            unsigned char   c;
+            /*
+             * Begin: Line things up on destination.
+             */
+            if (src_offset_modulo > dst_offset_modulo) {
+                bit_diff_ls = src_offset_modulo - dst_offset_modulo;
+                bit_diff_rs = CHAR_BIT - bit_diff_ls;
+
+                c = *src++ << bit_diff_ls;
+                c |= *src >> bit_diff_rs;
+                c     &= reverse_mask_xor[dst_offset_modulo];
+            } else {
+                bit_diff_rs = dst_offset_modulo - src_offset_modulo;
+                bit_diff_ls = CHAR_BIT - bit_diff_rs;
+
+                c = *src >> bit_diff_rs     &
+                    reverse_mask_xor[dst_offset_modulo];
+            }
+            PREPARE_FIRST_COPY();
+            *dst++ |= c;
+
+            /*
+             * Middle: copy with only shifting the source.
+             */
+            byte_len = src_len / CHAR_BIT;
+
+            while (--byte_len >= 0) {
+                c = *src++ << bit_diff_ls;
+                c |= *src >> bit_diff_rs;
+                *dst++ = c;
+            }
+
+            /*
+             * End: copy the remaing bits;
+             */
+            src_len_modulo = src_len % CHAR_BIT;
+            if (src_len_modulo) {
+                c = *src++ << bit_diff_ls;
+                c |= *src >> bit_diff_rs;
+                c     &= reverse_mask[src_len_modulo];
+
+                *dst     &= reverse_mask_xor[src_len_modulo];
+                *dst |= c;
+            }
+        }
+    }
+}
@@ -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;
+}
+
+}
@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/internal/marshalling/bit_stream.hpp>
+
+
+TEST(BitStream, ToString)
+{
+    {
+        uavcan::StaticTransferBuffer<8> buf;
+        uavcan::BitStream bs(buf);
+        ASSERT_EQ("", bs.toString());
+        ASSERT_EQ("", bs.toString());
+    }
+    {
+        const uint8_t data[] = {0xad};   // 10101101
+        uavcan::StaticTransferBuffer<8> buf;
+        uavcan::BitStream bs(buf);
+        ASSERT_EQ(0, bs.write(data, 8)); // all 8
+        ASSERT_EQ("10101101", bs.toString());
+    }
+    {
+        const uint8_t data[] = {0xad, 0xbe};  // 10101101 10111110
+        uavcan::StaticTransferBuffer<8> buf;
+        uavcan::BitStream bs(buf);
+        ASSERT_EQ(0, bs.write(data, 16));     // all 16
+        ASSERT_EQ("10101101 10111110", bs.toString());
+    }
+    {
+        const uint8_t data[] = {0xad, 0xbe, 0xfc};  // 10101101 10111110 11111100
+        uavcan::StaticTransferBuffer<8> buf;
+        uavcan::BitStream bs(buf);
+        ASSERT_EQ(0, bs.write(data, 20));           // 10101101 10111110 1111
+        ASSERT_EQ("10101101 10111110 11110000", bs.toString());
+    }
+}
+
+
+TEST(BitStream, BitOrder)
+{
+    /*
+     * a = 1010
+     * b = 1011
+     * c = 1100
+     * d = 1101
+     * e = 1110
+     * f = 1111
+     */
+    {   // Partial write
+        const uint8_t data[] = {0xad, 0xbe};            // adbe
+        uavcan::StaticTransferBuffer<32> buf;
+        uavcan::BitStream bs(buf);
+        ASSERT_EQ(0, bs.write(data, 12));               // adb0
+        ASSERT_EQ("10101101 10110000", bs.toString());  // adb0
+    }
+    {   // Multiple partial write
+        const uint8_t data1[] = {0xad, 0xbe};              // 10101101 10111110
+        const uint8_t data2[] = {0xfc};                    // 11111100
+        const uint8_t data3[] = {0xde, 0xad, 0xbe, 0xef};  // 11011110 10101101 10111110 11101111
+        const uint8_t data4[] = {0x12, 0x34, 0x56, 0x78,   // 00010010 00110100 01010110 01111000
+                                 0x9a, 0xbc, 0xde, 0xf0};  // 10011010 10111100 11011110 11110000
+
+        uavcan::StaticTransferBuffer<32> buf;
+        uavcan::BitStream bs(buf);
+        ASSERT_EQ(0, bs.write(data1, 11));  // 10101101 101
+        std::cout << bs.toString() << std::endl;
+        ASSERT_EQ(0, bs.write(data2, 6));   //             11111 1
+        std::cout << bs.toString() << std::endl;
+        ASSERT_EQ(0, bs.write(data3, 25));  //                    1101111 01010110 11011111 01
+        std::cout << bs.toString() << std::endl;
+        ASSERT_EQ(0, bs.write(data4, 64));  // all 64, total 42 + 64 = 106
+        std::cout << bs.toString() << std::endl;
+        ASSERT_EQ(0, bs.write(data4, 4));   // 0001
+        std::cout << bs.toString() << std::endl;
+
+        static const std::string REFERENCE =
+            "10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100";
+
+        std::cout << "Reference:\n" << REFERENCE << std::endl;
+
+        ASSERT_EQ(REFERENCE, bs.toString());
+    }
+}
+
+
+TEST(BitStream, BitByBit)
+{
+    static const int NUM_BYTES = 1024;
+    uavcan::StaticTransferBuffer<NUM_BYTES> buf;
+    uavcan::BitStream bs(buf);
+
+    std::string binary_string;
+    unsigned int counter = 0;
+    for (int byte = 0; byte < NUM_BYTES; byte++)
+    {
+        for (int bit = 0; bit < 8; bit++, counter++)
+        {
+            const bool value = counter % 3 == 0;
+            binary_string.push_back(value ? '1' : '0');
+            const uint8_t data[] = { value << 7 };
+            ASSERT_EQ(0, bs.write(data, 1));
+        }
+        binary_string.push_back(' ');
+    }
+    binary_string.erase(binary_string.length() - 1, 1);
+
+    /*
+     * Currently we have no free buffer space, so next write() must fail
+     */
+    const uint8_t data[] = { 0xFF };
+    ASSERT_EQ(-1, bs.write(data, 1));
+
+//    std::cout << bs.toString() << std::endl;
+//    std::cout << "Reference:\n" << binary_string << std::endl;
+
+    ASSERT_EQ(binary_string, bs.toString());
+}