uc_transfer_sender.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/transport/transfer_sender.hpp>
00006 #include <uavcan/debug.hpp>
00007 #include <cassert>
00008 
00009 
00010 namespace uavcan
00011 {
00012 
00013 void TransferSender::registerError() const
00014 {
00015     dispatcher_.getTransferPerfCounter().addError();
00016 }
00017 
00018 void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos)
00019 {
00020     UAVCAN_ASSERT(!isInitialized());
00021 
00022     qos_          = qos;
00023     data_type_id_ = dtid.getID();
00024     crc_base_     = dtid.getSignature().toTransferCRC();
00025 }
00026 
00027 int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
00028                          MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
00029                          TransferID tid) const
00030 {
00031     Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, tid);
00032 
00033     frame.setPriority(priority_);
00034     frame.setStartOfTransfer(true);
00035 
00036     UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str());
00037 
00038     /*
00039      * Checking if we're allowed to send.
00040      * In passive mode we can send only anonymous transfers, if they are enabled.
00041      */
00042     if (dispatcher_.isPassiveMode())
00043     {
00044         const bool allow = allow_anonymous_transfers_ &&
00045                            (transfer_type == TransferTypeMessageBroadcast) &&
00046                            (int(payload_len) <= frame.getPayloadCapacity());
00047         if (!allow)
00048         {
00049             return -ErrPassiveMode;
00050         }
00051     }
00052 
00053     dispatcher_.getTransferPerfCounter().addTxTransfer();
00054 
00055     /*
00056      * Sending frames
00057      */
00058     if (frame.getPayloadCapacity() >= payload_len)           // Single Frame Transfer
00059     {
00060         const int res = frame.setPayload(payload, payload_len);
00061         if (res != int(payload_len))
00062         {
00063             UAVCAN_ASSERT(0);
00064             UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res);
00065             registerError();
00066             return (res < 0) ? res : -ErrLogic;
00067         }
00068 
00069         frame.setEndOfTransfer(true);
00070         UAVCAN_ASSERT(frame.isStartOfTransfer() && frame.isEndOfTransfer() && !frame.getToggle());
00071 
00072         const CanIOFlags flags = frame.getSrcNodeID().isUnicast() ? flags_ : (flags_ | CanIOFlagAbortOnError);
00073 
00074         return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags, iface_mask_);
00075     }
00076     else                                                   // Multi Frame Transfer
00077     {
00078         UAVCAN_ASSERT(!dispatcher_.isPassiveMode());
00079         UAVCAN_ASSERT(frame.getSrcNodeID().isUnicast());
00080 
00081         int offset = 0;
00082         {
00083             TransferCRC crc = crc_base_;
00084             crc.add(payload, payload_len);
00085 
00086             static const int BUFLEN = sizeof(static_cast<CanFrame*>(0)->data);
00087             uint8_t buf[BUFLEN];
00088 
00089             buf[0] = uint8_t(crc.get() & 0xFFU);       // Transfer CRC, little endian
00090             buf[1] = uint8_t((crc.get() >> 8) & 0xFF);
00091             (void)copy(payload, payload + BUFLEN - 2, buf + 2);
00092 
00093             const int write_res = frame.setPayload(buf, BUFLEN);
00094             if (write_res < 2)
00095             {
00096                 UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
00097                 registerError();
00098                 return write_res;
00099             }
00100             offset = write_res - 2;
00101             UAVCAN_ASSERT(int(payload_len) > offset);
00102         }
00103 
00104         int num_sent = 0;
00105 
00106         while (true)
00107         {
00108             const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_);
00109             if (send_res < 0)
00110             {
00111                 registerError();
00112                 return send_res;
00113             }
00114 
00115             num_sent++;
00116             if (frame.isEndOfTransfer())
00117             {
00118                 return num_sent;  // Number of frames transmitted
00119             }
00120 
00121             frame.setStartOfTransfer(false);
00122             frame.flipToggle();
00123 
00124             UAVCAN_ASSERT(offset >= 0);
00125             const int write_res = frame.setPayload(payload + offset, payload_len - unsigned(offset));
00126             if (write_res < 0)
00127             {
00128                 UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
00129                 registerError();
00130                 return write_res;
00131             }
00132 
00133             offset += write_res;
00134             UAVCAN_ASSERT(offset <= int(payload_len));
00135             if (offset >= int(payload_len))
00136             {
00137                 frame.setEndOfTransfer(true);
00138             }
00139         }
00140     }
00141 
00142     UAVCAN_ASSERT(0);
00143     return -ErrLogic; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
00144 }
00145 
00146 int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
00147                          MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const
00148 {
00149     /*
00150      * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped?
00151      */
00152     const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id);
00153 
00154     UAVCAN_ASSERT(!tx_deadline.isZero());
00155     const MonotonicTime otr_deadline = tx_deadline + max(max_transfer_interval_ * 2,
00156                                                          OutgoingTransferRegistry::MinEntryLifetime);
00157 
00158     TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
00159     if (tid == UAVCAN_NULLPTR)
00160     {
00161         UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i",
00162                      int(data_type_id_.get()), int(transfer_type));
00163         return -ErrMemory;
00164     }
00165 
00166     const TransferID this_tid = tid->get();
00167     tid->increment();
00168 
00169     return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type,
00170                 dst_node_id, this_tid);
00171 }
00172 
00173 }