diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index d93bf997a8..d503566991 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -17,6 +17,7 @@ namespace uavcan class UAVCAN_EXPORT Frame { uint8_t payload_[sizeof(CanFrame::data)]; + TransferPriority transfer_priority_; TransferType transfer_type_; DataTypeID data_type_id_; uint_fast8_t payload_len_; @@ -27,10 +28,12 @@ class UAVCAN_EXPORT Frame bool last_frame_; public: - enum { MaxIndex = 62 }; // 63 (or 0b111111) is reserved + static const uint8_t MaxIndexForService = 62; // 63 is reserved + static const uint8_t MaxIndexForMessage = 15; Frame() - : transfer_type_(TransferType(NumTransferTypes)) // That is invalid value + : transfer_priority_(TransferPriority(NumTransferPriorities)) // Invalid value + , transfer_type_(TransferType(NumTransferTypes)) // Invalid value , payload_len_(0) , frame_index_(0) , transfer_id_(0) @@ -39,7 +42,8 @@ public: Frame(DataTypeID 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) + : transfer_priority_(getDefaultPriorityForTransferType(transfer_type)) + , transfer_type_(transfer_type) , data_type_id_(data_type_id) , payload_len_(0) , src_node_id_(src_node_id) @@ -51,9 +55,21 @@ public: UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type))); UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); - UAVCAN_ASSERT(frame_index <= MaxIndex); + UAVCAN_ASSERT(frame_index <= getMaxIndex()); } + static uint_fast8_t getMaxIndexForTransferType(const TransferType type); + + uint_fast8_t getMaxIndex() const { return getMaxIndexForTransferType(transfer_type_); } + + /** + * Priority can be set only for message transfers. + * Attempt to set priority of a service transfer will cause assertion failure in debug build; in release build + * it will be ignored. + */ + void setPriority(TransferPriority priority); + TransferPriority getPriority() const { return transfer_priority_; } + /** * Max payload length depends on the transfer type and frame index. */ diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index 6ffcd19175..c1cdc38abb 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -53,6 +53,33 @@ static inline unsigned getMaxPayloadLenForTransferType(const TransferType type) } +enum TransferPriority +{ + TransferPriorityHigh = 0, + TransferPriorityNormal = 1, + TransferPriorityService = 2, + TransferPriorityLow = 3, + NumTransferPriorities = 4 +}; + +static inline TransferPriority getDefaultPriorityForTransferType(const TransferType type) +{ + if (type == TransferTypeServiceResponse || type == TransferTypeServiceRequest) + { + return TransferPriorityService; + } + else if (type == TransferTypeMessageBroadcast || type == TransferTypeMessageUnicast) + { + return TransferPriorityNormal; + } + else + { + UAVCAN_ASSERT(0); + return TransferPriority(0); // whatever + } +} + + class UAVCAN_EXPORT TransferID { uint8_t value_; diff --git a/libuavcan/src/transport/uc_frame.cpp b/libuavcan/src/transport/uc_frame.cpp index 412e31df71..4298528fba 100644 --- a/libuavcan/src/transport/uc_frame.cpp +++ b/libuavcan/src/transport/uc_frame.cpp @@ -11,6 +11,48 @@ namespace uavcan /** * Frame */ +const uint8_t Frame::MaxIndexForService; +const uint8_t Frame::MaxIndexForMessage; + +uint_fast8_t Frame::getMaxIndexForTransferType(const TransferType type) +{ + if (type == TransferTypeMessageBroadcast || + type == TransferTypeMessageUnicast) + { + return MaxIndexForMessage; + } + else if (type == TransferTypeServiceRequest || + type == TransferTypeServiceResponse) + { + return MaxIndexForService; + } + else + { + UAVCAN_ASSERT(0); + return 0; + } +} + +void Frame::setPriority(const TransferPriority priority) +{ + if (transfer_type_ == TransferTypeMessageBroadcast || + transfer_type_ == TransferTypeMessageUnicast) + { + if (priority != TransferPriorityService) + { + transfer_priority_ = priority; + } + else + { + UAVCAN_ASSERT(0); + } + } + else + { + UAVCAN_ASSERT(0); + } +} + int Frame::getMaxPayloadLen() const { switch (getTransferType()) @@ -49,6 +91,9 @@ int Frame::setPayload(const uint8_t* data, unsigned len) template inline static uint32_t bitunpack(uint32_t val) { + StaticAssert<(OFFSET >= 0)>::check(); + StaticAssert<(WIDTH > 0)>::check(); + StaticAssert<((OFFSET + WIDTH) <= 29)>::check(); return (val >> OFFSET) & ((1UL << WIDTH) - 1); } @@ -71,10 +116,25 @@ bool Frame::parse(const CanFrame& can_frame) const uint32_t id = can_frame.id & CanFrame::MaskExtID; transfer_id_ = uint8_t(bitunpack<0, 3>(id)); last_frame_ = bitunpack<3, 1>(id) != 0; - frame_index_ = uint8_t(bitunpack<4, 6>(id)); - src_node_id_ = uint8_t(bitunpack<10, 7>(id)); - transfer_type_ = TransferType(bitunpack<17, 2>(id)); - data_type_id_ = uint16_t(bitunpack<19, 10>(id)); + // TT-specific fields skipped + transfer_priority_ = TransferPriority(bitunpack<27, 2>(id)); + + if (transfer_priority_ == TransferPriorityService) + { + frame_index_ = uint_fast8_t(bitunpack<4, 6>(id)); + src_node_id_ = uint8_t(bitunpack<10, 7>(id)); + data_type_id_ = uint16_t(bitunpack<17, 9>(id)); + // RequestNotResponse + transfer_type_ = (bitunpack<26, 1>(id) == 1U) ? TransferTypeServiceRequest : TransferTypeServiceResponse; + } + else + { + frame_index_ = uint_fast8_t(bitunpack<4, 4>(id)); + // BroadcastNotUnicast + transfer_type_ = (bitunpack<8, 1>(id) == 1U) ? TransferTypeMessageBroadcast : TransferTypeMessageUnicast; + src_node_id_ = uint8_t(bitunpack<9, 7>(id)); + data_type_id_ = uint16_t(bitunpack<16, 11>(id)); + } /* * CAN payload parsing @@ -111,12 +171,29 @@ bool Frame::parse(const CanFrame& can_frame) } } + /* + * Special case for anonymous transfers - trailing 8 bits of CAN ID must be ignored + * - Transfer ID (assumed zero) + * - Last Frame (assumed true) + * - Frame Index (assumed zero) + */ + if (src_node_id_.isBroadcast()) + { + transfer_id_ = TransferID(0); + last_frame_ = true; + frame_index_ = 0; + } + return isValid(); } template inline static uint32_t bitpack(uint32_t field) { + StaticAssert<(OFFSET >= 0)>::check(); + StaticAssert<(WIDTH > 0)>::check(); + StaticAssert<((OFFSET + WIDTH) <= 29)>::check(); + UAVCAN_ASSERT((field & ((1UL << WIDTH) - 1)) == field); return uint32_t((field & ((1UL << WIDTH) - 1)) << OFFSET); } @@ -128,15 +205,44 @@ bool Frame::compile(CanFrame& out_can_frame) const return false; } + /* + * Setting CAN ID field + */ + // Common fields for messages and services out_can_frame.id = CanFrame::FlagEFF | 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_.get()); + /* TT-specific fields skipped */ + bitpack<27, 2>(transfer_priority_); + if (transfer_type_ == TransferTypeServiceRequest || + transfer_type_ == TransferTypeServiceResponse) + { + out_can_frame.id |= + bitpack<4, 6>(frame_index_) | + bitpack<10, 7>(src_node_id_.get()) | + bitpack<17, 9>(data_type_id_.get()) | + bitpack<26, 1>((transfer_type_ == TransferTypeServiceRequest) ? 1U : 0U); + } + else if (transfer_type_ == TransferTypeMessageBroadcast || + transfer_type_ == TransferTypeMessageUnicast) + { + out_can_frame.id |= + bitpack<4, 4>(frame_index_) | + bitpack<8, 1>((transfer_type_ == TransferTypeMessageBroadcast) ? 1U : 0U) | + bitpack<9, 7>(src_node_id_.get()) | + bitpack<16, 11>(data_type_id_.get()); + } + else + { + UAVCAN_ASSERT(0); + return false; + } + + /* + * Setting payload + */ switch (transfer_type_) { case TransferTypeMessageBroadcast: @@ -161,40 +267,126 @@ bool Frame::compile(CanFrame& out_can_frame) const return false; } } + + /* + * Setting trailing bits of CAN ID for anonymous message + * This overrides the following fields: + * - Transfer ID (assumed zero) + * - Last Frame (assumed true) + * - Frame Index (assumed zero) + */ + if (src_node_id_.isBroadcast()) + { + uint8_t sum = 0; + out_can_frame.id &= ~bitpack<0, 8>(sum); // Clearing bits + for (uint_fast8_t i = 0; i < payload_len_; i++) + { + sum = static_cast(sum + payload_[i]); + } + out_can_frame.id &= ~bitpack<0, 8>(sum); // Setting the checksum + } + return true; } bool Frame::isValid() const { - // Refer to the specification for the detailed explanation of the checks - const bool invalid = - (frame_index_ > MaxIndex) || - ((frame_index_ == MaxIndex) && !last_frame_) || - (!src_node_id_.isValid()) || - (!dst_node_id_.isValid()) || - (src_node_id_.isUnicast() ? (src_node_id_ == dst_node_id_) : false) || - (src_node_id_.isBroadcast() - ? (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast)) - : false) || - ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) || - (transfer_type_ >= NumTransferTypes) || - (static_cast(payload_len_) > getMaxPayloadLen()) || - (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_))); + /* + * Frame index + */ + if (frame_index_ > getMaxIndex()) + { + return false; + } - return !invalid; + if ((frame_index_ == getMaxIndex()) && !last_frame_) + { + return false; + } + + /* + * Node ID + */ + if (!src_node_id_.isValid() || + !dst_node_id_.isValid()) + { + return false; + } + + if (src_node_id_.isUnicast() && + (src_node_id_ == dst_node_id_)) + { + return false; + } + + /* + * Transfer type + */ + if (transfer_type_ >= NumTransferTypes) + { + return false; + } + + if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) + { + return false; + } + + // Anonymous transfers + if (src_node_id_.isBroadcast() && + (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast))) + { + return false; + } + + /* + * Payload + */ + if (static_cast(payload_len_) > getMaxPayloadLen()) + { + return false; + } + + /* + * Data type ID + */ + if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_))) + { + return false; + } + + /* + * Priority + */ + if (transfer_priority_ >= NumTransferPriorities) + { + return false; + } + + if (transfer_type_ == TransferTypeServiceRequest || + transfer_type_ == TransferTypeServiceResponse) + { + if (transfer_priority_ != TransferPriorityService) + { + return false; + } + } + + return true; } 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_) && + (transfer_priority_ == rhs.transfer_priority_) && + (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_) && equal(payload_, payload_ + payload_len_, rhs.payload_); } @@ -202,7 +394,7 @@ bool Frame::operator==(const Frame& rhs) const std::string Frame::toString() const { /* - * Frame ID fields, according to UAVCAN specs: + * - Priority * - Data Type ID * - Transfer Type * - Source Node ID @@ -212,8 +404,8 @@ std::string Frame::toString() const */ static const int BUFLEN = 100; char buf[BUFLEN]; - int ofs = snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[", - int(data_type_id_.get()), int(transfer_type_), int(src_node_id_.get()), + int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d idx=%d last=%d tid=%d payload=[", + int(transfer_priority_), int(data_type_id_.get()), int(transfer_type_), int(src_node_id_.get()), int(dst_node_id_.get()), int(frame_index_), int(last_frame_), int(transfer_id_.get())); for (unsigned i = 0; i < payload_len_; i++) diff --git a/libuavcan/src/transport/uc_transfer_receiver.cpp b/libuavcan/src/transport/uc_transfer_receiver.cpp index de33f75dcd..7d85a0b749 100644 --- a/libuavcan/src/transport/uc_transfer_receiver.cpp +++ b/libuavcan/src/transport/uc_transfer_receiver.cpp @@ -77,7 +77,7 @@ bool TransferReceiver::validate(const RxFrame& frame) const registerError(); return false; } - if ((frame.getIndex() == Frame::MaxIndex) && !frame.isLast()) + if ((frame.getIndex() == frame.getMaxIndex()) && !frame.isLast()) { UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str()); registerError(); diff --git a/libuavcan/test/transport/frame.cpp b/libuavcan/test/transport/frame.cpp index bb54554fc9..e4a5c7e710 100644 --- a/libuavcan/test/transport/frame.cpp +++ b/libuavcan/test/transport/frame.cpp @@ -136,7 +136,8 @@ TEST(Frame, FrameParsing) * MFT invalid - unterminated transfer */ can.id = CanFrame::FlagEFF | - (2 << 0) | (0 << 3) | (Frame::MaxIndex << 4) | (42 << 10) | + (2 << 0) | (0 << 3) | + (unsigned(Frame::getMaxIndexForTransferType(uavcan::TransferTypeMessageUnicast)) << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19); ASSERT_FALSE(frame.parse(can)); @@ -226,12 +227,13 @@ TEST(Frame, FrameToString) // RX frame default RxFrame rx_frame; - EXPECT_EQ("dtid=65535 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", + EXPECT_EQ("prio=4 dtid=65535 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", rx_frame.toString()); // RX frame max len rx_frame = RxFrame(Frame(uavcan::DataTypeID::MaxPossibleDataTypeIDValue, uavcan::TransferTypeMessageUnicast, - uavcan::NodeID::Max, uavcan::NodeID::Max - 1, Frame::MaxIndex, + uavcan::NodeID::Max, uavcan::NodeID::Max - 1, + Frame::getMaxIndexForTransferType(uavcan::TransferTypeMessageUnicast), uavcan::TransferID::Max, true), uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3); @@ -242,15 +244,16 @@ TEST(Frame, FrameToString) } rx_frame.setPayload(data, sizeof(data)); - EXPECT_EQ("dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06] " + EXPECT_EQ("prio=1 dtid=2047 tt=3 snid=127 dnid=126 idx=15 last=1 tid=7 payload=[00 01 02 03 04 05 06] " "ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3", rx_frame.toString()); // Plain frame default Frame frame; - EXPECT_EQ("dtid=65535 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[]", frame.toString()); + EXPECT_EQ("prio=4 dtid=65535 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 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06]", frame.toString()); + EXPECT_EQ("prio=1 dtid=2047 tt=3 snid=127 dnid=126 idx=15 last=1 tid=7 payload=[00 01 02 03 04 05 06]", + frame.toString()); } diff --git a/libuavcan/test/transport/transfer_receiver.cpp b/libuavcan/test/transport/transfer_receiver.cpp index fb888ec0d1..495e585d5b 100644 --- a/libuavcan/test/transport/transfer_receiver.cpp +++ b/libuavcan/test/transport/transfer_receiver.cpp @@ -260,13 +260,15 @@ TEST(TransferReceiver, UnterminatedTransfer) uavcan::ITransferBufferManager& bufmgr = context.bufmgr; uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + const uint8_t MaxIndex = uavcan::Frame::getMaxIndexForTransferType(RxFrameGenerator::DEFAULT_KEY.getTransferType()); + std::string content; - for (uint8_t i = 0; i <= uavcan::Frame::MaxIndex; i++) + for (uint8_t i = 0; i <= MaxIndex; i++) { CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", i, false, 0, 1000U + i), bk)); // Last one will be dropped content += "12345678"; } - CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", uavcan::Frame::MaxIndex, true, 0, 1100), bk)); + CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", MaxIndex, true, 0, 1100), bk)); ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec()); ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), std::string(content, 2))); ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp index f27a603c49..c5252da303 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -226,7 +226,7 @@ std::vector serializeTransfer(const Transfer& transfer) } offset += unsigned(spres); - EXPECT_GE(uavcan::Frame::MaxIndex, frame_index); + EXPECT_GE(uavcan::Frame::getMaxIndexForTransferType(transfer.transfer_type), frame_index); frame_index++; const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);