uc_can_io.cpp

00001 /*
00002  * CAN bus IO logic.
00003  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00004  */
00005 
00006 #include <uavcan/transport/can_io.hpp>
00007 #include <uavcan/debug.hpp>
00008 #include <cassert>
00009 
00010 namespace uavcan
00011 {
00012 /*
00013  * CanRxFrame
00014  */
00015 #if UAVCAN_TOSTRING
00016 std::string CanRxFrame::toString(StringRepresentation mode) const
00017 {
00018     std::string out = CanFrame::toString(mode);
00019     out.reserve(128);
00020     out += " ts_m="   + ts_mono.toString();
00021     out += " ts_utc=" + ts_utc.toString();
00022     out += " iface=";
00023     out += char('0' + iface_index);
00024     return out;
00025 }
00026 #endif
00027 
00028 /*
00029  * CanTxQueue::Entry
00030  */
00031 void CanTxQueue::Entry::destroy(Entry*& obj, IPoolAllocator& allocator)
00032 {
00033     if (obj != UAVCAN_NULLPTR)
00034     {
00035         obj->~Entry();
00036         allocator.deallocate(obj);
00037         obj = UAVCAN_NULLPTR;
00038     }
00039 }
00040 
00041 bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const
00042 {
00043     if (qos != rhs_qos)
00044     {
00045         return qos > rhs_qos;
00046     }
00047     return frame.priorityHigherThan(rhs_frame);
00048 }
00049 
00050 bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const
00051 {
00052     if (qos != rhs_qos)
00053     {
00054         return qos < rhs_qos;
00055     }
00056     return frame.priorityLowerThan(rhs_frame);
00057 }
00058 
00059 #if UAVCAN_TOSTRING
00060 std::string CanTxQueue::Entry::toString() const
00061 {
00062     std::string str_qos;
00063     switch (qos)
00064     {
00065     case Volatile:
00066     {
00067         str_qos = "<volat> ";
00068         break;
00069     }
00070     case Persistent:
00071     {
00072         str_qos = "<perst> ";
00073         break;
00074     }
00075     default:
00076     {
00077         UAVCAN_ASSERT(0);
00078         str_qos = "<?WTF?> ";
00079         break;
00080     }
00081     }
00082     return str_qos + frame.toString();
00083 }
00084 #endif
00085 
00086 /*
00087  * CanTxQueue
00088  */
00089 CanTxQueue::~CanTxQueue()
00090 {
00091     Entry* p = queue_.get();
00092     while (p)
00093     {
00094         Entry* const next = p->getNextListNode();
00095         remove(p);
00096         p = next;
00097     }
00098 }
00099 
00100 void CanTxQueue::registerRejectedFrame()
00101 {
00102     if (rejected_frames_cnt_ < NumericTraits<uint32_t>::max())
00103     {
00104         rejected_frames_cnt_++;
00105     }
00106 }
00107 
00108 void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
00109 {
00110     const MonotonicTime timestamp = sysclock_.getMonotonic();
00111 
00112     if (timestamp >= tx_deadline)
00113     {
00114         UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
00115         registerRejectedFrame();
00116         return;
00117     }
00118 
00119     void* praw = allocator_.allocate(sizeof(Entry));
00120     if (praw == UAVCAN_NULLPTR)
00121     {
00122         UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup");
00123         // No memory left in the pool, so we try to remove expired frames
00124         Entry* p = queue_.get();
00125         while (p)
00126         {
00127             Entry* const next = p->getNextListNode();
00128             if (p->isExpired(timestamp))
00129             {
00130                 UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str());
00131                 registerRejectedFrame();
00132                 remove(p);
00133             }
00134             p = next;
00135         }
00136         praw = allocator_.allocate(sizeof(Entry));         // Try again
00137     }
00138 
00139     if (praw == UAVCAN_NULLPTR)
00140     {
00141         UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration");
00142         registerRejectedFrame();
00143 
00144         // Find a frame with lowest QoS
00145         Entry* p = queue_.get();
00146         if (p == UAVCAN_NULLPTR)
00147         {
00148             UAVCAN_TRACE("CanTxQueue", "Push rejected: Nothing to replace");
00149             return;
00150         }
00151         Entry* lowestqos = p;
00152         while (p)
00153         {
00154             if (lowestqos->qosHigherThan(*p))
00155             {
00156                 lowestqos = p;
00157             }
00158             p = p->getNextListNode();
00159         }
00160         // Note that frame with *equal* QoS will be replaced too.
00161         if (lowestqos->qosHigherThan(frame, qos))           // Frame that we want to transmit has lowest QoS
00162         {
00163             UAVCAN_TRACE("CanTxQueue", "Push rejected: low QoS");
00164             return;                                         // What a loser.
00165         }
00166         UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str());
00167         remove(lowestqos);
00168         praw = allocator_.allocate(sizeof(Entry));        // Try again
00169     }
00170 
00171     if (praw == UAVCAN_NULLPTR)
00172     {
00173         return;                                            // Seems that there is no memory at all.
00174     }
00175     Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags);
00176     UAVCAN_ASSERT(entry);
00177     queue_.insertBefore(entry, PriorityInsertionComparator(frame));
00178 }
00179 
00180 CanTxQueue::Entry* CanTxQueue::peek()
00181 {
00182     const MonotonicTime timestamp = sysclock_.getMonotonic();
00183     Entry* p = queue_.get();
00184     while (p)
00185     {
00186         if (p->isExpired(timestamp))
00187         {
00188             UAVCAN_TRACE("CanTxQueue", "Peek: Expired %s", p->toString().c_str());
00189             Entry* const next = p->getNextListNode();
00190             registerRejectedFrame();
00191             remove(p);
00192             p = next;
00193         }
00194         else
00195         {
00196             return p;
00197         }
00198     }
00199     return UAVCAN_NULLPTR;
00200 }
00201 
00202 void CanTxQueue::remove(Entry*& entry)
00203 {
00204     if (entry == UAVCAN_NULLPTR)
00205     {
00206         UAVCAN_ASSERT(0);
00207         return;
00208     }
00209     queue_.remove(entry);
00210     Entry::destroy(entry, allocator_);
00211 }
00212 
00213 const CanFrame* CanTxQueue::getTopPriorityPendingFrame() const
00214 {
00215     return (queue_.get() == UAVCAN_NULLPTR) ? UAVCAN_NULLPTR : &queue_.get()->frame;
00216 }
00217 
00218 bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
00219 {
00220     const Entry* entry = queue_.get();
00221     if (entry == UAVCAN_NULLPTR)
00222     {
00223         return false;
00224     }
00225     return !rhs_frame.priorityHigherThan(entry->frame);
00226 }
00227 
00228 /*
00229  * CanIOManager
00230  */
00231 int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags)
00232 {
00233     UAVCAN_ASSERT(iface_index < MaxCanIfaces);
00234     ICanIface* const iface = driver_.getIface(iface_index);
00235     if (iface == UAVCAN_NULLPTR)
00236     {
00237         UAVCAN_ASSERT(0);   // Nonexistent interface
00238         return -ErrLogic;
00239     }
00240     const int res = iface->send(frame, tx_deadline, flags);
00241     if (res != 1)
00242     {
00243         UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
00244                      res, iface_index, frame.toString().c_str());
00245     }
00246     if (res > 0)
00247     {
00248         counters_[iface_index].frames_tx += unsigned(res);
00249     }
00250     return res;
00251 }
00252 
00253 int CanIOManager::sendFromTxQueue(uint8_t iface_index)
00254 {
00255     UAVCAN_ASSERT(iface_index < MaxCanIfaces);
00256     CanTxQueue::Entry* entry = tx_queues_[iface_index]->peek();
00257     if (entry == UAVCAN_NULLPTR)
00258     {
00259         return 0;
00260     }
00261     const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags);
00262     if (res > 0)
00263     {
00264         tx_queues_[iface_index]->remove(entry);
00265     }
00266     return res;
00267 }
00268 
00269 int CanIOManager::callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces],
00270                              MonotonicTime blocking_deadline)
00271 {
00272     const CanSelectMasks in_masks = inout_masks;
00273 
00274     const int res = driver_.select(inout_masks, pending_tx, blocking_deadline);
00275     if (res < 0)
00276     {
00277         return -ErrDriver;
00278     }
00279 
00280     inout_masks.read  &= in_masks.read;  // Driver is not required to clean the masks
00281     inout_masks.write &= in_masks.write;
00282     return res;
00283 }
00284 
00285 CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
00286                            std::size_t mem_blocks_per_iface)
00287     : driver_(driver)
00288     , sysclock_(sysclock)
00289     , num_ifaces_(driver.getNumIfaces())
00290 {
00291     if (num_ifaces_ < 1 || num_ifaces_ > MaxCanIfaces)
00292     {
00293         handleFatalError("Num ifaces");
00294     }
00295 
00296     if (mem_blocks_per_iface == 0)
00297     {
00298         mem_blocks_per_iface = allocator.getBlockCapacity() / (num_ifaces_ + 1U) + 1U;
00299     }
00300     UAVCAN_TRACE("CanIOManager", "Memory blocks per iface: %u, total: %u",
00301                  unsigned(mem_blocks_per_iface), unsigned(allocator.getBlockCapacity()));
00302 
00303     for (int i = 0; i < num_ifaces_; i++)
00304     {
00305         tx_queues_[i].construct<IPoolAllocator&, ISystemClock&, std::size_t>
00306         (allocator, sysclock, mem_blocks_per_iface);
00307     }
00308 }
00309 
00310 uint8_t CanIOManager::makePendingTxMask() const
00311 {
00312     uint8_t write_mask = 0;
00313     for (uint8_t i = 0; i < getNumIfaces(); i++)
00314     {
00315         if (!tx_queues_[i]->isEmpty())
00316         {
00317             write_mask = uint8_t(write_mask | (1 << i));
00318         }
00319     }
00320     return write_mask;
00321 }
00322 
00323 CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const
00324 {
00325     ICanIface* const iface = driver_.getIface(iface_index);
00326     if (iface == UAVCAN_NULLPTR || iface_index >= MaxCanIfaces)
00327     {
00328         UAVCAN_ASSERT(0);
00329         return CanIfacePerfCounters();
00330     }
00331     CanIfacePerfCounters cnt;
00332     cnt.errors = iface->getErrorCount() + tx_queues_[iface_index]->getRejectedFrameCount();
00333     cnt.frames_rx = counters_[iface_index].frames_rx;
00334     cnt.frames_tx = counters_[iface_index].frames_tx;
00335     return cnt;
00336 }
00337 
00338 int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
00339                        uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
00340 {
00341     const uint8_t num_ifaces = getNumIfaces();
00342     const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1);
00343     iface_mask &= all_ifaces_mask;
00344 
00345     if (blocking_deadline > tx_deadline)
00346     {
00347         blocking_deadline = tx_deadline;
00348     }
00349 
00350     int retval = 0;
00351 
00352     while (true)        // Somebody please refactor this.
00353     {
00354         if (iface_mask == 0)
00355         {
00356             break;
00357         }
00358 
00359         CanSelectMasks masks;
00360         masks.write = iface_mask | makePendingTxMask();
00361         {
00362             // Building the list of next pending frames per iface.
00363             // The driver will give them a scrutinizing look before deciding whether he wants to accept them.
00364             const CanFrame* pending_tx[MaxCanIfaces] = {};
00365             for (int i = 0; i < num_ifaces; i++)
00366             {
00367                 CanTxQueue& q = *tx_queues_[i];
00368                 if (iface_mask & (1 << i))      // I hate myself so much right now.
00369                 {
00370                     pending_tx[i] = q.topPriorityHigherOrEqual(frame) ? q.getTopPriorityPendingFrame() : &frame;
00371                 }
00372                 else
00373                 {
00374                     pending_tx[i] = q.getTopPriorityPendingFrame();
00375                 }
00376             }
00377 
00378             const int select_res = callSelect(masks, pending_tx, blocking_deadline);
00379             if (select_res < 0)
00380             {
00381                 return -ErrDriver;
00382             }
00383             UAVCAN_ASSERT(masks.read == 0);
00384         }
00385 
00386         // Transmission
00387         for (uint8_t i = 0; i < num_ifaces; i++)
00388         {
00389             if (masks.write & (1 << i))
00390             {
00391                 int res = 0;
00392                 if (iface_mask & (1 << i))
00393                 {
00394                     if (tx_queues_[i]->topPriorityHigherOrEqual(frame))
00395                     {
00396                         res = sendFromTxQueue(i);                 // May return 0 if nothing to transmit (e.g. expired)
00397                     }
00398                     if (res <= 0)
00399                     {
00400                         res = sendToIface(i, frame, tx_deadline, flags);
00401                         if (res > 0)
00402                         {
00403                             iface_mask &= uint8_t(~(1 << i));     // Mark transmitted
00404                         }
00405                     }
00406                 }
00407                 else
00408                 {
00409                     res = sendFromTxQueue(i);
00410                 }
00411                 if (res > 0)
00412                 {
00413                     retval++;
00414                 }
00415             }
00416         }
00417 
00418         // Timeout. Enqueue the frame if wasn't transmitted and leave.
00419         const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline;
00420         if (masks.write == 0 || timed_out)
00421         {
00422             if (!timed_out)
00423             {
00424                 UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
00425                 continue;
00426             }
00427             for (uint8_t i = 0; i < num_ifaces; i++)
00428             {
00429                 if (iface_mask & (1 << i))
00430                 {
00431                     tx_queues_[i]->push(frame, tx_deadline, qos, flags);
00432                 }
00433             }
00434             break;
00435         }
00436     }
00437     return retval;
00438 }
00439 
00440 int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags)
00441 {
00442     const uint8_t num_ifaces = getNumIfaces();
00443 
00444     while (true)
00445     {
00446         CanSelectMasks masks;
00447         masks.write = makePendingTxMask();
00448         masks.read = uint8_t((1 << num_ifaces) - 1);
00449         {
00450             const CanFrame* pending_tx[MaxCanIfaces] = {};
00451             for (int i = 0; i < num_ifaces; i++)      // Dear compiler, kindly unroll this. Thanks.
00452             {
00453                 pending_tx[i] = tx_queues_[i]->getTopPriorityPendingFrame();
00454             }
00455 
00456             const int select_res = callSelect(masks, pending_tx, blocking_deadline);
00457             if (select_res < 0)
00458             {
00459                 return -ErrDriver;
00460             }
00461         }
00462 
00463         // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
00464         for (uint8_t i = 0; i < num_ifaces; i++)
00465         {
00466             if (masks.write & (1 << i))
00467             {
00468                 (void)sendFromTxQueue(i);  // It may fail, we don't care. Requested operation was receive, not send.
00469             }
00470         }
00471 
00472         // Read
00473         for (uint8_t i = 0; i < num_ifaces; i++)
00474         {
00475             if (masks.read & (1 << i))
00476             {
00477                 ICanIface* const iface = driver_.getIface(i);
00478                 if (iface == UAVCAN_NULLPTR)
00479                 {
00480                     UAVCAN_ASSERT(0);   // Nonexistent interface
00481                     continue;
00482                 }
00483 
00484                 const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags);
00485                 if (res == 0)
00486                 {
00487                     UAVCAN_ASSERT(0);   // select() reported that iface has pending RX frames, but receive() returned none
00488                     continue;
00489                 }
00490                 out_frame.iface_index = i;
00491 
00492                 if ((res > 0) && !(out_flags & CanIOFlagLoopback))
00493                 {
00494                     counters_[i].frames_rx += 1;
00495                 }
00496                 return (res < 0) ? -ErrDriver : res;
00497             }
00498         }
00499 
00500         // Timeout checked in the last order - this way we can operate with expired deadline:
00501         if (sysclock_.getMonotonic() >= blocking_deadline)
00502         {
00503             break;
00504         }
00505     }
00506     return 0;
00507 }
00508 
00509 }