From 3cf6a5ff608c8904d647bc667d3279db81afab07 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Feb 2014 19:03:43 +0400 Subject: [PATCH] Partially implemented BitStream class - only write() so far. --- .../internal/marshalling/bit_stream.hpp | 50 ++++++++ libuavcan/src/marshalling/bit_array_copy.c | 119 ++++++++++++++++++ libuavcan/src/marshalling/bit_stream.cpp | 62 +++++++++ libuavcan/test/marshalling/bit_stream.cpp | 119 ++++++++++++++++++ 4 files changed, 350 insertions(+) create mode 100644 libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp create mode 100644 libuavcan/src/marshalling/bit_array_copy.c create mode 100644 libuavcan/src/marshalling/bit_stream.cpp create mode 100644 libuavcan/test/marshalling/bit_stream.cpp diff --git a/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp b/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp new file mode 100644 index 0000000000..da095f662b --- /dev/null +++ b/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +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(src_org), src_offset, src_len, + reinterpret_cast(dst_org), dst_offset); + } + +public: + BitStream(ITransferBuffer& buf) + : buf_(buf) + , bit_offset_(0) + , byte_cache_(0) + { + StaticAssert::check(); + } + + int write(const uint8_t* bytes, const int bitlen); + + std::string toString() const; +}; + +} diff --git a/libuavcan/src/marshalling/bit_array_copy.c b/libuavcan/src/marshalling/bit_array_copy.c new file mode 100644 index 0000000000..15441a56d7 --- /dev/null +++ b/libuavcan/src/marshalling/bit_array_copy.c @@ -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 +#include +#include + +#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; + } + } + } +} diff --git a/libuavcan/src/marshalling/bit_stream.cpp b/libuavcan/src/marshalling/bit_stream.cpp new file mode 100644 index 0000000000..80a13c5229 --- /dev/null +++ b/libuavcan/src/marshalling/bit_stream.cpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +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; +} + +} diff --git a/libuavcan/test/marshalling/bit_stream.cpp b/libuavcan/test/marshalling/bit_stream.cpp new file mode 100644 index 0000000000..09c38e669a --- /dev/null +++ b/libuavcan/test/marshalling/bit_stream.cpp @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +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 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()); +}