From cd851312ff8f78a6aca254f50811966b3b25cdf3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 19 Feb 2014 17:05:19 +0400 Subject: [PATCH] TransferSender implementation and tests --- .../internal/transport/transfer_sender.hpp | 55 +++++++ libuavcan/src/transport/transfer_sender.cpp | 109 +++++++++++++ libuavcan/test/transport/transfer_sender.cpp | 143 ++++++++++++++++++ .../test/transport/transfer_test_helpers.hpp | 1 + 4 files changed, 308 insertions(+) create mode 100644 libuavcan/include/uavcan/internal/transport/transfer_sender.hpp create mode 100644 libuavcan/src/transport/transfer_sender.cpp create mode 100644 libuavcan/test/transport/transfer_sender.cpp diff --git a/libuavcan/include/uavcan/internal/transport/transfer_sender.hpp b/libuavcan/include/uavcan/internal/transport/transfer_sender.hpp new file mode 100644 index 0000000000..7252c5365a --- /dev/null +++ b/libuavcan/include/uavcan/internal/transport/transfer_sender.hpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class TransferSender +{ + static const uint64_t DEFAULT_MAX_TRANSFER_INTERVAL = 60 * 1000 * 1000; + + const uint64_t max_transfer_interval_; + const DataTypeDescriptor& data_type_; + const CanTxQueue::Qos qos_; + const Crc16 crc_base_; + + Dispatcher& dispatcher_; + +public: + TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, + uint64_t max_transfer_interval = DEFAULT_MAX_TRANSFER_INTERVAL) + : max_transfer_interval_(max_transfer_interval) + , data_type_(data_type) + , qos_(qos) + , crc_base_(data_type.hash.value, DataTypeHash::NUM_BYTES) + , dispatcher_(dispatcher) + { } + + /** + * Send with explicit Transfer ID. + * Should be used only for service responses, where response TID should match request TID. + */ + int send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline, + uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id, + TransferID tid); + + /** + * Send with automatic Transfer ID. + * TID is managed by OutgoingTransferRegistry. + */ + int send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline, + uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id); +}; + +} diff --git a/libuavcan/src/transport/transfer_sender.cpp b/libuavcan/src/transport/transfer_sender.cpp new file mode 100644 index 0000000000..deb7e96f6d --- /dev/null +++ b/libuavcan/src/transport/transfer_sender.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +namespace uavcan +{ + +int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline, + uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id, + TransferID tid) +{ + Frame frame(data_type_.id, transfer_type, dispatcher_.getSelfNodeID(), dst_node_id, 0, tid); + + if (frame.getMaxPayloadLen() >= payload_len) // Single Frame Transfer + { + const int res = frame.setPayload(payload, payload_len); + if (res != payload_len) + { + assert(0); + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res); + return (res < 0) ? res : -1; + } + frame.makeLast(); + assert(frame.isLast() && frame.isFirst()); + return dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_); + } + else // Multi Frame Transfer + { + int offset = 0; + { + Crc16 crc = crc_base_; + crc.add(payload, payload_len); + + static const int BUFLEN = sizeof(CanFrame::data); + uint8_t buf[BUFLEN]; + + buf[0] = crc.get() & 0xFF; // Transfer CRC, little endian + buf[1] = (crc.get() >> 8) & 0xFF; + std::copy(payload, payload + BUFLEN - 2, buf + 2); + + const int write_res = frame.setPayload(buf, BUFLEN); + if (write_res < 2) + { + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res); + return write_res; + } + offset = write_res - 2; + assert(payload_len > offset); + } + + int next_frame_index = 1; + + while (true) + { + const int send_res = dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_); + if (send_res < 0) + return send_res; + + if (frame.isLast()) + return next_frame_index; // Number of frames transmitted + + frame.setIndex(next_frame_index++); + + const int write_res = frame.setPayload(payload + offset, payload_len - offset); + if (write_res < 0) + { + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res); + return write_res; + } + + offset += write_res; + assert(offset <= payload_len); + if (offset >= payload_len) + frame.makeLast(); + } + } + + assert(0); + return -1; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable. +} + +int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline, + uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id) +{ + const OutgoingTransferRegistryKey otr_key(data_type_.id, transfer_type, dst_node_id); + + assert(monotonic_tx_deadline > 0); + const uint64_t otr_deadline = monotonic_tx_deadline + max_transfer_interval_; + + TransferID* const tid = dispatcher_.getOutgoingTransferRegistry()->accessOrCreate(otr_key, otr_deadline); + if (tid == NULL) + { + UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%i tt=%i", int(data_type_.id), int(transfer_type)); + return -1; + } + + const TransferID this_tid = tid->get(); + tid->increment(); + + return send(payload, payload_len, monotonic_tx_deadline, monotonic_blocking_deadline, transfer_type, + dst_node_id, this_tid); +} + +} diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp new file mode 100644 index 0000000000..7986496631 --- /dev/null +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "transfer_test_helpers.hpp" +#include "can/iface_mock.hpp" +#include + + +static int sendOne(uavcan::TransferSender& sender, const std::string& data, + uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, + uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id) +{ + return sender.send(reinterpret_cast(data.c_str()), data.length(), monotonic_tx_deadline, + monotonic_blocking_deadline, transfer_type, dst_node_id); +} + +static int sendOne(uavcan::TransferSender& sender, const std::string& data, + uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, + uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid) +{ + return sender.send(reinterpret_cast(data.c_str()), data.length(), monotonic_tx_deadline, + monotonic_blocking_deadline, transfer_type, dst_node_id, tid); +} + + +TEST(TransferSender, Basic) +{ + uavcan::PoolManager<1> poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::OutgoingTransferRegistry<8> out_trans_reg(&poolmgr); + + + static const uavcan::NodeID TX_NODE_ID(64); + static const uavcan::NodeID RX_NODE_ID(65); + uavcan::Dispatcher dispatcher_tx(&driver, &poolmgr, &clockmock, &out_trans_reg, TX_NODE_ID); + uavcan::Dispatcher dispatcher_rx(&driver, &poolmgr, &clockmock, &out_trans_reg, RX_NODE_ID); + + /* + * Test environment + */ + static const uavcan::DataTypeDescriptor TYPES[2] = + { + makeDataType(uavcan::DATA_TYPE_KIND_MESSAGE, 1), + makeDataType(uavcan::DATA_TYPE_KIND_SERVICE, 1) + }; + + uavcan::TransferSender senders[2] = + { + uavcan::TransferSender(dispatcher_tx, TYPES[0], uavcan::CanTxQueue::VOLATILE), + uavcan::TransferSender(dispatcher_tx, TYPES[1], uavcan::CanTxQueue::PERSISTENT) + }; + + static const std::string DATA[4] = + { + "Don't panic.", + + "The ships hung in the sky in much the same way that bricks don't.", + + "Would it save you a lot of time if I just gave up and went mad now?", + + "If there's anything more important than my ego around, I want it caught and shot now." + }; + + /* + * Transmission + */ + static const uint64_t TX_DEADLINE = 1000000; + + sendOne(senders[0], DATA[0], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 0); + sendOne(senders[0], DATA[1], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, RX_NODE_ID); + sendOne(senders[0], "123", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 0); + sendOne(senders[0], "456", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, RX_NODE_ID); + + sendOne(senders[1], DATA[2], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, RX_NODE_ID); + sendOne(senders[1], DATA[3], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, RX_NODE_ID, 1); + sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, RX_NODE_ID); + sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, RX_NODE_ID, 2); + + static const Transfer TRANSFERS[8] = + { + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 0, TX_NODE_ID, 0, DATA[0], TYPES[0]), + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 0, TX_NODE_ID, RX_NODE_ID, DATA[1], TYPES[0]), + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 1, TX_NODE_ID, 0, "123", TYPES[0]), + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 1, TX_NODE_ID, RX_NODE_ID, "456", TYPES[0]), + + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 0, TX_NODE_ID, RX_NODE_ID, DATA[2], TYPES[1]), + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 1, TX_NODE_ID, RX_NODE_ID, DATA[3], TYPES[1]), + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 1, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]), + Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 2, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]) + }; + + /* + * Receiving on the other side. + */ + for (int i = 0; i < driver.getNumIfaces(); i++) // Moving the frames from TX to RX side + { + CanIfaceMock& iface = driver.ifaces.at(i); + std::cout << "Num frames: " << iface.tx.size() << std::endl; + while (!iface.tx.empty()) + { + CanIfaceMock::FrameWithTime ft = iface.tx.front(); + iface.tx.pop(); + iface.rx.push(ft); + } + } + + TestSubscriber<512, 2, 2> sub_msg(TYPES + 0, &poolmgr); + TestSubscriber<512, 2, 2> sub_srv_req(TYPES + 1, &poolmgr); + TestSubscriber<512, 2, 2> sub_srv_resp(TYPES + 1, &poolmgr); + + dispatcher_rx.registerMessageListener(&sub_msg); + dispatcher_rx.registerServiceRequestListener(&sub_srv_req); + dispatcher_rx.registerServiceResponseListener(&sub_srv_resp); + + while (true) + { + const int res = dispatcher_rx.spin(0); + ASSERT_LE(0, res); + clockmock.advance(100); + if (res == 0) + break; + } + + /* + * Validation + */ + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[0])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[1])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[2])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[3])); + + ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[4])); + ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[6])); + + ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[5])); + ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[7])); +} diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp index ce9288dbc7..82ebd46ef0 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -85,6 +85,7 @@ struct Transfer << " tid=" << int(transfer_id.get()) << " snid=" << int(src_node_id.get()) << " dnid=" << int(dst_node_id.get()) + << " dtid=" << int(data_type.id) << "\n\t'" << payload << "'"; return os.str(); }