niklas/rmw_cyclonedds
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Implemented byte-swapping in deserializer (#31)

Browse source 
* Implemented byte-swapping in deserializer

Set the correct endianness flag when serializing ROS messages to CDR
and added support for byte-swapping in CDR deserializer when in a
mixed-endian network.

Signed-off-by: Dennis Potman <dennis.potman@adlinktech.com>

* Fix float deser byte-swap and add byte-swap in cycprint

Fixed the byte-swapping in float/double deserialization and added byte-
swapping in cycprint by creating a cycdeserbase that is shared with
cycdeser.

Signed-off-by: Dennis Potman <dennis.potman@adlinktech.com>
This commit is contained in:
dennis-adlink 2019-09-12 15:31:03 +02:00 • committed by eboasson
parent 60a87ab4ab
commit 883ef01105
2 changed files with 202 additions and 118 deletions
Download patch file Download diff file Expand all files Collapse all files

285
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp Normal file → Executable file
View file

@ -50,7 +50,7 @@ public:
template<class T, size_t S> template<class T, size_t S>
inline cycser & operator<<(const std::array<T, S> & x) {serialize(x); return *this;} inline cycser & operator<<(const std::array<T, S> & x) {serialize(x); return *this;}
#define SIMPLE(T) inline void serialize(T x) { \ #define SER(T) inline void serialize(T x) { \
if ((off % sizeof(T)) != 0) { \ if ((off % sizeof(T)) != 0) { \
off += sizeof(T) - (off % sizeof(T)); \ off += sizeof(T) - (off % sizeof(T)); \
} \ } \
@ -58,18 +58,18 @@ public:
*(reinterpret_cast<T *>(data() + off)) = x; \ *(reinterpret_cast<T *>(data() + off)) = x; \
off += sizeof(T); \ off += sizeof(T); \
} }
SIMPLE(char); SER(char);
SIMPLE(int8_t); SER(int8_t);
SIMPLE(uint8_t); SER(uint8_t);
SIMPLE(int16_t); SER(int16_t);
SIMPLE(uint16_t); SER(uint16_t);
SIMPLE(int32_t); SER(int32_t);
SIMPLE(uint32_t); SER(uint32_t);
SIMPLE(int64_t); SER(int64_t);
SIMPLE(uint64_t); SER(uint64_t);
SIMPLE(float); SER(float);
SIMPLE(double); SER(double);
#undef SIMPLE #undef SER
inline void serialize(bool x) inline void serialize(bool x)
{ {
@ -100,7 +100,7 @@ public:
off += sz * sizeof(wchar_t); off += sz * sizeof(wchar_t);
} }
#define SIMPLEA(T) inline void serializeA(const T * x, size_t cnt) { \ #define SER_A(T) inline void serializeA(const T * x, size_t cnt) { \
if (cnt > 0) { \ if (cnt > 0) { \
if ((off % sizeof(T)) != 0) { \ if ((off % sizeof(T)) != 0) { \
off += sizeof(T) - (off % sizeof(T)); \ off += sizeof(T) - (off % sizeof(T)); \
@ -110,18 +110,19 @@ public:
off += cnt * sizeof(T); \ off += cnt * sizeof(T); \
} \ } \
} }
SIMPLEA(char); SER_A(char);
SIMPLEA(int8_t); SER_A(int8_t);
SIMPLEA(uint8_t); SER_A(uint8_t);
SIMPLEA(int16_t); SER_A(int16_t);
SIMPLEA(uint16_t); SER_A(uint16_t);
SIMPLEA(int32_t); SER_A(int32_t);
SIMPLEA(uint32_t); SER_A(uint32_t);
SIMPLEA(int64_t); SER_A(int64_t);
SIMPLEA(uint64_t); SER_A(uint64_t);
SIMPLEA(float); SER_A(float);
SIMPLEA(double); SER_A(double);
#undef SIMPLEA #undef SER_A
template<class T> template<class T>
inline void serializeA(const T * x, size_t cnt) inline void serializeA(const T * x, size_t cnt)
{ {
@ -155,10 +156,55 @@ private:
size_t off; size_t off;
}; };
class cycdeser class cycdeserbase
{
public:
explicit cycdeserbase(const char * data_);
cycdeserbase() = delete;
protected:
inline void align(size_t a)
{
if ((pos % a) != 0) {
pos += a - (pos % a);
}
}
inline uint16_t bswap2u(uint16_t x)
{
return (uint16_t) ((x >> 8) | (x << 8));
}
inline int16_t bswap2(int16_t x)
{
return (int16_t) bswap2u((uint16_t) x);
}
inline uint32_t bswap4u(uint32_t x)
{
return (x >> 24) | ((x >> 8) & 0xff00) | ((x << 8) & 0xff0000) | (x << 24);
}
inline int32_t bswap4(int32_t x)
{
return (int32_t) bswap4u((uint32_t) x);
}
inline uint64_t bswap8u(uint64_t x)
{
const uint32_t newhi = bswap4u((uint32_t) x);
const uint32_t newlo = bswap4u((uint32_t) (x >> 32));
return ((uint64_t) newhi << 32) | (uint64_t) newlo;
}
inline int64_t bswap8(int64_t x)
{
return (int64_t) bswap8u((uint64_t) x);
}
const char * data;
size_t pos;
bool swap_bytes;
};
class cycdeser : cycdeserbase
{ {
public: public:
// FIXME: byteswapping
cycdeser(const void * data, size_t size); cycdeser(const void * data, size_t size);
cycdeser() = delete; cycdeser() = delete;
@ -182,28 +228,36 @@ public:
template<class T, size_t S> template<class T, size_t S>
inline cycdeser & operator>>(std::array<T, S> & x) {deserialize(x); return *this;} inline cycdeser & operator>>(std::array<T, S> & x) {deserialize(x); return *this;}
#define SIMPLE(T) inline void deserialize(T & x) { \ #define DESER8(T) DESER(T, )
#define DESER(T, fn_swap) inline void deserialize(T & x) { \
align(sizeof(x)); \ align(sizeof(x)); \
x = *reinterpret_cast<const T *>(data + pos); \ x = *reinterpret_cast<const T *>(data + pos); \
if (swap_bytes) {x = fn_swap(x);} \
pos += sizeof(x); \ pos += sizeof(x); \
} }
SIMPLE(char); DESER8(char);
SIMPLE(int8_t); DESER8(int8_t);
SIMPLE(uint8_t); DESER8(uint8_t);
SIMPLE(int16_t); DESER(int16_t, bswap2);
SIMPLE(uint16_t); DESER(uint16_t, bswap2u);
SIMPLE(int32_t); DESER(int32_t, bswap4);
SIMPLE(uint32_t); DESER(uint32_t, bswap4u);
SIMPLE(int64_t); DESER(int64_t, bswap8);
SIMPLE(uint64_t); DESER(uint64_t, bswap8u);
SIMPLE(float); #undef DESER
SIMPLE(double);
#undef SIMPLE
inline void deserialize(bool & x) inline void deserialize(bool & x)
{ {
unsigned char z; deserialize(z); x = (z != 0); unsigned char z; deserialize(z); x = (z != 0);
} }
inline void deserialize(float & x)
{
deserialize(*reinterpret_cast<uint32_t *>(&x));
}
inline void deserialize(double & x)
{
deserialize(*reinterpret_cast<uint64_t *>(&x));
}
inline uint32_t deserialize32() inline uint32_t deserialize32()
{ {
uint32_t sz; deserialize(sz); return sz; uint32_t sz; deserialize(sz); return sz;
@ -232,26 +286,42 @@ public:
pos += sz * sizeof(wchar_t); pos += sz * sizeof(wchar_t);
} }
#define SIMPLEA(T) inline void deserializeA(T * x, size_t cnt) { \ #define DESER8_A(T) DESER_A(T, )
#define DESER_A(T, fn_swap) inline void deserializeA(T * x, size_t cnt) { \
if (cnt > 0) { \ if (cnt > 0) { \
align(sizeof(T)); \ align(sizeof(T)); \
if (swap_bytes) { \
for (size_t i = 0; i < cnt; i++) { \
x[i] = fn_swap(*reinterpret_cast<const T *>(data + pos)); \
pos += sizeof(T); \
} \
} else { \
memcpy(reinterpret_cast<void *>(x), reinterpret_cast<const void *>(data + pos), \ memcpy(reinterpret_cast<void *>(x), reinterpret_cast<const void *>(data + pos), \
(cnt) * sizeof(T)); \ cnt * sizeof(T)); \
pos += (cnt) * sizeof(T); \ pos += cnt * sizeof(T); \
} \
} \ } \
} }
SIMPLEA(char); DESER8_A(char);
SIMPLEA(int8_t); DESER8_A(int8_t);
SIMPLEA(uint8_t); DESER8_A(uint8_t);
SIMPLEA(int16_t); DESER_A(int16_t, bswap2);
SIMPLEA(uint16_t); DESER_A(uint16_t, bswap2u);
SIMPLEA(int32_t); DESER_A(int32_t, bswap4);
SIMPLEA(uint32_t); DESER_A(uint32_t, bswap4u);
SIMPLEA(int64_t); DESER_A(int64_t, bswap8);
SIMPLEA(uint64_t); DESER_A(uint64_t, bswap8u);
SIMPLEA(float); #undef DESER_A
SIMPLEA(double);
#undef SIMPLEA inline void deserializeA(float * x, size_t cnt)
{
deserializeA(reinterpret_cast<uint32_t *>(x), cnt);
}
inline void deserializeA(double * x, size_t cnt)
{
deserializeA(reinterpret_cast<uint64_t *>(x), cnt);
}
template<class T> template<class T>
inline void deserializeA(T * x, size_t cnt) inline void deserializeA(T * x, size_t cnt)
{ {
@ -281,46 +351,12 @@ public:
} }
private: private:
inline void align(size_t a)
{
if ((pos % a) != 0) {
pos += a - (pos % a);
}
}
const char * data;
size_t pos;
size_t lim; // ignored for now ... better provide correct input size_t lim; // ignored for now ... better provide correct input
}; };
class cycprint class cycprint : cycdeserbase
{ {
private:
static bool prtf(char * __restrict * buf, size_t * __restrict bufsize, const char * fmt, ...)
{
va_list ap;
if (*bufsize == 0) {
return false;
}
va_start(ap, fmt);
int n = vsnprintf(*buf, *bufsize, fmt, ap);
va_end(ap);
if (n < 0) {
**buf = 0;
return false;
} else if ((size_t) n <= *bufsize) {
*buf += (size_t) n;
*bufsize -= (size_t) n;
return *bufsize > 0;
} else {
*buf += *bufsize;
*bufsize = 0;
return false;
}
}
public: public:
// FIXME: byteswapping
cycprint(char * buf, size_t bufsize, const void * data, size_t size); cycprint(char * buf, size_t bufsize, const void * data, size_t size);
cycprint() = delete; cycprint() = delete;
@ -349,35 +385,52 @@ public:
template<class T, size_t S> template<class T, size_t S>
inline cycprint & operator>>(std::array<T, S> & x) {print(x); return *this;} inline cycprint & operator>>(std::array<T, S> & x) {print(x); return *this;}
#define SIMPLE(T, F) inline void print(T & x) { \ #define PRNT8(T, F) PRNT(T, F, )
#define PRNT(T, F, fn_swap) inline void print(T & x) { \
align(sizeof(x)); \ align(sizeof(x)); \
x = *reinterpret_cast<const T *>(data + pos); \ x = *reinterpret_cast<const T *>(data + pos); \
if (swap_bytes) {x = fn_swap(x);} \
prtf(&buf, &bufsize, F, x); \ prtf(&buf, &bufsize, F, x); \
pos += sizeof(x); \ pos += sizeof(x); \
} }
SIMPLE(char, "'%c'"); PRNT8(char, "'%c'");
SIMPLE(int8_t, "%" PRId8); PRNT8(int8_t, "%" PRId8);
SIMPLE(uint8_t, "%" PRIu8); PRNT8(uint8_t, "%" PRIu8);
SIMPLE(int16_t, "%" PRId16); PRNT(int16_t, "%" PRId16, bswap2);
SIMPLE(uint16_t, "%" PRIu16); PRNT(uint16_t, "%" PRIu16, bswap2u);
SIMPLE(int32_t, "%" PRId32); PRNT(int32_t, "%" PRId32, bswap4);
SIMPLE(uint32_t, "%" PRIu32); PRNT(uint32_t, "%" PRIu32, bswap4u);
SIMPLE(int64_t, "%" PRId64); PRNT(int64_t, "%" PRId64, bswap8);
SIMPLE(uint64_t, "%" PRIu64); PRNT(uint64_t, "%" PRIu64, bswap8u);
SIMPLE(float, "%f"); #undef PRNT
SIMPLE(double, "%f");
#undef SIMPLE
inline void print(bool & x) inline void print(bool & x)
{ {
static_cast<void>(x); static_cast<void>(x);
unsigned char z; print(z); unsigned char z; print(z);
} }
inline void print(float & x)
{
align(sizeof(x));
uint32_t z = *reinterpret_cast<const uint32_t *>(data + pos);
if (swap_bytes) {z = bswap4u(z);}
prtf(&buf, &bufsize, "%f", *reinterpret_cast<float *>(&z));
pos += sizeof(x);
}
inline void print(double & x)
{
align(sizeof(x));
uint64_t z = *reinterpret_cast<const uint64_t *>(data + pos);
if (swap_bytes) {z = bswap8u(z);}
prtf(&buf, &bufsize, "%f", *reinterpret_cast<double *>(&z));
pos += sizeof(x);
}
inline uint32_t get32() inline uint32_t get32()
{ {
uint32_t sz; uint32_t sz;
align(sizeof(sz)); align(sizeof(sz));
sz = *reinterpret_cast<const uint32_t *>(data + pos); sz = *reinterpret_cast<const uint32_t *>(data + pos);
if (swap_bytes) {sz = bswap4u(sz);}
pos += sizeof(sz); pos += sizeof(sz);
return sz; return sz;
} }
@ -429,15 +482,29 @@ public:
} }
private: private:
inline void align(size_t a) static bool prtf(char * __restrict * buf, size_t * __restrict bufsize, const char * fmt, ...)
{ {
if ((pos % a) != 0) { va_list ap;
pos += a - (pos % a); if (*bufsize == 0) {
return false;
}
va_start(ap, fmt);
int n = vsnprintf(*buf, *bufsize, fmt, ap);
va_end(ap);
if (n < 0) {
**buf = 0;
return false;
} else if ((size_t) n <= *bufsize) {
*buf += (size_t) n;
*bufsize -= (size_t) n;
return *bufsize > 0;
} else {
*buf += *bufsize;
*bufsize = 0;
return false;
} }
} }
const char * data;
size_t pos;
char * buf; char * buf;
size_t bufsize; size_t bufsize;
}; };

