diff --git a/libuavcan/include/uavcan/data_type.hpp b/libuavcan/include/uavcan/data_type.hpp index 134be45aa9..a749347932 100644 --- a/libuavcan/include/uavcan/data_type.hpp +++ b/libuavcan/include/uavcan/data_type.hpp @@ -34,7 +34,7 @@ public: DataTypeID() : value_(0xFFFF) { } DataTypeID(uint16_t id) - : value_(id) + : value_(id) { assert(isValid()); } @@ -81,14 +81,18 @@ public: static const uint64_t Poly = 0x42F0E1EBA9EA3693; crc_ ^= uint64_t(byte) << 56; for (int i = 0; i < 8; i++) + { crc_ = (crc_ & (uint64_t(1) << 63)) ? (crc_ << 1) ^ Poly : crc_ << 1; + } } void add(const uint8_t* bytes, unsigned int len) { assert(bytes); while (len--) + { add(*bytes++); + } } uint64_t get() const { return crc_ ^ 0xFFFFFFFFFFFFFFFF; } @@ -127,15 +131,15 @@ public: enum { MaxFullNameLen = 80 }; DataTypeDescriptor() - : kind_(DataTypeKind(0)) - , full_name_("") + : kind_(DataTypeKind(0)) + , full_name_("") { } DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) - : kind_(kind) - , id_(id) - , signature_(signature) - , full_name_(name) + : kind_(kind) + , id_(id) + , signature_(signature) + , full_name_(name) { assert(kind < NumDataTypeKinds); assert(name); diff --git a/libuavcan/include/uavcan/debug.hpp b/libuavcan/include/uavcan/debug.hpp index ad77034935..777e65093b 100644 --- a/libuavcan/include/uavcan/debug.hpp +++ b/libuavcan/include/uavcan/debug.hpp @@ -7,12 +7,12 @@ #if UAVCAN_DEBUG -#include -#include +# include +# include -#if __GNUC__ -__attribute__ ((format (printf, 2, 3))) -#endif +# if __GNUC__ +__attribute__ ((format(printf, 2, 3))) +# endif static void UAVCAN_TRACE(const char* src, const char* fmt, ...) { va_list args; @@ -25,6 +25,6 @@ static void UAVCAN_TRACE(const char* src, const char* fmt, ...) #else -# define UAVCAN_TRACE(...) ((void)0) +# define UAVCAN_TRACE(...) ((void)0) #endif diff --git a/libuavcan/include/uavcan/driver/can.hpp b/libuavcan/include/uavcan/driver/can.hpp index 3e5420f590..d0a051035c 100644 --- a/libuavcan/include/uavcan/driver/can.hpp +++ b/libuavcan/include/uavcan/driver/can.hpp @@ -31,15 +31,15 @@ struct CanFrame uint8_t dlc; ///< Data Length Code CanFrame() - : id(0) - , dlc(0) + : id(0) + , dlc(0) { std::fill(data, data + sizeof(data), 0); } CanFrame(uint32_t id, const uint8_t* data, unsigned int dlc) - : id(id) - , dlc(dlc) + : id(id) + , dlc(dlc) { assert(data && dlc <= 8); std::copy(data, data + dlc, this->data); @@ -89,8 +89,8 @@ struct CanSelectMasks uint8_t write; CanSelectMasks() - : read(0) - , write(0) + : read(0) + , write(0) { } }; diff --git a/libuavcan/include/uavcan/dynamic_memory.hpp b/libuavcan/include/uavcan/dynamic_memory.hpp index 5af9223a09..6f6a19d61c 100644 --- a/libuavcan/include/uavcan/dynamic_memory.hpp +++ b/libuavcan/include/uavcan/dynamic_memory.hpp @@ -76,10 +76,14 @@ public: for (int i = 0; i < MaxPools; i++) { if (pools_[i] == NULL) + { break; + } void* const pmem = pools_[i]->allocate(size); if (pmem != NULL) + { return pmem; + } } return NULL; } @@ -123,18 +127,22 @@ public: static const int NumBlocks = int(PoolSize / BlockSize); PoolAllocator() - : free_list_(reinterpret_cast(pool_)) // TODO: alignment + : free_list_(reinterpret_cast(pool_)) // TODO: alignment { memset(pool_, 0, PoolSize); for (int i = 0; i < NumBlocks - 1; i++) + { free_list_[i].next = free_list_ + i + 1; + } free_list_[NumBlocks - 1].next = NULL; } void* allocate(std::size_t size) { if (free_list_ == NULL || size > BlockSize) + { return NULL; + } void* pmem = free_list_; free_list_ = free_list_->next; return pmem; @@ -143,7 +151,9 @@ public: void deallocate(const void* ptr) { if (ptr == NULL) + { return; + } Node* p = static_cast(const_cast(ptr)); #if DEBUG || UAVCAN_DEBUG std::memset(p, 0, sizeof(Node)); diff --git a/libuavcan/include/uavcan/linked_list.hpp b/libuavcan/include/uavcan/linked_list.hpp index 475055c726..11a36e2009 100644 --- a/libuavcan/include/uavcan/linked_list.hpp +++ b/libuavcan/include/uavcan/linked_list.hpp @@ -20,7 +20,7 @@ class LinkedListNode protected: LinkedListNode() - : next_(NULL) + : next_(NULL) { } public: @@ -42,7 +42,7 @@ class LinkedListRoot public: LinkedListRoot() - : root_(NULL) + : root_(NULL) { } T* get() const { return root_; } @@ -88,7 +88,9 @@ public: while (p->getNextListNode()) { if (predicate(p->getNextListNode())) + { break; + } p = p->getNextListNode(); } node->setNextListNode(p->getNextListNode()); @@ -99,7 +101,9 @@ public: bool remove(const T* node) { if (root_ == NULL || node == NULL) + { return false; + } if (root_ == node) { diff --git a/libuavcan/include/uavcan/map.hpp b/libuavcan/include/uavcan/map.hpp index 277df04bb9..bfb9ee92b4 100644 --- a/libuavcan/include/uavcan/map.hpp +++ b/libuavcan/include/uavcan/map.hpp @@ -24,13 +24,16 @@ namespace uavcan template class Map : Noncopyable { -UAVCAN_PACKED_BEGIN + UAVCAN_PACKED_BEGIN struct KVPair { Key key; Value value; KVPair() { } - KVPair(const Key& key, const Value& value) : key(key), value(value) { } + KVPair(const Key& key, const Value& value) + : key(key) + , value(value) + { } bool match(const Key& rhs) const { return rhs == key; } }; @@ -49,7 +52,9 @@ UAVCAN_PACKED_BEGIN { void* const praw = allocator.allocate(sizeof(KVGroup)); if (praw == NULL) + { return NULL; + } return new (praw) KVGroup(); } @@ -66,12 +71,16 @@ UAVCAN_PACKED_BEGIN KVPair* find(const Key& key) { for (int i = 0; i < NumKV; i++) + { if (kvs[i].match(key)) + { return kvs + i; + } + } return NULL; } }; -UAVCAN_PACKED_END + UAVCAN_PACKED_END LinkedListRoot list_; IAllocator& allocator_; @@ -80,15 +89,21 @@ UAVCAN_PACKED_END KVPair* find(const Key& key) { for (unsigned int i = 0; i < NumStaticEntries; i++) + { if (static_[i].match(key)) + { return static_ + i; + } + } KVGroup* p = list_.get(); while (p) { KVPair* const kv = p->find(key); if (kv) + { return kv; + } p = p->getNextListNode(); } return NULL; @@ -109,7 +124,9 @@ UAVCAN_PACKED_END } } if (stat == NULL) + { break; + } // Looking for the first NON-EMPTY dynamic entry, erasing immediately KVGroup* p = list_.get(); @@ -128,11 +145,15 @@ UAVCAN_PACKED_END } } if (stop) + { break; + } p = p->getNextListNode(); } if (dyn.match(Key())) + { break; + } // Migrating *stat = dyn; @@ -174,7 +195,7 @@ UAVCAN_PACKED_END public: Map(IAllocator& allocator) - : allocator_(allocator) + : allocator_(allocator) { assert(Key() == Key()); } @@ -203,7 +224,9 @@ public: KVGroup* const kvg = KVGroup::instantiate(allocator_); if (kvg == NULL) + { return NULL; + } list_.insert(kvg); kvg->kvs[0] = KVPair(key, value); return &kvg->kvs[0].value; @@ -313,8 +336,12 @@ public: { unsigned int num = 0; for (unsigned int i = 0; i < NumStaticEntries; i++) + { if (!static_[i].match(Key())) + { num++; + } + } return num; } @@ -329,7 +356,9 @@ public: { const KVPair* const kv = p->kvs + i; if (!kv->match(Key())) + { num++; + } } p = p->getNextListNode(); } diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index dd72754570..6b9704fe8e 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -42,7 +42,9 @@ protected: SizeType validateRange(SizeType pos) const { if (pos < Size) + { return pos; + } #if UAVCAN_EXCEPTIONS throw std::out_of_range("uavcan::Array"); #else @@ -71,7 +73,9 @@ protected: SizeType validateRange(SizeType pos) const { if (pos < size_) + { return pos; + } #if UAVCAN_EXCEPTIONS throw std::out_of_range("uavcan::Array"); #else @@ -83,15 +87,21 @@ protected: void grow() { if (size_ >= MaxSize) + { validateRange(MaxSize); // Will throw, assert() or do nothing + } else + { size_++; + } } void shrink() { if (size_ > 0) + { size_--; + } } public: @@ -122,8 +132,8 @@ class ArrayImpl : public Select::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) - && (ArrayMode == ArrayModeDynamic) + IsStringLike = IsIntegerSpec::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) && + (ArrayMode == ArrayModeDynamic) }; private: @@ -134,7 +144,9 @@ private: typename EnableIf::Type initialize(int) { if (ArrayMode != ArrayModeDynamic) + { std::fill(data_, data_ + MaxSize, U()); + } } template void initialize(...) { } @@ -233,7 +245,9 @@ class Array : public ArrayImpl const bool last_item = i == (size() - 1); const int res = RawValueType::encode(Base::at(i), codec, last_item ? tao_mode : TailArrayOptDisabled); if (res <= 0) + { return res; + } } return 1; } @@ -246,10 +260,14 @@ class Array : public ArrayImpl { const int res_sz = Base::RawSizeType::encode(size(), codec, TailArrayOptDisabled); if (res_sz <= 0) + { return res_sz; + } } if (size() == 0) + { return 1; + } return encodeImpl(codec, self_tao_enabled ? TailArrayOptDisabled : tao_mode, FalseType()); } @@ -262,7 +280,9 @@ class Array : public ArrayImpl ValueType value; // TODO: avoid extra copy const int res = RawValueType::decode(value, codec, last_item ? tao_mode : TailArrayOptDisabled); if (res <= 0) + { return res; + } Base::at(i) = value; } return 1; @@ -279,11 +299,17 @@ class Array : public ArrayImpl ValueType value; const int res = RawValueType::decode(value, codec, TailArrayOptDisabled); if (res < 0) + { return res; + } if (res == 0) // Success: End of stream reached (even if zero items were read) + { return 1; + } if (size() == MaxSize_) // Error: Max array length reached, but the end of stream is not + { return -1; + } push_back(value); } } @@ -292,12 +318,18 @@ class Array : public ArrayImpl typename StorageType::Type sz = 0; const int res_sz = Base::RawSizeType::decode(sz, codec, TailArrayOptDisabled); if (res_sz <= 0) + { return res_sz; + } if ((sz > 0) && ((sz - 1u) > (MaxSize_ - 1u))) // -Werror=type-limits + { return -1; + } resize(sz); if (sz == 0) + { return 1; + } return decodeImpl(codec, tao_mode, FalseType()); } assert(0); // Unreachable @@ -347,13 +379,17 @@ public: { unsigned int cnt = new_size - size(); while (cnt--) + { push_back(filler); + } } else if (new_size < size()) { unsigned int cnt = size() - new_size; while (cnt--) + { pop_back(); + } } } @@ -370,17 +406,25 @@ public: operator==(const R& rhs) const { if (size() != rhs.size()) + { return false; + } for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators + { if (!(Base::at(i) == rhs[i])) + { return false; + } + } return true; } bool operator==(const char* ch) const { if (ch == NULL) + { return false; + } return std::strcmp(Base::c_str(), ch) == 0; } @@ -395,9 +439,13 @@ public: StaticAssert::check(); Base::clear(); if (ch == NULL) + { handleFatalError("Null pointer in Array<>::operator=(const char*)"); + } while (*ch) + { push_back(*ch++); + } return *this; } @@ -406,9 +454,13 @@ public: StaticAssert::check(); StaticAssert::check(); if (ch == NULL) + { handleFatalError("Null pointer in Array<>::operator+=(const char*)"); + } while (*ch) + { push_back(*ch++); + } return *this; } @@ -420,7 +472,9 @@ public: SizeType, typename Rhs::SizeType>::Result CommonSizeType; StaticAssert::check(); for (CommonSizeType i = 0; i < rhs.size(); i++) + { push_back(rhs[i]); + } return *this; } @@ -435,7 +489,7 @@ public: StaticAssert::check(); StaticAssert::check(); // This check allows to weed out most non-trivial types - StaticAssert::check();// Another stupid check to catch non-trivial types + StaticAssert::check(); // Another stupid check to catch non-trivial types if (!format) { @@ -487,11 +541,17 @@ class YamlStreamer > static bool isNiceCharacter(int c) { if (c >= 32 && c <= 126) + { return true; + } static const char Good[] = {'\n', '\r', '\t'}; for (unsigned int i = 0; i < sizeof(Good) / sizeof(Good[0]); i++) + { if (Good[i] == c) + { return true; + } + } return false; } @@ -503,7 +563,9 @@ class YamlStreamer > { YamlStreamer::stream(s, array.at(i), 0); if ((i + 1) < array.size()) + { s << ", "; + } } s << ']'; } @@ -522,14 +584,18 @@ class YamlStreamer > { nibbles[i] += '0'; if (nibbles[i] > '9') + { nibbles[i] += 'A' - '9' - 1; + } } s << "\\x" << nibbles[0] << nibbles[1]; } else { if (c == '"' || c == '\\') // YAML requires to escape these two + { s << '\\'; + } s << char(c); } } @@ -582,7 +648,9 @@ class YamlStreamer > { s << '\n'; for (int pos = 0; pos < level; pos++) + { s << " "; + } s << "- "; YamlStreamer::stream(s, array.at(i), level + 1); } @@ -593,8 +661,9 @@ public: static void stream(Stream& s, const ArrayType& array, int level) { typedef typename Select::Result, SelectorPrimitives, - SelectorObjects>::Result >::Result Type; + typename Select::Result, + SelectorPrimitives, + SelectorObjects>::Result >::Result Type; genericStreamImpl(s, array, level, Type()); } }; diff --git a/libuavcan/include/uavcan/marshal/bit_stream.hpp b/libuavcan/include/uavcan/marshal/bit_stream.hpp index 7df928274b..6bcf2b385d 100644 --- a/libuavcan/include/uavcan/marshal/bit_stream.hpp +++ b/libuavcan/include/uavcan/marshal/bit_stream.hpp @@ -43,9 +43,9 @@ public: }; BitStream(ITransferBuffer& buf) - : buf_(buf) - , bit_offset_(0) - , byte_cache_(0) + : buf_(buf) + , bit_offset_(0) + , byte_cache_(0) { StaticAssert::check(); } diff --git a/libuavcan/include/uavcan/marshal/char_array_formatter.hpp b/libuavcan/include/uavcan/marshal/char_array_formatter.hpp index e29b401261..f5c61810e2 100644 --- a/libuavcan/include/uavcan/marshal/char_array_formatter.hpp +++ b/libuavcan/include/uavcan/marshal/char_array_formatter.hpp @@ -39,7 +39,9 @@ class CharArrayFormatter if (std::is_same()) { if (array_.size() != array_.capacity()) + { array_.push_back(value); + } } else if (std::is_signed()) { @@ -67,7 +69,7 @@ public: typedef ArrayType_ ArrayType; CharArrayFormatter(ArrayType& array) - : array_(array) + : array_(array) { } ArrayType& getArray() { return array_; } @@ -78,7 +80,7 @@ public: writeValue(text); } - template + template void write(const char* s, T value, Args... args) { while (s && *s) @@ -105,7 +107,7 @@ public: typedef ArrayType_ ArrayType; CharArrayFormatter(ArrayType& array) - : array_(array) + : array_(array) { } ArrayType& getArray() { return array_; } diff --git a/libuavcan/include/uavcan/marshal/float_spec.hpp b/libuavcan/include/uavcan/marshal/float_spec.hpp index 1ae873d751..e63755365f 100644 --- a/libuavcan/include/uavcan/marshal/float_spec.hpp +++ b/libuavcan/include/uavcan/marshal/float_spec.hpp @@ -124,9 +124,13 @@ public: { // cppcheck-suppress duplicateExpression if (CastMode == CastModeSaturate) + { saturate(value); + } else + { truncate(value); + } return codec.encode(IEEE754Converter::toIeee(value)); } @@ -135,7 +139,9 @@ public: typename IntegerSpec::StorageType ieee = 0; const int res = codec.decode(ieee); if (res <= 0) + { return res; + } out_value = IEEE754Converter::toNative(ieee); return res; } @@ -149,9 +155,13 @@ private: if (!IsExactRepresentation && isfinite(value)) { if (value > max()) + { value = max(); + } else if (value < -max()) + { value = -max(); + } } } @@ -161,9 +171,13 @@ private: if (!IsExactRepresentation && isfinite(value)) { if (value > max()) + { value = std::numeric_limits::infinity(); + } else if (value < -max()) + { value = -std::numeric_limits::infinity(); + } } } }; diff --git a/libuavcan/include/uavcan/marshal/integer_spec.hpp b/libuavcan/include/uavcan/marshal/integer_spec.hpp index b5e5ac296f..3a95fb0c6c 100644 --- a/libuavcan/include/uavcan/marshal/integer_spec.hpp +++ b/libuavcan/include/uavcan/marshal/integer_spec.hpp @@ -64,9 +64,13 @@ private: static void saturate(StorageType& value) { if (value > max()) + { value = max(); + } else if (value <= min()) // 'Less or Equal' allows to suppress compiler warning on unsigned types + { value = min(); + } } static void truncate(StorageType& value) { value &= mask(); } @@ -88,9 +92,13 @@ public: validate(); // cppcheck-suppress duplicateExpression if (CastMode == CastModeSaturate) + { saturate(value); + } else + { truncate(value); + } return codec.encode(value); } @@ -111,10 +119,16 @@ class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation template -struct IsIntegerSpec { enum { Result = 0 }; }; +struct IsIntegerSpec +{ + enum { Result = 0 }; +}; template -struct IsIntegerSpec > { enum { Result = 1 }; }; +struct IsIntegerSpec > +{ + enum { Result = 1 }; +}; template @@ -128,7 +142,7 @@ struct YamlStreamer > { // Get rid of character types - we want its integer representation, not ASCII code typedef typename Select<(sizeof(StorageType) >= sizeof(int)), StorageType, - typename Select::Result >::Result TempType; + typename Select::Result >::Result TempType; s << TempType(value); } }; diff --git a/libuavcan/include/uavcan/marshal/scalar_codec.hpp b/libuavcan/include/uavcan/marshal/scalar_codec.hpp index 8c9e450224..95970fa8b8 100644 --- a/libuavcan/include/uavcan/marshal/scalar_codec.hpp +++ b/libuavcan/include/uavcan/marshal/scalar_codec.hpp @@ -45,9 +45,10 @@ class ScalarCodec * It is likely to be OK anyway, so feel free to remove this assert() as needed. */ assert(big_endian == false); - if (big_endian) + { swapByteOrder(bytes); + } } template @@ -62,9 +63,10 @@ class ScalarCodec fixTwosComplement(T& value) { StaticAssert::is_integer>::check(); // Not applicable to floating point types - if (value & (T(1) << (BitLen - 1))) // The most significant bit is set --> negative + { value |= 0xFFFFFFFFFFFFFFFF & ~((T(1) << BitLen) - 1); + } } template @@ -98,7 +100,7 @@ class ScalarCodec public: ScalarCodec(BitStream& stream) - : stream_(stream) + : stream_(stream) { } template @@ -111,14 +113,13 @@ public: uint8_t bytes[sizeof(T)]; } byte_union; byte_union.value = value; - clearExtraBits(byte_union.value); convertByteOrder(byte_union.bytes); - // Underlying stream class assumes that more significant bits have lower index, so we need to shift some. if (BitLen % 8) + { byte_union.bytes[BitLen / 8] <<= (8 - (BitLen % 8)) & 7; - + } return stream_.write(byte_union.bytes, BitLen); } @@ -135,14 +136,15 @@ public: const int read_res = stream_.read(byte_union.bytes, BitLen); if (read_res <= 0) + { return read_res; - + } if (BitLen % 8) + { byte_union.bytes[BitLen / 8] >>= (8 - (BitLen % 8)) & 7; // As in encode(), vice versa - + } convertByteOrder(byte_union.bytes); fixTwosComplement(byte_union.value); - value = byte_union.value; return read_res; } diff --git a/libuavcan/include/uavcan/marshal/type_util.hpp b/libuavcan/include/uavcan/marshal/type_util.hpp index 440bde0e84..d037f7c219 100644 --- a/libuavcan/include/uavcan/marshal/type_util.hpp +++ b/libuavcan/include/uavcan/marshal/type_util.hpp @@ -15,17 +15,29 @@ enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled }; template -struct IntegerBitLen { enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; }; +struct IntegerBitLen +{ + enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; +}; template <> -struct IntegerBitLen<0> { enum { Result = 0 }; }; +struct IntegerBitLen<0> +{ + enum { Result = 0 }; +}; template -struct BitLenToByteLen { enum { Result = (BitLen + 7) / 8 }; }; +struct BitLenToByteLen +{ + enum { Result = (BitLen + 7) / 8 }; +}; template -struct StorageType { typedef T Type; }; +struct StorageType +{ + typedef T Type; +}; template struct StorageType::Type> diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index c08ed9814c..38874c59d5 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -24,8 +24,11 @@ namespace uavcan template class GenericPublisher { - enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? - CanTxQueue::Volatile : CanTxQueue::Persistent }; + enum + { + Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? + CanTxQueue::Volatile : CanTxQueue::Persistent + }; const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced MonotonicDuration tx_timeout_; @@ -35,7 +38,9 @@ class GenericPublisher bool checkInit() { if (sender_) + { return true; + } GlobalDataTypeRegistry::instance().freeze(); @@ -63,11 +68,15 @@ class GenericPublisher TransferID* tid, MonotonicTime blocking_deadline) { if (!checkInit()) + { return -1; + } IMarshalBuffer* const buf = getBuffer(); if (!buf) + { return -1; + } { BitStream bitstream(*buf); @@ -94,9 +103,9 @@ class GenericPublisher public: GenericPublisher(INode& node, MonotonicDuration tx_timeout, MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) - : max_transfer_interval_(max_transfer_interval) - , tx_timeout_(tx_timeout) - , node_(node) + : max_transfer_interval_(max_transfer_interval) + , tx_timeout_(tx_timeout) + , node_(node) { setTxTimeout(tx_timeout); #if UAVCAN_DEBUG diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 9ba10162a6..bdb1d8240d 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -22,7 +22,7 @@ class ReceivedDataStructure : public DataType_ { const IncomingTransfer* transfer_; - template + template Ret safeget() const { if (!transfer_) @@ -60,8 +60,8 @@ template static Stream& operator<<(Stream& s, const ReceivedDataStructure& rds) { s << "# Received struct ts_m=" << rds.getMonotonicTimestamp() - << " ts_utc=" << rds.getUtcTimestamp() - << " snid=" << int(rds.getSrcNodeID().get()) << "\n"; + << " ts_utc=" << rds.getUtcTimestamp() + << " snid=" << int(rds.getSrcNodeID().get()) << "\n"; s << static_cast(rds); return s; } @@ -98,8 +98,8 @@ class GenericSubscriber : Noncopyable public: TransferForwarder(SelfType& obj, const DataTypeDescriptor& data_type, IAllocator& allocator) - : TransferListenerType(data_type, allocator) - , obj_(obj) + : TransferListenerType(data_type, allocator) + , obj_(obj) { } }; @@ -116,7 +116,9 @@ class GenericSubscriber : Noncopyable bool checkInit() { if (forwarder_) + { return true; + } GlobalDataTypeRegistry::instance().freeze(); @@ -161,7 +163,7 @@ class GenericSubscriber : Noncopyable } } - int genericStart(bool(Dispatcher::*registration_method)(TransferListenerBase* listener)) + int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase* listener)) { stop(); @@ -182,8 +184,8 @@ class GenericSubscriber : Noncopyable protected: explicit GenericSubscriber(INode& node) - : node_(node) - , failure_count_(0) + : node_(node) + , failure_count_(0) { } virtual ~GenericSubscriber() { stop(); } diff --git a/libuavcan/include/uavcan/node/global_data_type_registry.hpp b/libuavcan/include/uavcan/node/global_data_type_registry.hpp index 1e210fa1d3..968fbbb11a 100644 --- a/libuavcan/include/uavcan/node/global_data_type_registry.hpp +++ b/libuavcan/include/uavcan/node/global_data_type_registry.hpp @@ -13,7 +13,7 @@ #include #include #if UAVCAN_DEBUG -#include +# include #endif namespace uavcan @@ -30,7 +30,7 @@ class GlobalDataTypeRegistry : Noncopyable Entry() { } Entry(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) - : descriptor(kind, id, signature, name) + : descriptor(kind, id, signature, name) { } }; @@ -75,19 +75,25 @@ public: RegistResult regist(DataTypeID id) { if (isFrozen()) + { return RegistResultFrozen; + } static Entry entry; { const RegistResult remove_res = remove(&entry); if (remove_res != RegistResultOk) + { return remove_res; + } } entry = Entry(DataTypeKind(Type::DataTypeKind), id, Type::getDataTypeSignature(), Type::getDataTypeFullName()); { const RegistResult remove_res = remove(&entry); if (remove_res != RegistResultOk) + { return remove_res; + } } return registImpl(&entry); } @@ -114,9 +120,13 @@ public: int(frozen_), getNumMessageTypes(), getNumServiceTypes()); frozen_ = false; while (msgs_.get()) + { msgs_.remove(msgs_.get()); + } while (srvs_.get()) + { srvs_.remove(srvs_.get()); + } } #endif }; @@ -131,7 +141,9 @@ struct DefaultDataTypeRegistrator GlobalDataTypeRegistry::instance().regist(Type::DefaultDataTypeID); if (res != GlobalDataTypeRegistry::RegistResultOk) + { handleFatalError("Type registration failed"); + } } }; diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index 78744fad94..cd8b7a0796 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -59,7 +59,9 @@ public: IMarshalBuffer* getBuffer(unsigned int size) { if (size > MaxSize) + { return NULL; + } buffer_.reset(); return &buffer_; } diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index 152936e525..9f7f1c6054 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -19,7 +19,7 @@ public: explicit Publisher(INode& node, MonotonicDuration tx_timeout = getDefaultTxTimeout(), MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) - : BaseType(node, tx_timeout, max_transfer_interval) + : BaseType(node, tx_timeout, max_transfer_interval) { #if UAVCAN_DEBUG assert(getTxTimeout() == tx_timeout); // Making sure default values are OK diff --git a/libuavcan/include/uavcan/node/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 0df72ea579..241d907897 100644 --- a/libuavcan/include/uavcan/node/scheduler.hpp +++ b/libuavcan/include/uavcan/node/scheduler.hpp @@ -20,7 +20,7 @@ protected: Scheduler& scheduler_; explicit DeadlineHandler(Scheduler& scheduler) - : scheduler_(scheduler) + : scheduler_(scheduler) { } virtual ~DeadlineHandler() { stop(); } @@ -76,10 +76,10 @@ class Scheduler : Noncopyable public: Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr) - : dispatcher_(can_driver, allocator, sysclock, otr) - , prev_cleanup_ts_(sysclock.getMonotonic()) - , deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs)) - , cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs)) + : dispatcher_(can_driver, allocator, sysclock, otr) + , prev_cleanup_ts_(sysclock.getMonotonic()) + , deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs)) + , cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs)) { } int spin(MonotonicTime deadline); diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index e2c08c6f5a..4d639186ef 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -29,9 +29,9 @@ struct ServiceCallResult ResponseFieldType& response; ///< Either response contents or unspecified response structure ServiceCallResult(Status status, NodeID server_node_id, ResponseFieldType& response) - : status(status) - , server_node_id(server_node_id) - , response(response) + : status(status) + , server_node_id(server_node_id) + , response(response) { assert(server_node_id.isUnicast()); assert(status == Success || status == ErrorTimeout); @@ -44,21 +44,25 @@ template static Stream& operator<<(Stream& s, const ServiceCallResult& scr) { s << "# Service call result [" << DataType::getDataTypeFullName() << "] " - << (scr.isSuccessful() ? "OK" : "FAILURE") - << " server_node_id=" << int(scr.server_node_id.get()) << "\n"; + << (scr.isSuccessful() ? "OK" : "FAILURE") + << " server_node_id=" << int(scr.server_node_id.get()) << "\n"; if (scr.isSuccessful()) + { s << scr.response; + } else + { s << "# (no data)"; + } return s; } -template &)> -class ServiceClient : - public GenericSubscriber::Type >, - public DeadlineHandler +template &)> +class ServiceClient + : public GenericSubscriber::Type > + , public DeadlineHandler { public: typedef DataType_ DataType; @@ -82,9 +86,13 @@ private: void invokeCallback(ServiceCallResultType& result) { if (isCallbackValid()) + { callback_(result); + } else + { handleFatalError("Invalid caller callback"); + } } void handleReceivedDataStruct(ReceivedDataStructure& response) @@ -128,12 +136,12 @@ private: public: explicit ServiceClient(INode& node, const Callback& callback = Callback()) - : SubscriberType(node) - , DeadlineHandler(node.getScheduler()) - , publisher_(node, getDefaultRequestTimeout()) - , callback_(callback) - , request_timeout_(getDefaultRequestTimeout()) - , pending_(false) + : SubscriberType(node) + , DeadlineHandler(node.getScheduler()) + , publisher_(node, getDefaultRequestTimeout()) + , callback_(callback) + , request_timeout_(getDefaultRequestTimeout()) + , pending_(false) { setRequestTimeout(getDefaultRequestTimeout()); #if UAVCAN_DEBUG @@ -181,7 +189,7 @@ public: const MonotonicTime otr_deadline = SubscriberType::getNode().getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval(); TransferID* const otr_tid = SubscriberType::getNode().getDispatcher().getOutgoingTransferRegistry() - .accessOrCreate(otr_key, otr_deadline); + .accessOrCreate(otr_key, otr_deadline); if (!otr_tid) { UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", descr->toString().c_str()); @@ -238,7 +246,9 @@ public: DeadlineHandler::stop(); TransferListenerType* const tl = SubscriberType::getTransferListener(); if (tl) + { tl->stopAcceptingAnything(); + } } bool isPending() const { return pending_; } diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index 7dddb3727b..b8462e4682 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -11,8 +11,8 @@ namespace uavcan { template &, - typename DataType_::Response&), + typename Callback = void (*)(const ReceivedDataStructure&, + typename DataType_::Response&), unsigned int NumStaticReceivers = 2, unsigned int NumStaticBufs = 1> class ServiceServer : public GenericSubscriber&), + typename Callback = void (*)(const ReceivedDataStructure&), unsigned int NumStaticReceivers = 2, unsigned int NumStaticBufs = 1> class Subscriber : public GenericSubscriber& msg) { if (try_implicit_cast(callback_, true)) + { callback_(msg); + } else + { handleFatalError("Invalid subscriber callback"); + } } public: typedef DataType_ DataType; explicit Subscriber(INode& node) - : BaseType(node) - , callback_() + : BaseType(node) + , callback_() { StaticAssert::check(); } diff --git a/libuavcan/include/uavcan/node/timer.hpp b/libuavcan/include/uavcan/node/timer.hpp index 7a3303065e..10525e963c 100644 --- a/libuavcan/include/uavcan/node/timer.hpp +++ b/libuavcan/include/uavcan/node/timer.hpp @@ -22,8 +22,8 @@ struct TimerEvent MonotonicTime real_time; TimerEvent(MonotonicTime scheduled_time, MonotonicTime real_time) - : scheduled_time(scheduled_time) - , real_time(real_time) + : scheduled_time(scheduled_time) + , real_time(real_time) { } }; @@ -41,8 +41,8 @@ public: using DeadlineHandler::getScheduler; explicit Timer(INode& node) - : DeadlineHandler(node.getScheduler()) - , period_(MonotonicDuration::getInfinite()) + : DeadlineHandler(node.getScheduler()) + , period_(MonotonicDuration::getInfinite()) { } void startOneShotWithDeadline(MonotonicTime deadline); @@ -63,20 +63,24 @@ class TimerEventForwarder : public Timer void handleTimerEvent(const TimerEvent& event) { if (try_implicit_cast(callback_, true)) + { callback_(event); + } else + { handleFatalError("Invalid timer callback"); + } } public: explicit TimerEventForwarder(INode& node) - : Timer(node) - , callback_() + : Timer(node) + , callback_() { } TimerEventForwarder(INode& node, Callback callback) - : Timer(node) - , callback_(callback) + : Timer(node) + , callback_(callback) { } const Callback& getCallback() const { return callback_; } diff --git a/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp index 6f07c3c8a8..eaf1cfe2c3 100644 --- a/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp +++ b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp @@ -16,13 +16,13 @@ namespace uavcan class DataTypeInfoProvider : Noncopyable { typedef MethodBinder + void (DataTypeInfoProvider::*)(const protocol::ComputeAggregateTypeSignature::Request&, + protocol::ComputeAggregateTypeSignature::Response&)> ComputeAggregateTypeSignatureCallback; typedef MethodBinder GetDataTypeInfoCallback; + void (DataTypeInfoProvider::*)(const protocol::GetDataTypeInfo::Request&, + protocol::GetDataTypeInfo::Response&)> GetDataTypeInfoCallback; ServiceServer cats_srv_; ServiceServer gdti_srv_; @@ -39,8 +39,8 @@ class DataTypeInfoProvider : Noncopyable public: DataTypeInfoProvider(INode& node) - : cats_srv_(node) - , gdti_srv_(node) + : cats_srv_(node) + , gdti_srv_(node) { } int start(); diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp index a193a8de77..d968894619 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -26,8 +26,8 @@ class GlobalTimeSyncMaster : protected LoopbackFrameListenerBase public: IfaceMaster(INode& node, uint8_t iface_index) - : pub_(node) - , iface_index_(iface_index) + : pub_(node) + , iface_index_(iface_index) { assert(iface_index < MaxCanIfaces); } @@ -48,9 +48,9 @@ class GlobalTimeSyncMaster : protected LoopbackFrameListenerBase public: GlobalTimeSyncMaster(INode& node) - : LoopbackFrameListenerBase(node.getDispatcher()) - , node_(node) - , initialized_(false) + : LoopbackFrameListenerBase(node.getDispatcher()) + , node_(node) + , initialized_(false) { } int init(); diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp index ae1128205b..303fb40b48 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -14,7 +14,8 @@ namespace uavcan class GlobalTimeSyncSlave : Noncopyable { typedef MethodBinder&)> GlobalTimeSyncCallback; + void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure&)> + GlobalTimeSyncCallback; // Static buffers are explicitly disabled because time should never be unicasted. Subscriber sub_; @@ -39,9 +40,9 @@ class GlobalTimeSyncSlave : Noncopyable public: GlobalTimeSyncSlave(INode& node) - : sub_(node) - , state_(Update) - , prev_iface_index_(0xFF) + : sub_(node) + , state_(Update) + , prev_iface_index_(0xFF) { } int start(); diff --git a/libuavcan/include/uavcan/protocol/logger.hpp b/libuavcan/include/uavcan/protocol/logger.hpp index ddd896557e..7f82687e0e 100644 --- a/libuavcan/include/uavcan/protocol/logger.hpp +++ b/libuavcan/include/uavcan/protocol/logger.hpp @@ -41,8 +41,8 @@ private: public: Logger(INode& node) - : logmsg_pub_(node) - , external_sink_(NULL) + : logmsg_pub_(node) + , external_sink_(NULL) { level_ = protocol::debug::LogLevel::ERROR; setTxTimeout(MonotonicDuration::fromMSec(DefaultTxTimeoutMs)); diff --git a/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/libuavcan/include/uavcan/protocol/node_status_provider.hpp index 51d49b2aef..c5a61387a0 100644 --- a/libuavcan/include/uavcan/protocol/node_status_provider.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_provider.hpp @@ -19,11 +19,11 @@ namespace uavcan class NodeStatusProvider : private Timer { - typedef MethodBinder + typedef MethodBinder GlobalDiscoveryRequestCallback; typedef MethodBinder GetNodeInfoCallback; + void (NodeStatusProvider::*)(const protocol::GetNodeInfo::Request&, + protocol::GetNodeInfo::Response&)> GetNodeInfoCallback; const MonotonicTime creation_timestamp_; @@ -46,11 +46,11 @@ class NodeStatusProvider : private Timer public: NodeStatusProvider(INode& node) - : Timer(node) - , creation_timestamp_(node.getMonotonicTime()) - , node_status_pub_(node) - , gdr_sub_(node) - , gni_srv_(node) + : Timer(node) + , creation_timestamp_(node.getMonotonicTime()) + , node_status_pub_(node) + , gdr_sub_(node) + , gni_srv_(node) { assert(!creation_timestamp_.isZero()); diff --git a/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp index 793517fcee..1529f6461f 100644 --- a/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp +++ b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp @@ -22,8 +22,8 @@ class PanicBroadcaster : private Timer public: PanicBroadcaster(INode& node) - : Timer(node) - , pub_(node) + : Timer(node) + , pub_(node) { pub_.setTxTimeout(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS - 10)); } diff --git a/libuavcan/include/uavcan/protocol/panic_listener.hpp b/libuavcan/include/uavcan/protocol/panic_listener.hpp index 9ca604a8b5..a57189b29b 100644 --- a/libuavcan/include/uavcan/protocol/panic_listener.hpp +++ b/libuavcan/include/uavcan/protocol/panic_listener.hpp @@ -4,6 +4,8 @@ #pragma once +#include +#include #include #include #include @@ -17,10 +19,10 @@ namespace uavcan * void (const protocol::Panic&) * The listener can be stopped from the callback. */ -template &)> +template &)> class PanicListener : Noncopyable { - typedef MethodBinder&)> + typedef MethodBinder&)> PanicMsgCallback; Subscriber sub_; @@ -73,9 +75,9 @@ class PanicListener : Noncopyable public: PanicListener(INode& node) - : sub_(node) - , callback_() - , num_subsequent_msgs_(0) + : sub_(node) + , callback_() + , num_subsequent_msgs_(0) { } int start(const Callback& callback) diff --git a/libuavcan/include/uavcan/protocol/restart_request_server.hpp b/libuavcan/include/uavcan/protocol/restart_request_server.hpp index be7d9e8407..fae1eb3c89 100644 --- a/libuavcan/include/uavcan/protocol/restart_request_server.hpp +++ b/libuavcan/include/uavcan/protocol/restart_request_server.hpp @@ -22,8 +22,8 @@ public: class RestartRequestServer : Noncopyable { typedef MethodBinder&, - protocol::RestartNode::Response&) const> RestartNodeCallback; + void (RestartRequestServer::*)(const ReceivedDataStructure&, + protocol::RestartNode::Response&) const> RestartNodeCallback; ServiceServer srv_; IRestartRequestHandler* handler_; @@ -33,8 +33,8 @@ class RestartRequestServer : Noncopyable public: RestartRequestServer(INode& node) - : srv_(node) - , handler_(NULL) + : srv_(node) + , handler_(NULL) { } IRestartRequestHandler* getHandler() const { return handler_; } diff --git a/libuavcan/include/uavcan/time.hpp b/libuavcan/include/uavcan/time.hpp index 7a06a2726e..9b7c1af440 100644 --- a/libuavcan/include/uavcan/time.hpp +++ b/libuavcan/include/uavcan/time.hpp @@ -23,7 +23,7 @@ class DurationBase public: DurationBase() - : usec_(0) + : usec_(0) { StaticAssert<(sizeof(D) == 8)>::check(); } @@ -55,17 +55,17 @@ public: bool operator<=(const D& r) const { return usec_ <= r.usec_; } bool operator>=(const D& r) const { return usec_ >= r.usec_; } - D operator+(const D &r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check - D operator-(const D &r) const { return fromUSec(usec_ - r.usec_); } // ditto + D operator+(const D& r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check + D operator-(const D& r) const { return fromUSec(usec_ - r.usec_); } // ditto D operator-() const { return fromUSec(-usec_); } - D& operator+=(const D &r) + D& operator+=(const D& r) { *this = *this + r; return *static_cast(this); } - D& operator-=(const D &r) + D& operator-=(const D& r) { *this = *this - r; return *static_cast(this); @@ -92,7 +92,7 @@ class TimeBase public: TimeBase() - : usec_(0) + : usec_(0) { StaticAssert<(sizeof(T) == 8)>::check(); StaticAssert<(sizeof(D) == 8)>::check(); @@ -126,12 +126,16 @@ public: if (r.isNegative()) { if (uint64_t(r.getAbs().toUSec()) > usec_) + { return fromUSec(0); + } } else { if (uint64_t(usec_ + r.toUSec()) < usec_) + { return fromUSec(std::numeric_limits::max()); + } } return fromUSec(usec_ + r.toUSec()); } @@ -202,7 +206,9 @@ inline Stream& operator<<(Stream& s, DurationBase d) char buf[8]; std::snprintf(buf, sizeof(buf), "%06lu", static_cast(std::abs(d.toUSec() % 1000000L))); if (d.isNegative()) + { s << '-'; + } s << std::abs(d.toUSec() / 1000000L) << '.' << buf; return s; } diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index e3798ea3fe..c162566a37 100644 --- a/libuavcan/include/uavcan/transport/can_io.hpp +++ b/libuavcan/include/uavcan/transport/can_io.hpp @@ -27,7 +27,7 @@ struct CanRxFrame : public CanFrame uint8_t iface_index; CanRxFrame() - : iface_index(0) + : iface_index(0) { } std::string toString(StringRepresentation mode = StrTight) const; @@ -47,10 +47,10 @@ public: CanIOFlags flags; Entry(const CanFrame& frame, MonotonicTime deadline, Qos qos, CanIOFlags flags) - : deadline(deadline) - , frame(frame) - , qos(uint8_t(qos)) - , flags(flags) + : deadline(deadline) + , frame(frame) + , qos(uint8_t(qos)) + , flags(flags) { assert(qos == Volatile || qos == Persistent); IsDynamicallyAllocatable::check(); @@ -90,15 +90,15 @@ private: public: CanTxQueue() - : allocator_(NULL) - , sysclock_(NULL) - , rejected_frames_cnt_(0) + : allocator_(NULL) + , sysclock_(NULL) + , rejected_frames_cnt_(0) { } CanTxQueue(IAllocator* allocator, ISystemClock* sysclock) - : allocator_(allocator) - , sysclock_(sysclock) - , rejected_frames_cnt_(0) + : allocator_(allocator) + , sysclock_(sysclock) + , rejected_frames_cnt_(0) { } ~CanTxQueue(); @@ -133,8 +133,8 @@ class CanIOManager : Noncopyable public: CanIOManager(ICanDriver& driver, IAllocator& allocator, ISystemClock& sysclock) - : driver_(driver) - , sysclock_(sysclock) + : driver_(driver) + , sysclock_(sysclock) { assert(driver.getNumIfaces() <= MaxCanIfaces); // We can't initialize member array with non-default constructors in C++03 diff --git a/libuavcan/include/uavcan/transport/crc.hpp b/libuavcan/include/uavcan/transport/crc.hpp index e0299a5396..b6575238cc 100644 --- a/libuavcan/include/uavcan/transport/crc.hpp +++ b/libuavcan/include/uavcan/transport/crc.hpp @@ -32,7 +32,7 @@ public: enum { NumBytes = 2 }; TransferCRC() - : value_(0xFFFF) + : value_(0xFFFF) { } void add(uint8_t byte) @@ -44,7 +44,9 @@ public: { assert(bytes); while (len--) + { add(*bytes++); + } } uint16_t get() const { return value_; } diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index bb90b1e1a9..c10a7e1118 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -22,7 +22,7 @@ class LoopbackFrameListenerBase : public LinkedListNode(this) = frame; } diff --git a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp index f3a399ead5..481116b2f8 100644 --- a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp +++ b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp @@ -25,13 +25,13 @@ class OutgoingTransferRegistryKey public: OutgoingTransferRegistryKey() - : transfer_type_(0xFF) + : transfer_type_(0xFF) { } OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id) - : data_type_id_(data_type_id) - , transfer_type_(transfer_type) - , destination_node_id_(destination_node_id) + : data_type_id_(data_type_id) + , transfer_type_(transfer_type) + , destination_node_id_(destination_node_id) { assert((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast()); @@ -56,8 +56,8 @@ public: { std::ostringstream os; os << "dtid=" << int(data_type_id_.get()) - << " tt=" << int(transfer_type_) - << " dnid=" << int(destination_node_id_.get()); + << " tt=" << int(transfer_type_) + << " dnid=" << int(destination_node_id_.get()); return os.str(); } }; @@ -77,13 +77,13 @@ public: template class OutgoingTransferRegistry : public IOutgoingTransferRegistry, Noncopyable { -UAVCAN_PACKED_BEGIN + UAVCAN_PACKED_BEGIN struct Value { MonotonicTime deadline; TransferID tid; }; -UAVCAN_PACKED_END + UAVCAN_PACKED_END class DeadlineExpiredPredicate { @@ -91,7 +91,7 @@ UAVCAN_PACKED_END public: DeadlineExpiredPredicate(MonotonicTime ts) - : ts_(ts) + : ts_(ts) { } bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const @@ -115,8 +115,8 @@ UAVCAN_PACKED_END public: ExistenceCheckingPredicate(DataTypeID dtid, TransferType tt) - : dtid_(dtid) - , tt_(tt) + : dtid_(dtid) + , tt_(tt) { } bool operator()(const OutgoingTransferRegistryKey& key, const Value&) const @@ -129,7 +129,7 @@ UAVCAN_PACKED_END public: OutgoingTransferRegistry(IAllocator& allocator) - : map_(allocator) + : map_(allocator) { } TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline) @@ -140,7 +140,9 @@ public: { p = map_.insert(key, Value()); if (p == NULL) + { return NULL; + } UAVCAN_TRACE("OutgoingTransferRegistry", "Created %s", key.toString().c_str()); } p->deadline = new_deadline; diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index 1348e1c8da..d73ee19011 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -33,11 +33,11 @@ public: enum { Max = (1 << BitLen) - 1 }; TransferID() - : value_(0) + : value_(0) { } TransferID(uint8_t value) // implicit - : value_(value) + : value_(value) { value_ &= Max; assert(value == value_); @@ -82,16 +82,16 @@ public: NodeID() : value_(ValueInvalid) { } NodeID(uint8_t value) - : value_(value) + : value_(value) { assert(isValid()); } uint8_t get() const { return value_; } - bool isValid() const { return value_ <= Max; } + bool isValid() const { return value_ <= Max; } bool isBroadcast() const { return value_ == ValueBroadcast; } - bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); } + bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); } bool operator!=(NodeID rhs) const { return !operator==(rhs); } bool operator==(NodeID rhs) const { return value_ == rhs.value_; } diff --git a/libuavcan/include/uavcan/transport/transfer_buffer.hpp b/libuavcan/include/uavcan/transport/transfer_buffer.hpp index 66afe08562..1226167b19 100644 --- a/libuavcan/include/uavcan/transport/transfer_buffer.hpp +++ b/libuavcan/include/uavcan/transport/transfer_buffer.hpp @@ -38,14 +38,14 @@ class TransferBufferManagerKey public: TransferBufferManagerKey() - : transfer_type_(TransferType(0)) + : transfer_type_(TransferType(0)) { assert(isEmpty()); } TransferBufferManagerKey(NodeID node_id, TransferType ttype) - : node_id_(node_id) - , transfer_type_(ttype) + : node_id_(node_id) + , transfer_type_(ttype) { assert(!isEmpty()); } @@ -77,7 +77,7 @@ public: TransferBufferManagerEntry() { } TransferBufferManagerEntry(const TransferBufferManagerKey& key) - : key_(key) + : key_(key) { } const TransferBufferManagerKey& getKey() const { return key_; } @@ -95,8 +95,9 @@ public: * reset() call releases all memory blocks. * Supports unordered write operations - from higher to lower offsets */ -class DynamicTransferBufferManagerEntry : public TransferBufferManagerEntry, - public LinkedListNode +class DynamicTransferBufferManagerEntry + : public TransferBufferManagerEntry + , public LinkedListNode { struct Block : LinkedListNode { @@ -121,9 +122,9 @@ class DynamicTransferBufferManagerEntry : public TransferBufferManagerEntry, public: DynamicTransferBufferManagerEntry(IAllocator& allocator, unsigned int max_size) - : allocator_(allocator) - , max_write_pos_(0) - , max_size_(max_size) + : allocator_(allocator) + , max_write_pos_(0) + , max_size_(max_size) { StaticAssert<(Block::Size > 8)>::check(); IsDynamicallyAllocatable::check(); @@ -154,7 +155,7 @@ class StaticTransferBuffer : public ITransferBuffer public: StaticTransferBuffer() - : max_write_pos_(0) + : max_write_pos_(0) { StaticAssert<(Size > 0)>::check(); std::fill(data_, data_ + Size, 0); @@ -168,9 +169,13 @@ public: return -1; } if (offset >= max_write_pos_) + { return 0; + } if ((offset + len) > max_write_pos_) + { len = max_write_pos_ - offset; + } assert((offset + len) <= max_write_pos_); std::copy(data_ + offset, data_ + offset + len, data); return len; @@ -184,9 +189,13 @@ public: return -1; } if (offset >= Size) + { return 0; + } if ((offset + len) > Size) + { len = Size - offset; + } assert((offset + len) <= Size); std::copy(data, data + len, data_ + offset); max_write_pos_ = std::max(offset + len, max_write_pos_); @@ -250,7 +259,9 @@ public: } buf_.setMaxWritePos(res); if (res < int(Size)) + { return true; + } // Now we need to make sure that all data can fit the storage uint8_t dummy = 0; @@ -285,8 +296,8 @@ class TransferBufferAccessor public: TransferBufferAccessor(ITransferBufferManager& bufmgr, TransferBufferManagerKey key) - : bufmgr_(bufmgr) - , key_(key) + : bufmgr_(bufmgr) + , key_(key) { assert(!key.isEmpty()); } @@ -312,7 +323,9 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable for (unsigned int i = 0; i < NumStaticBufs; i++) { if (static_buffers_[i].getKey() == key) + { return static_buffers_ + i; + } } return NULL; } @@ -324,7 +337,9 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable { assert(!dyn->isEmpty()); if (dyn->getKey() == key) + { return dyn; + } dyn = dyn->getNextListNode(); } return NULL; @@ -336,7 +351,9 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable { StaticBufferType* const sb = findFirstStatic(TransferBufferManagerKey()); if (sb == NULL) + { break; + } DynamicTransferBufferManagerEntry* dyn = dynamic_buffers_.get(); assert(dyn); assert(!dyn->isEmpty()); @@ -354,7 +371,7 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable * This should never happen during normal operation because dynamic buffers are limited in growth. */ UAVCAN_TRACE("TransferBufferManager", "Storage optimization: MIGRATION FAILURE %s MAXSIZE %u", - dyn->getKey().toString().c_str(), MaxBufSize); + dyn->getKey().toString().c_str(), MaxBufSize); assert(0); sb->reset(); break; @@ -364,7 +381,7 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable public: TransferBufferManager(IAllocator& allocator) - : allocator_(allocator) + : allocator_(allocator) { StaticAssert<(MaxBufSize > 0)>::check(); StaticAssert<(NumStaticBufs > 0)>::check(); @@ -391,7 +408,9 @@ public: } TransferBufferManagerEntry* tbme = findFirstStatic(key); if (tbme) + { return tbme; + } return findFirstDynamic(key); } @@ -407,10 +426,13 @@ public: TransferBufferManagerEntry* tbme = findFirstStatic(TransferBufferManagerKey()); if (tbme == NULL) { - DynamicTransferBufferManagerEntry* dyn = DynamicTransferBufferManagerEntry::instantiate(allocator_, MaxBufSize); + DynamicTransferBufferManagerEntry* dyn = + DynamicTransferBufferManagerEntry::instantiate(allocator_, MaxBufSize); tbme = dyn; if (dyn == NULL) + { return NULL; // Epic fail. + } dynamic_buffers_.insert(dyn); UAVCAN_TRACE("TransferBufferManager", "Dynamic buffer created [st=%u, dyn=%u], %s", getNumStaticBuffers(), getNumDynamicBuffers(), key.toString().c_str()); @@ -461,7 +483,9 @@ public: for (unsigned int i = 0; i < NumStaticBufs; i++) { if (!static_buffers_[i].isEmpty()) + { res++; + } } return res; } diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 1b241d3a23..f8183c5734 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -34,12 +34,12 @@ class IncomingTransfer : public ITransferBuffer protected: IncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, TransferType transfer_type, TransferID transfer_id, NodeID source_node_id, uint8_t iface_index) - : ts_mono_(ts_mono) - , ts_utc_(ts_utc) - , transfer_type_(transfer_type) - , transfer_id_(transfer_id) - , src_node_id_(source_node_id) - , iface_index_(iface_index) + : ts_mono_(ts_mono) + , ts_utc_(ts_utc) + , transfer_type_(transfer_type) + , transfer_id_(transfer_id) + , src_node_id_(source_node_id) + , iface_index_(iface_index) { } public: @@ -93,8 +93,8 @@ class TransferListenerBase : public LinkedListNode, Noncop protected: TransferListenerBase(const DataTypeDescriptor& data_type) - : data_type_(data_type) - , crc_base_(data_type.getSignature().toTransferCRC()) + : data_type_(data_type) + , crc_base_(data_type.getSignature().toTransferCRC()) { } virtual ~TransferListenerBase() { } @@ -127,8 +127,8 @@ class TransferListener : public TransferListenerBase public: TimedOutReceiverPredicate(MonotonicTime ts, BufferManager& bufmgr) - : ts_(ts) - , bufmgr_(bufmgr) + : ts_(ts) + , bufmgr_(bufmgr) { } bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const @@ -164,7 +164,9 @@ protected: if (recv == NULL) { if (!frame.isFirst()) + { return; + } TransferReceiver new_recv; recv = receivers_.insert(key, new_recv); @@ -180,9 +182,9 @@ protected: public: TransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator) - : TransferListenerBase(data_type) - , bufmgr_(allocator) - , receivers_(allocator) + : TransferListenerBase(data_type) + , bufmgr_(allocator) + , receivers_(allocator) { StaticAssert<(NumStaticReceivers >= NumStaticBufs)>::check(); // Otherwise it would be meaningless } @@ -212,8 +214,8 @@ public: } ExpectedResponseParams(NodeID src_node_id, TransferID transfer_id) - : src_node_id(src_node_id) - , transfer_id(transfer_id) + : src_node_id(src_node_id) + , transfer_id(transfer_id) { assert(src_node_id.isUnicast()); } @@ -245,7 +247,7 @@ private: public: ServiceResponseTransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator) - : BaseType(data_type, allocator) + : BaseType(data_type, allocator) { } void setExpectedResponseParams(const ExpectedResponseParams& erp) diff --git a/libuavcan/include/uavcan/transport/transfer_receiver.hpp b/libuavcan/include/uavcan/transport/transfer_receiver.hpp index 73d118f309..a8817e175e 100644 --- a/libuavcan/include/uavcan/transport/transfer_receiver.hpp +++ b/libuavcan/include/uavcan/transport/transfer_receiver.hpp @@ -55,11 +55,11 @@ private: public: TransferReceiver() - : transfer_interval_usec_(DefaultTransferIntervalUSec) - , this_transfer_crc_(0) - , buffer_write_pos_(0) - , iface_index_(IfaceIndexNotSet) - , next_frame_index_(0) + : transfer_interval_usec_(DefaultTransferIntervalUSec) + , this_transfer_crc_(0) + , buffer_write_pos_(0) + , iface_index_(IfaceIndexNotSet) + , next_frame_index_(0) { } bool isTimedOut(MonotonicTime current_ts) const; diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index baddb65462..003e352ba7 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -35,13 +35,13 @@ public: TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) - : max_transfer_interval_(max_transfer_interval) - , data_type_(data_type) - , qos_(qos) - , crc_base_(data_type.getSignature().toTransferCRC()) - , flags_(CanIOFlags(0)) - , iface_mask_(AllIfacesMask) - , dispatcher_(dispatcher) + : max_transfer_interval_(max_transfer_interval) + , data_type_(data_type) + , qos_(qos) + , crc_base_(data_type.getSignature().toTransferCRC()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , dispatcher_(dispatcher) { } CanIOFlags getCanIOFlags() const { return flags_; } diff --git a/libuavcan/include/uavcan/util/compile_time.hpp b/libuavcan/include/uavcan/util/compile_time.hpp index f1c9284a13..1cbcb19325 100644 --- a/libuavcan/include/uavcan/util/compile_time.hpp +++ b/libuavcan/include/uavcan/util/compile_time.hpp @@ -23,7 +23,7 @@ struct StaticAssert * Usage: * ShowIntegerAsError::foobar(); */ -template struct ShowIntegerAsError; +template struct ShowIntegerAsError; /** * Prevents copying when inherited @@ -39,15 +39,21 @@ protected: /** * Compile time conditions */ -template +template struct EnableIf { }; -template -struct EnableIf { typedef T Type; }; +template +struct EnableIf +{ + typedef T Type; +}; -template -struct EnableIfType { typedef R Type; }; +template +struct EnableIfType +{ + typedef R Type; +}; template @@ -68,7 +74,7 @@ struct Select /** * Value types */ -template struct BooleanType { }; +template struct BooleanType { }; typedef BooleanType TrueType; typedef BooleanType FalseType; @@ -102,14 +108,14 @@ template To try_implicit_cast(const From& from, const To& default_) { return TryImplicitCastImpl::impl(from, default_, - BooleanType::Result>()); + BooleanType::Result>()); } template To try_implicit_cast(const From& from) { return TryImplicitCastImpl::impl(from, To(), - BooleanType::Result>()); + BooleanType::Result>()); } } diff --git a/libuavcan/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 394ab90635..751802917d 100644 --- a/libuavcan/include/uavcan/util/lazy_constructor.hpp +++ b/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -24,13 +24,17 @@ class LazyConstructor void ensureConstructed() const { if (!ptr_) + { handleFatalError("LazyConstructor is not constructed"); + } } void ensureNotConstructed() const { if (ptr_) + { handleFatalError("LazyConstructor is already constructed"); + } } template struct ParamType { typedef const U& Type; }; @@ -41,17 +45,19 @@ class LazyConstructor public: LazyConstructor() - : ptr_(NULL) + : ptr_(NULL) { std::fill(data_, data_ + sizeof(T), 0); } LazyConstructor(const LazyConstructor& rhs) - : ptr_(NULL) + : ptr_(NULL) { std::fill(data_, data_ + sizeof(T), 0); if (rhs) + { construct(*rhs); // Invoke copy constructor + } } ~LazyConstructor() { destroy(); } @@ -60,7 +66,9 @@ public: { destroy(); if (rhs) + { construct(*rhs); // Invoke copy constructor + } return *this; } @@ -77,7 +85,9 @@ public: void destroy() { if (ptr_) + { ptr_->~T(); + } ptr_ = NULL; std::fill(data_, data_ + sizeof(T), 0); } diff --git a/libuavcan/include/uavcan/util/method_binder.hpp b/libuavcan/include/uavcan/util/method_binder.hpp index 52d00ca5fe..031a2261e8 100644 --- a/libuavcan/include/uavcan/util/method_binder.hpp +++ b/libuavcan/include/uavcan/util/method_binder.hpp @@ -20,18 +20,20 @@ class MethodBinder void validateBeforeCall() const { if (!operator bool()) + { handleFatalError("Null method binder"); + } } public: MethodBinder() - : obj_() - , fun_() + : obj_() + , fun_() { } MethodBinder(ObjectPtr o, MemFunPtr f) - : obj_(o) - , fun_(f) + : obj_(o) + , fun_(f) { } operator bool() const diff --git a/libuavcan/src/data_type.cpp b/libuavcan/src/data_type.cpp index da52f71e5f..736da7ee86 100644 --- a/libuavcan/src/data_type.cpp +++ b/libuavcan/src/data_type.cpp @@ -18,7 +18,9 @@ void DataTypeSignature::mixin64(uint64_t x) { DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_); for (int i = 0; i < 64; i += 8) // LSB first + { crc.add((x >> i) & 0xFF); + } value_ = crc.get(); } @@ -33,7 +35,9 @@ TransferCRC DataTypeSignature::toTransferCRC() const { TransferCRC tcrc; for (int i = 0; i < 64; i += 8) // LSB first + { tcrc.add((value_ >> i) & 0xFF); + } return tcrc; } @@ -71,7 +75,7 @@ std::string DataTypeDescriptor::toString() const std::ostringstream os; os << full_name_ << ":" << id_.get() << kindch << ":" << std::hex - << std::setfill('0') << std::setw(16) << signature_.get(); + << std::setfill('0') << std::setw(16) << signature_.get(); return os.str(); } diff --git a/libuavcan/src/driver/can.cpp b/libuavcan/src/driver/can.cpp index efc3b8ad4a..8a71df50db 100644 --- a/libuavcan/src/driver/can.cpp +++ b/libuavcan/src/driver/can.cpp @@ -52,17 +52,23 @@ std::string CanFrame::toString(StringRepresentation mode) const else { for (int dlen = 0; dlen < dlc; dlen++) // hex bytes + { wpos += snprintf(wpos, epos - wpos, " %02x", (unsigned int)data[dlen]); + } while (mode == StrAligned && wpos < buf + ASCII_COLUMN_OFFSET) // alignment + { *wpos++ = ' '; + } wpos += snprintf(wpos, epos - wpos, " \'"); // ascii for (int dlen = 0; dlen < dlc; dlen++) { uint8_t ch = data[dlen]; if (ch < 0x20 || ch > 0x7E) + { ch = '.'; + } wpos += snprintf(wpos, epos - wpos, "%c", ch); } wpos += snprintf(wpos, epos - wpos, "\'"); diff --git a/libuavcan/src/marshal/bit_array_copy.cpp b/libuavcan/src/marshal/bit_array_copy.cpp index e1cc6fd641..83fc639cb1 100644 --- a/libuavcan/src/marshal/bit_array_copy.cpp +++ b/libuavcan/src/marshal/bit_array_copy.cpp @@ -15,7 +15,7 @@ } else { \ *dst &= reverse_mask[dst_offset_modulo] | \ reverse_mask_xor[dst_offset_modulo + src_len + 1]; \ - c &= reverse_mask[dst_offset_modulo + src_len]; \ + c &= reverse_mask[dst_offset_modulo + src_len]; \ src_len = 0; \ } } while (0) diff --git a/libuavcan/src/marshal/bit_stream.cpp b/libuavcan/src/marshal/bit_stream.cpp index d765041844..37211a8c5f 100644 --- a/libuavcan/src/marshal/bit_stream.cpp +++ b/libuavcan/src/marshal/bit_stream.cpp @@ -38,9 +38,13 @@ int BitStream::write(const uint8_t* bytes, const int bitlen) */ const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen); if (write_res < 0) + { return write_res; + } if (write_res < bytelen) + { return ResultOutOfBuffer; + } bit_offset_ = new_bit_offset; return ResultOk; @@ -55,9 +59,13 @@ int BitStream::read(uint8_t* bytes, const int bitlen) const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen); if (read_res < 0) + { return read_res; + } if (read_res < bytelen) + { return ResultOutOfBuffer; + } std::fill(bytes, bytes + bitlenToBytelen(bitlen), 0); copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0); @@ -72,14 +80,20 @@ std::string BitStream::toString() const { 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/src/marshal/float_spec.cpp b/libuavcan/src/marshal/float_spec.cpp index 57b8a0e870..ae1e3983a1 100644 --- a/libuavcan/src/marshal/float_spec.cpp +++ b/libuavcan/src/marshal/float_spec.cpp @@ -22,17 +22,27 @@ uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value) { uint16_t hbits = signbit(value) << 15; if (value == 0.0f) + { return hbits; + } if (isnanf(value)) + { return hbits | 0x7FFF; + } if (isinff(value)) + { return hbits | 0x7C00; + } int exp; std::frexp(value, &exp); if (exp > 16) + { return hbits | 0x7C00; + } if (exp < -13) + { value = std::ldexp(value, 24); + } else { value = std::ldexp(value, 11 - exp); @@ -50,14 +60,22 @@ float IEEE754Converter::halfToNativeNonIeee(uint16_t value) float out; int abs = value & 0x7FFF; if (abs > 0x7C00) + { out = std::numeric_limits::has_quiet_NaN ? std::numeric_limits::quiet_NaN() : 0.0f; + } else if (abs == 0x7C00) + { out = std::numeric_limits::has_infinity ? - std::numeric_limits::infinity() : std::numeric_limits::max(); + std::numeric_limits::infinity() : std::numeric_limits::max(); + } else if (abs > 0x3FF) + { out = std::ldexp(static_cast((value & 0x3FF) | 0x400), (abs >> 10) - 25); + } else + { out = std::ldexp(static_cast(abs), -24); + } return (value & 0x8000) ? -out : out; } diff --git a/libuavcan/src/node/global_data_type_registry.cpp b/libuavcan/src/node/global_data_type_registry.cpp index 2f1f7dd0c9..43e5c9c44c 100644 --- a/libuavcan/src/node/global_data_type_registry.cpp +++ b/libuavcan/src/node/global_data_type_registry.cpp @@ -35,11 +35,15 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd) return RegistResultInvalidParams; } if (isFrozen()) + { return RegistResultFrozen; + } List* list = selectList(dtd->descriptor.getKind()); if (!list) + { return RegistResultInvalidParams; + } list->remove(dtd); // If this call came from regist<>(), that would be enough Entry* p = list->get(); // But anyway @@ -47,7 +51,9 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd) { Entry* const next = p->getNextListNode(); if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName())) + { list->remove(p); + } p = next; } return RegistResultOk; @@ -61,20 +67,28 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::registImpl(Entry* d return RegistResultInvalidParams; } if (isFrozen()) + { return RegistResultFrozen; + } List* list = selectList(dtd->descriptor.getKind()); if (!list) + { return RegistResultInvalidParams; + } { // Collision check Entry* p = list->get(); while (p) { if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision + { return RegistResultCollision; + } if (!std::strcmp(p->descriptor.getFullName(), dtd->descriptor.getFullName())) // Name collision + { return RegistResultCollision; + } p = p->getNextListNode(); } } @@ -132,7 +146,9 @@ const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const while (p) { if (p->descriptor.match(kind, name)) + { return &p->descriptor; + } p = p->getNextListNode(); } return NULL; @@ -150,7 +166,9 @@ const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, DataTy while (p) { if (p->descriptor.match(kind, dtid)) + { return &p->descriptor; + } p = p->getNextListNode(); } return NULL; @@ -180,23 +198,31 @@ DataTypeSignature GlobalDataTypeRegistry::computeAggregateSignature(DataTypeKind if (inout_id_mask[dtid]) { if (signature_initialized) + { signature.extend(desc.getSignature()); + } else + { signature = DataTypeSignature(desc.getSignature()); + } signature_initialized = true; } assert(prev_dtid < dtid); // Making sure that list is ordered properly prev_dtid++; while (prev_dtid < dtid) + { inout_id_mask[prev_dtid++] = false; // Erasing bits for missing types + } assert(prev_dtid == dtid); p = p->getNextListNode(); } prev_dtid++; while (prev_dtid <= DataTypeID::Max) + { inout_id_mask[prev_dtid++] = false; + } return signature; } diff --git a/libuavcan/src/node/scheduler.cpp b/libuavcan/src/node/scheduler.cpp index 6937ac1641..2708082b7e 100644 --- a/libuavcan/src/node/scheduler.cpp +++ b/libuavcan/src/node/scheduler.cpp @@ -70,11 +70,15 @@ bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const { #if UAVCAN_DEBUG if (prev_deadline > p->getDeadline()) // Self check + { std::abort(); + } prev_deadline = p->getDeadline(); #endif if (p == mdh) + { return true; + } p = p->getNextListNode(); } return false; @@ -86,15 +90,21 @@ MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock) { DeadlineHandler* const mdh = handlers_.get(); if (!mdh) + { return sysclock.getMonotonic(); + } #if UAVCAN_DEBUG if (mdh->getNextListNode()) // Order check + { assert(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline()); + } #endif const MonotonicTime ts = sysclock.getMonotonic(); if (ts < mdh->getDeadline()) + { return ts; + } handlers_.remove(mdh); mdh->handleDeadline(ts); // This handler can be re-registered immediately @@ -107,7 +117,9 @@ MonotonicTime DeadlineScheduler::getEarliestDeadline() const { const DeadlineHandler* const mdh = handlers_.get(); if (mdh) + { return mdh->getDeadline(); + } return MonotonicTime::getMax(); } @@ -121,7 +133,9 @@ MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadli if (earliest > ts) { if (ts - earliest > deadline_resolution_) + { return ts + deadline_resolution_; + } } return earliest; } @@ -146,12 +160,16 @@ int Scheduler::spin(MonotonicTime deadline) const MonotonicTime dl = computeDispatcherSpinDeadline(deadline); retval = dispatcher_.spin(dl); if (retval < 0) + { break; + } const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock()); pollCleanup(ts, retval); if (ts >= deadline) + { break; + } } return retval; } diff --git a/libuavcan/src/node/timer.cpp b/libuavcan/src/node/timer.cpp index 93eb0fa822..7b8e115aa8 100644 --- a/libuavcan/src/node/timer.cpp +++ b/libuavcan/src/node/timer.cpp @@ -15,7 +15,9 @@ void Timer::handleDeadline(MonotonicTime current) const MonotonicTime scheduled_time = getDeadline(); if (period_ < MonotonicDuration::getInfinite()) + { startWithDeadline(scheduled_time + period_); + } // Application can re-register the timer with different params, it's OK handleTimerEvent(TimerEvent(scheduled_time, current)); diff --git a/libuavcan/src/protocol/data_type_info_provider.cpp b/libuavcan/src/protocol/data_type_info_provider.cpp index 5e6d8ec4a5..691853aa22 100644 --- a/libuavcan/src/protocol/data_type_info_provider.cpp +++ b/libuavcan/src/protocol/data_type_info_provider.cpp @@ -89,16 +89,20 @@ int DataTypeInfoProvider::start() res = cats_srv_.start( ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest)); if (res < 0) + { goto fail; + } res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); if (res < 0) + { goto fail; + } assert(res >= 0); return res; - fail: +fail: assert(res < 0); cats_srv_.stop(); gdti_srv_.stop(); diff --git a/libuavcan/src/protocol/node_status_provider.cpp b/libuavcan/src/protocol/node_status_provider.cpp index a494b381b9..eef38956a5 100644 --- a/libuavcan/src/protocol/node_status_provider.cpp +++ b/libuavcan/src/protocol/node_status_provider.cpp @@ -13,8 +13,8 @@ bool NodeStatusProvider::isNodeInfoInitialized() const { // Hardware version is not required return (node_info_.software_version != protocol::SoftwareVersion()) && - (node_info_.uavcan_version != protocol::SoftwareVersion()) && - (!node_info_.name.empty()); + (node_info_.uavcan_version != protocol::SoftwareVersion()) && + (!node_info_.name.empty()); } int NodeStatusProvider::publish() @@ -32,7 +32,9 @@ void NodeStatusProvider::publishWithErrorHandling() { const int res = publish(); if (res < 0) + { getNode().registerInternalFailure("NodeStatus publication failed"); + } } void NodeStatusProvider::handleTimerEvent(const TimerEvent&) @@ -67,21 +69,27 @@ int NodeStatusProvider::startAndPublish() res = publish(); // Initial broadcast if (res < 0) + { goto fail; + } res = gdr_sub_.start(GlobalDiscoveryRequestCallback(this, &NodeStatusProvider::handleGlobalDiscoveryRequest)); if (res < 0) + { goto fail; + } res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest)); if (res < 0) + { goto fail; + } Timer::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS)); return res; - fail: +fail: assert(res < 0); gdr_sub_.stop(); gni_srv_.stop(); diff --git a/libuavcan/src/protocol/panic_broadcaster.cpp b/libuavcan/src/protocol/panic_broadcaster.cpp index 1437ab14a3..1be2762bc3 100644 --- a/libuavcan/src/protocol/panic_broadcaster.cpp +++ b/libuavcan/src/protocol/panic_broadcaster.cpp @@ -29,7 +29,9 @@ void PanicBroadcaster::panic(const char* short_reason) while (p && *p) { if (msg_.reason_text.size() == msg_.reason_text.capacity()) + { break; + } msg_.reason_text.push_back(*p); p++; } diff --git a/libuavcan/src/transport/can_io.cpp b/libuavcan/src/transport/can_io.cpp index c07ba54aa5..afd01a71bc 100644 --- a/libuavcan/src/transport/can_io.cpp +++ b/libuavcan/src/transport/can_io.cpp @@ -19,7 +19,7 @@ std::string CanRxFrame::toString(StringRepresentation mode) const { std::ostringstream os; os << CanFrame::toString(mode) - << " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index); + << " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index); return os.str(); } @@ -39,14 +39,18 @@ void CanTxQueue::Entry::destroy(Entry*& obj, IAllocator& allocator) bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const { if (qos != rhs_qos) + { return qos > rhs_qos; + } return frame.priorityHigherThan(rhs_frame); } bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const { if (qos != rhs_qos) + { return qos < rhs_qos; + } return frame.priorityLowerThan(rhs_frame); } @@ -56,11 +60,15 @@ std::string CanTxQueue::Entry::toString() const switch (qos) { case Volatile: + { str_qos = " "; break; + } case Persistent: + { str_qos = " "; break; + } default: assert(0); str_qos = " "; @@ -85,7 +93,9 @@ CanTxQueue::~CanTxQueue() void CanTxQueue::registerRejectedFrame() { if (rejected_frames_cnt_ < std::numeric_limits::max()) + { rejected_frames_cnt_++; + } } void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags) @@ -130,7 +140,9 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, while (p) { if (lowestqos->qosHigherThan(*p)) + { lowestqos = p; + } p = p->getNextListNode(); } // Note that frame with *equal* QoS will be replaced too. @@ -145,8 +157,10 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, } if (praw == NULL) + { return; // Seems that there is no memory at all. + } Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags); assert(entry); queue_.insertBefore(entry, PriorityInsertionComparator(frame)); @@ -167,7 +181,9 @@ CanTxQueue::Entry* CanTxQueue::peek() p = next; } else + { return p; + } } return NULL; } @@ -187,7 +203,9 @@ bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const { const Entry* entry = queue_.get(); if (entry == NULL) + { return false; + } return !rhs_frame.priorityHigherThan(entry->frame); } @@ -217,10 +235,14 @@ int CanIOManager::sendFromTxQueue(int iface_index) assert(iface_index >= 0 && iface_index < MaxCanIfaces); CanTxQueue::Entry* entry = tx_queues_[iface_index].peek(); if (entry == NULL) + { return 0; + } const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags); if (res > 0) + { tx_queues_[iface_index].remove(entry); + } return res; } @@ -230,7 +252,9 @@ int CanIOManager::makePendingTxMask() const for (int i = 0; i < getNumIfaces(); i++) { if (!tx_queues_[i].isEmpty()) + { write_mask |= 1 << i; + } } return write_mask; } @@ -254,27 +278,33 @@ uint64_t CanIOManager::getNumErrors(int iface_index) const } int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, - int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) + int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) { const int num_ifaces = getNumIfaces(); const int all_ifaces_mask = (1 << num_ifaces) - 1; iface_mask &= all_ifaces_mask; if (blocking_deadline > tx_deadline) + { blocking_deadline = tx_deadline; + } int retval = 0; while (true) { if (iface_mask == 0) + { break; + } CanSelectMasks masks; masks.write = iface_mask | makePendingTxMask(); { const int select_res = driver_.select(masks, blocking_deadline); if (select_res < 0) + { return select_res; + } assert(masks.read == 0); } @@ -294,7 +324,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton { res = sendToIface(i, frame, tx_deadline, flags); if (res > 0) + { iface_mask &= ~(1 << i); // Mark transmitted + } } } else @@ -302,7 +334,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton res = sendFromTxQueue(i); } if (res > 0) + { retval++; + } } } @@ -318,7 +352,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton for (int i = 0; i < num_ifaces; i++) { if (iface_mask & (1 << i)) + { tx_queues_[i].push(frame, tx_deadline, qos, flags); + } } break; } @@ -338,14 +374,18 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline { const int select_res = driver_.select(masks, blocking_deadline); if (select_res < 0) + { return select_res; + } } // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call for (int i = 0; i < num_ifaces; i++) { if (masks.write & (1 << i)) + { sendFromTxQueue(i); + } } // Read @@ -372,7 +412,9 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline // Timeout checked in the last order - this way we can operate with expired deadline: if (sysclock_.getMonotonic() >= blocking_deadline) + { break; + } } return 0; } diff --git a/libuavcan/src/transport/dispatcher.cpp b/libuavcan/src/transport/dispatcher.cpp index cca313ed35..981d8708e9 100644 --- a/libuavcan/src/transport/dispatcher.cpp +++ b/libuavcan/src/transport/dispatcher.cpp @@ -48,7 +48,9 @@ bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* l while (p) { if (p == listener) + { return true; + } p = p->getNextListNode(); } return false; @@ -76,7 +78,9 @@ bool Dispatcher::ListenerRegistry::add(TransferListenerBase* listener, Mode mode while (p) { if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID()) + { return false; + } p = p->getNextListNode(); } } @@ -96,7 +100,9 @@ bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const while (p) { if (p->getDataTypeDescriptor().getID() == dtid) + { return true; + } p = p->getNextListNode(); } return false; @@ -118,9 +124,13 @@ void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame) while (p) { if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID()) + { p->handleFrame(frame); + } else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id! + { break; + } p = p->getNextListNode(); } } @@ -147,16 +157,22 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) { case TransferTypeMessageBroadcast: case TransferTypeMessageUnicast: + { lmsg_.handleFrame(frame); break; + } case TransferTypeServiceRequest: + { lsrv_req_.handleFrame(frame); break; + } case TransferTypeServiceResponse: + { lsrv_resp_.handleFrame(frame); break; + } default: assert(0); @@ -185,7 +201,9 @@ int Dispatcher::spin(MonotonicTime deadline) CanRxFrame frame; const int res = canio_.receive(frame, deadline, flags); if (res < 0) + { return res; + } if (res > 0) { if (flags & CanIOFlagLoopback) @@ -283,8 +301,8 @@ bool Dispatcher::hasSubscriber(DataTypeID dtid) const bool Dispatcher::hasPublisher(DataTypeID dtid) const { - return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast) - || outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast); + return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast) || + outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast); } bool Dispatcher::hasServer(DataTypeID dtid) const diff --git a/libuavcan/src/transport/frame.cpp b/libuavcan/src/transport/frame.cpp index 16f152257a..ed592e5ff4 100644 --- a/libuavcan/src/transport/frame.cpp +++ b/libuavcan/src/transport/frame.cpp @@ -18,15 +18,15 @@ int Frame::getMaxPayloadLen() const switch (getTransferType()) { case TransferTypeMessageBroadcast: + { return sizeof(payload_); - break; - + } case TransferTypeServiceResponse: case TransferTypeServiceRequest: case TransferTypeMessageUnicast: + { return sizeof(payload_) - 1; - break; - + } default: assert(0); return -1; @@ -53,7 +53,9 @@ inline static uint32_t bitunpack(uint32_t val) bool Frame::parse(const CanFrame& can_frame) { if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended()) + { return false; + } if (can_frame.dlc > sizeof(CanFrame::data)) { @@ -78,22 +80,30 @@ bool Frame::parse(const CanFrame& can_frame) switch (transfer_type_) { case TransferTypeMessageBroadcast: + { dst_node_id_ = NodeID::Broadcast; payload_len_ = can_frame.dlc; std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload_); break; + } case TransferTypeServiceResponse: case TransferTypeServiceRequest: case TransferTypeMessageUnicast: + { if (can_frame.dlc < 1) + { return false; + } if (can_frame.data[0] & 0x80) // RESERVED, must be zero + { return false; + } dst_node_id_ = can_frame.data[0] & 0x7F; payload_len_ = can_frame.dlc - 1; std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_); break; + } default: return false; @@ -128,18 +138,22 @@ bool Frame::compile(CanFrame& out_can_frame) const switch (transfer_type_) { case TransferTypeMessageBroadcast: + { out_can_frame.dlc = payload_len_; std::copy(payload_, payload_ + payload_len_, out_can_frame.data); break; + } case TransferTypeServiceResponse: case TransferTypeServiceRequest: case TransferTypeMessageUnicast: + { assert((payload_len_ + 1) <= sizeof(CanFrame::data)); out_can_frame.data[0] = dst_node_id_.get(); out_can_frame.dlc = payload_len_ + 1; std::copy(payload_, payload_ + payload_len_, out_can_frame.data + 1); break; + } default: assert(0); @@ -200,7 +214,9 @@ std::string Frame::toString() const { ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload_[i]); if ((i + 1) < payload_len_) + { ofs += std::snprintf(buf + ofs, BUFLEN - ofs, " "); + } } ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "]"); return std::string(buf); diff --git a/libuavcan/src/transport/transfer.cpp b/libuavcan/src/transport/transfer.cpp index 0fa81ad180..f42d0e83e1 100644 --- a/libuavcan/src/transport/transfer.cpp +++ b/libuavcan/src/transport/transfer.cpp @@ -22,7 +22,9 @@ int TransferID::computeForwardDistance(TransferID rhs) const { int d = int(rhs.get()) - int(get()); if (d < 0) + { d += 1 << BitLen; + } assert(((get() + d) & Max) == rhs.get()); return d; diff --git a/libuavcan/src/transport/transfer_buffer.cpp b/libuavcan/src/transport/transfer_buffer.cpp index ba477f26ba..b3b61bd6fc 100644 --- a/libuavcan/src/transport/transfer_buffer.cpp +++ b/libuavcan/src/transport/transfer_buffer.cpp @@ -27,7 +27,9 @@ DynamicTransferBufferManagerEntry::Block* DynamicTransferBufferManagerEntry::Blo { void* const praw = allocator.allocate(sizeof(Block)); if (praw == NULL) + { return NULL; + } return new (praw) Block; } @@ -77,7 +79,9 @@ DynamicTransferBufferManagerEntry* DynamicTransferBufferManagerEntry::instantiat { void* const praw = allocator.allocate(sizeof(DynamicTransferBufferManagerEntry)); if (praw == NULL) + { return NULL; + } return new (praw) DynamicTransferBufferManagerEntry(allocator, max_size); } @@ -112,9 +116,13 @@ int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data, return -1; } if (offset >= max_write_pos_) + { return 0; + } if ((offset + len) > max_write_pos_) + { len = max_write_pos_ - offset; + } assert((offset + len) <= max_write_pos_); // This shall be optimized. @@ -125,7 +133,9 @@ int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data, { p->read(outptr, offset, total_offset, left_to_read); if (left_to_read == 0) + { break; + } p = p->getNextListNode(); } @@ -142,9 +152,13 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t* } if (offset >= max_size_) + { return 0; + } if ((offset + len) > max_size_) + { len = max_size_ - offset; + } assert((offset + len) <= max_size_); unsigned int total_offset = 0; @@ -159,7 +173,9 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t* last_written_block = p; p->write(inptr, offset, total_offset, left_to_write); if (left_to_write == 0) + { break; + } p = p->getNextListNode(); } @@ -172,8 +188,10 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t* // Allocating the chunk Block* new_block = Block::instantiate(allocator_); if (new_block == NULL) + { break; // We're in deep shit. + } // Appending the chain with the new block if (last_written_block != NULL) { diff --git a/libuavcan/src/transport/transfer_listener.cpp b/libuavcan/src/transport/transfer_listener.cpp index 411f174310..95f937cbe3 100644 --- a/libuavcan/src/transport/transfer_listener.cpp +++ b/libuavcan/src/transport/transfer_listener.cpp @@ -22,10 +22,10 @@ int IncomingTransfer::write(unsigned int, const uint8_t*, unsigned int) * SingleFrameIncomingTransfer */ SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm) -: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(), - frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex()) -, payload_(frm.getPayloadPtr()) -, payload_len_(frm.getPayloadLen()) + : IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(), + frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex()) + , payload_(frm.getPayloadPtr()) + , payload_len_(frm.getPayloadLen()) { assert(frm.isValid()); } @@ -38,9 +38,13 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign return -1; } if (offset >= payload_len_) + { return 0; + } if ((offset + len) > payload_len_) + { len = payload_len_ - offset; + } assert((offset + len) <= payload_len_); std::copy(payload_ + offset, payload_ + offset + len, data); return len; @@ -51,9 +55,9 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign */ MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, const RxFrame& last_frame, TransferBufferAccessor& tba) -: IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(), - last_frame.getSrcNodeID(), last_frame.getIfaceIndex()) -, buf_acc_(tba) + : IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(), + last_frame.getSrcNodeID(), last_frame.getIfaceIndex()) + , buf_acc_(tba) { assert(last_frame.isValid()); assert(last_frame.isLast()); @@ -87,7 +91,9 @@ bool TransferListenerBase::checkPayloadCrc(const uint16_t compare_with, const IT return false; } if (res == 0) + { break; + } offset += res; crc.add(buf, res); } @@ -106,8 +112,10 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF switch (receiver.addFrame(frame, tba)) { case TransferReceiver::ResultNotComplete: + { return; + } case TransferReceiver::ResultSingleFrame: { SingleFrameIncomingTransfer it(frame); @@ -138,9 +146,11 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF } default: + { assert(0); break; } + } } } diff --git a/libuavcan/src/transport/transfer_receiver.cpp b/libuavcan/src/transport/transfer_receiver.cpp index cab435a4c9..ababd67a4f 100644 --- a/libuavcan/src/transport/transfer_receiver.cpp +++ b/libuavcan/src/transport/transfer_receiver.cpp @@ -21,9 +21,13 @@ TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& fr { const int distance = tid_.computeForwardDistance(frame.getTransferID()); if (distance == 0) + { return TidSame; + } if (distance < ((1 << TransferID::BitLen) / 2)) + { return TidFuture; + } return TidRepeat; } @@ -53,7 +57,9 @@ void TransferReceiver::prepareForNextTransfer() bool TransferReceiver::validate(const RxFrame& frame) const { if (iface_index_ != frame.getIfaceIndex()) + { return false; + } if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes)) { @@ -90,15 +96,19 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf) if (frame.isFirst()) // First frame contains CRC, we need to extract it now { if (frame.getPayloadLen() < TransferCRC::NumBytes) + { return false; // Must have been validated earlier though. I think I'm paranoid. + } this_transfer_crc_ = (payload[0] & 0xFF) | (uint16_t(payload[1] & 0xFF) << 8); // Little endian. const int effective_payload_len = payload_len - TransferCRC::NumBytes; const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len); const bool success = res == effective_payload_len; if (success) + { buffer_write_pos_ += effective_payload_len; + } return success; } else @@ -106,7 +116,9 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf) const int res = buf.write(buffer_write_pos_, payload, payload_len); const bool success = res == payload_len; if (success) + { buffer_write_pos_ += payload_len; + } return success; } } @@ -132,7 +144,9 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra // Payload write ITransferBuffer* buf = tba.access(); if (buf == NULL) + { buf = tba.create(); + } if (buf == NULL) { UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str()); @@ -161,7 +175,9 @@ bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const { static const uint64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1; if (current_ts <= this_transfer_ts_) + { return false; + } return (current_ts - this_transfer_ts_).toUSec() > (uint64_t(transfer_interval_usec_) * INTERVAL_MULT); } @@ -209,7 +225,9 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr } if (!validate(frame)) + { return ResultNotComplete; + } return receive(frame, tba); } diff --git a/libuavcan/src/transport/transfer_sender.cpp b/libuavcan/src/transport/transfer_sender.cpp index 7f4d32dae1..85301f7d50 100644 --- a/libuavcan/src/transport/transfer_sender.cpp +++ b/libuavcan/src/transport/transfer_sender.cpp @@ -59,11 +59,15 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime { const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_); if (send_res < 0) + { return send_res; + } if (frame.isLast()) + { return next_frame_index; // Number of frames transmitted + } frame.setIndex(next_frame_index++); const int write_res = frame.setPayload(payload + offset, payload_len - offset); @@ -76,7 +80,9 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime offset += write_res; assert(offset <= payload_len); if (offset >= payload_len) + { frame.makeLast(); + } } }