uc_scalar_codec.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/marshal/scalar_codec.hpp>
00006 
00007 namespace uavcan
00008 {
00009 
00010 void ScalarCodec::swapByteOrder(uint8_t* const bytes, const unsigned len)
00011 {
00012     UAVCAN_ASSERT(bytes);
00013     for (unsigned i = 0, j = len - 1; i < j; i++, j--)
00014     {
00015         const uint8_t c = bytes[i];
00016         bytes[i] = bytes[j];
00017         bytes[j] = c;
00018     }
00019 }
00020 
00021 int ScalarCodec::encodeBytesImpl(uint8_t* const bytes, const unsigned bitlen)
00022 {
00023     UAVCAN_ASSERT(bytes);
00024     // Underlying stream class assumes that more significant bits have lower index, so we need to shift some.
00025     if (bitlen % 8)
00026     {
00027         bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] << ((8 - (bitlen % 8)) & 7));
00028     }
00029     return stream_.write(bytes, bitlen);
00030 }
00031 
00032 int ScalarCodec::decodeBytesImpl(uint8_t* const bytes, const unsigned bitlen)
00033 {
00034     UAVCAN_ASSERT(bytes);
00035     const int read_res = stream_.read(bytes, bitlen);
00036     if (read_res > 0)
00037     {
00038         if (bitlen % 8)
00039         {
00040             bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] >> ((8 - (bitlen % 8)) & 7));  // As in encode(), vice versa
00041         }
00042     }
00043     return read_res;
00044 }
00045 
00046 }