uc_bit_stream.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/marshal/bit_stream.hpp>
00006 #include <uavcan/transport/transfer_buffer.hpp>
00007 #include <cassert>
00008 
00009 namespace uavcan
00010 {
00011 
00012 const unsigned BitStream::MaxBytesPerRW;
00013 const unsigned BitStream::MaxBitsPerRW;
00014 
00015 int BitStream::write(const uint8_t* bytes, const unsigned bitlen)
00016 {
00017     // Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_)
00018     uint8_t tmp[MaxBytesPerRW + 1];
00019 
00020     // Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write()
00021     const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
00022     UAVCAN_ASSERT(MaxBytesPerRW >= bytelen);
00023     tmp[0] = tmp[bytelen - 1] = 0;
00024 
00025     fill(tmp, tmp + bytelen, uint8_t(0));
00026     copyBitArrayAlignedToUnaligned(bytes, bitlen, tmp, bit_offset_ % 8);
00027 
00028     const unsigned new_bit_offset = bit_offset_ + bitlen;
00029 
00030     // Bitcopy algorithm resets skipped bits in the first byte. Restore them back.
00031     tmp[0] |= byte_cache_;
00032 
00033     // (new_bit_offset % 8 == 0) means that this write was perfectly aligned.
00034     byte_cache_ = uint8_t((new_bit_offset % 8) ? tmp[bytelen - 1] : 0);
00035 
00036     /*
00037      * Dump the data into the destination buffer.
00038      * Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value
00039      * within the next write() operation.
00040      */
00041     const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
00042     if (write_res < 0)
00043     {
00044         return write_res;
00045     }
00046     if (static_cast<unsigned>(write_res) < bytelen)
00047     {
00048         return ResultOutOfBuffer;
00049     }
00050 
00051     bit_offset_ = new_bit_offset;
00052     return ResultOk;
00053 }
00054 
00055 int BitStream::read(uint8_t* bytes, const unsigned bitlen)
00056 {
00057     uint8_t tmp[MaxBytesPerRW + 1];
00058 
00059     const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
00060     UAVCAN_ASSERT(MaxBytesPerRW >= bytelen);
00061 
00062     const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen);
00063     if (read_res < 0)
00064     {
00065         return read_res;
00066     }
00067     if (static_cast<unsigned>(read_res) < bytelen)
00068     {
00069         return ResultOutOfBuffer;
00070     }
00071 
00072     fill(bytes, bytes + bitlenToBytelen(bitlen), uint8_t(0));
00073     copyBitArrayUnalignedToAligned(tmp, bit_offset_ % 8, bitlen, bytes);
00074     bit_offset_ += bitlen;
00075     return ResultOk;
00076 }
00077 
00078 #if UAVCAN_TOSTRING
00079 std::string BitStream::toString() const
00080 {
00081     std::string out;
00082     out.reserve(128);
00083 
00084     for (unsigned offset = 0; true; offset++)
00085     {
00086         uint8_t byte = 0;
00087         if (1 != buf_.read(offset, &byte, 1U))
00088         {
00089             break;
00090         }
00091         for (int i = 7; i >= 0; i--)     // Most significant goes first
00092         {
00093             out += (byte & (1 << i)) ? '1' : '0';
00094         }
00095         out += ' ';
00096     }
00097     if (out.length() > 0)
00098     {
00099         (void)out.erase(out.length() - 1, 1);
00100     }
00101     return out;
00102 }
00103 #endif
00104 
00105 }