33
rmw_cyclonedds_cpp/src/serdes.cpp Normal file → Executable file
View file

@ -11,8 +11,10 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include <exception>
#include <vector> #include <vector>
#include "dds/ddsrt/endian.h"
#include "rmw_cyclonedds_cpp/serdes.hpp" #include "rmw_cyclonedds_cpp/serdes.hpp"
cycser::cycser(std::vector<unsigned char> & dst_) cycser::cycser(std::vector<unsigned char> & dst_)
@ -20,24 +22,39 @@ cycser::cycser(std::vector<unsigned char> & dst_)
off(0) off(0)
{ {
dst.reserve(4); dst.reserve(4);
/* FIXME: hard code to little endian ... and ignoring endianness in deser */
dst.push_back(0); /* endianness: 0x0000 for BE, 0x0001 for LE */
dst.push_back(1); dst.push_back(0x00);
/* options: */ dst.push_back((DDSRT_ENDIAN == DDSRT_LITTLE_ENDIAN) ? 0x01 : 0x00);
/* options: defaults to 0x0000 */
dst.push_back(0); dst.push_back(0);
dst.push_back(0); dst.push_back(0);
} }
cycdeser::cycdeser(const void * data_, size_t size_) cycdeserbase::cycdeserbase(const char * data_)
: data(static_cast<const char *>(data_) + 4), : data(data_),
pos(0), pos(0),
swap_bytes(false)
{
/* Get the endianness byte (skip unused first byte in data[0]) */
uint32_t data_endianness = (data[1] == 0x01) ? DDSRT_LITTLE_ENDIAN : DDSRT_BIG_ENDIAN;
/* If endianness of data differs from our endianness: swap bytes when deserializing */
swap_bytes = (DDSRT_ENDIAN != data_endianness);
/* Ignore representation options (data_[2] and data_[3]) */
data += 4;
}
cycdeser::cycdeser(const void * data_, size_t size_)
: cycdeserbase(static_cast<const char *>(data_)),
lim(size_ - 4) lim(size_ - 4)
{ {
} }
cycprint::cycprint(char * buf_, size_t bufsize_, const void * data_, size_t size_) cycprint::cycprint(char * buf_, size_t bufsize_, const void * data_, size_t size_)
: data(static_cast<const char *>(data_) + 4), : cycdeserbase(static_cast<const char *>(data_)),
pos(0),
buf(buf_), buf(buf_),
bufsize(bufsize_) bufsize(bufsize_)
{ {