uc_transfer_listener.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/transport/transfer_listener.hpp>
00006 #include <uavcan/debug.hpp>
00007 #include <cstdlib>
00008 #include <cassert>
00009 
00010 namespace uavcan
00011 {
00012 /*
00013  * IncomingTransfer
00014  */
00015 int IncomingTransfer::write(unsigned, const uint8_t*, unsigned)
00016 {
00017     UAVCAN_ASSERT(0);  // Incoming transfer container is read-only
00018     return -ErrLogic;
00019 }
00020 
00021 /*
00022  * SingleFrameIncomingTransfer
00023  */
00024 SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm)
00025     : IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getPriority(),
00026                        frm.getTransferType(), frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex())
00027     , payload_(frm.getPayloadPtr())
00028     , payload_len_(uint8_t(frm.getPayloadLen()))
00029 {
00030     UAVCAN_ASSERT(frm.isValid());
00031 }
00032 
00033 int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const
00034 {
00035     if (data == UAVCAN_NULLPTR)
00036     {
00037         UAVCAN_ASSERT(0);
00038         return -ErrInvalidParam;
00039     }
00040     if (offset >= payload_len_)
00041     {
00042         return 0;
00043     }
00044     if ((offset + len) > payload_len_)
00045     {
00046         len = payload_len_ - offset;
00047     }
00048     UAVCAN_ASSERT((offset + len) <= payload_len_);
00049     (void)copy(payload_ + offset, payload_ + offset + len, data);
00050     return int(len);
00051 }
00052 
00053 bool SingleFrameIncomingTransfer::isAnonymousTransfer() const
00054 {
00055     return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast();
00056 }
00057 
00058 /*
00059  * MultiFrameIncomingTransfer
00060  */
00061 MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc,
00062                                                        const RxFrame& last_frame, TransferBufferAccessor& tba)
00063     : IncomingTransfer(ts_mono, ts_utc, last_frame.getPriority(), last_frame.getTransferType(),
00064                        last_frame.getTransferID(), last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
00065     , buf_acc_(tba)
00066 {
00067     UAVCAN_ASSERT(last_frame.isValid());
00068     UAVCAN_ASSERT(last_frame.isEndOfTransfer());
00069 }
00070 
00071 int MultiFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const
00072 {
00073     const ITransferBuffer* const tbb = const_cast<TransferBufferAccessor&>(buf_acc_).access();
00074     if (tbb == UAVCAN_NULLPTR)
00075     {
00076         UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer");
00077         return -ErrLogic;
00078     }
00079     return tbb->read(offset, data, len);
00080 }
00081 
00082 /*
00083  * TransferListener::TimedOutReceiverPredicate
00084  */
00085 bool TransferListener::TimedOutReceiverPredicate::operator()(const TransferBufferManagerKey& key,
00086                                                              const TransferReceiver& value) const
00087 {
00088     if (value.isTimedOut(ts_))
00089     {
00090         UAVCAN_TRACE("TransferListener", "Timed out receiver: %s", key.toString().c_str());
00091         /*
00092          * TransferReceivers do not own their buffers - this helps the Map<> container to copy them
00093          * around quickly and safely (using default assignment operator). Downside is that we need to
00094          * destroy the buffers manually.
00095          * Maybe it is not good that the predicate has side effects, but I ran out of better ideas.
00096          */
00097         parent_bufmgr_.remove(key);
00098         return true;
00099     }
00100     return false;
00101 }
00102 
00103 /*
00104  * TransferListener
00105  */
00106 bool TransferListener::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const
00107 {
00108     TransferCRC crc = crc_base_;
00109     unsigned offset = 0;
00110     while (true)
00111     {
00112         uint8_t buf[16];
00113         const int res = tbb.read(offset, buf, sizeof(buf));
00114         if (res < 0)
00115         {
00116             UAVCAN_TRACE("TransferListener", "Failed to check CRC: Buffer read failure %i", res);
00117             return false;
00118         }
00119         if (res == 0)
00120         {
00121             break;
00122         }
00123         offset += unsigned(res);
00124         crc.add(buf, unsigned(res));
00125     }
00126     if (crc.get() != compare_with)
00127     {
00128         UAVCAN_TRACE("TransferListener", "CRC mismatch, expected=0x%04x, got=0x%04x",
00129                      int(compare_with), int(crc.get()));
00130         return false;
00131     }
00132     return true;
00133 }
00134 
00135 void TransferListener::handleReception(TransferReceiver& receiver, const RxFrame& frame,
00136                                            TransferBufferAccessor& tba)
00137 {
00138     switch (receiver.addFrame(frame, tba))
00139     {
00140     case TransferReceiver::ResultNotComplete:
00141     {
00142         perf_.addErrors(receiver.yieldErrorCount());
00143         break;
00144     }
00145     case TransferReceiver::ResultSingleFrame:
00146     {
00147         perf_.addRxTransfer();
00148         SingleFrameIncomingTransfer it(frame);
00149         handleIncomingTransfer(it);
00150         break;
00151     }
00152     case TransferReceiver::ResultComplete:
00153     {
00154         perf_.addRxTransfer();
00155         const ITransferBuffer* tbb = tba.access();
00156         if (tbb == UAVCAN_NULLPTR)
00157         {
00158             UAVCAN_TRACE("TransferListener", "Buffer access failure, last frame: %s", frame.toString().c_str());
00159             break;
00160         }
00161         if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb))
00162         {
00163             UAVCAN_TRACE("TransferListener", "CRC error, last frame: %s", frame.toString().c_str());
00164             break;
00165         }
00166         MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(),
00167                                       receiver.getLastTransferTimestampUtc(), frame, tba);
00168         handleIncomingTransfer(it);
00169         it.release();
00170         break;
00171     }
00172     default:
00173     {
00174         UAVCAN_ASSERT(0);
00175         break;
00176     }
00177     }
00178 }
00179 
00180 void TransferListener::handleAnonymousTransferReception(const RxFrame& frame)
00181 {
00182     if (allow_anonymous_transfers_)
00183     {
00184         perf_.addRxTransfer();
00185         SingleFrameIncomingTransfer it(frame);
00186         handleIncomingTransfer(it);
00187     }
00188 }
00189 
00190 TransferListener::~TransferListener()
00191 {
00192     // Map must be cleared before bufmgr is destroyed
00193     receivers_.clear();
00194 }
00195 
00196 void TransferListener::cleanup(MonotonicTime ts)
00197 {
00198     receivers_.removeAllWhere(TimedOutReceiverPredicate(ts, bufmgr_));
00199     UAVCAN_ASSERT(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
00200 }
00201 
00202 void TransferListener::handleFrame(const RxFrame& frame)
00203 {
00204     if (frame.getSrcNodeID().isUnicast())       // Normal transfer
00205     {
00206         const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
00207 
00208         TransferReceiver* recv = receivers_.access(key);
00209         if (recv == UAVCAN_NULLPTR)
00210         {
00211             if (!frame.isStartOfTransfer())
00212             {
00213                 return;
00214             }
00215 
00216             TransferReceiver new_recv;
00217             recv = receivers_.insert(key, new_recv);
00218             if (recv == UAVCAN_NULLPTR)
00219             {
00220                 UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
00221                 return;
00222             }
00223         }
00224         TransferBufferAccessor tba(bufmgr_, key);
00225         handleReception(*recv, frame, tba);
00226     }
00227     else if (frame.getSrcNodeID().isBroadcast() &&
00228              frame.isStartOfTransfer() &&
00229              frame.isEndOfTransfer() &&
00230              frame.getDstNodeID().isBroadcast())        // Anonymous transfer
00231     {
00232         handleAnonymousTransferReception(frame);
00233     }
00234     else
00235     {
00236         UAVCAN_TRACE("TransferListener", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame
00237     }
00238 }
00239 
00240 /*
00241  * TransferListenerWithFilter
00242  */
00243 void TransferListenerWithFilter::handleFrame(const RxFrame& frame)
00244 {
00245     if (filter_ != UAVCAN_NULLPTR)
00246     {
00247         if (filter_->shouldAcceptFrame(frame))
00248         {
00249             TransferListener::handleFrame(frame);
00250         }
00251     }
00252     else
00253     {
00254         UAVCAN_ASSERT(0);
00255     }
00256 }
00257 
00258 }