uc_transfer_receiver.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/transport/transfer_receiver.hpp>
00006 #include <uavcan/transport/crc.hpp>
00007 #include <uavcan/debug.hpp>
00008 #include <cstdlib>
00009 #include <cassert>
00010 
00011 namespace uavcan
00012 {
00013 
00014 const uint16_t TransferReceiver::MinTransferIntervalMSec;
00015 const uint16_t TransferReceiver::MaxTransferIntervalMSec;
00016 const uint16_t TransferReceiver::DefaultTransferIntervalMSec;
00017 const uint16_t TransferReceiver::DefaultTidTimeoutMSec;
00018 
00019 MonotonicDuration TransferReceiver::getIfaceSwitchDelay() const
00020 {
00021     return MonotonicDuration::fromMSec(transfer_interval_msec_);
00022 }
00023 
00024 MonotonicDuration TransferReceiver::getTidTimeout() const
00025 {
00026     return MonotonicDuration::fromMSec(DefaultTidTimeoutMSec);
00027 }
00028 
00029 void TransferReceiver::registerError() const
00030 {
00031     error_cnt_ = static_cast<uint8_t>(error_cnt_ + 1) & ErrorCntMask;
00032 }
00033 
00034 void TransferReceiver::updateTransferTimings()
00035 {
00036     UAVCAN_ASSERT(!this_transfer_ts_.isZero());
00037 
00038     const MonotonicTime prev_prev_ts = prev_transfer_ts_;
00039     prev_transfer_ts_ = this_transfer_ts_;
00040 
00041     if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts))
00042     {
00043         uint64_t interval_msec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toMSec());
00044         interval_msec = min(interval_msec, uint64_t(MaxTransferIntervalMSec));
00045         interval_msec = max(interval_msec, uint64_t(MinTransferIntervalMSec));
00046         transfer_interval_msec_ = static_cast<uint16_t>((uint64_t(transfer_interval_msec_) * 7U + interval_msec) / 8U);
00047     }
00048 }
00049 
00050 void TransferReceiver::prepareForNextTransfer()
00051 {
00052     tid_.increment();
00053     next_toggle_ = false;
00054     buffer_write_pos_ = 0;
00055 }
00056 
00057 bool TransferReceiver::validate(const RxFrame& frame) const
00058 {
00059     if (iface_index_ != frame.getIfaceIndex())
00060     {
00061         return false;
00062     }
00063     if (frame.isStartOfTransfer() && !frame.isEndOfTransfer() && (frame.getPayloadLen() < TransferCRC::NumBytes))
00064     {
00065         UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
00066         registerError();
00067         return false;
00068     }
00069     if (frame.isStartOfTransfer() && frame.getToggle())
00070     {
00071         UAVCAN_TRACE("TransferReceiver", "Toggle bit is not cleared, %s", frame.toString().c_str());
00072         registerError();
00073         return false;
00074     }
00075     if (frame.isStartOfTransfer() && isMidTransfer())
00076     {
00077         UAVCAN_TRACE("TransferReceiver", "Unexpected start of transfer, %s", frame.toString().c_str());
00078         registerError();
00079     }
00080     if (frame.getToggle() != next_toggle_)
00081     {
00082         UAVCAN_TRACE("TransferReceiver", "Unexpected toggle bit (not %i), %s",
00083                      int(next_toggle_), frame.toString().c_str());
00084         registerError();
00085         return false;
00086     }
00087     if (frame.getTransferID() != tid_)
00088     {
00089         UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str());
00090         registerError();
00091         return false;
00092     }
00093     return true;
00094 }
00095 
00096 bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
00097 {
00098     const uint8_t* const payload = frame.getPayloadPtr();
00099     const unsigned payload_len = frame.getPayloadLen();
00100 
00101     if (frame.isStartOfTransfer())     // First frame contains CRC, we need to extract it now
00102     {
00103         if (frame.getPayloadLen() < TransferCRC::NumBytes)
00104         {
00105             return false;    // Must have been validated earlier though. I think I'm paranoid.
00106         }
00107         this_transfer_crc_ = static_cast<uint16_t>(payload[0] & 0xFF);
00108         this_transfer_crc_ |= static_cast<uint16_t>(static_cast<uint16_t>(payload[1] & 0xFF) << 8);  // Little endian.
00109 
00110         const unsigned effective_payload_len = payload_len - TransferCRC::NumBytes;
00111         const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len);
00112         const bool success = res == static_cast<int>(effective_payload_len);
00113         if (success)
00114         {
00115             buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len);
00116         }
00117         return success;
00118     }
00119     else
00120     {
00121         const int res = buf.write(buffer_write_pos_, payload, payload_len);
00122         const bool success = res == static_cast<int>(payload_len);
00123         if (success)
00124         {
00125             buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len);
00126         }
00127         return success;
00128     }
00129 }
00130 
00131 TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
00132 {
00133     // Transfer timestamps are derived from the first frame
00134     if (frame.isStartOfTransfer())
00135     {
00136         this_transfer_ts_ = frame.getMonotonicTimestamp();
00137         first_frame_ts_   = frame.getUtcTimestamp();
00138     }
00139 
00140     if (frame.isStartOfTransfer() && frame.isEndOfTransfer())
00141     {
00142         tba.remove();
00143         updateTransferTimings();
00144         prepareForNextTransfer();
00145         this_transfer_crc_ = 0;         // SFT has no CRC
00146         return ResultSingleFrame;
00147     }
00148 
00149     // Payload write
00150     ITransferBuffer* buf = tba.access();
00151     if (buf == UAVCAN_NULLPTR)
00152     {
00153         buf = tba.create();
00154     }
00155     if (buf == UAVCAN_NULLPTR)
00156     {
00157         UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
00158         prepareForNextTransfer();
00159         registerError();
00160         return ResultNotComplete;
00161     }
00162     if (!writePayload(frame, *buf))
00163     {
00164         UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str());
00165         tba.remove();
00166         prepareForNextTransfer();
00167         registerError();
00168         return ResultNotComplete;
00169     }
00170     next_toggle_ = !next_toggle_;
00171 
00172     if (frame.isEndOfTransfer())
00173     {
00174         updateTransferTimings();
00175         prepareForNextTransfer();
00176         return ResultComplete;
00177     }
00178     return ResultNotComplete;
00179 }
00180 
00181 bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
00182 {
00183     return (current_ts - this_transfer_ts_) > getTidTimeout();
00184 }
00185 
00186 TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
00187 {
00188     if ((frame.getMonotonicTimestamp().isZero()) ||
00189         (frame.getMonotonicTimestamp() < prev_transfer_ts_) ||
00190         (frame.getMonotonicTimestamp() < this_transfer_ts_))
00191     {
00192         UAVCAN_TRACE("TransferReceiver", "Invalid frame, %s", frame.toString().c_str());
00193         return ResultNotComplete;
00194     }
00195 
00196     const bool not_initialized = !isInitialized();
00197     const bool tid_timed_out = isTimedOut(frame.getMonotonicTimestamp());
00198     const bool same_iface = frame.getIfaceIndex() == iface_index_;
00199     const bool first_frame = frame.isStartOfTransfer();
00200     const bool non_wrapped_tid = tid_.computeForwardDistance(frame.getTransferID()) < TransferID::Half;
00201     const bool not_previous_tid = frame.getTransferID().computeForwardDistance(tid_) > 1;
00202     const bool iface_switch_allowed = (frame.getMonotonicTimestamp() - this_transfer_ts_) > getIfaceSwitchDelay();
00203 
00204     // FSM, the hard way
00205     const bool need_restart =
00206         (not_initialized) ||
00207         (tid_timed_out) ||
00208         (same_iface && first_frame && not_previous_tid) ||
00209         (iface_switch_allowed && first_frame && non_wrapped_tid);
00210 
00211     if (need_restart)
00212     {
00213         if (!not_initialized && (tid_ != frame.getTransferID()))
00214         {
00215             registerError();
00216         }
00217         UAVCAN_TRACE("TransferReceiver", "Restart [ni=%d, isa=%d, tt=%d, si=%d, ff=%d, nwtid=%d, nptid=%d, tid=%d], %s",
00218                      int(not_initialized), int(iface_switch_allowed), int(tid_timed_out), int(same_iface),
00219                      int(first_frame), int(non_wrapped_tid), int(not_previous_tid), int(tid_.get()),
00220                      frame.toString().c_str());
00221         tba.remove();
00222         iface_index_ = frame.getIfaceIndex() & IfaceIndexMask;
00223         tid_ = frame.getTransferID();
00224         next_toggle_ = false;
00225         buffer_write_pos_ = 0;
00226         this_transfer_crc_ = 0;
00227         if (!first_frame)
00228         {
00229             tid_.increment();
00230             return ResultNotComplete;
00231         }
00232     }
00233 
00234     if (!validate(frame))
00235     {
00236         return ResultNotComplete;
00237     }
00238     return receive(frame, tba);
00239 }
00240 
00241 uint8_t TransferReceiver::yieldErrorCount()
00242 {
00243     const uint8_t ret = error_cnt_;
00244     error_cnt_ = 0;
00245     return ret;
00246 }
00247 
00248 }