From 06d757b78eabc985c16a87a4459190a8b3cee553 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2014 21:04:12 +0400 Subject: [PATCH] Tests refactoring - mocks and helpers were separated from testing logic --- libuavcan/test/transport/can/iface_mock.cpp | 59 ++++ libuavcan/test/transport/can/iface_mock.hpp | 145 +++++++++ libuavcan/test/transport/can/io.cpp | 191 +----------- .../test/transport/transfer_listener.cpp | 287 +----------------- .../test/transport/transfer_test_helpers.cpp | 95 ++++++ .../test/transport/transfer_test_helpers.hpp | 210 +++++++++++++ 6 files changed, 511 insertions(+), 476 deletions(-) create mode 100644 libuavcan/test/transport/can/iface_mock.cpp create mode 100644 libuavcan/test/transport/can/iface_mock.hpp create mode 100644 libuavcan/test/transport/transfer_test_helpers.cpp create mode 100644 libuavcan/test/transport/transfer_test_helpers.hpp diff --git a/libuavcan/test/transport/can/iface_mock.cpp b/libuavcan/test/transport/can/iface_mock.cpp new file mode 100644 index 0000000000..02ee98b97e --- /dev/null +++ b/libuavcan/test/transport/can/iface_mock.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "iface_mock.hpp" + +TEST(CanIOManager, CanDriverMock) +{ + using uavcan::CanFrame; + + SystemClockMock clockmock; + CanDriverMock driver(3, clockmock); + + ASSERT_EQ(3, driver.getNumIfaces()); + + // All WR, no RD + int mask_wr = 7; + int mask_rd = 7; + EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100)); + EXPECT_EQ(7, mask_wr); + EXPECT_EQ(0, mask_rd); + + for (int i = 0; i < 3; i++) + driver.ifaces.at(i).writeable = false; + + // No WR, no RD + mask_wr = 7; + mask_rd = 7; + EXPECT_EQ(0, driver.select(mask_wr, mask_rd, 100)); + EXPECT_EQ(0, mask_wr); + EXPECT_EQ(0, mask_rd); + EXPECT_EQ(100, clockmock.monotonic); + EXPECT_EQ(100, clockmock.utc); + + // No WR, #1 RD + const CanFrame fr1 = makeCanFrame(123, "foo", EXT); + driver.ifaces.at(1).pushRx(fr1); + mask_wr = 7; + mask_rd = 6; + EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100)); + EXPECT_EQ(0, mask_wr); + EXPECT_EQ(2, mask_rd); + CanFrame fr2; + uint64_t ts_monotonic, ts_utc; + EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc)); + EXPECT_EQ(fr1, fr2); + EXPECT_EQ(100, ts_monotonic); + EXPECT_EQ(0, ts_utc); + + // #0 WR, #1 RD, Select failure + driver.ifaces.at(0).writeable = true; + driver.select_failure = true; + mask_wr = 1; + mask_rd = 7; + EXPECT_EQ(-1, driver.select(mask_wr, mask_rd, 100)); + EXPECT_EQ(1, mask_wr); // Leaving masks unchanged - library must ignore them + EXPECT_EQ(7, mask_rd); +} diff --git a/libuavcan/test/transport/can/iface_mock.hpp b/libuavcan/test/transport/can/iface_mock.hpp new file mode 100644 index 0000000000..4d098acc8a --- /dev/null +++ b/libuavcan/test/transport/can/iface_mock.hpp @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include "../../common.hpp" + + +class CanIfaceMock : public uavcan::ICanIface +{ +public: + struct FrameWithTime + { + uavcan::CanFrame frame; + uint64_t time; + + FrameWithTime(const uavcan::CanFrame& frame, uint64_t time) + : frame(frame) + , time(time) + { } + }; + + std::queue tx; ///< Queue of outgoing frames (bus <-- library) + std::queue rx; ///< Queue of incoming frames (bus --> library) + bool writeable; + bool tx_failure; + bool rx_failure; + uint64_t num_errors; + SystemClockMock& clockmock; + + CanIfaceMock(SystemClockMock& clockmock) + : writeable(true) + , tx_failure(false) + , rx_failure(false) + , num_errors(0) + , clockmock(clockmock) + { } + + void pushRx(uavcan::CanFrame frame) + { + rx.push(FrameWithTime(frame, clockmock.utc)); + } + + bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline) + { + if (tx.empty()) + { + std::cout << "Tx buffer is empty" << std::endl; + return false; + } + const FrameWithTime frame_time = tx.front(); + tx.pop(); + return (frame_time.frame == frame) && (frame_time.time == tx_deadline); + } + + int send(const uavcan::CanFrame& frame, uint64_t tx_timeout_usec) + { + assert(this); + EXPECT_TRUE(writeable); // Shall never be called when not writeable + if (tx_failure) + return -1; + if (!writeable) + return 0; + const uint64_t monotonic_deadline = tx_timeout_usec + clockmock.monotonic; + tx.push(FrameWithTime(frame, monotonic_deadline)); + return 1; + } + + int receive(uavcan::CanFrame& out_frame, uint64_t& out_ts_monotonic_usec, uint64_t& out_ts_utc_usec) + { + assert(this); + EXPECT_TRUE(rx.size()); // Shall never be called when not readable + if (rx_failure) + return -1; + if (rx.empty()) + return 0; + const FrameWithTime frame = rx.front(); + rx.pop(); + out_frame = frame.frame; + out_ts_monotonic_usec = frame.time; + out_ts_utc_usec = 0; + return 1; + } + + // cppcheck-suppress unusedFunction + // cppcheck-suppress functionConst + int configureFilters(const uavcan::CanFilterConfig* filter_configs, int num_configs) { return -1; } + // cppcheck-suppress unusedFunction + int getNumFilters() const { return 0; } + uint64_t getNumErrors() const { return num_errors; } +}; + +class CanDriverMock : public uavcan::ICanDriver +{ +public: + std::vector ifaces; + SystemClockMock& clockmock; + bool select_failure; + + CanDriverMock(int num_ifaces, SystemClockMock& clockmock) + : ifaces(num_ifaces, CanIfaceMock(clockmock)) + , clockmock(clockmock) + , select_failure(false) + { } + + int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uint64_t timeout_usec) + { + assert(this); + std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl; + + if (select_failure) + return -1; + + const int valid_iface_mask = (1 << getNumIfaces()) - 1; + EXPECT_FALSE(inout_write_iface_mask & ~valid_iface_mask); + EXPECT_FALSE(inout_read_iface_mask & ~valid_iface_mask); + + int out_write_mask = 0; + int out_read_mask = 0; + for (int i = 0; i < getNumIfaces(); i++) + { + const int mask = 1 << i; + if ((inout_write_iface_mask & mask) && ifaces.at(i).writeable) + out_write_mask |= mask; + if ((inout_read_iface_mask & mask) && ifaces.at(i).rx.size()) + out_read_mask |= mask; + } + inout_write_iface_mask = out_write_mask; + inout_read_iface_mask = out_read_mask; + if ((out_write_mask | out_read_mask) == 0) + { + clockmock.advance(timeout_usec); // Emulating timeout + return 0; + } + return 1; // This value is not being checked anyway, it just has to be greater than zero + } + + uavcan::ICanIface* getIface(int iface_index) { return &ifaces.at(iface_index); } + int getNumIfaces() const { return ifaces.size(); } +}; diff --git a/libuavcan/test/transport/can/io.cpp b/libuavcan/test/transport/can/io.cpp index f175a501c4..31242179b9 100644 --- a/libuavcan/test/transport/can/io.cpp +++ b/libuavcan/test/transport/can/io.cpp @@ -2,199 +2,10 @@ * Copyright (C) 2014 Pavel Kirienko */ -#include -#include #include -#include -#include "../../common.hpp" +#include "iface_mock.hpp" -class CanIfaceMock : public uavcan::ICanIface -{ -public: - struct FrameWithTime - { - uavcan::CanFrame frame; - uint64_t time; - - FrameWithTime(const uavcan::CanFrame& frame, uint64_t time) - : frame(frame) - , time(time) - { } - }; - - std::queue tx; ///< Queue of outgoing frames (bus <-- library) - std::queue rx; ///< Queue of incoming frames (bus --> library) - bool writeable; - bool tx_failure; - bool rx_failure; - uint64_t num_errors; - SystemClockMock& clockmock; - - CanIfaceMock(SystemClockMock& clockmock) - : writeable(true) - , tx_failure(false) - , rx_failure(false) - , num_errors(0) - , clockmock(clockmock) - { } - - void pushRx(uavcan::CanFrame frame) - { - rx.push(FrameWithTime(frame, clockmock.utc)); - } - - bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline) - { - if (tx.empty()) - { - std::cout << "Tx buffer is empty" << std::endl; - return false; - } - const FrameWithTime frame_time = tx.front(); - tx.pop(); - return (frame_time.frame == frame) && (frame_time.time == tx_deadline); - } - - int send(const uavcan::CanFrame& frame, uint64_t tx_timeout_usec) - { - assert(this); - EXPECT_TRUE(writeable); // Shall never be called when not writeable - if (tx_failure) - return -1; - if (!writeable) - return 0; - const uint64_t monotonic_deadline = tx_timeout_usec + clockmock.monotonic; - tx.push(FrameWithTime(frame, monotonic_deadline)); - return 1; - } - - int receive(uavcan::CanFrame& out_frame, uint64_t& out_ts_monotonic_usec, uint64_t& out_ts_utc_usec) - { - assert(this); - EXPECT_TRUE(rx.size()); // Shall never be called when not readable - if (rx_failure) - return -1; - if (rx.empty()) - return 0; - const FrameWithTime frame = rx.front(); - rx.pop(); - out_frame = frame.frame; - out_ts_monotonic_usec = frame.time; - out_ts_utc_usec = 0; - return 1; - } - - // cppcheck-suppress unusedFunction - // cppcheck-suppress functionConst - int configureFilters(const uavcan::CanFilterConfig* filter_configs, int num_configs) { return -1; } - // cppcheck-suppress unusedFunction - int getNumFilters() const { return 0; } - uint64_t getNumErrors() const { return num_errors; } -}; - -class CanDriverMock : public uavcan::ICanDriver -{ -public: - std::vector ifaces; - SystemClockMock& clockmock; - bool select_failure; - - CanDriverMock(int num_ifaces, SystemClockMock& clockmock) - : ifaces(num_ifaces, CanIfaceMock(clockmock)) - , clockmock(clockmock) - , select_failure(false) - { } - - int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uint64_t timeout_usec) - { - assert(this); - std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl; - - if (select_failure) - return -1; - - const int valid_iface_mask = (1 << getNumIfaces()) - 1; - EXPECT_FALSE(inout_write_iface_mask & ~valid_iface_mask); - EXPECT_FALSE(inout_read_iface_mask & ~valid_iface_mask); - - int out_write_mask = 0; - int out_read_mask = 0; - for (int i = 0; i < getNumIfaces(); i++) - { - const int mask = 1 << i; - if ((inout_write_iface_mask & mask) && ifaces.at(i).writeable) - out_write_mask |= mask; - if ((inout_read_iface_mask & mask) && ifaces.at(i).rx.size()) - out_read_mask |= mask; - } - inout_write_iface_mask = out_write_mask; - inout_read_iface_mask = out_read_mask; - if ((out_write_mask | out_read_mask) == 0) - { - clockmock.advance(timeout_usec); // Emulating timeout - return 0; - } - return 1; // This value is not being checked anyway, it just has to be greater than zero - } - - uavcan::ICanIface* getIface(int iface_index) { return &ifaces.at(iface_index); } - int getNumIfaces() const { return ifaces.size(); } -}; - -TEST(CanIOManager, CanDriverMock) -{ - using uavcan::CanFrame; - - SystemClockMock clockmock; - CanDriverMock driver(3, clockmock); - - ASSERT_EQ(3, driver.getNumIfaces()); - - // All WR, no RD - int mask_wr = 7; - int mask_rd = 7; - EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100)); - EXPECT_EQ(7, mask_wr); - EXPECT_EQ(0, mask_rd); - - for (int i = 0; i < 3; i++) - driver.ifaces.at(i).writeable = false; - - // No WR, no RD - mask_wr = 7; - mask_rd = 7; - EXPECT_EQ(0, driver.select(mask_wr, mask_rd, 100)); - EXPECT_EQ(0, mask_wr); - EXPECT_EQ(0, mask_rd); - EXPECT_EQ(100, clockmock.monotonic); - EXPECT_EQ(100, clockmock.utc); - - // No WR, #1 RD - const CanFrame fr1 = makeCanFrame(123, "foo", EXT); - driver.ifaces.at(1).pushRx(fr1); - mask_wr = 7; - mask_rd = 6; - EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100)); - EXPECT_EQ(0, mask_wr); - EXPECT_EQ(2, mask_rd); - CanFrame fr2; - uint64_t ts_monotonic, ts_utc; - EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc)); - EXPECT_EQ(fr1, fr2); - EXPECT_EQ(100, ts_monotonic); - EXPECT_EQ(0, ts_utc); - - // #0 WR, #1 RD, Select failure - driver.ifaces.at(0).writeable = true; - driver.select_failure = true; - mask_wr = 1; - mask_rd = 7; - EXPECT_EQ(-1, driver.select(mask_wr, mask_rd, 100)); - EXPECT_EQ(1, mask_wr); // Leaving masks unchanged - library must ignore them - EXPECT_EQ(7, mask_rd); -} - static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame, uint64_t timestamp, int iface_index) { diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index f12e336bcc..d47f571aaa 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -2,293 +2,8 @@ * Copyright (C) 2014 Pavel Kirienko */ -#include -#include #include -#include - - -struct Transfer -{ - uint64_t ts_monotonic; - uint64_t ts_utc; - uavcan::TransferType transfer_type; - uavcan::TransferID transfer_id; - uint8_t source_node_id; - std::string payload; - - Transfer(const uavcan::IncomingTransfer& tr) - : ts_monotonic(tr.getMonotonicTimestamp()) - , ts_utc(tr.getUtcTimestamp()) - , transfer_type(tr.getTransferType()) - , transfer_id(tr.getTransferID()) - , source_node_id(tr.getSourceNodeID()) - { - unsigned int offset = 0; - while (true) - { - uint8_t buf[256]; - int res = tr.read(offset, buf, sizeof(buf)); - if (res < 0) - { - std::cout << "IncomingTransferContainer: read failure " << res << std::endl; - exit(1); - } - if (res == 0) - break; - payload += std::string(reinterpret_cast(buf), res); - offset += res; - } - } - - Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferType transfer_type, - uavcan::TransferID transfer_id, uint8_t source_node_id, const std::string& payload) - : ts_monotonic(ts_monotonic) - , ts_utc(ts_utc) - , transfer_type(transfer_type) - , transfer_id(transfer_id) - , source_node_id(source_node_id) - , payload(payload) - { } - - bool operator==(const Transfer& rhs) const - { - return - (ts_monotonic == rhs.ts_monotonic) && - (ts_utc == rhs.ts_utc) && - (transfer_type == rhs.transfer_type) && - (transfer_id == rhs.transfer_id) && - (source_node_id == rhs.source_node_id) && - (payload == rhs.payload); - } - - std::string toString() const - { - std::ostringstream os; - os << "ts_m=" << ts_monotonic - << " ts_utc=" << ts_utc - << " tt=" << transfer_type - << " tid=" << int(transfer_id.get()) - << " snid=" << int(source_node_id) - << "\n\t'" << payload << "'"; - return os.str(); - } -}; - - -TEST(TransferListener, TestingEnvironmentTransfer) -{ - uavcan::PoolAllocator pool; - uavcan::PoolManager<1> poolmgr; - poolmgr.addPool(&pool); - - uavcan::TransferBufferManager<128, 1> mgr(&poolmgr); - uavcan::TransferBufferAccessor tba(&mgr, uavcan::TransferBufferManagerKey(0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST)); - - uavcan::RxFrame frame; - frame.last_frame = true; - uavcan::MultiFrameIncomingTransfer mfit(10, 1000, frame, tba); - - // Filling the buffer with data - static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!"; - ASSERT_TRUE(tba.create()); - ASSERT_EQ(TEST_DATA.length(), - tba.access()->write(0, reinterpret_cast(TEST_DATA.c_str()), TEST_DATA.length())); - - // Reading back - const Transfer transfer(mfit); - ASSERT_EQ(TEST_DATA, transfer.payload); -} - - -static std::vector serializeTransfer(const Transfer& transfer, uint8_t target_node_id, - const uavcan::DataTypeDescriptor& type) -{ - uavcan::Crc16 payload_crc(type.hash.value, uavcan::DataTypeHash::NUM_BYTES); - payload_crc.add(reinterpret_cast(transfer.payload.c_str()), transfer.payload.length()); - - std::vector raw_payload; - bool need_crc = false; - - switch (transfer.transfer_type) - { - case uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST: - need_crc = transfer.payload.length() > uavcan::Frame::PAYLOAD_LEN_MAX; - break; - case uavcan::TRANSFER_TYPE_SERVICE_RESPONSE: - case uavcan::TRANSFER_TYPE_SERVICE_REQUEST: - case uavcan::TRANSFER_TYPE_MESSAGE_UNICAST: - need_crc = transfer.payload.length() > (uavcan::Frame::PAYLOAD_LEN_MAX - 1); - raw_payload.push_back(target_node_id); - break; - default: - std::cerr << "X_X" << std::endl; - std::exit(1); - } - - if (need_crc) - { - // Little endian - raw_payload.push_back(payload_crc.get() & 0xFF); - raw_payload.push_back((payload_crc.get() >> 8) & 0xFF); - } - - raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end()); - - std::vector output; - unsigned int frame_index = 0; - unsigned int offset = 0; - uint64_t ts_monotonic = transfer.ts_monotonic; - uint64_t ts_utc = transfer.ts_utc; - - while (true) - { - int bytes_left = raw_payload.size() - offset; - EXPECT_TRUE(bytes_left >= 0); - if (bytes_left <= 0 && !raw_payload.empty()) - break; - - uavcan::RxFrame frm; - - frm.data_type_id = type.id; - frm.frame_index = frame_index++; - EXPECT_TRUE(uavcan::Frame::FRAME_INDEX_MAX >= frame_index); - - frm.iface_index = 0; - frm.last_frame = bytes_left <= uavcan::Frame::PAYLOAD_LEN_MAX; - frm.payload_len = std::min(int(uavcan::Frame::PAYLOAD_LEN_MAX), bytes_left); - std::copy(raw_payload.begin() + offset, raw_payload.begin() + offset + frm.payload_len, frm.payload); - offset += frm.payload_len; - - frm.source_node_id = transfer.source_node_id; - frm.transfer_id = transfer.transfer_id; - frm.transfer_type = transfer.transfer_type; - - frm.ts_monotonic = ts_monotonic; - frm.ts_utc = ts_utc; - ts_monotonic += 1; - ts_utc += 1; - - output.push_back(frm); - if (raw_payload.empty()) - break; - } - return output; -} - - -TEST(TransferListener, TestingEnvironmentMFTSerialization) -{ - uavcan::DataTypeDescriptor type(uavcan::DATA_TYPE_KIND_MESSAGE, 123, uavcan::DataTypeHash()); - for (int i = 0; i < uavcan::DataTypeHash::NUM_BYTES; i++) - type.hash.value[i] = i; - - static const std::string DATA = "To go wrong in one's own way is better than to go right in someone else's."; - const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 12, 42, DATA); - - const std::vector ser = serializeTransfer(transfer, 127, type); - - std::cout << "Serialized transfer:\n"; - for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) - std::cout << "\t" << it->toString() << "\n"; - - for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) - { - std::cout << "\t'"; - for (int i = 0; i < it->payload_len; i++) - { - uint8_t ch = it->payload[i]; - if (ch < 0x20 || ch > 0x7E) - ch = '.'; - std::cout << static_cast(ch); - } - std::cout << "'\n"; - } - std::cout << std::flush; -} - - -TEST(TransferListener, TestingEnvironmentSFTSerialization) -{ - uavcan::DataTypeDescriptor type(uavcan::DATA_TYPE_KIND_MESSAGE, 123, uavcan::DataTypeHash()); - for (int i = 0; i < uavcan::DataTypeHash::NUM_BYTES; i++) - type.hash.value[i] = i; - - { - const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 12, 42, "Nvrfrget"); - const std::vector ser = serializeTransfer(transfer, 127, type); - ASSERT_EQ(1, ser.size()); - std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; - } - { - const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 12, 42, "7-chars"); - const std::vector ser = serializeTransfer(transfer, 127, type); - ASSERT_EQ(1, ser.size()); - std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; - } - { - const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 12, 42, ""); - const std::vector ser = serializeTransfer(transfer, 127, type); - ASSERT_EQ(1, ser.size()); - std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; - } - { - const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 12, 42, ""); - const std::vector ser = serializeTransfer(transfer, 127, type); - ASSERT_EQ(1, ser.size()); - std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; - } -} - - -/** - * This subscriber accepts any types of transfers - this makes testing easier. - * In reality, uavcan::TransferListener should accept only specific transfer types - * which are dispatched/filtered by uavcan::Dispatcher. - */ -template -class TestSubscriber : public uavcan::TransferListener -{ - typedef uavcan::TransferListener Base; - - std::queue transfers_; - -public: - TestSubscriber(const uavcan::DataTypeDescriptor* data_type, uavcan::IAllocator* allocator) - : Base(data_type, allocator) - { } - - void handleIncomingTransfer(uavcan::IncomingTransfer& transfer) - { - const Transfer rx(transfer); - transfers_.push(rx); - std::cout << "Received transfer: " << rx.toString() << std::endl; - } - - bool matchAndPop(const Transfer& reference) - { - if (transfers_.empty()) - { - std::cout << "No received transfers" << std::endl; - return false; - } - - const Transfer tr = transfers_.front(); - transfers_.pop(); - - const bool res = (tr == reference); - if (!res) - { - std::cout << "TestSubscriber: Transfer mismatch:\n" - << "Expected: " << reference.toString() << "\n" - << "Received: " << tr.toString() << std::endl; - } - return res; - } - - int getNumReceivedTransfers() const { return transfers_.size(); } - bool isEmpty() const { return transfers_.empty(); } -}; +#include "transfer_test_helpers.hpp" class Emulator diff --git a/libuavcan/test/transport/transfer_test_helpers.cpp b/libuavcan/test/transport/transfer_test_helpers.cpp new file mode 100644 index 0000000000..4b1206ceba --- /dev/null +++ b/libuavcan/test/transport/transfer_test_helpers.cpp @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "transfer_test_helpers.hpp" + + +TEST(TransferTestHelpers, Transfer) +{ + uavcan::PoolAllocator pool; + uavcan::PoolManager<1> poolmgr; + poolmgr.addPool(&pool); + + uavcan::TransferBufferManager<128, 1> mgr(&poolmgr); + uavcan::TransferBufferAccessor tba(&mgr, uavcan::TransferBufferManagerKey(0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST)); + + uavcan::RxFrame frame; + frame.last_frame = true; + uavcan::MultiFrameIncomingTransfer mfit(10, 1000, frame, tba); + + // Filling the buffer with data + static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!"; + ASSERT_TRUE(tba.create()); + ASSERT_EQ(TEST_DATA.length(), + tba.access()->write(0, reinterpret_cast(TEST_DATA.c_str()), TEST_DATA.length())); + + // Reading back + const Transfer transfer(mfit); + ASSERT_EQ(TEST_DATA, transfer.payload); +} + + +TEST(TransferTestHelpers, MFTSerialization) +{ + uavcan::DataTypeDescriptor type(uavcan::DATA_TYPE_KIND_MESSAGE, 123, uavcan::DataTypeHash()); + for (int i = 0; i < uavcan::DataTypeHash::NUM_BYTES; i++) + type.hash.value[i] = i; + + static const std::string DATA = "To go wrong in one's own way is better than to go right in someone else's."; + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 12, 42, DATA); + + const std::vector ser = serializeTransfer(transfer, 127, type); + + std::cout << "Serialized transfer:\n"; + for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) + std::cout << "\t" << it->toString() << "\n"; + + for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) + { + std::cout << "\t'"; + for (int i = 0; i < it->payload_len; i++) + { + uint8_t ch = it->payload[i]; + if (ch < 0x20 || ch > 0x7E) + ch = '.'; + std::cout << static_cast(ch); + } + std::cout << "'\n"; + } + std::cout << std::flush; +} + + +TEST(TransferTestHelpers, SFTSerialization) +{ + uavcan::DataTypeDescriptor type(uavcan::DATA_TYPE_KIND_MESSAGE, 123, uavcan::DataTypeHash()); + for (int i = 0; i < uavcan::DataTypeHash::NUM_BYTES; i++) + type.hash.value[i] = i; + + { + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 12, 42, "Nvrfrget"); + const std::vector ser = serializeTransfer(transfer, 127, type); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 12, 42, "7-chars"); + const std::vector ser = serializeTransfer(transfer, 127, type); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 12, 42, ""); + const std::vector ser = serializeTransfer(transfer, 127, type); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 12, 42, ""); + const std::vector ser = serializeTransfer(transfer, 127, type); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } +} diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp new file mode 100644 index 0000000000..64279cf50c --- /dev/null +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + + +struct Transfer +{ + uint64_t ts_monotonic; + uint64_t ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uint8_t source_node_id; + std::string payload; + + Transfer(const uavcan::IncomingTransfer& tr) + : ts_monotonic(tr.getMonotonicTimestamp()) + , ts_utc(tr.getUtcTimestamp()) + , transfer_type(tr.getTransferType()) + , transfer_id(tr.getTransferID()) + , source_node_id(tr.getSourceNodeID()) + { + unsigned int offset = 0; + while (true) + { + uint8_t buf[256]; + int res = tr.read(offset, buf, sizeof(buf)); + if (res < 0) + { + std::cout << "IncomingTransferContainer: read failure " << res << std::endl; + exit(1); + } + if (res == 0) + break; + payload += std::string(reinterpret_cast(buf), res); + offset += res; + } + } + + Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferType transfer_type, + uavcan::TransferID transfer_id, uint8_t source_node_id, const std::string& payload) + : ts_monotonic(ts_monotonic) + , ts_utc(ts_utc) + , transfer_type(transfer_type) + , transfer_id(transfer_id) + , source_node_id(source_node_id) + , payload(payload) + { } + + bool operator==(const Transfer& rhs) const + { + return + (ts_monotonic == rhs.ts_monotonic) && + (ts_utc == rhs.ts_utc) && + (transfer_type == rhs.transfer_type) && + (transfer_id == rhs.transfer_id) && + (source_node_id == rhs.source_node_id) && + (payload == rhs.payload); + } + + std::string toString() const + { + std::ostringstream os; + os << "ts_m=" << ts_monotonic + << " ts_utc=" << ts_utc + << " tt=" << transfer_type + << " tid=" << int(transfer_id.get()) + << " snid=" << int(source_node_id) + << "\n\t'" << payload << "'"; + return os.str(); + } +}; + +/** + * This subscriber accepts any types of transfers - this makes testing easier. + * In reality, uavcan::TransferListener should accept only specific transfer types + * which are dispatched/filtered by uavcan::Dispatcher. + */ +template +class TestSubscriber : public uavcan::TransferListener +{ + typedef uavcan::TransferListener Base; + + std::queue transfers_; + +public: + TestSubscriber(const uavcan::DataTypeDescriptor* data_type, uavcan::IAllocator* allocator) + : Base(data_type, allocator) + { } + + void handleIncomingTransfer(uavcan::IncomingTransfer& transfer) + { + const Transfer rx(transfer); + transfers_.push(rx); + std::cout << "Received transfer: " << rx.toString() << std::endl; + } + + bool matchAndPop(const Transfer& reference) + { + if (transfers_.empty()) + { + std::cout << "No received transfers" << std::endl; + return false; + } + + const Transfer tr = transfers_.front(); + transfers_.pop(); + + const bool res = (tr == reference); + if (!res) + { + std::cout << "TestSubscriber: Transfer mismatch:\n" + << "Expected: " << reference.toString() << "\n" + << "Received: " << tr.toString() << std::endl; + } + return res; + } + + int getNumReceivedTransfers() const { return transfers_.size(); } + bool isEmpty() const { return transfers_.empty(); } +}; + + +namespace +{ + +std::vector serializeTransfer(const Transfer& transfer, uint8_t target_node_id, + const uavcan::DataTypeDescriptor& type) +{ + uavcan::Crc16 payload_crc(type.hash.value, uavcan::DataTypeHash::NUM_BYTES); + payload_crc.add(reinterpret_cast(transfer.payload.c_str()), transfer.payload.length()); + + std::vector raw_payload; + bool need_crc = false; + + switch (transfer.transfer_type) + { + case uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST: + need_crc = transfer.payload.length() > uavcan::Frame::PAYLOAD_LEN_MAX; + break; + case uavcan::TRANSFER_TYPE_SERVICE_RESPONSE: + case uavcan::TRANSFER_TYPE_SERVICE_REQUEST: + case uavcan::TRANSFER_TYPE_MESSAGE_UNICAST: + need_crc = transfer.payload.length() > (uavcan::Frame::PAYLOAD_LEN_MAX - 1); + raw_payload.push_back(target_node_id); + break; + default: + std::cerr << "X_X" << std::endl; + std::exit(1); + } + + if (need_crc) + { + // Little endian + raw_payload.push_back(payload_crc.get() & 0xFF); + raw_payload.push_back((payload_crc.get() >> 8) & 0xFF); + } + + raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end()); + + std::vector output; + unsigned int frame_index = 0; + unsigned int offset = 0; + uint64_t ts_monotonic = transfer.ts_monotonic; + uint64_t ts_utc = transfer.ts_utc; + + while (true) + { + int bytes_left = raw_payload.size() - offset; + EXPECT_TRUE(bytes_left >= 0); + if (bytes_left <= 0 && !raw_payload.empty()) + break; + + uavcan::RxFrame frm; + + frm.data_type_id = type.id; + frm.frame_index = frame_index++; + EXPECT_TRUE(uavcan::Frame::FRAME_INDEX_MAX >= frame_index); + + frm.iface_index = 0; + frm.last_frame = bytes_left <= uavcan::Frame::PAYLOAD_LEN_MAX; + frm.payload_len = std::min(int(uavcan::Frame::PAYLOAD_LEN_MAX), bytes_left); + std::copy(raw_payload.begin() + offset, raw_payload.begin() + offset + frm.payload_len, frm.payload); + offset += frm.payload_len; + + frm.source_node_id = transfer.source_node_id; + frm.transfer_id = transfer.transfer_id; + frm.transfer_type = transfer.transfer_type; + + frm.ts_monotonic = ts_monotonic; + frm.ts_utc = ts_utc; + ts_monotonic += 1; + ts_utc += 1; + + output.push_back(frm); + if (raw_payload.empty()) + break; + } + return output; +} + +} +