mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-01 20:51:35 +08:00
Some more renamings: marshalling --> marshal
This commit is contained in:
Pavel Kirienko
@@ -0,0 +1,342 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <bitset>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <typeinfo>
|
||||
#include <uavcan/internal/impl_constants.hpp>
|
||||
#include <uavcan/internal/util.hpp>
|
||||
#include <uavcan/internal/marshal/type_util.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
enum ArrayMode { ArrayModeStatic, ArrayModeDynamic };
|
||||
|
||||
|
||||
template <unsigned int Size>
|
||||
class StaticArrayBase
|
||||
{
|
||||
public:
|
||||
enum { SizeBitLen = 0 };
|
||||
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 <unsigned int MaxSize>
|
||||
class DynamicArrayBase
|
||||
{
|
||||
protected:
|
||||
typedef IntegerSpec<IntegerBitLen<MaxSize>::Result, SignednessUnsigned, CastModeSaturate> RawSizeType;
|
||||
public:
|
||||
typedef typename StorageType<RawSizeType>::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:
|
||||
enum { SizeBitLen = RawSizeType::BitLen };
|
||||
|
||||
SizeType size() const
|
||||
{
|
||||
assert(size_ ? ((size_ - 1u) <= (MaxSize - 1u)) : 1); // -Werror=type-limits
|
||||
return size_;
|
||||
}
|
||||
|
||||
SizeType capacity() const { return MaxSize; }
|
||||
|
||||
void clear() { size_ = 0; }
|
||||
};
|
||||
|
||||
/**
|
||||
* Statically allocated array with optional dynamic-like behavior
|
||||
*/
|
||||
template <typename T, ArrayMode ArrayMode, unsigned int MaxSize>
|
||||
class ArrayImpl : public StaticIf<ArrayMode == ArrayModeDynamic,
|
||||
DynamicArrayBase<MaxSize>,
|
||||
StaticArrayBase<MaxSize> >::Result
|
||||
{
|
||||
typedef ArrayImpl<T, ArrayMode, MaxSize> SelfType;
|
||||
typedef typename StaticIf<ArrayMode == ArrayModeDynamic,
|
||||
DynamicArrayBase<MaxSize>,
|
||||
StaticArrayBase<MaxSize> >::Result Base;
|
||||
|
||||
typename StorageType<T>::Type data_[MaxSize];
|
||||
|
||||
template <typename U>
|
||||
typename EnableIf<sizeof(U(0) == U())>::Type initialize(int) { std::fill(data_, data_ + MaxSize, U()); }
|
||||
template <typename> void initialize(...) { }
|
||||
|
||||
public:
|
||||
typedef typename StorageType<T>::Type ValueType;
|
||||
typedef typename Base::SizeType SizeType;
|
||||
|
||||
using Base::size;
|
||||
using Base::capacity;
|
||||
|
||||
ArrayImpl() { initialize<ValueType>(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 <typename R>
|
||||
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 <unsigned int MaxSize, ArrayMode ArrayMode, CastMode CastMode>
|
||||
class ArrayImpl<IntegerSpec<1, SignednessUnsigned, CastMode>, ArrayMode, MaxSize>
|
||||
: public std::bitset<MaxSize>
|
||||
, public StaticIf<ArrayMode == ArrayModeDynamic, DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result
|
||||
{
|
||||
typedef typename StaticIf<ArrayMode == ArrayModeDynamic,
|
||||
DynamicArrayBase<MaxSize>,
|
||||
StaticArrayBase<MaxSize> >::Result ArrayBase;
|
||||
|
||||
public:
|
||||
typedef typename std::bitset<MaxSize>::reference Reference;
|
||||
typedef typename ArrayBase::SizeType SizeType;
|
||||
|
||||
using ArrayBase::size;
|
||||
using ArrayBase::capacity;
|
||||
|
||||
Reference at(SizeType pos) { return std::bitset<MaxSize>::operator[](ArrayBase::validateRange(pos)); }
|
||||
bool at(SizeType pos) const { return std::bitset<MaxSize>::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 <typename T, ArrayMode ArrayMode> class ArrayImpl<T, ArrayMode, 0>;
|
||||
|
||||
|
||||
template <typename T, ArrayMode ArrayMode, unsigned int MaxSize_>
|
||||
class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
{
|
||||
typedef ArrayImpl<T, ArrayMode, MaxSize_> Base;
|
||||
typedef Array<T, ArrayMode, MaxSize_> SelfType;
|
||||
|
||||
static bool isOptimizedTailArray(TailArrayOptimizationMode tao_mode)
|
||||
{
|
||||
return (T::MinBitLen >= 8) && (tao_mode == TailArrayOptEnabled);
|
||||
}
|
||||
|
||||
int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) const /// Static
|
||||
{
|
||||
assert(size() > 0);
|
||||
for (SizeType i = 0; i < size(); i++)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) const /// Dynamic
|
||||
{
|
||||
StaticAssert<IsDynamic>::check();
|
||||
const bool self_tao_enabled = isOptimizedTailArray(tao_mode);
|
||||
if (!self_tao_enabled)
|
||||
{
|
||||
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());
|
||||
}
|
||||
|
||||
int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) /// Static
|
||||
{
|
||||
assert(size() > 0);
|
||||
for (SizeType i = 0; i < size(); i++)
|
||||
{
|
||||
const bool last_item = i == (size() - 1);
|
||||
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;
|
||||
}
|
||||
|
||||
int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) /// Dynamic
|
||||
{
|
||||
StaticAssert<IsDynamic>::check();
|
||||
Base::clear();
|
||||
if (isOptimizedTailArray(tao_mode))
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
typename StorageType<typename Base::RawSizeType>::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
|
||||
return -1;
|
||||
}
|
||||
|
||||
public:
|
||||
typedef T RawValueType;
|
||||
typedef typename StorageType<T>::Type ValueType;
|
||||
typedef typename Base::SizeType SizeType;
|
||||
|
||||
using Base::size;
|
||||
|
||||
enum { IsDynamic = ArrayMode == ArrayModeDynamic };
|
||||
enum { MaxSize = MaxSize_ };
|
||||
enum { MinBitLen = IsDynamic ? 0 : (RawValueType::MinBitLen * MaxSize) };
|
||||
enum { MaxBitLen = Base::SizeBitLen + RawValueType::MaxBitLen * MaxSize };
|
||||
|
||||
static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
|
||||
{
|
||||
return array.encodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
|
||||
}
|
||||
|
||||
static int decode(SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
|
||||
{
|
||||
return array.decodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
template <typename R>
|
||||
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 <typename R> bool operator!=(const R& rhs) const { return !operator==(rhs); }
|
||||
|
||||
typedef ValueType value_type;
|
||||
typedef SizeType size_type;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <stdint.h>
|
||||
#include <string>
|
||||
#include <uavcan/internal/util.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
extern "C" void bitarray_copy(const unsigned char *src_org, int src_offset, int src_len,
|
||||
unsigned char *dst_org, int dst_offset);
|
||||
|
||||
class BitStream
|
||||
{
|
||||
enum { MaxBytesPerRW = 16 };
|
||||
|
||||
ITransferBuffer& buf_;
|
||||
int bit_offset_;
|
||||
uint8_t byte_cache_;
|
||||
|
||||
static inline unsigned int bitlenToBytelen(unsigned int bits) { return (bits + 7) / 8; }
|
||||
|
||||
static inline void copyBitArray(const uint8_t* src_org, int src_offset, int src_len,
|
||||
uint8_t* dst_org, int dst_offset)
|
||||
{
|
||||
bitarray_copy(reinterpret_cast<const unsigned char*>(src_org), src_offset, src_len,
|
||||
reinterpret_cast<unsigned char*>(dst_org), dst_offset);
|
||||
}
|
||||
|
||||
public:
|
||||
enum { MaxBitsPerRW = MaxBytesPerRW * 8 };
|
||||
|
||||
enum
|
||||
{
|
||||
ResultOutOfBuffer = 0,
|
||||
ResultOk = 1
|
||||
};
|
||||
|
||||
BitStream(ITransferBuffer& buf)
|
||||
: buf_(buf)
|
||||
, bit_offset_(0)
|
||||
, byte_cache_(0)
|
||||
{
|
||||
StaticAssert<sizeof(uint8_t) == 1>::check();
|
||||
}
|
||||
|
||||
/**
|
||||
* Write/read calls interpret bytes as bit arrays, 8 bits per byte, where the most
|
||||
* significant bits have lower index, i.e.:
|
||||
* Hex: 55 2d
|
||||
* Bits: 01010101 00101101
|
||||
* Indices: 0 .. 7 8 .. 15
|
||||
* Return values:
|
||||
* Negative - Error
|
||||
* Zero - Out of buffer space
|
||||
* Positive - OK
|
||||
*/
|
||||
int write(const uint8_t* bytes, const int bitlen);
|
||||
int read(uint8_t* bytes, const int bitlen);
|
||||
|
||||
std::string toString() const;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,157 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <limits>
|
||||
#include <math.h> // Needed for isfinite
|
||||
#include <uavcan/internal/util.hpp>
|
||||
#include <uavcan/internal/marshal/type_util.hpp>
|
||||
#include <uavcan/internal/marshal/integer_spec.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
template <unsigned int BitLen>
|
||||
struct NativeFloatSelector
|
||||
{
|
||||
struct ErrorNoSuchFloat;
|
||||
typedef typename StaticIf<(sizeof(float) * 8 >= BitLen), float,
|
||||
typename StaticIf<(sizeof(double) * 8 >= BitLen), double,
|
||||
typename StaticIf<(sizeof(long double) * 8 >= BitLen), long double,
|
||||
ErrorNoSuchFloat>::Result>::Result>::Result Type;
|
||||
};
|
||||
|
||||
|
||||
class IEEE754Converter
|
||||
{
|
||||
// TODO: Non-IEEE float support for float32 and float64
|
||||
static uint16_t nativeNonIeeeToHalf(float value);
|
||||
static float halfToNativeNonIeee(uint16_t value);
|
||||
|
||||
IEEE754Converter();
|
||||
|
||||
public:
|
||||
/// UAVCAN requires rounding to nearest for all float conversions
|
||||
static std::float_round_style roundstyle() { return std::round_to_nearest; }
|
||||
|
||||
template <unsigned int BitLen>
|
||||
static typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType
|
||||
toIeee(typename NativeFloatSelector<BitLen>::Type value)
|
||||
{
|
||||
typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType IntType;
|
||||
typedef typename NativeFloatSelector<BitLen>::Type FloatType;
|
||||
StaticAssert<sizeof(FloatType) * 8 == BitLen && std::numeric_limits<FloatType>::is_iec559>::check();
|
||||
union { IntType i; FloatType f; } u;
|
||||
u.f = value;
|
||||
return u.i;
|
||||
}
|
||||
|
||||
template <unsigned int BitLen>
|
||||
static typename NativeFloatSelector<BitLen>::Type
|
||||
toNative(typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType value)
|
||||
{
|
||||
typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType IntType;
|
||||
typedef typename NativeFloatSelector<BitLen>::Type FloatType;
|
||||
StaticAssert<sizeof(FloatType) * 8 == BitLen && std::numeric_limits<FloatType>::is_iec559>::check();
|
||||
union { IntType i; FloatType f; } u;
|
||||
u.i = value;
|
||||
return u.f;
|
||||
}
|
||||
};
|
||||
template <>
|
||||
inline typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType
|
||||
IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value)
|
||||
{
|
||||
return nativeNonIeeeToHalf(value);
|
||||
}
|
||||
template <>
|
||||
inline typename NativeFloatSelector<16>::Type
|
||||
IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value)
|
||||
{
|
||||
return halfToNativeNonIeee(value);
|
||||
}
|
||||
|
||||
template <unsigned int BitLen> struct IEEE754Limits;
|
||||
template <> struct IEEE754Limits<16>
|
||||
{
|
||||
static typename NativeFloatSelector<16>::Type max() { return 65504.0; }
|
||||
static typename NativeFloatSelector<16>::Type epsilon() { return 9.77e-04; }
|
||||
};
|
||||
template <> struct IEEE754Limits<32>
|
||||
{
|
||||
static typename NativeFloatSelector<32>::Type max() { return 3.40282346638528859812e+38; }
|
||||
static typename NativeFloatSelector<32>::Type epsilon() { return 1.19209289550781250000e-7; }
|
||||
};
|
||||
template <> struct IEEE754Limits<64>
|
||||
{
|
||||
static typename NativeFloatSelector<64>::Type max() { return 1.79769313486231570815e+308L; }
|
||||
static typename NativeFloatSelector<64>::Type epsilon() { return 2.22044604925031308085e-16L; }
|
||||
};
|
||||
|
||||
|
||||
template <unsigned int BitLen_, CastMode CastMode>
|
||||
class FloatSpec : public IEEE754Limits<BitLen_>
|
||||
{
|
||||
FloatSpec();
|
||||
|
||||
public:
|
||||
enum { BitLen = BitLen_ };
|
||||
enum { MinBitLen = BitLen };
|
||||
enum { MaxBitLen = BitLen };
|
||||
|
||||
typedef typename NativeFloatSelector<BitLen>::Type StorageType;
|
||||
|
||||
enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits<StorageType>::is_iec559 };
|
||||
|
||||
using IEEE754Limits<BitLen>::max;
|
||||
using IEEE754Limits<BitLen>::epsilon;
|
||||
static std::float_round_style roundstyle() { return IEEE754Converter::roundstyle(); }
|
||||
|
||||
static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
// cppcheck-suppress duplicateExpression
|
||||
if (CastMode == CastModeSaturate)
|
||||
saturate(value);
|
||||
else
|
||||
truncate(value);
|
||||
return codec.encode<BitLen>(IEEE754Converter::toIeee<BitLen>(value));
|
||||
}
|
||||
|
||||
static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType ieee = 0;
|
||||
const int res = codec.decode<BitLen>(ieee);
|
||||
if (res <= 0)
|
||||
return res;
|
||||
out_value = IEEE754Converter::toNative<BitLen>(ieee);
|
||||
return res;
|
||||
}
|
||||
|
||||
private:
|
||||
static inline void saturate(StorageType& value)
|
||||
{
|
||||
if (!IsExactRepresentation && isfinite(value))
|
||||
{
|
||||
if (value > max())
|
||||
value = max();
|
||||
else if (value < -max())
|
||||
value = -max();
|
||||
}
|
||||
}
|
||||
|
||||
static inline void truncate(StorageType& value)
|
||||
{
|
||||
if (!IsExactRepresentation && isfinite(value))
|
||||
{
|
||||
if (value > max())
|
||||
value = std::numeric_limits<StorageType>::infinity();
|
||||
else if (value < -max())
|
||||
value = -std::numeric_limits<StorageType>::infinity();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <limits>
|
||||
#include <uavcan/internal/util.hpp>
|
||||
#include <uavcan/internal/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/internal/marshal/type_util.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
enum Signedness { SignednessUnsigned, SignednessSigned };
|
||||
|
||||
template <unsigned int BitLen_, Signedness Signedness, CastMode CastMode>
|
||||
class IntegerSpec
|
||||
{
|
||||
enum { IsSigned = Signedness == SignednessSigned };
|
||||
|
||||
struct ErrorNoSuchInteger;
|
||||
|
||||
public:
|
||||
enum { BitLen = BitLen_ };
|
||||
enum { MinBitLen = BitLen };
|
||||
enum { MaxBitLen = BitLen };
|
||||
|
||||
typedef typename StaticIf<(BitLen <= 8), typename StaticIf<IsSigned, int8_t, uint8_t>::Result,
|
||||
typename StaticIf<(BitLen <= 16), typename StaticIf<IsSigned, int16_t, uint16_t>::Result,
|
||||
typename StaticIf<(BitLen <= 32), typename StaticIf<IsSigned, int32_t, uint32_t>::Result,
|
||||
typename StaticIf<(BitLen <= 64), typename StaticIf<IsSigned, int64_t, uint64_t>::Result,
|
||||
ErrorNoSuchInteger>::Result>::Result>::Result>::Result StorageType;
|
||||
|
||||
typedef typename IntegerSpec<BitLen, SignednessUnsigned, CastMode>::StorageType UnsignedStorageType;
|
||||
|
||||
private:
|
||||
IntegerSpec();
|
||||
|
||||
struct LimitsImplGeneric
|
||||
{
|
||||
static StorageType max()
|
||||
{
|
||||
StaticAssert<(sizeof(uintmax_t) >= 8)>::check();
|
||||
return StorageType(IsSigned ? ((uintmax_t(1) << (BitLen - 1)) - 1) : ((uintmax_t(1) << BitLen) - 1));
|
||||
}
|
||||
static UnsignedStorageType mask()
|
||||
{
|
||||
StaticAssert<(sizeof(uintmax_t) >= 8)>::check();
|
||||
return UnsignedStorageType((uintmax_t(1) << BitLen) - 1);
|
||||
}
|
||||
};
|
||||
|
||||
struct LimitsImpl64
|
||||
{
|
||||
static StorageType max() { return StorageType(IsSigned ? 0x7FFFFFFFFFFFFFFF : 0xFFFFFFFFFFFFFFFF); }
|
||||
static UnsignedStorageType mask() { return 0xFFFFFFFFFFFFFFFF; }
|
||||
};
|
||||
|
||||
typedef typename StaticIf<(BitLen == 64), LimitsImpl64, LimitsImplGeneric>::Result Limits;
|
||||
|
||||
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(); }
|
||||
|
||||
static void validate()
|
||||
{
|
||||
StaticAssert<(BitLen <= (sizeof(StorageType) * 8))>::check();
|
||||
assert(max() <= std::numeric_limits<StorageType>::max());
|
||||
assert(min() >= std::numeric_limits<StorageType>::min());
|
||||
}
|
||||
|
||||
public:
|
||||
static StorageType max() { return Limits::max(); }
|
||||
static StorageType min() { return IsSigned ? StorageType(-max() - 1) : 0; }
|
||||
static UnsignedStorageType mask() { return Limits::mask(); }
|
||||
|
||||
static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
validate();
|
||||
// cppcheck-suppress duplicateExpression
|
||||
if (CastMode == CastModeSaturate)
|
||||
saturate(value);
|
||||
else
|
||||
truncate(value);
|
||||
return codec.encode<BitLen>(value);
|
||||
}
|
||||
|
||||
static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode)
|
||||
{
|
||||
validate();
|
||||
return codec.decode<BitLen>(out_value);
|
||||
}
|
||||
};
|
||||
|
||||
template <CastMode CastMode>
|
||||
class IntegerSpec<1, SignednessSigned, CastMode>; // Invalid instantiation
|
||||
|
||||
template <Signedness Signedness, CastMode CastMode>
|
||||
class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation
|
||||
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <stdint.h>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <uavcan/internal/util.hpp>
|
||||
#include <uavcan/internal/marshal/bit_stream.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
class ScalarCodec
|
||||
{
|
||||
BitStream& stream_;
|
||||
|
||||
template <int Size>
|
||||
static void swapByteOrder(uint8_t (&bytes)[Size])
|
||||
{
|
||||
for (int i = 0, j = Size - 1; i < j; i++, j--)
|
||||
{
|
||||
const uint8_t c = bytes[i];
|
||||
bytes[i] = bytes[j];
|
||||
bytes[j] = c;
|
||||
}
|
||||
}
|
||||
|
||||
template <int BitLen, int Size>
|
||||
static typename EnableIf<(BitLen > 8)>::Type
|
||||
convertByteOrder(uint8_t (&bytes)[Size])
|
||||
{
|
||||
#if defined(BYTE_ORDER) && defined(BIG_ENDIAN)
|
||||
static const bool big_endian = BYTE_ORDER == BIG_ENDIAN;
|
||||
#else
|
||||
union { long int l; char c[sizeof(long int)]; } u;
|
||||
u.l = 1;
|
||||
const bool big_endian = u.c[sizeof(long int) - 1] == 1;
|
||||
#endif
|
||||
/*
|
||||
* I didn't have any big endian machine nearby, so big endian support wasn't tested yet.
|
||||
* 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 <int BitLen, int Size>
|
||||
static typename EnableIf<(BitLen <= 8)>::Type
|
||||
convertByteOrder(uint8_t (&bytes)[Size])
|
||||
{
|
||||
(void)bytes;
|
||||
}
|
||||
|
||||
template <int BitLen, typename T>
|
||||
static typename EnableIf<std::numeric_limits<T>::is_signed && ((sizeof(T) * 8) > BitLen)>::Type
|
||||
fixTwosComplement(T& value)
|
||||
{
|
||||
StaticAssert<std::numeric_limits<T>::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 <int BitLen, typename T>
|
||||
static typename EnableIf<!std::numeric_limits<T>::is_signed || ((sizeof(T) * 8) == BitLen)>::Type
|
||||
fixTwosComplement(T& value)
|
||||
{
|
||||
(void)value;
|
||||
}
|
||||
|
||||
template <int BitLen, typename T>
|
||||
static typename EnableIf<((sizeof(T) * 8) > BitLen)>::Type
|
||||
clearExtraBits(T& value)
|
||||
{
|
||||
value &= (T(1) << BitLen) - 1; // Signedness doesn't matter
|
||||
}
|
||||
|
||||
template <int BitLen, typename T>
|
||||
static typename EnableIf<((sizeof(T) * 8) == BitLen)>::Type
|
||||
clearExtraBits(T& value)
|
||||
{
|
||||
(void)value;
|
||||
}
|
||||
|
||||
template <int BitLen, typename T>
|
||||
void validate()
|
||||
{
|
||||
StaticAssert<((sizeof(T) * 8) >= BitLen)>::check();
|
||||
StaticAssert<(BitLen <= BitStream::MaxBitsPerRW)>::check();
|
||||
StaticAssert<std::numeric_limits<T>::is_signed ? (BitLen > 1) : 1>::check();
|
||||
}
|
||||
|
||||
public:
|
||||
ScalarCodec(BitStream& stream)
|
||||
: stream_(stream)
|
||||
{ }
|
||||
|
||||
template <int BitLen, typename T>
|
||||
int encode(const T value)
|
||||
{
|
||||
validate<BitLen, T>();
|
||||
union ByteUnion
|
||||
{
|
||||
T value;
|
||||
uint8_t bytes[sizeof(T)];
|
||||
} byte_union;
|
||||
byte_union.value = value;
|
||||
|
||||
clearExtraBits<BitLen>(byte_union.value);
|
||||
convertByteOrder<BitLen>(byte_union.bytes);
|
||||
|
||||
// Underlying stream class assumes that more significant bits have lower index, so we need to shift some.
|
||||
byte_union.bytes[BitLen / 8] <<= (8 - (BitLen % 8)) & 7;
|
||||
|
||||
return stream_.write(byte_union.bytes, BitLen);
|
||||
}
|
||||
|
||||
template <int BitLen, typename T>
|
||||
int decode(T& value)
|
||||
{
|
||||
validate<BitLen, T>();
|
||||
union ByteUnion
|
||||
{
|
||||
T value;
|
||||
uint8_t bytes[sizeof(T)];
|
||||
} byte_union;
|
||||
std::fill(byte_union.bytes, byte_union.bytes + sizeof(T), 0);
|
||||
|
||||
const int read_res = stream_.read(byte_union.bytes, BitLen);
|
||||
if (read_res <= 0)
|
||||
return read_res;
|
||||
|
||||
byte_union.bytes[BitLen / 8] >>= (8 - (BitLen % 8)) & 7; // As in encode(), vice versa
|
||||
|
||||
convertByteOrder<BitLen>(byte_union.bytes);
|
||||
fixTwosComplement<BitLen>(byte_union.value);
|
||||
|
||||
value = byte_union.value;
|
||||
return read_res;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/internal/util.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
enum CastMode { CastModeSaturate, CastModeTruncate };
|
||||
|
||||
enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled };
|
||||
|
||||
|
||||
template <unsigned long Num>
|
||||
struct IntegerBitLen { enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; };
|
||||
template <>
|
||||
struct IntegerBitLen<0> { enum { Result = 0 }; };
|
||||
|
||||
|
||||
template <typename T, typename Enable = void>
|
||||
struct StorageType { typedef T Type; };
|
||||
|
||||
template <typename T>
|
||||
struct StorageType<T, typename EnableIfType<typename T::StorageType>::Type>
|
||||
{
|
||||
typedef typename T::StorageType Type;
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/internal/marshal/integer_spec.hpp>
|
||||
#include <uavcan/internal/marshal/float_spec.hpp>
|
||||
#include <uavcan/internal/marshal/array.hpp>
|
||||
#include <uavcan/internal/marshal/type_util.hpp>
|
||||
Reference in New Issue
Block a user