uc_frame.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/transport/frame.hpp>
00006 #include <uavcan/transport/can_io.hpp>
00007 #include <uavcan/transport/crc.hpp>
00008 #include <uavcan/debug.hpp>
00009 #include <cassert>
00010 
00011 namespace uavcan
00012 {
00013 /**
00014  * Frame
00015  */
00016 uint8_t Frame::setPayload(const uint8_t* data, unsigned len)
00017 {
00018     const uint8_t maxlen = getPayloadCapacity();
00019     len = min(unsigned(maxlen), len);
00020     (void)copy(data, data + len, payload_);
00021     payload_len_ = uint_fast8_t(len);
00022     return static_cast<uint8_t>(len);
00023 }
00024 
00025 template <int OFFSET, int WIDTH>
00026 inline static uint32_t bitunpack(uint32_t val)
00027 {
00028     StaticAssert<(OFFSET >= 0)>::check();
00029     StaticAssert<(WIDTH > 0)>::check();
00030     StaticAssert<((OFFSET + WIDTH) <= 29)>::check();
00031     return (val >> OFFSET) & ((1UL << WIDTH) - 1);
00032 }
00033 
00034 bool Frame::parse(const CanFrame& can_frame)
00035 {
00036     if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended())
00037     {
00038         UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__);
00039         return false;
00040     }
00041 
00042     if (can_frame.dlc > sizeof(can_frame.data))
00043     {
00044         UAVCAN_ASSERT(0);  // This is not a protocol error, so UAVCAN_ASSERT() is ok
00045         return false;
00046     }
00047 
00048     if (can_frame.dlc < 1)
00049     {
00050         UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__);
00051         return false;
00052     }
00053 
00054     /*
00055      * CAN ID parsing
00056      */
00057     const uint32_t id = can_frame.id & CanFrame::MaskExtID;
00058 
00059     transfer_priority_ = static_cast<uint8_t>(bitunpack<24, 5>(id));
00060     src_node_id_ = static_cast<uint8_t>(bitunpack<0, 7>(id));
00061 
00062     const bool service_not_message = bitunpack<7, 1>(id) != 0U;
00063     if (service_not_message)
00064     {
00065         const bool request_not_response = bitunpack<15, 1>(id) != 0U;
00066         transfer_type_ = request_not_response ? TransferTypeServiceRequest : TransferTypeServiceResponse;
00067 
00068         dst_node_id_ = static_cast<uint8_t>(bitunpack<8, 7>(id));
00069         data_type_id_ = static_cast<uint16_t>(bitunpack<16, 8>(id));
00070     }
00071     else
00072     {
00073         transfer_type_ = TransferTypeMessageBroadcast;
00074         dst_node_id_ = NodeID::Broadcast;
00075 
00076         data_type_id_ = static_cast<uint16_t>(bitunpack<8, 16>(id));
00077 
00078         if (src_node_id_.isBroadcast())
00079         {
00080             // Removing the discriminator
00081             data_type_id_ = static_cast<uint16_t>(data_type_id_.get() & 3U);
00082         }
00083     }
00084 
00085     /*
00086      * CAN payload parsing
00087      */
00088     payload_len_ = static_cast<uint8_t>(can_frame.dlc - 1U);
00089     (void)copy(can_frame.data, can_frame.data + payload_len_, payload_);
00090 
00091     const uint8_t tail = can_frame.data[can_frame.dlc - 1U];
00092 
00093     start_of_transfer_ = (tail & (1U << 7)) != 0;
00094     end_of_transfer_   = (tail & (1U << 6)) != 0;
00095     toggle_            = (tail & (1U << 5)) != 0;
00096 
00097     transfer_id_ = tail & TransferID::Max;
00098 
00099     return isValid();
00100 }
00101 
00102 template <int OFFSET, int WIDTH>
00103 inline static uint32_t bitpack(uint32_t field)
00104 {
00105     StaticAssert<(OFFSET >= 0)>::check();
00106     StaticAssert<(WIDTH > 0)>::check();
00107     StaticAssert<((OFFSET + WIDTH) <= 29)>::check();
00108     UAVCAN_ASSERT((field & ((1UL << WIDTH) - 1)) == field);
00109     return uint32_t((field & ((1UL << WIDTH) - 1)) << OFFSET);
00110 }
00111 
00112 bool Frame::compile(CanFrame& out_can_frame) const
00113 {
00114     if (!isValid())
00115     {
00116         UAVCAN_ASSERT(0);        // This is an application error, so we need to maximize it.
00117         return false;
00118     }
00119 
00120     /*
00121      * CAN ID field
00122      */
00123     out_can_frame.id = CanFrame::FlagEFF |
00124         bitpack<0, 7>(src_node_id_.get()) |
00125         bitpack<24, 5>(transfer_priority_.get());
00126 
00127     if (transfer_type_ == TransferTypeMessageBroadcast)
00128     {
00129         out_can_frame.id |=
00130             bitpack<7, 1>(0U) |
00131             bitpack<8, 16>(data_type_id_.get());
00132     }
00133     else
00134     {
00135         const bool request_not_response = transfer_type_ == TransferTypeServiceRequest;
00136         out_can_frame.id |=
00137             bitpack<7, 1>(1U) |
00138             bitpack<8, 7>(dst_node_id_.get()) |
00139             bitpack<15, 1>(request_not_response ? 1U : 0U) |
00140             bitpack<16, 8>(data_type_id_.get());
00141     }
00142 
00143     /*
00144      * Payload
00145      */
00146     uint8_t tail = transfer_id_.get();
00147     if (start_of_transfer_)
00148     {
00149         tail |= (1U << 7);
00150     }
00151     if (end_of_transfer_)
00152     {
00153         tail |= (1U << 6);
00154     }
00155     if (toggle_)
00156     {
00157         tail |= (1U << 5);
00158     }
00159 
00160     UAVCAN_ASSERT(payload_len_ < sizeof(static_cast<CanFrame*>(UAVCAN_NULLPTR)->data));
00161 
00162     out_can_frame.dlc = static_cast<uint8_t>(payload_len_);
00163     (void)copy(payload_, payload_ + payload_len_, out_can_frame.data);
00164 
00165     out_can_frame.data[out_can_frame.dlc] = tail;
00166     out_can_frame.dlc++;
00167 
00168     /*
00169      * Discriminator
00170      */
00171     if (src_node_id_.isBroadcast())
00172     {
00173         TransferCRC crc;
00174         crc.add(out_can_frame.data, out_can_frame.dlc);
00175         out_can_frame.id |= bitpack<10, 14>(crc.get() & ((1U << 14) - 1U));
00176     }
00177 
00178     return true;
00179 }
00180 
00181 bool Frame::isValid() const
00182 {
00183     /*
00184      * Toggle
00185      */
00186     if (start_of_transfer_ && toggle_)
00187     {
00188         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00189         return false;
00190     }
00191 
00192     /*
00193      * Node ID
00194      */
00195     if (!src_node_id_.isValid() || !dst_node_id_.isValid())
00196     {
00197         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00198         return false;
00199     }
00200 
00201     if (src_node_id_.isUnicast() && (src_node_id_ == dst_node_id_))
00202     {
00203         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00204         return false;
00205     }
00206 
00207     /*
00208      * Transfer type
00209      */
00210     if (transfer_type_ >= NumTransferTypes)
00211     {
00212         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00213         return false;
00214     }
00215 
00216     if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast())
00217     {
00218         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00219         return false;
00220     }
00221 
00222     // Anonymous transfers
00223     if (src_node_id_.isBroadcast() &&
00224         (!start_of_transfer_ || !end_of_transfer_ || (transfer_type_ != TransferTypeMessageBroadcast)))
00225     {
00226         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00227         return false;
00228     }
00229 
00230     /*
00231      * Payload
00232      */
00233     if (payload_len_ > getPayloadCapacity())
00234     {
00235         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00236         return false;
00237     }
00238 
00239     /*
00240      * Data type ID
00241      */
00242     if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_)))
00243     {
00244         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00245         return false;
00246     }
00247 
00248     /*
00249      * Priority
00250      */
00251     if (!transfer_priority_.isValid())
00252     {
00253         UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
00254         return false;
00255     }
00256 
00257     return true;
00258 }
00259 
00260 bool Frame::operator==(const Frame& rhs) const
00261 {
00262     return
00263         (transfer_priority_ == rhs.transfer_priority_) &&
00264         (transfer_type_     == rhs.transfer_type_) &&
00265         (data_type_id_      == rhs.data_type_id_) &&
00266         (src_node_id_       == rhs.src_node_id_) &&
00267         (dst_node_id_       == rhs.dst_node_id_) &&
00268         (transfer_id_       == rhs.transfer_id_) &&
00269         (toggle_            == rhs.toggle_) &&
00270         (start_of_transfer_ == rhs.start_of_transfer_) &&
00271         (end_of_transfer_   == rhs.end_of_transfer_) &&
00272         (payload_len_       == rhs.payload_len_) &&
00273         equal(payload_, payload_ + payload_len_, rhs.payload_);
00274 }
00275 
00276 #if UAVCAN_TOSTRING
00277 std::string Frame::toString() const
00278 {
00279     static const int BUFLEN = 100;
00280     char buf[BUFLEN];
00281     int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d sot=%d eot=%d togl=%d tid=%d payload=[",
00282                        int(transfer_priority_.get()), int(data_type_id_.get()), int(transfer_type_),
00283                        int(src_node_id_.get()), int(dst_node_id_.get()),
00284                        int(start_of_transfer_), int(end_of_transfer_), int(toggle_), int(transfer_id_.get()));
00285 
00286     for (unsigned i = 0; i < payload_len_; i++)
00287     {
00288         // Coverity Scan complains about payload_ being not default initialized. This is OK.
00289         // coverity[read_parm_fld]
00290         ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), "%02x", payload_[i]);
00291         if ((i + 1) < payload_len_)
00292         {
00293             ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), " ");
00294         }
00295     }
00296     (void)snprintf(buf + ofs, unsigned(BUFLEN - ofs), "]");
00297     return std::string(buf);
00298 }
00299 #endif
00300 
00301 /**
00302  * RxFrame
00303  */
00304 bool RxFrame::parse(const CanRxFrame& can_frame)
00305 {
00306     if (!Frame::parse(can_frame))
00307     {
00308         return false;
00309     }
00310     if (can_frame.ts_mono.isZero())  // Monotonic timestamps are mandatory.
00311     {
00312         UAVCAN_ASSERT(0);                   // If it is not set, it's a driver failure.
00313         return false;
00314     }
00315     ts_mono_ = can_frame.ts_mono;
00316     ts_utc_ = can_frame.ts_utc;
00317     iface_index_ = can_frame.iface_index;
00318     return true;
00319 }
00320 
00321 #if UAVCAN_TOSTRING
00322 std::string RxFrame::toString() const
00323 {
00324     std::string out = Frame::toString();
00325     out.reserve(128);
00326     out += " ts_m="   + ts_mono_.toString();
00327     out += " ts_utc=" + ts_utc_.toString();
00328     out += " iface=";
00329     out += char('0' + iface_index_);
00330     return out;
00331 }
00332 #endif
00333 
00334 }