From 697a55aebb976bf5dde78d2fded80325652dcf5f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2014 12:35:12 +0400 Subject: [PATCH] Refactoring: Frame Index field size increased, Trnasfer ID field size reduced. NodeID class added, Frame class rewritten with stricter runtime checks. All tests were updated accordingly. --- .../uavcan/internal/transport/dispatcher.hpp | 6 +- .../transport/outgoing_transfer_registry.hpp | 8 +- .../uavcan/internal/transport/transfer.hpp | 181 ++++++++++------ .../internal/transport/transfer_buffer.hpp | 12 +- .../internal/transport/transfer_listener.hpp | 18 +- .../internal/transport/transfer_receiver.hpp | 2 - libuavcan/src/transport/dispatcher.cpp | 46 ++-- libuavcan/src/transport/transfer.cpp | 197 +++++++++++++++--- libuavcan/src/transport/transfer_buffer.cpp | 2 +- libuavcan/src/transport/transfer_listener.cpp | 34 ++- libuavcan/src/transport/transfer_receiver.cpp | 132 ++++-------- .../test/transport/incoming_transfer.cpp | 35 ++-- libuavcan/test/transport/transfer.cpp | 126 +++++------ libuavcan/test/transport/transfer_buffer.cpp | 2 +- .../test/transport/transfer_listener.cpp | 23 +- .../test/transport/transfer_receiver.cpp | 120 +++++------ .../test/transport/transfer_test_helpers.cpp | 29 ++- .../test/transport/transfer_test_helpers.hpp | 60 +++--- 18 files changed, 544 insertions(+), 489 deletions(-) diff --git a/libuavcan/include/uavcan/internal/transport/dispatcher.hpp b/libuavcan/include/uavcan/internal/transport/dispatcher.hpp index ed9a89d433..b9895b4ffd 100644 --- a/libuavcan/include/uavcan/internal/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/internal/transport/dispatcher.hpp @@ -49,13 +49,13 @@ class Dispatcher : Noncopyable ListenerRegister lsrv_req_; ListenerRegister lsrv_resp_; - const uint8_t self_node_id_; + const NodeID self_node_id_; void handleFrame(const CanRxFrame& can_frame); public: Dispatcher(ICanDriver* driver, IAllocator* allocator, ISystemClock* sysclock, IOutgoingTransferRegistry* otr, - uint8_t self_node_id) + NodeID self_node_id) : canio_(driver, allocator, sysclock) , sysclock_(sysclock) , outgoing_transfer_reg_(otr) @@ -87,7 +87,7 @@ public: IOutgoingTransferRegistry* getOutgoingTransferRegistry() { return outgoing_transfer_reg_; } - uint8_t getSelfNodeID() const { return self_node_id_; } + NodeID getSelfNodeID() const { return self_node_id_; } }; } diff --git a/libuavcan/include/uavcan/internal/transport/outgoing_transfer_registry.hpp b/libuavcan/include/uavcan/internal/transport/outgoing_transfer_registry.hpp index 3378355959..61cf44539b 100644 --- a/libuavcan/include/uavcan/internal/transport/outgoing_transfer_registry.hpp +++ b/libuavcan/include/uavcan/internal/transport/outgoing_transfer_registry.hpp @@ -18,22 +18,20 @@ class OutgoingTransferRegistryKey { uint16_t data_type_id_; uint8_t transfer_type_; - uint8_t destination_node_id_; ///< Not applicable for message broadcasting + NodeID destination_node_id_; ///< Not applicable for message broadcasting public: OutgoingTransferRegistryKey() : data_type_id_(0xFFFF) , transfer_type_(0xFF) - , destination_node_id_(NODE_ID_INVALID) { } - OutgoingTransferRegistryKey(uint16_t data_type_id, TransferType transfer_type, uint8_t destination_node_id) + OutgoingTransferRegistryKey(uint16_t data_type_id, TransferType transfer_type, NodeID destination_node_id) : data_type_id_(data_type_id) , transfer_type_(transfer_type) , destination_node_id_(destination_node_id) { - assert(destination_node_id != NODE_ID_INVALID); - assert((transfer_type == TRANSFER_TYPE_MESSAGE_BROADCAST) == (destination_node_id == NODE_ID_BROADCAST)); + assert((transfer_type == TRANSFER_TYPE_MESSAGE_BROADCAST) == destination_node_id.isBroadcast()); /* Service response transfers must use the same Transfer ID as matching service request transfer, * so this registry is not applicable for service response transfers at all. diff --git a/libuavcan/include/uavcan/internal/transport/transfer.hpp b/libuavcan/include/uavcan/internal/transport/transfer.hpp index 8e4a58e1e6..7aa6797826 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer.hpp @@ -12,13 +12,6 @@ namespace uavcan { -enum NodeIDConstants -{ - NODE_ID_BROADCAST = 0, - NODE_ID_MAX = 127, - NODE_ID_INVALID = 255 -}; - enum TransferType { TRANSFER_TYPE_SERVICE_RESPONSE = 0, @@ -29,12 +22,46 @@ enum TransferType }; +class NodeID +{ + enum + { + VALUE_BROADCAST = 0, + VALUE_INVALID = 0xFF + }; + uint8_t value_; + +public: + enum { BITLEN = 7 }; + enum { MAX = (1 << BITLEN) - 1 }; + + static const NodeID BROADCAST; + + NodeID() : value_(VALUE_INVALID) { } + + NodeID(uint8_t value) + : value_(value) + { + assert(isValid()); + } + + uint8_t get() const { return value_; } + + bool isValid() const { return value_ <= MAX; } + bool isBroadcast() const { return value_ == VALUE_BROADCAST; } + bool isUnicast() const { return (value_ <= MAX) && (value_ != VALUE_BROADCAST); } + + bool operator!=(NodeID rhs) const { return !operator==(rhs); } + bool operator==(NodeID rhs) const { return value_ == rhs.value_; } +}; + + class TransferID { uint8_t value_; public: - enum { BITLEN = 4 }; + enum { BITLEN = 3 }; enum { MAX = (1 << BITLEN) - 1 }; TransferID() @@ -69,94 +96,114 @@ public: }; -struct Frame +class Frame { - enum { DATA_TYPE_ID_MAX = 1023 }; - enum { FRAME_INDEX_MAX = 31 }; - enum { PAYLOAD_LEN_MAX = 8 }; + uint8_t payload_[sizeof(CanFrame::data)]; + TransferType transfer_type_; + uint_fast16_t data_type_id_; + uint_fast8_t payload_len_; + NodeID src_node_id_; + NodeID dst_node_id_; + uint_fast8_t frame_index_; + TransferID transfer_id_; + bool last_frame_; - uint8_t payload[PAYLOAD_LEN_MAX]; - TransferType transfer_type; - uint_fast16_t data_type_id; - uint_fast8_t payload_len; - uint_fast8_t source_node_id; - uint_fast8_t frame_index; - TransferID transfer_id; - bool last_frame; +public: + enum { DATA_TYPE_ID_MAX = 1023 }; + enum { FRAME_INDEX_MAX = 62 }; // 63 (or 0b111111) is reserved 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) + : transfer_type_(TransferType(NUM_TRANSFER_TYPES)) // That is invalid value + , data_type_id_(0) + , payload_len_(0) + , frame_index_(0) + , transfer_id_(0) + , last_frame_(false) + { } + + Frame(uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false) + : transfer_type_(transfer_type) + , data_type_id_(data_type_id) + , payload_len_(0) + , src_node_id_(src_node_id) + , dst_node_id_(dst_node_id) + , frame_index_(frame_index) + , transfer_id_(transfer_id) + , last_frame_(last_frame) { - std::fill(payload, payload + sizeof(payload), 0); + assert((transfer_type == TRANSFER_TYPE_MESSAGE_BROADCAST) == dst_node_id.isBroadcast()); + assert(data_type_id <= DATA_TYPE_ID_MAX); + assert(src_node_id != dst_node_id); + assert(frame_index <= FRAME_INDEX_MAX); } - 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) - , data_type_id(data_type_id) - , payload_len(payload_len) - , source_node_id(source_node_id) - , frame_index(frame_index) - , transfer_id(transfer_id) - , last_frame(last_frame) - { - 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); - } + int getMaxPayloadLen() const; + int setPayload(const uint8_t* data, int len); + + int getPayloadLen() const { return payload_len_; } + const uint8_t* getPayloadPtr() const { return payload_; } + + TransferType getTransferType() const { return transfer_type_; } + uint_fast16_t getDataTypeID() const { return data_type_id_; } + NodeID getSrcNodeID() const { return src_node_id_; } + NodeID getDstNodeID() const { return dst_node_id_; } + TransferID getTransferID() const { return transfer_id_; } + uint_fast8_t getFrameIndex() const { return frame_index_; } + bool isLastFrame() const { return last_frame_; } + + void makeLast() { last_frame_ = true; } + + bool isFirstFrame() const { return frame_index_ == 0; } bool parse(const CanFrame& can_frame); + bool compile(CanFrame& can_frame) const; - CanFrame compile() const; + bool isValid() const; bool operator!=(const Frame& rhs) const { return !operator==(rhs); } - bool operator==(const Frame& rhs) const - { - return - (transfer_type == rhs.transfer_type) && - (data_type_id == rhs.data_type_id) && - (source_node_id == rhs.source_node_id) && - (frame_index == rhs.frame_index) && - (transfer_id == rhs.transfer_id) && - (last_frame == rhs.last_frame) && - (payload_len == rhs.payload_len) && - std::equal(payload, payload + payload_len, rhs.payload); - } + bool operator==(const Frame& rhs) const; std::string toString() const; }; -struct RxFrame : public Frame +class RxFrame : public Frame { - uint_fast64_t ts_monotonic; - uint_fast64_t ts_utc; - uint_fast8_t iface_index; + uint64_t ts_monotonic_; + uint64_t ts_utc_; + uint8_t iface_index_; +public: RxFrame() - : ts_monotonic(0) - , ts_utc(0) - , iface_index(0) + : ts_monotonic_(0) + , ts_utc_(0) + , iface_index_(0) { } + RxFrame(const Frame& frame, uint64_t ts_monotonic, uint64_t ts_utc, uint8_t iface_index) + : ts_monotonic_(ts_monotonic) + , ts_utc_(ts_utc) + , iface_index_(iface_index) + { + *static_cast(this) = frame; + } + bool parse(const CanRxFrame& can_frame) { if (!Frame::parse(can_frame)) return false; - ts_monotonic = can_frame.ts_monotonic; - ts_utc = can_frame.ts_utc; - iface_index = can_frame.iface_index; + ts_monotonic_ = can_frame.ts_monotonic; + ts_utc_ = can_frame.ts_utc; + iface_index_ = can_frame.iface_index; return true; } + uint64_t getMonotonicTimestamp() const { return ts_monotonic_; } + uint64_t getUtcTimestamp() const { return ts_utc_; } + + uint8_t getIfaceIndex() const { return iface_index_; } + std::string toString() const; }; diff --git a/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp b/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp index bba3fcb490..bf58fe2cd5 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp @@ -32,22 +32,20 @@ public: */ class TransferBufferManagerKey { - uint8_t node_id_; + NodeID node_id_; uint8_t transfer_type_; public: TransferBufferManagerKey() - : node_id_(NODE_ID_INVALID) - , transfer_type_(TransferType(0)) + : transfer_type_(TransferType(0)) { assert(isEmpty()); } - TransferBufferManagerKey(uint8_t node_id, TransferType ttype) + TransferBufferManagerKey(NodeID node_id, TransferType ttype) : node_id_(node_id) , transfer_type_(ttype) { - assert(node_id <= NODE_ID_MAX && node_id != NODE_ID_INVALID); assert(!isEmpty()); } @@ -56,9 +54,9 @@ public: return node_id_ == rhs.node_id_ && transfer_type_ == rhs.transfer_type_; } - bool isEmpty() const { return node_id_ == NODE_ID_INVALID; } + bool isEmpty() const { return !node_id_.isValid(); } - uint8_t getNodeID() const { return node_id_; } + NodeID getNodeID() const { return node_id_; } TransferType getTransferType() const { return TransferType(transfer_type_); } std::string toString() const; diff --git a/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp b/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp index e0e898abf6..d05f798da2 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp @@ -25,16 +25,16 @@ class IncomingTransfer uint64_t ts_utc_; TransferType transfer_type_; TransferID transfer_id_; - uint8_t source_node_id_; + NodeID src_node_id_; protected: IncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc, TransferType transfer_type, - TransferID transfer_id, uint8_t source_node_id) + TransferID transfer_id, NodeID source_node_id) : ts_monotonic_(ts_monotonic) , ts_utc_(ts_utc) , transfer_type_(transfer_type) , transfer_id_(transfer_id) - , source_node_id_(source_node_id) + , src_node_id_(source_node_id) { } public: @@ -54,7 +54,7 @@ public: uint64_t getUtcTimestamp() const { return ts_utc_; } TransferType getTransferType() const { return transfer_type_; } TransferID getTransferID() const { return transfer_id_; } - uint8_t getSourceNodeID() const { return source_node_id_; } + NodeID getSrcNodeID() const { return src_node_id_; } }; /** @@ -65,7 +65,7 @@ class SingleFrameIncomingTransfer : public IncomingTransfer const uint8_t* const payload_; const uint8_t payload_len_; public: - SingleFrameIncomingTransfer(const RxFrame& frm, const uint8_t* payload, unsigned int payload_len); + SingleFrameIncomingTransfer(const RxFrame& frm); int read(unsigned int offset, uint8_t* data, unsigned int len) const; }; @@ -125,16 +125,12 @@ class TransferListener : public TransferListenerBase, Noncopyable void handleFrame(const RxFrame& frame) { - const TransferBufferManagerKey key(frame.source_node_id, frame.transfer_type); + const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); TransferReceiver* recv = receivers_.access(key); if (recv == NULL) { - /* Adding new registrations mid-transfer (i.e. not upon first frame reception) is not only - * pointless, but plain wrong: non-first frames do not carry address information, so we - * have no chance to detect whether a transfer is addressed to our node or not. - */ - if (frame.frame_index != 0) + if (!frame.isFirstFrame()) return; TransferReceiver new_recv; diff --git a/libuavcan/include/uavcan/internal/transport/transfer_receiver.hpp b/libuavcan/include/uavcan/internal/transport/transfer_receiver.hpp index 4961d5e622..8f8c857972 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer_receiver.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer_receiver.hpp @@ -68,8 +68,6 @@ public: uint16_t getLastTransferCrc() const { return this_transfer_crc_; } uint32_t getInterval() const { return transfer_interval_; } - - static bool extractSingleFrameTransferPayload(const RxFrame& frame, uint8_t* out_data, unsigned int& out_len); }; #pragma pack(pop) diff --git a/libuavcan/src/transport/dispatcher.cpp b/libuavcan/src/transport/dispatcher.cpp index 34ab0768e2..0b87ce4853 100644 --- a/libuavcan/src/transport/dispatcher.cpp +++ b/libuavcan/src/transport/dispatcher.cpp @@ -48,9 +48,9 @@ void Dispatcher::ListenerRegister::handleFrame(const RxFrame& frame) TransferListenerBase* p = list_.get(); while (p) { - if (p->getDataTypeDescriptor()->id == frame.data_type_id) + if (p->getDataTypeDescriptor()->id == frame.getDataTypeID()) p->handleFrame(frame); - else if (p->getDataTypeDescriptor()->id < frame.data_type_id) // Listeners are ordered by data type id! + else if (p->getDataTypeDescriptor()->id < frame.getDataTypeID()) // Listeners are ordered by data type id! break; p = p->getNextListNode(); } @@ -68,39 +68,13 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) return; } - /* - * Target Node ID checking - */ - switch (frame.transfer_type) + if ((frame.getDstNodeID() != NodeID::BROADCAST) && + (frame.getDstNodeID() != getSelfNodeID())) { - case TRANSFER_TYPE_MESSAGE_BROADCAST: - // Always accepted - break; - - case TRANSFER_TYPE_SERVICE_RESPONSE: - case TRANSFER_TYPE_SERVICE_REQUEST: - case TRANSFER_TYPE_MESSAGE_UNICAST: - /* Here we drop transfer headers that aren't addressed to us. Non-first frames must be accepted in - * any case, because they lack address information in order to reduce payload overhead. - */ - if (frame.frame_index == 0) - { - const uint8_t target_node_id = frame.payload[0]; - if (target_node_id != getSelfNodeID()) - return; - } - break; - - default: - // This means that RxFrame::parse() accepted an invalid frame. Too bad! - assert(0); return; } - /* - * Target register selection - */ - switch (frame.transfer_type) + switch (frame.getTransferType()) { case TRANSFER_TYPE_MESSAGE_BROADCAST: case TRANSFER_TYPE_MESSAGE_UNICAST: @@ -143,13 +117,19 @@ int Dispatcher::spin(uint64_t monotonic_deadline) int Dispatcher::send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, CanTxQueue::Qos qos) { - if (frame.source_node_id != getSelfNodeID()) + if (frame.getSrcNodeID() != getSelfNodeID()) { assert(0); return -1; } - const CanFrame can_frame = frame.compile(); + CanFrame can_frame; + if (!frame.compile(can_frame)) + { + UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str()); + assert(0); + return -1; + } const int iface_mask = (1 << canio_.getNumIfaces()) - 1; return canio_.send(can_frame, monotonic_tx_deadline, monotonic_blocking_deadline, iface_mask, qos); diff --git a/libuavcan/src/transport/transfer.cpp b/libuavcan/src/transport/transfer.cpp index 39b7f88328..21e7b8bb81 100644 --- a/libuavcan/src/transport/transfer.cpp +++ b/libuavcan/src/transport/transfer.cpp @@ -9,7 +9,14 @@ namespace uavcan { +/** + * NodeID + */ +const NodeID NodeID::BROADCAST(VALUE_BROADCAST); +/** + * TransferID + */ int TransferID::forwardDistance(TransferID rhs) const { int d = int(rhs.get()) - int(get()); @@ -20,6 +27,39 @@ int TransferID::forwardDistance(TransferID rhs) const return d; } +/** + * Frame + */ +int Frame::getMaxPayloadLen() const +{ + switch (getTransferType()) + { + case TRANSFER_TYPE_MESSAGE_BROADCAST: + return sizeof(payload_); + break; + + case TRANSFER_TYPE_SERVICE_RESPONSE: + case TRANSFER_TYPE_SERVICE_REQUEST: + case TRANSFER_TYPE_MESSAGE_UNICAST: + return sizeof(payload_) - 1; + break; + + default: + assert(0); + return -1; + } +} + +int Frame::setPayload(const uint8_t* data, int len) +{ + len = std::min(getMaxPayloadLen(), len); + if (len >= 0) + { + std::copy(data, data + len, payload_); + payload_len_ = len; + } + return len; +} template inline static uint32_t bitunpack(uint32_t val) @@ -32,23 +72,65 @@ 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) + if (can_frame.dlc > sizeof(CanFrame::data)) { - assert(0); + assert(0); // This is not a protocol error, so assert() is ok return false; } + /* + * CAN ID parsing + */ const uint32_t id = can_frame.id & CanFrame::MASK_EXTID; + transfer_id_ = bitunpack<0, 3>(id); + last_frame_ = bitunpack<3, 1>(id); + frame_index_ = bitunpack<4, 6>(id); + src_node_id_ = bitunpack<10, 7>(id); + transfer_type_ = TransferType(bitunpack<17, 2>(id)); + data_type_id_ = bitunpack<19, 10>(id); - 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); + /* + * Validation + */ + if (frame_index_ > FRAME_INDEX_MAX) + return false; - payload_len = can_frame.dlc; - std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload); + if ((frame_index_ == FRAME_INDEX_MAX) && !last_frame_) // Unterminated transfer + return false; + + if (!src_node_id_.isUnicast()) + return false; + + /* + * CAN payload parsing + */ + switch (transfer_type_) + { + case TRANSFER_TYPE_MESSAGE_BROADCAST: + dst_node_id_ = NodeID::BROADCAST; + payload_len_ = can_frame.dlc; + std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload_); + break; + + case TRANSFER_TYPE_SERVICE_RESPONSE: + case TRANSFER_TYPE_SERVICE_REQUEST: + case TRANSFER_TYPE_MESSAGE_UNICAST: + if (can_frame.dlc < 1) + return false; + if (can_frame.data[0] & 0x80) // RESERVED, must be zero + return false; + + dst_node_id_ = can_frame.data[0] & 0x7F; + if (!dst_node_id_.isUnicast() || (dst_node_id_ == src_node_id_)) + return false; + + payload_len_ = can_frame.dlc - 1; + std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_); + break; + + default: + return false; + } return true; } @@ -58,22 +140,74 @@ inline static uint32_t bitpack(uint32_t field) return (field & ((1UL << WIDTH) - 1)) << OFFSET; } -CanFrame Frame::compile() const +bool Frame::compile(CanFrame& out_can_frame) const { - CanFrame frame; + if (!isValid()) + { + assert(0); // This is an application error, so we need to maximize it. + return false; + } - 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); + out_can_frame.id = CanFrame::FLAG_EFF | + bitpack<0, 3>(transfer_id_.get()) | + bitpack<3, 1>(last_frame_) | + bitpack<4, 6>(frame_index_) | + bitpack<10, 7>(src_node_id_.get()) | + 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; + switch (transfer_type_) + { + case TRANSFER_TYPE_MESSAGE_BROADCAST: + out_can_frame.dlc = payload_len_; + std::copy(payload_, payload_ + payload_len_, out_can_frame.data); + break; + + case TRANSFER_TYPE_SERVICE_RESPONSE: + case TRANSFER_TYPE_SERVICE_REQUEST: + case TRANSFER_TYPE_MESSAGE_UNICAST: + assert((payload_len_ + 1) <= sizeof(CanFrame::data)); + out_can_frame.data[0] = dst_node_id_.get(); + out_can_frame.dlc = payload_len_ + 1; + std::copy(payload_, payload_ + payload_len_, out_can_frame.data + 1); + break; + + default: + assert(0); + return false; + } + return true; +} + +bool Frame::isValid() const +{ + // Refer to the specification for the detailed explanation of the checks + const bool invalid = + (frame_index_ > FRAME_INDEX_MAX) || + ((frame_index_ == FRAME_INDEX_MAX) && !last_frame_) || + (!src_node_id_.isUnicast()) || + (!dst_node_id_.isValid()) || + (src_node_id_ == dst_node_id_) || + ((transfer_type_ == TRANSFER_TYPE_MESSAGE_BROADCAST) != dst_node_id_.isBroadcast()) || + (transfer_type_ >= NUM_TRANSFER_TYPES) || + (payload_len_ > getMaxPayloadLen()) || + (data_type_id_ > DATA_TYPE_ID_MAX); + + return !invalid; +} + +bool Frame::operator==(const Frame& rhs) const +{ + return + (transfer_type_ == rhs.transfer_type_) && + (data_type_id_ == rhs.data_type_id_) && + (src_node_id_ == rhs.src_node_id_) && + (dst_node_id_ == rhs.dst_node_id_) && + (frame_index_ == rhs.frame_index_) && + (transfer_id_ == rhs.transfer_id_) && + (last_frame_ == rhs.last_frame_) && + (payload_len_ == rhs.payload_len_) && + std::equal(payload_, payload_ + payload_len_, rhs.payload_); } std::string Frame::toString() const @@ -89,24 +223,27 @@ std::string Frame::toString() const */ static const int BUFLEN = 100; char buf[BUFLEN]; - int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i idx=%i last=%i tid=%i payload=[", - int(data_type_id), int(transfer_type), int(source_node_id), int(frame_index), - int(last_frame), int(transfer_id.get())); + int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[", + int(data_type_id_), int(transfer_type_), int(src_node_id_.get()), int(dst_node_id_.get()), + int(frame_index_), int(last_frame_), int(transfer_id_.get())); - for (int i = 0; i < payload_len; i++) + for (int i = 0; i < payload_len_; i++) { - ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload[i]); - if ((i + 1) < payload_len) + ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload_[i]); + if ((i + 1) < payload_len_) ofs += std::snprintf(buf + ofs, BUFLEN - ofs, " "); } ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "]"); return std::string(buf); } +/** + * RxFrame + */ std::string RxFrame::toString() const { std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp - os << Frame::toString() << " ts_m=" << ts_monotonic << " ts_utc=" << ts_utc << " iface=" << int(iface_index); + os << Frame::toString() << " ts_m=" << ts_monotonic_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_); return os.str(); } diff --git a/libuavcan/src/transport/transfer_buffer.cpp b/libuavcan/src/transport/transfer_buffer.cpp index f613c36a72..a0612d7999 100644 --- a/libuavcan/src/transport/transfer_buffer.cpp +++ b/libuavcan/src/transport/transfer_buffer.cpp @@ -16,7 +16,7 @@ namespace uavcan std::string TransferBufferManagerKey::toString() const { char buf[24]; - std::snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_), int(transfer_type_)); + std::snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_)); return std::string(buf); } diff --git a/libuavcan/src/transport/transfer_listener.cpp b/libuavcan/src/transport/transfer_listener.cpp index c1d82c26ce..c332333b8b 100644 --- a/libuavcan/src/transport/transfer_listener.cpp +++ b/libuavcan/src/transport/transfer_listener.cpp @@ -3,7 +3,9 @@ */ #include +#include #include +#include namespace uavcan { @@ -11,12 +13,14 @@ namespace uavcan /* * SingleFrameIncomingTransfer */ -SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm, const uint8_t* payload, - unsigned int payload_len) -: IncomingTransfer(frm.ts_monotonic, frm.ts_utc, frm.transfer_type, frm.transfer_id, frm.source_node_id) -, payload_(payload) -, payload_len_(payload_len) -{ } +SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm) +: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(), + frm.getTransferID(), frm.getSrcNodeID()) +, payload_(frm.getPayloadPtr()) +, payload_len_(frm.getPayloadLen()) +{ + assert(frm.isValid()); +} int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const { @@ -39,10 +43,12 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign */ MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc, const RxFrame& last_frame, TransferBufferAccessor& tba) -: IncomingTransfer(ts_monotonic, ts_utc, last_frame.transfer_type, last_frame.transfer_id, last_frame.source_node_id) +: IncomingTransfer(ts_monotonic, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(), + last_frame.getSrcNodeID()) , buf_acc_(tba) { - assert(last_frame.last_frame); + assert(last_frame.isValid()); + assert(last_frame.isLastFrame()); } int MultiFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const @@ -90,17 +96,7 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF case TransferReceiver::RESULT_SINGLE_FRAME: { - uint8_t payload[Frame::PAYLOAD_LEN_MAX]; - unsigned int payload_len = 0; - const bool success = TransferReceiver::extractSingleFrameTransferPayload(frame, payload, payload_len); - if (!success) - { - UAVCAN_TRACE("TransferListenerBase", "SFT payload extraction failed, frame: %s", frame.toString().c_str()); - return; - } - assert(payload_len <= Frame::PAYLOAD_LEN_MAX); - - SingleFrameIncomingTransfer it(frame, payload, payload_len); + SingleFrameIncomingTransfer it(frame); handleIncomingTransfer(it); return; } diff --git a/libuavcan/src/transport/transfer_receiver.cpp b/libuavcan/src/transport/transfer_receiver.cpp index ae409b12b8..aff2fc6f77 100644 --- a/libuavcan/src/transport/transfer_receiver.cpp +++ b/libuavcan/src/transport/transfer_receiver.cpp @@ -12,13 +12,15 @@ namespace uavcan { +static const int CRC_LEN = 2; + const uint32_t TransferReceiver::DEFAULT_TRANSFER_INTERVAL; const uint32_t TransferReceiver::MIN_TRANSFER_INTERVAL; const uint32_t TransferReceiver::MAX_TRANSFER_INTERVAL; TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const { - const int distance = tid_.forwardDistance(frame.transfer_id); + const int distance = tid_.forwardDistance(frame.getTransferID()); if (distance == 0) return TID_SAME; if (distance < ((1 << TransferID::BITLEN) / 2)) @@ -52,36 +54,31 @@ void TransferReceiver::prepareForNextTransfer() bool TransferReceiver::validate(const RxFrame& frame) const { - if (iface_index_ != frame.iface_index) + if (iface_index_ != frame.getIfaceIndex()) return false; - if (frame.source_node_id == 0) + if (frame.isFirstFrame() && !frame.isLastFrame() && (frame.getPayloadLen() < CRC_LEN)) { - UAVCAN_TRACE("TransferReceiver", "Invalid frame NID, %s", frame.toString().c_str()); + UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str()); return false; } - if (frame.frame_index != next_frame_index_) + if ((frame.getFrameIndex() == Frame::FRAME_INDEX_MAX) && !frame.isLastFrame()) { - UAVCAN_TRACE("TransferReceiver", "Unexpected frame index, %s", frame.toString().c_str()); + UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str()); return false; } - if (!frame.last_frame && frame.payload_len != Frame::PAYLOAD_LEN_MAX) + if (frame.getFrameIndex() != next_frame_index_) { - UAVCAN_TRACE("TransferReceiver", "Unexpected payload len, %s", frame.toString().c_str()); - return false; - } - - if (!frame.last_frame && frame.frame_index == Frame::FRAME_INDEX_MAX) - { - UAVCAN_TRACE("TransferReceiver", "Expected end of transfer, %s", frame.toString().c_str()); + UAVCAN_TRACE("TransferReceiver", "Unexpected frame index (not %i), %s", + int(next_frame_index_), frame.toString().c_str()); return false; } if (getTidRelation(frame) != TID_SAME) { - UAVCAN_TRACE("TransferReceiver", "Unexpected TID, %s", frame.toString().c_str()); + UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str()); return false; } return true; @@ -89,34 +86,18 @@ bool TransferReceiver::validate(const RxFrame& frame) const bool TransferReceiver::writePayload(const RxFrame& frame, TransferBufferBase& buf) { - if (frame.frame_index == 0) + const uint8_t* const payload = frame.getPayloadPtr(); + const int payload_len = frame.getPayloadLen(); + + if (frame.isFirstFrame()) // First frame contains CRC, we need to extract it now { - unsigned int payload_offset = 0; - unsigned int crc_offset = 0; + if (frame.getPayloadLen() < CRC_LEN) // Must have been validated earlier though. I think I'm paranoid. + return false; - switch (frame.transfer_type) - { - case TRANSFER_TYPE_MESSAGE_BROADCAST: - payload_offset = 2; - crc_offset = 0; - break; - case TRANSFER_TYPE_SERVICE_RESPONSE: // Addressed transfers have 1-byte overhead for Destination Node ID - case TRANSFER_TYPE_SERVICE_REQUEST: - case TRANSFER_TYPE_MESSAGE_UNICAST: - payload_offset = 3; - crc_offset = 1; - break; - default: - UAVCAN_TRACE("TransferReceiver", "Invalid transfer type, %s", frame.toString().c_str()); - return RESULT_NOT_COMPLETE; - } + this_transfer_crc_ = (payload[0] & 0xFF) | (uint16_t(payload[1] & 0xFF) << 8); // Little endian. - this_transfer_crc_ = - (frame.payload[crc_offset] & 0xFF) | - (uint16_t(frame.payload[crc_offset + 1] & 0xFF) << 8); // little endian - - const int effective_payload_len = frame.payload_len - payload_offset; - const int res = buf.write(buffer_write_pos_, frame.payload + payload_offset, effective_payload_len); + const int effective_payload_len = payload_len - CRC_LEN; + const int res = buf.write(buffer_write_pos_, payload + CRC_LEN, effective_payload_len); const bool success = res == effective_payload_len; if (success) buffer_write_pos_ += effective_payload_len; @@ -124,10 +105,10 @@ bool TransferReceiver::writePayload(const RxFrame& frame, TransferBufferBase& bu } else { - const int res = buf.write(buffer_write_pos_, frame.payload, frame.payload_len); - const bool success = res == frame.payload_len; + const int res = buf.write(buffer_write_pos_, payload, payload_len); + const bool success = res == payload_len; if (success) - buffer_write_pos_ += frame.payload_len; + buffer_write_pos_ += payload_len; return success; } } @@ -135,14 +116,13 @@ bool TransferReceiver::writePayload(const RxFrame& frame, TransferBufferBase& bu TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba) { // Transfer timestamps are derived from the first frame - if (frame.frame_index == 0) + if (frame.isFirstFrame()) { - this_transfer_ts_monotonic_ = frame.ts_monotonic; - first_frame_ts_utc_ = frame.ts_utc; + this_transfer_ts_monotonic_ = frame.getMonotonicTimestamp(); + first_frame_ts_utc_ = frame.getUtcTimestamp(); } - // Single-frame transfer - if ((frame.frame_index == 0) && frame.last_frame) + if (frame.isFirstFrame() && frame.isLastFrame()) { tba.remove(); updateTransferTimings(); @@ -170,7 +150,7 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra } next_frame_index_++; - if (frame.last_frame) + if (frame.isLastFrame()) { updateTransferTimings(); prepareForNextTransfer(); @@ -181,7 +161,7 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra bool TransferReceiver::isTimedOut(uint64_t ts_monotonic) const { - static const uint64_t INTERVAL_MULT = (1 << TransferID::BITLEN) / 2 - 1; + static const uint64_t INTERVAL_MULT = (1 << TransferID::BITLEN) / 2 + 1; const uint64_t ts = this_transfer_ts_monotonic_; if (ts_monotonic <= ts) return false; @@ -190,19 +170,20 @@ bool TransferReceiver::isTimedOut(uint64_t ts_monotonic) const TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba) { - if ((frame.ts_monotonic == 0) || - (frame.ts_monotonic < prev_transfer_ts_monotonic_) || - (frame.ts_monotonic < this_transfer_ts_monotonic_)) + if ((frame.getMonotonicTimestamp() == 0) || + (frame.getMonotonicTimestamp() < prev_transfer_ts_monotonic_) || + (frame.getMonotonicTimestamp() < this_transfer_ts_monotonic_)) { return RESULT_NOT_COMPLETE; } const bool not_initialized = !isInitialized(); - const bool iface_timed_out = (frame.ts_monotonic - this_transfer_ts_monotonic_) > (uint64_t(transfer_interval_) * 2); - const bool receiver_timed_out = isTimedOut(frame.ts_monotonic); - const bool same_iface = frame.iface_index == iface_index_; - const bool first_fame = frame.frame_index == 0; + const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp()); + const bool same_iface = frame.getIfaceIndex() == iface_index_; + const bool first_fame = frame.isFirstFrame(); const TidRelation tid_rel = getTidRelation(frame); + const bool iface_timed_out = + (frame.getMonotonicTimestamp() - this_transfer_ts_monotonic_) > (uint64_t(transfer_interval_) * 2); const bool need_restart = // FSM, the hard way (not_initialized) || @@ -217,8 +198,8 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr int(not_initialized), int(iface_timed_out), int(receiver_timed_out), int(same_iface), int(first_fame), int(tid_rel), frame.toString().c_str()); tba.remove(); - iface_index_ = frame.iface_index; - tid_ = frame.transfer_id; + iface_index_ = frame.getIfaceIndex(); + tid_ = frame.getTransferID(); next_frame_index_ = 0; buffer_write_pos_ = 0; this_transfer_crc_ = 0; @@ -235,37 +216,4 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr return receive(frame, tba); } -bool TransferReceiver::extractSingleFrameTransferPayload(const RxFrame& frame, uint8_t* out_data, - unsigned int& out_len) -{ - if (out_data == NULL) - { - assert(0); - return false; - } - - out_len = 0; - unsigned int offset = 0; - switch (frame.transfer_type) - { - case TRANSFER_TYPE_MESSAGE_BROADCAST: - offset = 0; - break; - case TRANSFER_TYPE_SERVICE_RESPONSE: // Addressed transfers have 1-byte overhead for Destination Node ID - case TRANSFER_TYPE_SERVICE_REQUEST: - case TRANSFER_TYPE_MESSAGE_UNICAST: - offset = 1; - break; - default: - return false; - } - - if (frame.payload_len < offset) - return false; - - out_len = frame.payload_len - offset; - std::copy(frame.payload + offset, frame.payload + offset + out_len, out_data); - return true; -} - } diff --git a/libuavcan/test/transport/incoming_transfer.cpp b/libuavcan/test/transport/incoming_transfer.cpp index 2bf4c73b62..b917af6ba2 100644 --- a/libuavcan/test/transport/incoming_transfer.cpp +++ b/libuavcan/test/transport/incoming_transfer.cpp @@ -9,16 +9,13 @@ static uavcan::RxFrame makeFrame() { - uavcan::RxFrame frame; - frame.data_type_id = 123; - frame.last_frame = true; - frame.source_node_id = 42; - frame.transfer_id.increment(); - frame.ts_monotonic = 123; - frame.ts_utc = 456; - frame.payload_len = uavcan::RxFrame::PAYLOAD_LEN_MAX; - for (int i = 0; i < uavcan::RxFrame::PAYLOAD_LEN_MAX; i++) - frame.payload[i] = i; + uavcan::RxFrame frame( + uavcan::Frame(123, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 1, uavcan::NodeID::BROADCAST, 0, 1, true), + 123, 456, 0); + uint8_t data[8]; + for (unsigned int i = 0; i < sizeof(data); i++) + data[i] = i; + frame.setPayload(data, sizeof(data)); return frame; } @@ -27,11 +24,11 @@ static bool match(const uavcan::IncomingTransfer& it, const uavcan::RxFrame& fra const uint8_t* payload, unsigned int payload_len) { // Fields extracted from the frame struct - EXPECT_EQ(it.getMonotonicTimestamp(), frame.ts_monotonic); - EXPECT_EQ(it.getUtcTimestamp(), frame.ts_utc); - EXPECT_EQ(it.getSourceNodeID(), frame.source_node_id); - EXPECT_EQ(it.getTransferID(), frame.transfer_id); - EXPECT_EQ(it.getTransferType(), frame.transfer_type); + EXPECT_EQ(it.getMonotonicTimestamp(), frame.getMonotonicTimestamp()); + EXPECT_EQ(it.getUtcTimestamp(), frame.getUtcTimestamp()); + EXPECT_EQ(it.getSrcNodeID(), frame.getSrcNodeID()); + EXPECT_EQ(it.getTransferID(), frame.getTransferID()); + EXPECT_EQ(it.getTransferType(), frame.getTransferType()); // Payload comparison static const unsigned int BUFLEN = 1024; @@ -60,9 +57,9 @@ TEST(SingleFrameIncomingTransfer, Basic) using uavcan::SingleFrameIncomingTransfer; const RxFrame frame = makeFrame(); - SingleFrameIncomingTransfer it(frame, frame.payload, frame.payload_len); + SingleFrameIncomingTransfer it(frame); - ASSERT_TRUE(match(it, frame, frame.payload, frame.payload_len)); + ASSERT_TRUE(match(it, frame, frame.getPayloadPtr(), frame.getPayloadLen())); } @@ -75,10 +72,10 @@ TEST(MultiFrameIncomingTransfer, Basic) uavcan::TransferBufferManager<256, 1> bufmgr(&poolmgr); const RxFrame frame = makeFrame(); - uavcan::TransferBufferManagerKey bufmgr_key(frame.source_node_id, frame.transfer_type); + uavcan::TransferBufferManagerKey bufmgr_key(frame.getSrcNodeID(), frame.getTransferType()); uavcan::TransferBufferAccessor tba(&bufmgr, bufmgr_key); - MultiFrameIncomingTransfer it(frame.ts_monotonic, frame.ts_utc, frame, tba); + MultiFrameIncomingTransfer it(frame.getMonotonicTimestamp(), frame.getUtcTimestamp(), frame, tba); /* * Empty read must fail diff --git a/libuavcan/test/transport/transfer.cpp b/libuavcan/test/transport/transfer.cpp index 6d5cad5649..d962dc719b 100644 --- a/libuavcan/test/transport/transfer.cpp +++ b/libuavcan/test/transport/transfer.cpp @@ -13,40 +13,29 @@ TEST(Transfer, TransferID) using uavcan::TransferID; // Tests below are based on this assumption - ASSERT_EQ(16, 1 << TransferID::BITLEN); + ASSERT_EQ(8, 1 << TransferID::BITLEN); /* * forwardDistance() */ EXPECT_EQ(0, TransferID(0).forwardDistance(0)); EXPECT_EQ(1, TransferID(0).forwardDistance(1)); - EXPECT_EQ(15, TransferID(0).forwardDistance(15)); + EXPECT_EQ(7, TransferID(0).forwardDistance(7)); EXPECT_EQ(0, TransferID(7).forwardDistance(7)); - EXPECT_EQ(15, TransferID(7).forwardDistance(6)); - EXPECT_EQ(1, TransferID(7).forwardDistance(8)); + EXPECT_EQ(7, TransferID(7).forwardDistance(6)); + EXPECT_EQ(1, TransferID(7).forwardDistance(0)); - EXPECT_EQ(9, TransferID(10).forwardDistance(3)); - EXPECT_EQ(7, TransferID(3).forwardDistance(10)); - - EXPECT_EQ(8, TransferID(6).forwardDistance(14)); - EXPECT_EQ(8, TransferID(14).forwardDistance(6)); - - EXPECT_EQ(1, TransferID(14).forwardDistance(15)); - EXPECT_EQ(2, TransferID(14).forwardDistance(0)); - EXPECT_EQ(4, TransferID(14).forwardDistance(2)); - - EXPECT_EQ(15, TransferID(15).forwardDistance(14)); - EXPECT_EQ(14, TransferID(0).forwardDistance(14)); - EXPECT_EQ(12, TransferID(2).forwardDistance(14)); + EXPECT_EQ(7, TransferID(7).forwardDistance(6)); + EXPECT_EQ(5, TransferID(0).forwardDistance(5)); /* * Misc */ EXPECT_TRUE(TransferID(2) == TransferID(2)); EXPECT_FALSE(TransferID(2) != TransferID(2)); - EXPECT_FALSE(TransferID(2) == TransferID(8)); - EXPECT_TRUE(TransferID(2) != TransferID(8)); + EXPECT_FALSE(TransferID(2) == TransferID(0)); + EXPECT_TRUE(TransferID(2) != TransferID(0)); TransferID tid; for (int i = 0; i < 999; i++) @@ -55,7 +44,7 @@ TEST(Transfer, TransferID) const TransferID copy = tid; tid.increment(); ASSERT_EQ(1, copy.forwardDistance(tid)); - ASSERT_EQ(15, tid.forwardDistance(copy)); + ASSERT_EQ(7, tid.forwardDistance(copy)); ASSERT_EQ(0, tid.forwardDistance(tid)); } } @@ -71,10 +60,10 @@ TEST(Transfer, FrameParseCompile) const uint32_t can_id = (2 << 0) | // Transfer ID - (1 << 4) | // Last Frame - (29 << 5) | // Frame Index + (1 << 3) | // Last Frame + (29 << 4) | // Frame Index (42 << 10) | // Source Node ID - (3 << 17) | // Transfer Type + (uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST << 17) | (456 << 19); // Data Type ID const std::string payload_string = "hello"; @@ -89,32 +78,24 @@ TEST(Transfer, FrameParseCompile) // 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(TransferID(2), frame.getTransferID()); + EXPECT_TRUE(frame.isLastFrame()); + EXPECT_EQ(29, frame.getFrameIndex()); + EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID()); + EXPECT_EQ(uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, frame.getTransferType()); + EXPECT_EQ(456, frame.getDataTypeID()); - 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); + EXPECT_EQ(payload_string.length(), frame.getPayloadLen()); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + payload_string.begin())); /* * Compile */ - // Default - frame = Frame(); - CanFrame can_frame = frame.compile(); - ASSERT_EQ(can_frame.id, CanFrame::FLAG_EFF); - - // Custom + CanFrame can_frame; ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); - can_frame = frame.compile(); + ASSERT_TRUE(frame.compile(can_frame)); ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT)); EXPECT_EQ(payload_string.length(), can_frame.dlc); @@ -144,26 +125,30 @@ TEST(Transfer, RxFrameParse) // Failure ASSERT_FALSE(rx_frame.parse(can_rx_frame)); - // Default - can_rx_frame.id = CanFrame::FLAG_EFF; - ASSERT_TRUE(rx_frame.parse(can_rx_frame)); - ASSERT_EQ(0, rx_frame.ts_monotonic); - ASSERT_EQ(0, rx_frame.iface_index); + // Valid + can_rx_frame.id = CanFrame::FLAG_EFF | + (2 << 0) | // Transfer ID + (1 << 3) | // Last Frame + (29 << 4) | // Frame Index + (42 << 10) | // Source Node ID + (uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST << 17) | + (456 << 19); // Data Type ID + + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(0, rx_frame.getMonotonicTimestamp()); + ASSERT_EQ(0, rx_frame.getIfaceIndex()); - // Custom can_rx_frame.ts_monotonic = 123; can_rx_frame.iface_index = 2; - Frame frame; - frame.data_type_id = 456; - frame.transfer_type = uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST; - *static_cast(&can_rx_frame) = frame.compile(); + Frame frame(456, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 1, uavcan::NodeID::BROADCAST, 0, 0); + ASSERT_TRUE(frame.compile(can_rx_frame)); ASSERT_TRUE(rx_frame.parse(can_rx_frame)); - ASSERT_EQ(123, rx_frame.ts_monotonic); - ASSERT_EQ(2, rx_frame.iface_index); - ASSERT_EQ(456, rx_frame.data_type_id); - ASSERT_EQ(uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, rx_frame.transfer_type); + ASSERT_EQ(123, rx_frame.getMonotonicTimestamp()); + ASSERT_EQ(2, rx_frame.getIfaceIndex()); + ASSERT_EQ(456, rx_frame.getDataTypeID()); + ASSERT_EQ(uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, rx_frame.getTransferType()); } @@ -174,31 +159,28 @@ TEST(Transfer, FrameToString) // RX frame default RxFrame rx_frame; - EXPECT_EQ("dtid=0 tt=0 snid=0 idx=0 last=0 tid=0 payload=[] ts_m=0 ts_utc=0 iface=0", rx_frame.toString()); + EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0 ts_utc=0 iface=0", rx_frame.toString()); // RX frame max len - rx_frame.data_type_id = Frame::DATA_TYPE_ID_MAX; - rx_frame.transfer_type = uavcan::TransferType(uavcan::NUM_TRANSFER_TYPES - 1); - rx_frame.source_node_id = uavcan::NODE_ID_MAX; - rx_frame.frame_index = Frame::FRAME_INDEX_MAX; - rx_frame.last_frame = true; - rx_frame.transfer_id = uavcan::TransferID::MAX; - rx_frame.payload_len = Frame::PAYLOAD_LEN_MAX; - for (int i = 0; i < rx_frame.payload_len; i++) - rx_frame.payload[i] = i; - rx_frame.ts_monotonic = 0xFFFFFFFFFFFFFFFF; - rx_frame.ts_utc = 0xFFFFFFFFFFFFFFFF; - rx_frame.iface_index = 3; + rx_frame = RxFrame(Frame(Frame::DATA_TYPE_ID_MAX, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, + uavcan::NodeID::MAX, uavcan::NodeID::MAX - 1, Frame::FRAME_INDEX_MAX, + uavcan::TransferID::MAX, true), + 0xFFFFFFFFFFFFFFFF, 0xFFFFFFFFFFFFFFFF, 3); + + uint8_t data[8]; + for (unsigned int i = 0; i < sizeof(data); i++) + data[i] = i; + rx_frame.setPayload(data, sizeof(data)); EXPECT_EQ( - "dtid=1023 tt=3 snid=127 idx=31 last=1 tid=15 payload=[00 01 02 03 04 05 06 07] ts_m=18446744073709551615 ts_utc=18446744073709551615 iface=3", + "dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06] ts_m=18446744073709551615 ts_utc=18446744073709551615 iface=3", rx_frame.toString()); // Plain frame default Frame frame; - EXPECT_EQ("dtid=0 tt=0 snid=0 idx=0 last=0 tid=0 payload=[]", frame.toString()); + EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[]", frame.toString()); // Plain frame max len frame = rx_frame; - EXPECT_EQ("dtid=1023 tt=3 snid=127 idx=31 last=1 tid=15 payload=[00 01 02 03 04 05 06 07]", frame.toString()); + EXPECT_EQ("dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06]", frame.toString()); } diff --git a/libuavcan/test/transport/transfer_buffer.cpp b/libuavcan/test/transport/transfer_buffer.cpp index 8c9e1278c7..cb791ccd89 100644 --- a/libuavcan/test/transport/transfer_buffer.cpp +++ b/libuavcan/test/transport/transfer_buffer.cpp @@ -243,7 +243,7 @@ TEST(TransferBufferManager, Basic) // Empty ASSERT_FALSE(mgr->access(TransferBufferManagerKey(0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST))); - ASSERT_FALSE(mgr->access(TransferBufferManagerKey(uavcan::NODE_ID_MAX, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST))); + ASSERT_FALSE(mgr->access(TransferBufferManagerKey(127, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST))); TransferBufferBase* tbb = NULL; diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index d47f571aaa..35ebf34c9e 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -12,10 +12,11 @@ class Emulator const uavcan::DataTypeDescriptor type_; uint64_t ts_; uavcan::TransferID tid_; - uint8_t target_node_id_; + uavcan::NodeID target_node_id_; public: - Emulator(uavcan::TransferListenerBase& target, const uavcan::DataTypeDescriptor& type, uint8_t target_node_id = 127) + Emulator(uavcan::TransferListenerBase& target, const uavcan::DataTypeDescriptor& type, + uavcan::NodeID target_node_id = 127) : target_(target) , type_(type) , ts_(0) @@ -55,7 +56,11 @@ public: { std::vector > sers; while (num_transfers--) - sers.push_back(serializeTransfer(*transfers++, target_node_id_, type_)); + { + const uavcan::NodeID dnid = (transfers->transfer_type == uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST) + ? uavcan::NodeID::BROADCAST : target_node_id_; + sers.push_back(serializeTransfer(*transfers++, dnid, type_)); + } send(sers); } @@ -146,8 +151,9 @@ TEST(TransferListener, CrcFailure) ASSERT_TRUE(ser_mft.size() > 1); ASSERT_TRUE(ser_sft.size() == 1); - ser_mft[1].payload[0] = ~ser_mft[1].payload[0]; // CRC is no longer valid - ser_sft[0].payload[2] = ~ser_sft[0].payload[2]; // SFT has no CRC, so the corruption will be undetected + // Fuck my brain. + const_cast(ser_mft[1].getPayloadPtr())[1] = ~ser_mft[1].getPayloadPtr()[1];// CRC is no longer valid + const_cast(ser_sft[0].getPayloadPtr())[2] = ~ser_sft[0].getPayloadPtr()[2];// no CRC - will be undetected /* * Sending and making sure that MFT was not received, but SFT was. @@ -161,7 +167,7 @@ TEST(TransferListener, CrcFailure) emulator.send(sers); Transfer tr_sft_damaged = tr_sft; - tr_sft_damaged.payload[1] = ~tr_sft.payload[1]; // Damaging the data similarly, so that it can be matched + tr_sft_damaged.payload[2] = ~tr_sft.payload[2]; // Damaging the data similarly, so that it can be matched ASSERT_TRUE(subscriber.matchAndPop(tr_sft_damaged)); ASSERT_TRUE(subscriber.isEmpty()); @@ -185,7 +191,6 @@ TEST(TransferListener, BasicSFT) emulator.makeTransfer(uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 2, ""), emulator.makeTransfer(uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 3, "abc"), emulator.makeTransfer(uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 4, ""), - emulator.makeTransfer(uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 5, ""), // New NID, ignored due to OOM emulator.makeTransfer(uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 2, ""), // New TT, ignored due to OOM emulator.makeTransfer(uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 2, "foo"), // Same as 2, not ignored emulator.makeTransfer(uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 2, "123456789abc"),// Same as 2, not SFT - ignore @@ -199,8 +204,8 @@ TEST(TransferListener, BasicSFT) ASSERT_TRUE(subscriber.matchAndPop(transfers[2])); ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); ASSERT_TRUE(subscriber.matchAndPop(transfers[4])); - ASSERT_TRUE(subscriber.matchAndPop(transfers[7])); - ASSERT_TRUE(subscriber.matchAndPop(transfers[9])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[6])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[8])); ASSERT_TRUE(subscriber.isEmpty()); } diff --git a/libuavcan/test/transport/transfer_receiver.cpp b/libuavcan/test/transport/transfer_receiver.cpp index c1bf7f448e..13f40bca59 100644 --- a/libuavcan/test/transport/transfer_receiver.cpp +++ b/libuavcan/test/transport/transfer_receiver.cpp @@ -14,6 +14,7 @@ struct RxFrameGenerator { static const uavcan::TransferBufferManagerKey DEFAULT_KEY; + enum { TARGET_NODE_ID = 126 }; uint16_t data_type_id; uavcan::TransferBufferManagerKey bufmgr_key; @@ -27,28 +28,16 @@ struct RxFrameGenerator uavcan::RxFrame operator()(int iface_index, const std::string& data, uint8_t frame_index, bool last, uint8_t transfer_id, uint64_t ts_monotonic, uint64_t ts_utc = 0) { - if (data.length() > uavcan::Frame::PAYLOAD_LEN_MAX) - { - std::cerr << "RxFrameGenerator(): Data is too long" << std::endl; - std::exit(1); - } + const uavcan::NodeID dst_nid = + (bufmgr_key.getTransferType() == uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST) ? + uavcan::NodeID::BROADCAST : TARGET_NODE_ID; - uavcan::RxFrame frm; + uavcan::Frame frame(data_type_id, bufmgr_key.getTransferType(), bufmgr_key.getNodeID(), + dst_nid, frame_index, transfer_id, last); - frm.iface_index = iface_index; - frm.ts_monotonic = ts_monotonic; - frm.ts_utc = ts_utc; + EXPECT_EQ(data.length(), frame.setPayload(reinterpret_cast(data.c_str()), data.length())); - frm.data_type_id = data_type_id; - frm.frame_index = frame_index; - frm.last_frame = last; - std::copy(data.begin(), data.end(), frm.payload); - frm.payload_len = data.length(); - frm.source_node_id = bufmgr_key.getNodeID(); - frm.transfer_id = uavcan::TransferID(transfer_id); - frm.transfer_type = bufmgr_key.getTransferType(); - - return frm; + return uavcan::RxFrame(frame, ts_monotonic, ts_utc, iface_index); } }; @@ -171,37 +160,37 @@ TEST(TransferReceiver, Basic) ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic()); CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 0, 3000), bk)); // Old TID - CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 15,3100), bk)); // Old TID + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 1, 3100), bk)); // Old TID CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 3, 3200), bk)); // Old TID CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 0, 3300), bk)); // Old TID, wrong iface - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 15,3400), bk)); // Old TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 2, 3400), bk)); // Old TID, wrong iface CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 3, 3500), bk)); // Old TID, wrong iface - CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 8, 3600), bk)); + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 4, 3600), bk)); ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic()); /* * Timeouts */ - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 9, 100000), bk)); // Wrong iface - ignored - CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 10, 600000), bk)); // Accepted due to iface timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 1, 100000), bk)); // Wrong iface - ignored + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 6, 600000), bk)); // Accepted due to iface timeout ASSERT_EQ(600000, rcv.getLastTransferTimestampMonotonic()); - CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 0, true, 11, 600100), bk));// Ignored - old iface 0 - CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 11, 600100), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 0, true, 7, 600100), bk));// Ignored - old iface 0 + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 7, 600100), bk)); ASSERT_EQ(600100, rcv.getLastTransferTimestampMonotonic()); - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 11, 600100), bk));// Old TID - CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 11, 100000000), bk));// Accepted - global timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 7, 600100), bk)); // Old TID + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 7, 100000000), bk));// Accepted - global timeout ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic()); - CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 12, 100000100), bk)); + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 0, 100000100), bk)); ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic()); ASSERT_TRUE(rcv.isTimedOut(900000000)); - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "345678", 0, false, 11, 900000000), bk));// Global timeout - CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, 11, 900000100), bk));// Wrong iface - CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 1, true, 11, 900000200), bk));// Wrong iface - CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", 1, true, 11, 900000200), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "345678", 0, false, 0, 900000000), bk));// Global timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, 0, 900000100), bk));// Wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 1, true, 0, 900000200), bk));// Wrong iface + CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", 1, true, 0, 900000200), bk)); ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic()); ASSERT_FALSE(rcv.isTimedOut(1000)); ASSERT_FALSE(rcv.isTimedOut(900000200)); @@ -232,11 +221,11 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes) /* * Simple transfer, maximum buffer length */ - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 10, 100000000), bk)); // 6 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 1, false, 10, 100000100), bk)); // 14 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 2, false, 10, 100000200), bk)); // 22 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 10, 100000300), bk)); // 30 - CHECK_COMPLETE( rcv.addFrame(gen(1, "12", 4, true, 10, 100000400), bk)); // 32 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 1, 100000000), bk)); // 6 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 1, false, 1, 100000100), bk)); // 14 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 2, false, 1, 100000200), bk)); // 22 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 1, 100000300), bk)); // 30 + CHECK_COMPLETE( rcv.addFrame(gen(1, "12", 4, true, 1, 100000400), bk)); // 32 ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic()); ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567812345678123456781234567812")); @@ -245,11 +234,11 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes) /* * Transfer longer than available buffer space */ - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 11, 100001000), bk)); // 6 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 1, false, 11, 100001100), bk)); // 14 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 2, false, 11, 100001200), bk)); // 22 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 11, 100001200), bk)); // 30 - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 4, true, 11, 100001300), bk)); // 38 // EOT, ignored - lost sync + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 2, 100001000), bk)); // 6 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 1, false, 2, 100001100), bk)); // 14 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 2, false, 2, 100001200), bk)); // 22 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 2, 100001200), bk)); // 30 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 4, true, 2, 100001300), bk)); // 38 // EOT, ignored - lost sync ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic()); // Timestamp will not be overriden ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed @@ -258,7 +247,7 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes) TEST(TransferReceiver, UnterminatedTransfer) { - Context<256> context; + Context<512> context; RxFrameGenerator gen(789, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST); uavcan::TransferReceiver& rcv = context.receiver; uavcan::ITransferBufferManager& bufmgr = context.bufmgr; @@ -285,12 +274,12 @@ TEST(TransferReceiver, OutOfOrderFrames) uavcan::ITransferBufferManager& bufmgr = context.bufmgr; uavcan::TransferBufferAccessor bk(&context.bufmgr, RxFrameGenerator::DEFAULT_KEY); - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 10, 100000000), bk)); - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 3, false, 10, 100000100), bk)); // Out of order - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 2, true, 10, 100000200), bk)); // Out of order - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyui", 1, false, 10, 100000300), bk)); - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 4, true, 10, 100000200), bk)); // Out of order - CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 10, 100000400), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 7, 100000000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 3, false, 7, 100000100), bk)); // Out of order + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 2, true, 7, 100000200), bk)); // Out of order + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyui", 1, false, 7, 100000300), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 4, true, 7, 100000200), bk)); // Out of order + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 7, 100000400), bk)); ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic()); ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678qwertyuiabcd")); @@ -347,8 +336,8 @@ TEST(TransferReceiver, Restart) * Begins immediately after, gets an iface timeout but completes OK */ CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 0, 10000300), bk));// Begin - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 1, false, 0, 13000300), bk));// 3 sec later, iface timeout - CHECK_COMPLETE( rcv.addFrame(gen(1, "12345678", 2, true, 0, 13000400), bk));// OK nevertheless + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 1, false, 0, 12000300), bk));// 2 sec later, iface timeout + CHECK_COMPLETE( rcv.addFrame(gen(1, "12345678", 2, true, 0, 12000400), bk));// OK nevertheless ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456781234567812345678")); ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); @@ -465,11 +454,11 @@ TEST(TransferReceiver, HeaderParsing) const uint64_t ts_monotonic = i + 10; - CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, tid.get(), ts_monotonic), bk2)); - CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", 1, true, tid.get(), ts_monotonic), bk2)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", 0, false, tid.get(), ts_monotonic), bk2)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", 1, true, tid.get(), ts_monotonic), bk2)); - ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "45678abcd")); - ASSERT_EQ(0x3332, rcv.getLastTransferCrc()); // Second and third bytes + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); tid.increment(); bk2.remove(); @@ -478,24 +467,21 @@ TEST(TransferReceiver, HeaderParsing) /* * SFT, message broadcasting */ - static const std::string SFT_PAYLOAD = "12345678"; + static const std::string SFT_PAYLOAD_BROADCAST = "12345678"; + static const std::string SFT_PAYLOAD_UNICAST = "1234567"; { gen.bufmgr_key = uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST); uavcan::TransferBufferAccessor bk(&context.bufmgr, gen.bufmgr_key); - const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, 0, true, tid.get(), 1000); + const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD_BROADCAST, 0, true, tid.get(), 1000); CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk)); ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero - uint8_t payload[uavcan::Frame::PAYLOAD_LEN_MAX]; - unsigned int payload_len = 0xFFFF; - ASSERT_TRUE(uavcan::TransferReceiver::extractSingleFrameTransferPayload(frame, payload, payload_len)); - // All bytes are payload, zero overhead - ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), payload)); + ASSERT_TRUE(std::equal(SFT_PAYLOAD_BROADCAST.begin(), SFT_PAYLOAD_BROADCAST.end(), frame.getPayloadPtr())); tid.increment(); } @@ -509,17 +495,13 @@ TEST(TransferReceiver, HeaderParsing) uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]); uavcan::TransferBufferAccessor bk(&context.bufmgr, gen.bufmgr_key); - const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, 0, true, tid.get(), i + 10000); + const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD_UNICAST, 0, true, tid.get(), i + 10000); CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk)); ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero - uint8_t payload[uavcan::Frame::PAYLOAD_LEN_MAX]; - unsigned int payload_len = 0xFFFF; - ASSERT_TRUE(uavcan::TransferReceiver::extractSingleFrameTransferPayload(frame, payload, payload_len)); - // First byte must be ignored - ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin() + 1, SFT_PAYLOAD.end(), payload)); + ASSERT_TRUE(std::equal(SFT_PAYLOAD_UNICAST.begin(), SFT_PAYLOAD_UNICAST.end(), frame.getPayloadPtr())); tid.increment(); } diff --git a/libuavcan/test/transport/transfer_test_helpers.cpp b/libuavcan/test/transport/transfer_test_helpers.cpp index 4b1206ceba..85149be0fa 100644 --- a/libuavcan/test/transport/transfer_test_helpers.cpp +++ b/libuavcan/test/transport/transfer_test_helpers.cpp @@ -15,8 +15,7 @@ TEST(TransferTestHelpers, Transfer) 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::RxFrame frame(uavcan::Frame(123, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 1, 0, 0, 0, true), 0, 0, 0); uavcan::MultiFrameIncomingTransfer mfit(10, 1000, frame, tba); // Filling the buffer with data @@ -38,7 +37,7 @@ TEST(TransferTestHelpers, MFTSerialization) 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 Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 2, 42, DATA); const std::vector ser = serializeTransfer(transfer, 127, type); @@ -49,9 +48,9 @@ TEST(TransferTestHelpers, MFTSerialization) for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) { std::cout << "\t'"; - for (int i = 0; i < it->payload_len; i++) + for (int i = 0; i < it->getPayloadLen(); i++) { - uint8_t ch = it->payload[i]; + uint8_t ch = it->getPayloadPtr()[i]; if (ch < 0x20 || ch > 0x7E) ch = '.'; std::cout << static_cast(ch); @@ -69,25 +68,25 @@ TEST(TransferTestHelpers, SFTSerialization) type.hash.value[i] = i; { - const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 12, 42, "Nvrfrget"); + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 7, 42, "Nvrfrget"); + const std::vector ser = serializeTransfer(transfer, 0, 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, 7, 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_SERVICE_REQUEST, 12, 42, "7-chars"); - const std::vector ser = serializeTransfer(transfer, 127, type); + const Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 7, 42, ""); + const std::vector ser = serializeTransfer(transfer, 0, 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 Transfer transfer(1, 100000, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 7, 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 index 64279cf50c..be71c3e201 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -17,7 +17,7 @@ struct Transfer uint64_t ts_utc; uavcan::TransferType transfer_type; uavcan::TransferID transfer_id; - uint8_t source_node_id; + uavcan::NodeID source_node_id; std::string payload; Transfer(const uavcan::IncomingTransfer& tr) @@ -25,7 +25,7 @@ struct Transfer , ts_utc(tr.getUtcTimestamp()) , transfer_type(tr.getTransferType()) , transfer_id(tr.getTransferID()) - , source_node_id(tr.getSourceNodeID()) + , source_node_id(tr.getSrcNodeID()) { unsigned int offset = 0; while (true) @@ -45,7 +45,7 @@ struct Transfer } 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) + uavcan::TransferID transfer_id, uavcan::NodeID source_node_id, const std::string& payload) : ts_monotonic(ts_monotonic) , ts_utc(ts_utc) , transfer_type(transfer_type) @@ -72,7 +72,7 @@ struct Transfer << " ts_utc=" << ts_utc << " tt=" << transfer_type << " tid=" << int(transfer_id.get()) - << " snid=" << int(source_node_id) + << " snid=" << int(source_node_id.get()) << "\n\t'" << payload << "'"; return os.str(); } @@ -131,38 +131,34 @@ public: namespace { -std::vector serializeTransfer(const Transfer& transfer, uint8_t target_node_id, +std::vector serializeTransfer(const Transfer& transfer, uavcan::NodeID dst_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; + need_crc = transfer.payload.length() > sizeof(uavcan::CanFrame::data); 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); + need_crc = transfer.payload.length() > (sizeof(uavcan::CanFrame::data) - 1); break; default: std::cerr << "X_X" << std::endl; std::exit(1); } + std::vector raw_payload; if (need_crc) { + uavcan::Crc16 payload_crc(type.hash.value, uavcan::DataTypeHash::NUM_BYTES); + payload_crc.add(reinterpret_cast(transfer.payload.c_str()), transfer.payload.length()); // 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; @@ -173,34 +169,30 @@ std::vector serializeTransfer(const Transfer& transfer, uint8_t while (true) { - int bytes_left = raw_payload.size() - offset; + const int bytes_left = raw_payload.size() - offset; EXPECT_TRUE(bytes_left >= 0); - if (bytes_left <= 0 && !raw_payload.empty()) - break; - uavcan::RxFrame frm; + uavcan::Frame frm(type.id, transfer.transfer_type, transfer.source_node_id, dst_node_id, frame_index, + transfer.transfer_id); + const int spres = frm.setPayload(&*(raw_payload.begin() + offset), bytes_left); + if (spres < 0) + { + std::cerr << ">_<" << std::endl; + std::exit(1); + } + if (spres == bytes_left) + frm.makeLast(); - frm.data_type_id = type.id; - frm.frame_index = frame_index++; + offset += spres; + 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; + const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0); ts_monotonic += 1; ts_utc += 1; - output.push_back(frm); - if (raw_payload.empty()) + output.push_back(rxfrm); + if (frm.isLastFrame()) break; } return output;