From 8794c7eab94c3f754b8d946f2ae698a42dde9fd4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 2 Feb 2014 22:54:27 +0400 Subject: [PATCH] Frame parse()/compile(), some renamings --- .../uavcan/internal/transport/can_io.hpp | 5 + .../uavcan/internal/transport/transfer.hpp | 44 ++++++--- libuavcan/src/can_driver.cpp | 6 ++ libuavcan/src/transport/transfer.cpp | 56 +++++++++++ libuavcan/test/can_driver.cpp | 22 ++--- libuavcan/test/common.hpp | 2 +- libuavcan/test/transport/can/io.cpp | 10 +- libuavcan/test/transport/can/tx_queue.cpp | 20 ++-- libuavcan/test/transport/transfer.cpp | 99 +++++++++++++++++++ 9 files changed, 226 insertions(+), 38 deletions(-) diff --git a/libuavcan/include/uavcan/internal/transport/can_io.hpp b/libuavcan/include/uavcan/internal/transport/can_io.hpp index 20883d23e9..bfdfcf4ef7 100644 --- a/libuavcan/include/uavcan/internal/transport/can_io.hpp +++ b/libuavcan/include/uavcan/internal/transport/can_io.hpp @@ -20,6 +20,11 @@ struct CanRxFrame CanFrame frame; uint64_t timestamp; uint8_t iface_index; + + CanRxFrame() + : timestamp(0) + , iface_index(0) + { } }; diff --git a/libuavcan/include/uavcan/internal/transport/transfer.hpp b/libuavcan/include/uavcan/internal/transport/transfer.hpp index 69f2b2af1d..b7c553ce8f 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer.hpp @@ -22,7 +22,7 @@ enum TransferType class TransferID { - uint_fast8_t value_; + uint8_t value_; public: enum { BITLEN = 4 }; @@ -32,7 +32,7 @@ public: : value_(0) { } - TransferID(uint_fast8_t value) // implicit + TransferID(uint8_t value) // implicit : value_(value) { value_ &= MAX; @@ -47,7 +47,7 @@ public: value_ = (value_ + 1) & MAX; } - uint_fast8_t get() const + uint8_t get() const { assert(value_ <= MAX); return value_; @@ -62,6 +62,10 @@ public: struct Frame { + enum { DATA_TYPE_ID_MAX = 1023 }; + enum { NODE_ID_MAX = 127 }; + enum { FRAME_INDEX_MAX = 31 }; + uint8_t payload[8]; TransferType transfer_type; uint_fast16_t data_type_id; @@ -71,6 +75,18 @@ struct Frame TransferID transfer_id; bool last_frame; + Frame() + : transfer_type(TransferType(0)) + , data_type_id(0) + , payload_len(0) + , source_node_id(0) + , frame_index(0) + , transfer_id(0) + , last_frame(false) + { + std::fill(payload, payload + sizeof(payload), 0); + } + Frame(const uint8_t* payload, uint_fast8_t payload_len, uint_fast16_t data_type_id, TransferType transfer_type, uint_fast8_t source_node_id, uint_fast8_t frame_index, TransferID transfer_id, bool last_frame) : transfer_type(transfer_type) @@ -81,11 +97,14 @@ struct Frame , transfer_id(transfer_id) , last_frame(last_frame) { - assert(payload && payload_len <= 8); + assert(data_type_id <= DATA_TYPE_ID_MAX); + assert(source_node_id <= NODE_ID_MAX); + assert(frame_index <= FRAME_INDEX_MAX); + assert(payload && payload_len <= sizeof(payload)); std::copy(payload, payload + payload_len, this->payload); } - static Frame parse(const CanFrame& can_frame); + bool parse(const CanFrame& can_frame); CanFrame compile() const; @@ -111,15 +130,18 @@ struct RxFrame Frame frame; uint_fast8_t iface_index; - RxFrame(const Frame& frame, uint_fast64_t timestamp, uint_fast8_t iface_index) - : timestamp(timestamp) - , frame(frame) - , iface_index(iface_index) + RxFrame() + : timestamp(0) + , iface_index(0) { } - static RxFrame parse(const CanRxFrame& can_frame) + bool parse(const CanRxFrame& can_frame) { - return RxFrame(Frame::parse(can_frame.frame), can_frame.timestamp, can_frame.iface_index); + if (!frame.parse(can_frame.frame)) + return false; + timestamp = can_frame.timestamp; + iface_index = can_frame.iface_index; + return true; } }; diff --git a/libuavcan/src/can_driver.cpp b/libuavcan/src/can_driver.cpp index 338a8680d2..38858d1a3f 100644 --- a/libuavcan/src/can_driver.cpp +++ b/libuavcan/src/can_driver.cpp @@ -10,6 +10,12 @@ namespace uavcan { +const uint32_t CanFrame::MASK_STDID; +const uint32_t CanFrame::MASK_EXTID; +const uint32_t CanFrame::FLAG_EFF; +const uint32_t CanFrame::FLAG_RTR; + + std::string CanFrame::toString(StringRepresentation mode) const { using std::snprintf; diff --git a/libuavcan/src/transport/transfer.cpp b/libuavcan/src/transport/transfer.cpp index cff09f30be..4195317f37 100644 --- a/libuavcan/src/transport/transfer.cpp +++ b/libuavcan/src/transport/transfer.cpp @@ -18,4 +18,60 @@ int TransferID::forwardDistance(TransferID rhs) const return d; } + +template +inline static uint32_t bitunpack(uint32_t val) +{ + return (val >> OFFSET) & ((1UL << WIDTH) - 1); +} + +bool Frame::parse(const CanFrame& can_frame) +{ + if ((can_frame.id & CanFrame::FLAG_RTR) || !(can_frame.id & CanFrame::FLAG_EFF)) + return false; + + if (can_frame.dlc > 8) + { + assert(0); + return false; + } + + const uint32_t id = can_frame.id & CanFrame::MASK_EXTID; + + transfer_id = bitunpack<0, 4>(id); + last_frame = bitunpack<4, 1>(id); + frame_index = bitunpack<5, 5>(id); + source_node_id = bitunpack<10, 7>(id); + transfer_type = TransferType(bitunpack<17, 2>(id)); + data_type_id = bitunpack<19, 10>(id); + + payload_len = can_frame.dlc; + std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload); + return true; +} + +template +inline static uint32_t bitpack(uint32_t field) +{ + return (field & ((1UL << WIDTH) - 1)) << OFFSET; +} + +CanFrame Frame::compile() const +{ + CanFrame frame; + + frame.id = CanFrame::FLAG_EFF | + bitpack<0, 4>(transfer_id.get()) | + bitpack<4, 1>(last_frame) | + bitpack<5, 5>(frame_index) | + bitpack<10, 7>(source_node_id) | + bitpack<17, 2>(transfer_type) | + bitpack<19, 10>(data_type_id); + + assert(payload_len <= sizeof(payload)); + frame.dlc = payload_len; + std::copy(payload, payload + payload_len, frame.data); + return frame; +} + } diff --git a/libuavcan/test/can_driver.cpp b/libuavcan/test/can_driver.cpp index 47c2140097..7ff0f0805d 100644 --- a/libuavcan/test/can_driver.cpp +++ b/libuavcan/test/can_driver.cpp @@ -8,35 +8,35 @@ TEST(CanFrame, FrameProperties) { - EXPECT_TRUE(makeFrame(0, "", EXT).isExtended()); - EXPECT_FALSE(makeFrame(0, "", STD).isExtended()); - EXPECT_FALSE(makeFrame(0, "", EXT).isRemoteTransmissionRequest()); - EXPECT_FALSE(makeFrame(0, "", STD).isRemoteTransmissionRequest()); + EXPECT_TRUE(makeCanFrame(0, "", EXT).isExtended()); + EXPECT_FALSE(makeCanFrame(0, "", STD).isExtended()); + EXPECT_FALSE(makeCanFrame(0, "", EXT).isRemoteTransmissionRequest()); + EXPECT_FALSE(makeCanFrame(0, "", STD).isRemoteTransmissionRequest()); - uavcan::CanFrame frame = makeFrame(123, "", STD); + uavcan::CanFrame frame = makeCanFrame(123, "", STD); frame.id |= uavcan::CanFrame::FLAG_RTR; EXPECT_TRUE(frame.isRemoteTransmissionRequest()); } TEST(CanFrame, ToString) { - uavcan::CanFrame frame = makeFrame(123, "\x01\x02\x03\x04""1234", EXT); + uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04""1234", EXT); EXPECT_EQ("0x0000007b 01 02 03 04 31 32 33 34 '....1234'", frame.toString()); EXPECT_TRUE(frame.toString() == frame.toString(uavcan::CanFrame::STR_ALIGNED)); - frame = makeFrame(123, "z", EXT); + frame = makeCanFrame(123, "z", EXT); EXPECT_EQ("0x0000007b 7a 'z'", frame.toString(uavcan::CanFrame::STR_ALIGNED)); EXPECT_EQ("0x0000007b 7a 'z'", frame.toString()); EXPECT_EQ(" 0x141 61 62 63 64 aa bb cc dd 'abcd....'", - makeFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::STR_ALIGNED)); + makeCanFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::STR_ALIGNED)); EXPECT_EQ(" 0x100 ''", - makeFrame(256, "", STD).toString(uavcan::CanFrame::STR_ALIGNED)); + makeCanFrame(256, "", STD).toString(uavcan::CanFrame::STR_ALIGNED)); EXPECT_EQ("0x100 ''", - makeFrame(256, "", STD).toString()); + makeCanFrame(256, "", STD).toString()); EXPECT_EQ("0x141 61 62 63 64 aa bb cc dd 'abcd....'", - makeFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString()); + makeCanFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString()); } diff --git a/libuavcan/test/common.hpp b/libuavcan/test/common.hpp index 1d3cce8ede..e93efccfb1 100644 --- a/libuavcan/test/common.hpp +++ b/libuavcan/test/common.hpp @@ -44,7 +44,7 @@ public: }; enum FrameType { STD, EXT }; -static uavcan::CanFrame makeFrame(uint32_t id, const std::string& str_data, FrameType type) +static uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type) { id |= (type == EXT) ? uavcan::CanFrame::FLAG_EFF : 0; return uavcan::CanFrame(id, reinterpret_cast(str_data.c_str()), str_data.length()); diff --git a/libuavcan/test/transport/can/io.cpp b/libuavcan/test/transport/can/io.cpp index c66dbfe053..4d337f9259 100644 --- a/libuavcan/test/transport/can/io.cpp +++ b/libuavcan/test/transport/can/io.cpp @@ -170,7 +170,7 @@ TEST(CanIOManager, CanDriverMock) EXPECT_EQ(100, clockmock.utc); // No WR, #1 RD - const CanFrame fr1 = makeFrame(123, "foo", EXT); + const CanFrame fr1 = makeCanFrame(123, "foo", EXT); driver.ifaces.at(1).pushRx(fr1); mask_wr = 7; mask_rd = 6; @@ -232,8 +232,8 @@ TEST(CanIOManager, Reception) * Non empty from multiple ifaces */ const uavcan::CanFrame frames[2][3] = { - { makeFrame(1, "a0", EXT), makeFrame(99, "a1", EXT), makeFrame(803, "a2", STD) }, - { makeFrame(6341, "b0", EXT), makeFrame(196, "b1", STD), makeFrame(73, "b2", EXT) }, + { makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) }, + { makeCanFrame(6341, "b0", EXT), makeCanFrame(196, "b1", STD), makeCanFrame(73, "b2", EXT) }, }; clockmock.advance(10); @@ -307,7 +307,7 @@ TEST(CanIOManager, Transmission) const int ALL_IFACES_MASK = 3; const uavcan::CanFrame frames[] = { - makeFrame(1, "a0", EXT), makeFrame(99, "a1", EXT), makeFrame(803, "a2", STD) + makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) }; /* @@ -396,7 +396,7 @@ TEST(CanIOManager, Transmission) // Preparing the driver mock for receive() call driver.ifaces.at(0).writeable = true; driver.ifaces.at(1).writeable = true; - const uavcan::CanFrame rx_frames[] = { makeFrame(123, "rx0", STD), makeFrame(321, "rx1", EXT) }; + const uavcan::CanFrame rx_frames[] = { makeCanFrame(123, "rx0", STD), makeCanFrame(321, "rx1", EXT) }; driver.ifaces.at(0).pushRx(rx_frames[0]); driver.ifaces.at(1).pushRx(rx_frames[1]); diff --git a/libuavcan/test/transport/can/tx_queue.cpp b/libuavcan/test/transport/can/tx_queue.cpp index 3ccc8f8b85..62dd2f1b6f 100644 --- a/libuavcan/test/transport/can/tx_queue.cpp +++ b/libuavcan/test/transport/can/tx_queue.cpp @@ -33,8 +33,8 @@ static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame) TEST(CanTxQueue, Qos) { - uavcan::CanTxQueue::Entry e1(makeFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE); - uavcan::CanTxQueue::Entry e2(makeFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE); + uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE); + uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE); EXPECT_FALSE(e1.qosHigherThan(e2)); EXPECT_FALSE(e2.qosHigherThan(e1)); @@ -74,14 +74,14 @@ TEST(CanTxQueue, TxQueue) EXPECT_TRUE(queue.isEmpty()); // Descending priority - const CanFrame f0 = makeFrame(0, "f0", EXT); - const CanFrame f1 = makeFrame(10, "f1", EXT); - const CanFrame f2 = makeFrame(20, "f2", EXT); - const CanFrame f3 = makeFrame(100, "f3", EXT); - const CanFrame f4 = makeFrame(10000, "f4", EXT); - const CanFrame f5 = makeFrame(99999, "f5", EXT); - const CanFrame f5a = makeFrame(99999, "f5a", EXT); - const CanFrame f6 = makeFrame(999999, "f6", EXT); + const CanFrame f0 = makeCanFrame(0, "f0", EXT); + const CanFrame f1 = makeCanFrame(10, "f1", EXT); + const CanFrame f2 = makeCanFrame(20, "f2", EXT); + const CanFrame f3 = makeCanFrame(100, "f3", EXT); + const CanFrame f4 = makeCanFrame(10000, "f4", EXT); + const CanFrame f5 = makeCanFrame(99999, "f5", EXT); + const CanFrame f5a = makeCanFrame(99999, "f5a", EXT); + const CanFrame f6 = makeCanFrame(999999, "f6", EXT); /* * Priority insertion diff --git a/libuavcan/test/transport/transfer.cpp b/libuavcan/test/transport/transfer.cpp index 14bdc77ead..86ec044e2a 100644 --- a/libuavcan/test/transport/transfer.cpp +++ b/libuavcan/test/transport/transfer.cpp @@ -5,6 +5,7 @@ #include #include #include +#include "../common.hpp" TEST(Transfer, TransferID) @@ -58,3 +59,101 @@ TEST(Transfer, TransferID) ASSERT_EQ(0, tid.forwardDistance(tid)); } } + +TEST(Transfer, FrameParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + const uint32_t can_id = + (2 << 0) | // Transfer ID + (1 << 4) | // Last Frame + (29 << 5) | // Frame Index + (42 << 10) | // Source Node ID + (3 << 17) | // Transfer Type + (456 << 19); // Data Type ID + + const std::string payload_string = "hello"; + + /* + * Parse + */ + // Invalid CAN frames + ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FLAG_RTR, (const uint8_t*)"", 0))); + ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD))); + + // Valid + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(2), frame.transfer_id); + EXPECT_TRUE(frame.last_frame); + EXPECT_EQ(29, frame.frame_index); + EXPECT_EQ(42, frame.source_node_id); + EXPECT_EQ(TransferType(3), frame.transfer_type); + EXPECT_EQ(456, frame.data_type_id); + + EXPECT_EQ(payload_string.length(), frame.payload_len); + EXPECT_TRUE(std::equal(frame.payload, frame.payload + frame.payload_len, payload_string.begin())); + + // Default + ASSERT_TRUE(frame.parse(CanFrame(CanFrame::FLAG_EFF, (const uint8_t*)"", 0))); + ASSERT_EQ(Frame(), frame); + + /* + * Compile + */ + // Default + frame = Frame(); + CanFrame can_frame = frame.compile(); + ASSERT_EQ(can_frame.id, CanFrame::FLAG_EFF); + + // Custom + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + can_frame = frame.compile(); + ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT)); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, payload_string.begin())); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Transfer, RxFrameParseCompile) +{ + using uavcan::Frame; + using uavcan::RxFrame; + using uavcan::CanFrame; + using uavcan::CanRxFrame; + + CanRxFrame can_rx_frame; + RxFrame rx_frame; + + // Failure + ASSERT_FALSE(rx_frame.parse(can_rx_frame)); + + // Default + can_rx_frame.frame.id = CanFrame::FLAG_EFF; + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(0, rx_frame.timestamp); + ASSERT_EQ(0, rx_frame.iface_index); + + // Custom + can_rx_frame.timestamp = 123; + can_rx_frame.iface_index = 2; + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(123, rx_frame.timestamp); + ASSERT_EQ(2, rx_frame.iface_index); +}