diff --git a/libuavcan/include/uavcan/internal/marshalling/array.hpp b/libuavcan/include/uavcan/internal/marshalling/array.hpp new file mode 100644 index 0000000000..b42b8df672 --- /dev/null +++ b/libuavcan/include/uavcan/internal/marshalling/array.hpp @@ -0,0 +1,290 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +enum ArrayMode { ArrayModeStatic, ArrayModeDynamic }; + + +template +class StaticArrayBase +{ +public: + typedef unsigned int SizeType; + SizeType size() const { return Size; } + SizeType capacity() const { return Size; } + +protected: + StaticArrayBase() { } + ~StaticArrayBase() { } + + SizeType validateRange(SizeType pos) const + { + if (pos < Size) + return pos; +#if UAVCAN_EXCEPTIONS + throw std::out_of_range(typeid(*this).name()); +#else + assert(0); + return Size - 1; // Ha ha +#endif + } +}; + + +template +class DynamicArrayBase +{ +protected: + typedef IntegerSpec::Result, SignednessUnsigned, CastModeSaturate> RawSizeType; +public: + typedef typename StorageType::Type SizeType; + +private: + SizeType size_; + +protected: + DynamicArrayBase() : size_(0) { } + ~DynamicArrayBase() { } + + SizeType validateRange(SizeType pos) const + { + if (pos < size_) + return pos; +#if UAVCAN_EXCEPTIONS + throw std::out_of_range(typeid(*this).name()); +#else + assert(0); + return (size_ == 0) ? 0 : (size_ - 1); +#endif + } + + void grow() + { + if (size_ >= MaxSize) + validateRange(MaxSize); // Will throw, assert() or do nothing + else + size_++; + } + + void shrink() + { + if (size_ > 0) + size_--; + } + +public: + SizeType size() const + { + assert(size_ <= MaxSize); + return size_; + } + + SizeType capacity() const { return MaxSize; } +}; + +/** + * Statically allocated array with optional dynamic-like behavior + */ +template +class ArrayImpl : public StaticIf, + StaticArrayBase >::Result +{ + typedef ArrayImpl SelfType; + typedef typename StaticIf, + StaticArrayBase >::Result Base; + + typename StorageType::Type data_[MaxSize]; + + template + typename EnableIf::Type initialize(int) { std::fill(data_, data_ + MaxSize, U()); } + template void initialize(...) { } + +public: + typedef typename StorageType::Type ValueType; + typedef typename Base::SizeType SizeType; + + using Base::size; + using Base::capacity; + + ArrayImpl() { initialize(0); } + + ValueType& at(SizeType pos) { return data_[validateRange(pos)]; } + const ValueType& at(SizeType pos) const { return data_[Base::validateRange(pos)]; } + + ValueType& operator[](SizeType pos) { return at(pos); } + const ValueType& operator[](SizeType pos) const { return at(pos); } + + ValueType* begin() { return data_; } + const ValueType* begin() const { return data_; } + ValueType* end() { return data_ + Base::size(); } + const ValueType* end() const { return data_ + Base::size(); } + + ValueType& front() { return at(0); } + const ValueType& front() const { return at(0); } + ValueType& back() { return at(Base::size() - 1); } + const ValueType& back() const { return at(Base::size() - 1); } + + template + bool operator<(const R& rhs) const + { + return std::lexicographical_compare(begin(), end(), rhs.begin(), rhs.end()); + } + + typedef ValueType* iterator; + typedef const ValueType* const_iterator; +}; + +/** + * Bit array specialization + */ +template +class ArrayImpl, ArrayMode, MaxSize> + : public std::bitset + , public StaticIf, StaticArrayBase >::Result +{ + typedef typename StaticIf, + StaticArrayBase >::Result ArrayBase; + +public: + typedef typename std::bitset::reference Reference; + typedef typename ArrayBase::SizeType SizeType; + + using ArrayBase::size; + using ArrayBase::capacity; + + Reference at(SizeType pos) { return std::bitset::operator[](ArrayBase::validateRange(pos)); } + bool at(SizeType pos) const { return std::bitset::operator[](ArrayBase::validateRange(pos)); } + + Reference operator[](SizeType pos) { return at(pos); } + bool operator[](SizeType pos) const { return at(pos); } +}; + +/** + * Zero length arrays are not allowed + */ +template class ArrayImpl; + + +template +class Array : public ArrayImpl +{ + typedef ArrayImpl Base; + typedef Array SelfType; + + int encodeSize(ScalarCodec&, FalseType) const { return 1; } + int encodeSize(ScalarCodec& codec, TrueType) const // TODO: Tail array optimization + { + return Base::RawSizeType::encode(size(), codec); + } + + int decodeSize(ScalarCodec&, FalseType) { return 1; } + int decodeSize(ScalarCodec& codec, TrueType) + { + typename StorageType::Type sz = 0; + const int res = Base::RawSizeType::decode(sz, codec); + if (res > 0) + resize(sz); + return res; + } + +public: + typedef T RawValueType; + typedef typename StorageType::Type ValueType; + typedef typename Base::SizeType SizeType; + + using Base::size; + + enum { IsDynamic = ArrayMode == ArrayModeDynamic }; + enum { MaxSize = MaxSize_ }; + enum { MinBitLen = IsDynamic ? 0 : (RawValueType::MinBitLen * MaxSize) }; + + static int encode(const SelfType& array, ScalarCodec& codec) + { + const int res_sz = array.encodeSize(codec, BooleanType()); + if (res_sz <= 0) + return res_sz; + for (SizeType i = 0; i < array.size(); i++) + { + const int res = RawValueType::encode(array[i], codec); + if (res <= 0) + return res; + } + return 1; + } + + static int decode(SelfType& array, ScalarCodec& codec) + { + const int res_sz = array.decodeSize(codec, BooleanType()); + if (res_sz <= 0) + return res_sz; + for (SizeType i = 0; i < array.size(); i++) + { + // TODO: avoid excessive copy + ValueType value; + const int res = RawValueType::decode(value, codec); + array[i] = value; + if (res <= 0) + return res; + } + return 1; + } + + bool empty() const { return size() == 0; } + + void pop_back() { Base::shrink(); } + void push_back(const ValueType& value) + { + Base::grow(); + Base::at(size() - 1) = value; + } + + void resize(SizeType new_size, ValueType filler = ValueType()) + { + if (new_size > size()) + { + 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(); + } + } + + void clear() { resize(0); } + + template + bool 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; + } + template bool operator!=(const R& rhs) const { return !operator==(rhs); } + + typedef ValueType value_type; + typedef SizeType size_type; +}; + +} diff --git a/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp b/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp index 9d7f4f13b3..6929e4e785 100644 --- a/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp +++ b/libuavcan/include/uavcan/internal/marshalling/float_spec.hpp @@ -99,6 +99,7 @@ class FloatSpec : public IEEE754Limits public: enum { BitLen = BitLen_ }; + enum { MinBitLen = BitLen }; typedef typename NativeFloatSelector::Type StorageType; diff --git a/libuavcan/include/uavcan/internal/marshalling/integer_spec.hpp b/libuavcan/include/uavcan/internal/marshalling/integer_spec.hpp index 3aecd0edf1..ab5958dbab 100644 --- a/libuavcan/include/uavcan/internal/marshalling/integer_spec.hpp +++ b/libuavcan/include/uavcan/internal/marshalling/integer_spec.hpp @@ -24,6 +24,7 @@ class IntegerSpec public: enum { BitLen = BitLen_ }; + enum { MinBitLen = BitLen }; typedef typename StaticIf<(BitLen <= 8), typename StaticIf::Result, typename StaticIf<(BitLen <= 16), typename StaticIf::Result, diff --git a/libuavcan/include/uavcan/internal/marshalling/static_array.hpp b/libuavcan/include/uavcan/internal/marshalling/static_array.hpp deleted file mode 100644 index 1bc405ef76..0000000000 --- a/libuavcan/include/uavcan/internal/marshalling/static_array.hpp +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace uavcan -{ - -template -class StaticArray -{ - typedef StaticArray SelfType; - -public: - enum { IsDynamic = 0 }; - enum { Size = Size_ }; - - typedef T RawValueType; - typedef typename StorageType::Type ValueType; - typedef unsigned int SizeType; - -private: - ValueType data_[Size]; - - void checkOrAdjustRange(SizeType& pos) const - { - if (pos < Size) - return; -#if UAVCAN_EXCEPTIONS - throw std::out_of_range(typeid(*this).name()); -#else - assert(0); - pos = Size - 1; // Ha ha -#endif - } - - template - typename EnableIf::Type initialize(int) { std::fill(begin(), end(), U()); } - template void initialize(...) { } - -public: - StaticArray() { initialize(0); } - - static int encode(const SelfType& array, ScalarCodec& codec) - { - for (SizeType i = 0; i < Size; i++) - { - const int res = RawValueType::encode(array.data_[i], codec); - if (res <= 0) - return res; - } - return 1; - } - - static int decode(SelfType& array, ScalarCodec& codec) - { - for (SizeType i = 0; i < Size; i++) - { - const int res = RawValueType::decode(array.data_[i], codec); - if (res <= 0) - return res; - } - return 1; - } - - template - bool operator==(const R& rhs) const - { - return (size() == rhs.size()) && std::equal(begin(), end(), rhs.begin()); - } - template bool operator!=(const R& rhs) const { return !operator==(rhs); } - - template - bool operator<(const R& rhs) const - { - return std::lexicographical_compare(begin(), end(), rhs.begin(), rhs.end()); - } - - ValueType& at(SizeType pos) - { - checkOrAdjustRange(pos); - return data_[pos]; - } - const ValueType& at(SizeType pos) const - { - checkOrAdjustRange(pos); - return data_[pos]; - } - - ValueType& operator[](SizeType pos) { return at(pos); } - const ValueType& operator[](SizeType pos) const { return at(pos); } - - ValueType* begin() { return data_; } - ValueType* end() { return data_ + Size; } - const ValueType* begin() const { return data_; } - const ValueType* end() const { return data_ + Size; } - - SizeType size() const { return Size; } - ValueType* data() { return data_; } - const ValueType* data() const { return data_; } - - /** - * STL compatibility - */ - typedef ValueType value_type; - typedef SizeType size_type; - typedef ValueType* iterator; - typedef const ValueType* const_iterator; -}; - - -/// Special case - bit array -template -class StaticArray, Size_ > : public std::bitset -{ -public: - enum { IsDynamic = 0 }; - enum { Size = Size_ }; - - typedef IntegerSpec<1, SignednessUnsigned, CastMode> RawValueType; - - static int encode(const StaticArray& array, ScalarCodec& codec) - { - for (std::size_t i = 0; i < Size; i++) - { - const int res = RawValueType::encode(bool(array[i]), codec); - if (res <= 0) - return res; - } - return 1; - } - - static int decode(StaticArray& array, ScalarCodec& codec) - { - for (std::size_t i = 0; i < Size; i++) - { - typename StorageType::Type value = 0; - const int res = RawValueType::decode(value, codec); - array[i] = value; - if (res <= 0) - return res; - } - return 1; - } -}; - - -template class StaticArray; // Invalid instantiation - -} diff --git a/libuavcan/include/uavcan/internal/marshalling/type_util.hpp b/libuavcan/include/uavcan/internal/marshalling/type_util.hpp index 5d6d2d2a12..82b28b8d4a 100644 --- a/libuavcan/include/uavcan/internal/marshalling/type_util.hpp +++ b/libuavcan/include/uavcan/internal/marshalling/type_util.hpp @@ -12,6 +12,12 @@ namespace uavcan enum CastMode { CastModeSaturate, CastModeTruncate }; +template +struct IntegerBitLen { enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; }; +template <> +struct IntegerBitLen<0> { enum { Result = 0 }; }; + + template struct StorageTypeImpl { typedef T Type; }; diff --git a/libuavcan/include/uavcan/internal/marshalling/types.hpp b/libuavcan/include/uavcan/internal/marshalling/types.hpp index 207323ab8a..f3de6374f5 100644 --- a/libuavcan/include/uavcan/internal/marshalling/types.hpp +++ b/libuavcan/include/uavcan/internal/marshalling/types.hpp @@ -6,5 +6,5 @@ #include #include -#include +#include #include diff --git a/libuavcan/test/marshalling/static_array.cpp b/libuavcan/test/marshalling/array.cpp similarity index 76% rename from libuavcan/test/marshalling/static_array.cpp rename to libuavcan/test/marshalling/array.cpp index 7cf6beb06a..b158915aac 100644 --- a/libuavcan/test/marshalling/static_array.cpp +++ b/libuavcan/test/marshalling/array.cpp @@ -10,7 +10,11 @@ struct CustomType { typedef uavcan::IntegerSpec<8, uavcan::SignednessSigned, uavcan::CastModeTruncate> A; typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> B; - typedef uavcan::StaticArray, 8> C; + // Dynamic array of max len 5 --> 3 bits for len, 5 bits for data --> 1 byte max len + typedef uavcan::Array, + uavcan::ArrayModeDynamic, 5> C; + + enum { MinBitLen = A::MinBitLen + B::MinBitLen + C::MinBitLen }; uavcan::StorageType::Type a; uavcan::StorageType::Type b; @@ -60,9 +64,23 @@ struct CustomType }; +TEST(StaticArray, IntegerBitLen) +{ + using uavcan::IntegerBitLen; + + ASSERT_EQ(0, IntegerBitLen<0>::Result); + ASSERT_EQ(1, IntegerBitLen<1>::Result); + ASSERT_EQ(6, IntegerBitLen<42>::Result); + ASSERT_EQ(8, IntegerBitLen<232>::Result); + ASSERT_EQ(32, IntegerBitLen<0x81234567>::Result); +} + + TEST(StaticArray, Basic) { - using uavcan::StaticArray; + using uavcan::Array; + using uavcan::ArrayModeDynamic; + using uavcan::ArrayModeStatic; using uavcan::IntegerSpec; using uavcan::FloatSpec; using uavcan::SignednessSigned; @@ -70,9 +88,9 @@ TEST(StaticArray, Basic) using uavcan::CastModeSaturate; using uavcan::CastModeTruncate; - typedef StaticArray, 4> A1; - typedef StaticArray, 2> A2; - typedef StaticArray A3; + typedef Array, ArrayModeStatic, 4> A1; + typedef Array, ArrayModeStatic, 2> A2; + typedef Array A3; A1 a1; A2 a2; @@ -93,8 +111,7 @@ TEST(StaticArray, Basic) { ASSERT_EQ(0, it->a); ASSERT_EQ(0, it->b); - for (int i = 0; i < 8; i++) - ASSERT_EQ(0, it->c[i]); + ASSERT_EQ(0, it->c.size()); } /* @@ -110,8 +127,8 @@ TEST(StaticArray, Basic) { a3[i].a = i; a3[i].b = i; - for (int i2 = 0; i2 < 8; i2++) - a3[i].c[i2] = i2 & 1; + for (int i2 = 0; i2 < 5; i2++) + a3[i].c.push_back(i2 & 1); } /* @@ -130,8 +147,8 @@ TEST(StaticArray, Basic) static const std::string Reference = "00000000 00000001 00000010 00000011 " // A1 (0, 1, 2, 3) "00000000 00000000 00000000 00111100 " // A2 (0, 1) - "00000000 00000000 00000000 01010101 " // A3[0] (0, 0, bool[8]) - "00000001 00000000 00111100 01010101"; // A3[1] (1, 1, bool[8]) + "00000000 00000000 00000000 10101010 " // A3[0] (0, 0, bool[5]) + "00000001 00000000 00111100 10101010"; // A3[1] (1, 1, bool[5]) ASSERT_EQ(Reference, bs_wr.toString());