diff --git a/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp b/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp index da095f662b..f7a09b2c01 100644 --- a/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp +++ b/libuavcan/include/uavcan/internal/marshalling/bit_stream.hpp @@ -43,6 +43,7 @@ public: } int write(const uint8_t* bytes, const int bitlen); + int read(uint8_t* bytes, const int bitlen); std::string toString() const; }; diff --git a/libuavcan/src/marshalling/bit_stream.cpp b/libuavcan/src/marshalling/bit_stream.cpp index 80a13c5229..08dd627b20 100644 --- a/libuavcan/src/marshalling/bit_stream.cpp +++ b/libuavcan/src/marshalling/bit_stream.cpp @@ -2,6 +2,7 @@ * Copyright (C) 2014 Pavel Kirienko */ +#include #include #include @@ -18,6 +19,7 @@ int BitStream::write(const uint8_t* bytes, const int bitlen) assert(MAX_BYTES_PER_RW >= bytelen); tmp[0] = tmp[bytelen - 1] = 0; + std::fill(tmp, tmp + bytelen, 0); copyBitArray(bytes, 0, bitlen, tmp, bit_offset_ % 8); const int new_bit_offset = bit_offset_ + bitlen; @@ -34,13 +36,32 @@ int BitStream::write(const uint8_t* bytes, const int bitlen) * within the next write() operation. */ const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen); - bit_offset_ = new_bit_offset; + bit_offset_ = new_bit_offset; // TODO: DO NOT UPDATE ON FAILURE if (write_res < 0) return write_res; return (write_res == bytelen) ? 0 : -1; } +int BitStream::read(uint8_t* bytes, const int bitlen) +{ + uint8_t tmp[MAX_BYTES_PER_RW + 1]; + + const int bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); + assert(MAX_BYTES_PER_RW >= bytelen); + + const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen); + if (read_res < 0) + return read_res; + if (read_res != bytelen) + return -1; + + std::fill(bytes, bytes + bitlenToBytelen(bitlen), 0); + copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0); + bit_offset_ += bitlen; + return 0; +} + std::string BitStream::toString() const { std::ostringstream os; diff --git a/libuavcan/test/marshalling/bit_stream.cpp b/libuavcan/test/marshalling/bit_stream.cpp index 09c38e669a..99c69d8cb6 100644 --- a/libuavcan/test/marshalling/bit_stream.cpp +++ b/libuavcan/test/marshalling/bit_stream.cpp @@ -38,7 +38,7 @@ TEST(BitStream, ToString) } -TEST(BitStream, BitOrder) +TEST(BitStream, BitOrderSimple) { /* * a = 1010 @@ -48,21 +48,37 @@ TEST(BitStream, BitOrder) * e = 1110 * f = 1111 */ - { // Partial write + uavcan::StaticTransferBuffer<32> buf; + { // 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 + { // Read + uavcan::BitStream bs(buf); + ASSERT_EQ("10101101 10110000", bs.toString()); // Same data + uint8_t data[] = {0xFF, 0xFF}; // Uninitialized + ASSERT_EQ(0, bs.read(data, 12)); + ASSERT_EQ(0xad, data[0]); + ASSERT_EQ(0xb0, data[1]); + } +} + + +TEST(BitStream, BitOrderComplex) +{ + static const std::string REFERENCE = + "10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100"; + + uavcan::StaticTransferBuffer<32> buf; + { // 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; @@ -75,13 +91,39 @@ TEST(BitStream, BitOrder) 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()); } + { // Read back in the same order + uint8_t data[8]; + std::fill(data, data + sizeof(data), 0xA5); // Filling with garbage + uavcan::BitStream bs(buf); + ASSERT_EQ(REFERENCE, bs.toString()); + + ASSERT_EQ(0, bs.read(data, 11)); // 10101101 10100000 + ASSERT_EQ(0xad, data[0]); + ASSERT_EQ(0xa0, data[1]); + + ASSERT_EQ(0, bs.read(data, 6)); // 11111100 + ASSERT_EQ(0xfc, data[0]); + + ASSERT_EQ(0, bs.read(data, 25)); // 11011110 10101101 10111110 10000000 + ASSERT_EQ(0xde, data[0]); + ASSERT_EQ(0xad, data[1]); + ASSERT_EQ(0xbe, data[2]); + ASSERT_EQ(0x80, data[3]); + + ASSERT_EQ(0, bs.read(data, 64)); // Data - see above + ASSERT_EQ(0x12, data[0]); + ASSERT_EQ(0x34, data[1]); + ASSERT_EQ(0x56, data[2]); + ASSERT_EQ(0x78, data[3]); + ASSERT_EQ(0x9a, data[4]); + ASSERT_EQ(0xbc, data[5]); + ASSERT_EQ(0xde, data[6]); + ASSERT_EQ(0xf0, data[7]); + } } @@ -89,7 +131,7 @@ TEST(BitStream, BitByBit) { static const int NUM_BYTES = 1024; uavcan::StaticTransferBuffer buf; - uavcan::BitStream bs(buf); + uavcan::BitStream bs_wr(buf); std::string binary_string; unsigned int counter = 0; @@ -100,7 +142,7 @@ TEST(BitStream, BitByBit) 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)); + ASSERT_EQ(0, bs_wr.write(data, 1)); } binary_string.push_back(' '); } @@ -109,11 +151,43 @@ TEST(BitStream, BitByBit) /* * Currently we have no free buffer space, so next write() must fail */ - const uint8_t data[] = { 0xFF }; - ASSERT_EQ(-1, bs.write(data, 1)); + const uint8_t dummy_data_wr[] = { 0xFF }; + ASSERT_EQ(-1, bs_wr.write(dummy_data_wr, 1)); + /* + * Bitstream content validation + */ // std::cout << bs.toString() << std::endl; // std::cout << "Reference:\n" << binary_string << std::endl; + ASSERT_EQ(binary_string, bs_wr.toString()); - ASSERT_EQ(binary_string, bs.toString()); + /* + * Read back + */ + uavcan::BitStream bs_rd(buf); + counter = 0; + for (int byte = 0; byte < NUM_BYTES; byte++) + { + for (int bit = 0; bit < 8; bit++, counter++) + { + const bool value = counter % 3 == 0; + uint8_t data[1]; + ASSERT_EQ(0, bs_rd.read(data, 1)); + if (value) + { + ASSERT_EQ(0x80, data[0]); + } + else + { + ASSERT_EQ(0, data[0]); + } + } + } + + /* + * Making sure that reading out of buffer range will fail with error + */ + uint8_t dummy_data_rd[] = { 0xFF }; + ASSERT_EQ(-1, bs_wr.read(dummy_data_rd, 1)); + ASSERT_EQ(0xFF, dummy_data_rd[0]); }