socketcan.hpp

00001 /*
00002  * Copyright (C) 2014-2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  *                         Ilia Sheremet <illia.sheremet@gmail.com>
00004  */
00005 
00006 #pragma once
00007 
00008 #include <cassert>
00009 #include <cstdint>
00010 #include <queue>
00011 #include <vector>
00012 #include <map>
00013 #include <unordered_set>
00014 #include <memory>
00015 #include <algorithm>
00016 
00017 #include <fcntl.h>
00018 #include <sys/socket.h>
00019 #include <sys/ioctl.h>
00020 #include <net/if.h>
00021 #include <linux/can.h>
00022 #include <linux/can/raw.h>
00023 #include <poll.h>
00024 
00025 #include <uavcan/uavcan.hpp>
00026 #include <uavcan_linux/clock.hpp>
00027 #include <uavcan_linux/exception.hpp>
00028 
00029 
00030 namespace uavcan_linux
00031 {
00032 /**
00033  * SocketCan driver class keeps number of each kind of errors occurred since the object was created.
00034  */
00035 enum class SocketCanError
00036 {
00037     SocketReadFailure,
00038     SocketWriteFailure,
00039     TxTimeout
00040 };
00041 
00042 /**
00043  * Single SocketCAN socket interface.
00044  *
00045  * SocketCAN socket adapter maintains TX and RX queues in user space. At any moment socket's buffer contains
00046  * no more than 'max_frames_in_socket_tx_queue_' TX frames, rest is waiting in the user space TX queue; when the
00047  * socket produces loopback for the previously sent TX frame the next frame from the user space TX queue will
00048  * be sent into the socket.
00049  *
00050  * This approach allows to properly maintain TX timeouts (http://stackoverflow.com/questions/19633015/).
00051  * TX timestamping is implemented by means of reading RX timestamps of loopback frames (see "TX timestamping" on
00052  * linux-can mailing list, http://permalink.gmane.org/gmane.linux.can/5322).
00053  *
00054  * Note that if max_frames_in_socket_tx_queue_ is greater than one, frame reordering may occur (depending on the
00055  * unrderlying logic).
00056  *
00057  * This class is too complex and needs to be refactored later. At least, basic socket IO and configuration
00058  * should be extracted into a different class.
00059  */
00060 class SocketCanIface : public uavcan::ICanIface
00061 {
00062     static inline ::can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame)
00063     {
00064         ::can_frame sockcan_frame = ::can_frame();
00065         sockcan_frame.can_id = uavcan_frame.id & uavcan::CanFrame::MaskExtID;
00066         sockcan_frame.can_dlc = uavcan_frame.dlc;
00067         (void)std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
00068         if (uavcan_frame.isExtended())
00069         {
00070             sockcan_frame.can_id |= CAN_EFF_FLAG;
00071         }
00072         if (uavcan_frame.isErrorFrame())
00073         {
00074             sockcan_frame.can_id |= CAN_ERR_FLAG;
00075         }
00076         if (uavcan_frame.isRemoteTransmissionRequest())
00077         {
00078             sockcan_frame.can_id |= CAN_RTR_FLAG;
00079         }
00080         return sockcan_frame;
00081     }
00082 
00083     static inline uavcan::CanFrame makeUavcanFrame(const ::can_frame& sockcan_frame)
00084     {
00085         uavcan::CanFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc);
00086         if (sockcan_frame.can_id & CAN_EFF_FLAG)
00087         {
00088             uavcan_frame.id |= uavcan::CanFrame::FlagEFF;
00089         }
00090         if (sockcan_frame.can_id & CAN_ERR_FLAG)
00091         {
00092             uavcan_frame.id |= uavcan::CanFrame::FlagERR;
00093         }
00094         if (sockcan_frame.can_id & CAN_RTR_FLAG)
00095         {
00096             uavcan_frame.id |= uavcan::CanFrame::FlagRTR;
00097         }
00098         return uavcan_frame;
00099     }
00100 
00101     struct TxItem
00102     {
00103         uavcan::CanFrame frame;
00104         uavcan::MonotonicTime deadline;
00105         uavcan::CanIOFlags flags = 0;
00106         std::uint64_t order = 0;
00107 
00108         TxItem(const uavcan::CanFrame& arg_frame, uavcan::MonotonicTime arg_deadline,
00109                uavcan::CanIOFlags arg_flags, std::uint64_t arg_order)
00110             : frame(arg_frame)
00111             , deadline(arg_deadline)
00112             , flags(arg_flags)
00113             , order(arg_order)
00114         { }
00115 
00116         bool operator<(const TxItem& rhs) const
00117         {
00118             if (frame.priorityLowerThan(rhs.frame))
00119             {
00120                 return true;
00121             }
00122             if (frame.priorityHigherThan(rhs.frame))
00123             {
00124                 return false;
00125             }
00126             return order > rhs.order;
00127         }
00128     };
00129 
00130     struct RxItem
00131     {
00132         uavcan::CanFrame frame;
00133         uavcan::MonotonicTime ts_mono;
00134         uavcan::UtcTime ts_utc;
00135         uavcan::CanIOFlags flags;
00136 
00137         RxItem()
00138             : flags(0)
00139         { }
00140     };
00141 
00142     const SystemClock& clock_;
00143     const int fd_;
00144 
00145     const unsigned max_frames_in_socket_tx_queue_;
00146     unsigned frames_in_socket_tx_queue_ = 0;
00147 
00148     std::uint64_t tx_frame_counter_ = 0;        ///< Increments with every frame pushed into the TX queue
00149 
00150     std::map<SocketCanError, std::uint64_t> errors_;
00151 
00152     std::priority_queue<TxItem> tx_queue_;                          // TODO: Use pool allocator
00153     std::queue<RxItem> rx_queue_;                                   // TODO: Use pool allocator
00154     std::unordered_multiset<std::uint32_t> pending_loopback_ids_;   // TODO: Use pool allocator
00155 
00156     std::vector<::can_filter> hw_filters_container_;
00157 
00158     void registerError(SocketCanError e) { errors_[e]++; }
00159 
00160     void incrementNumFramesInSocketTxQueue()
00161     {
00162         assert(frames_in_socket_tx_queue_ < max_frames_in_socket_tx_queue_);
00163         frames_in_socket_tx_queue_++;
00164     }
00165 
00166     void confirmSentFrame()
00167     {
00168         if (frames_in_socket_tx_queue_ > 0)
00169         {
00170             frames_in_socket_tx_queue_--;
00171         }
00172         else
00173         {
00174             assert(0); // Loopback for a frame that we didn't send.
00175         }
00176     }
00177 
00178     bool wasInPendingLoopbackSet(const uavcan::CanFrame& frame)
00179     {
00180         if (pending_loopback_ids_.count(frame.id) > 0)
00181         {
00182             (void)pending_loopback_ids_.erase(frame.id);
00183             return true;
00184         }
00185         return false;
00186     }
00187 
00188     int write(const uavcan::CanFrame& frame) const
00189     {
00190         errno = 0;
00191 
00192         const ::can_frame sockcan_frame = makeSocketCanFrame(frame);
00193 
00194         const int res = ::write(fd_, &sockcan_frame, sizeof(sockcan_frame));
00195         if (res <= 0)
00196         {
00197             if (errno == ENOBUFS || errno == EAGAIN)    // Writing is not possible atm, not an error
00198             {
00199                 return 0;
00200             }
00201             return res;
00202         }
00203         if (res != sizeof(sockcan_frame))
00204         {
00205             return -1;
00206         }
00207         return 1;
00208     }
00209 
00210     /**
00211      * SocketCAN git show 1e55659ce6ddb5247cee0b1f720d77a799902b85
00212      *    MSG_DONTROUTE is set for any packet from localhost,
00213      *    MSG_CONFIRM is set for any pakcet of your socket.
00214      * Diff: https://git.ucsd.edu/abuss/linux/commit/1e55659ce6ddb5247cee0b1f720d77a799902b85
00215      * Man: https://www.kernel.org/doc/Documentation/networking/can.txt (chapter 4.1.6).
00216      */
00217     int read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const
00218     {
00219         auto iov = ::iovec();
00220         auto sockcan_frame = ::can_frame();
00221         iov.iov_base = &sockcan_frame;
00222         iov.iov_len  = sizeof(sockcan_frame);
00223 
00224         static constexpr size_t ControlSize = sizeof(cmsghdr) + sizeof(::timeval);
00225         using ControlStorage = typename std::aligned_storage<ControlSize>::type;
00226         ControlStorage control_storage;
00227         auto control = reinterpret_cast<std::uint8_t *>(&control_storage);
00228         std::fill(control, control + ControlSize, 0x00);
00229 
00230         auto msg = ::msghdr();
00231         msg.msg_iov    = &iov;
00232         msg.msg_iovlen = 1;
00233         msg.msg_control = control;
00234         msg.msg_controllen = ControlSize;
00235 
00236         const int res = ::recvmsg(fd_, &msg, MSG_DONTWAIT);
00237         if (res <= 0)
00238         {
00239             return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
00240         }
00241         /*
00242          * Flags
00243          */
00244         loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
00245 
00246         if (!loopback && !checkHWFilters(sockcan_frame))
00247         {
00248             return 0;
00249         }
00250 
00251         frame = makeUavcanFrame(sockcan_frame);
00252         /*
00253          * Timestamp
00254          */
00255         const ::cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
00256         assert(cmsg != nullptr);
00257         if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP)
00258         {
00259             auto tv = ::timeval();
00260             (void)std::memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv));  // Copy to avoid alignment problems
00261             assert(tv.tv_sec >= 0 && tv.tv_usec >= 0);
00262             ts_utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * 1000000ULL + tv.tv_usec);
00263         }
00264         else
00265         {
00266             assert(0);
00267             return -1;
00268         }
00269         return 1;
00270     }
00271 
00272     void pollWrite()
00273     {
00274         while (hasReadyTx())
00275         {
00276             const TxItem tx = tx_queue_.top();
00277 
00278             if (tx.deadline >= clock_.getMonotonic())
00279             {
00280                 const int res = write(tx.frame);
00281                 if (res == 1)                   // Transmitted successfully
00282                 {
00283                     incrementNumFramesInSocketTxQueue();
00284                     if (tx.flags & uavcan::CanIOFlagLoopback)
00285                     {
00286                         (void)pending_loopback_ids_.insert(tx.frame.id);
00287                     }
00288                 }
00289                 else if (res == 0)              // Not transmitted, nor is it an error
00290                 {
00291                     break;                      // Leaving the loop, the frame remains enqueued for the next retry
00292                 }
00293                 else                            // Transmission error
00294                 {
00295                     registerError(SocketCanError::SocketWriteFailure);
00296                 }
00297             }
00298             else
00299             {
00300                 registerError(SocketCanError::TxTimeout);
00301             }
00302 
00303             // Removing the frame from the queue even if transmission failed
00304             tx_queue_.pop();
00305         }
00306     }
00307 
00308     void pollRead()
00309     {
00310         while (true)
00311         {
00312             RxItem rx;
00313             rx.ts_mono = clock_.getMonotonic();  // Monotonic timestamp is not required to be precise (unlike UTC)
00314             bool loopback = false;
00315             const int res = read(rx.frame, rx.ts_utc, loopback);
00316             if (res == 1)
00317             {
00318                 assert(!rx.ts_utc.isZero());
00319                 bool accept = true;
00320                 if (loopback)                   // We receive loopback for all CAN frames
00321                 {
00322                     confirmSentFrame();
00323                     rx.flags |= uavcan::CanIOFlagLoopback;
00324                     accept = wasInPendingLoopbackSet(rx.frame); // Do we need to send this loopback into the lib?
00325                 }
00326                 if (accept)
00327                 {
00328                     rx.ts_utc += clock_.getPrivateAdjustment();
00329                     rx_queue_.push(rx);
00330                 }
00331             }
00332             else if (res == 0)
00333             {
00334                 break;
00335             }
00336             else
00337             {
00338                 registerError(SocketCanError::SocketReadFailure);
00339                 break;
00340             }
00341         }
00342     }
00343 
00344     /**
00345      * Returns true if a frame accepted by HW filters
00346      */
00347     bool checkHWFilters(const ::can_frame& frame) const
00348     {
00349         if (!hw_filters_container_.empty())
00350         {
00351             for (auto& f : hw_filters_container_)
00352             {
00353                 if (((frame.can_id & f.can_mask) ^ f.can_id) == 0)
00354                 {
00355                     return true;
00356                 }
00357             }
00358             return false;
00359         }
00360         else
00361         {
00362             return true;
00363         }
00364     }
00365 
00366 public:
00367     /**
00368      * Takes ownership of socket's file descriptor.
00369      *
00370      * @ref max_frames_in_socket_tx_queue       See a note in the class comment.
00371      */
00372     SocketCanIface(const SystemClock& clock, int socket_fd, int max_frames_in_socket_tx_queue = 2)
00373         : clock_(clock)
00374         , fd_(socket_fd)
00375         , max_frames_in_socket_tx_queue_(max_frames_in_socket_tx_queue)
00376     {
00377         assert(fd_ >= 0);
00378     }
00379 
00380     /**
00381      * Socket file descriptor will be closed.
00382      */
00383     virtual ~SocketCanIface()
00384     {
00385         UAVCAN_TRACE("SocketCAN", "SocketCanIface: Closing fd %d", fd_);
00386         (void)::close(fd_);
00387     }
00388 
00389     /**
00390      * Assumes that the socket is writeable
00391      */
00392     std::int16_t send(const uavcan::CanFrame& frame, const uavcan::MonotonicTime tx_deadline,
00393                       const uavcan::CanIOFlags flags) override
00394     {
00395         tx_queue_.emplace(frame, tx_deadline, flags, tx_frame_counter_);
00396         tx_frame_counter_++;
00397         pollRead();     // Read poll is necessary because it can release the pending TX flag
00398         pollWrite();
00399         return 1;
00400     }
00401 
00402     /**
00403      * Will read the socket only if RX queue is empty.
00404      * Normally, poll() needs to be executed first.
00405      */
00406     std::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
00407                          uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override
00408     {
00409         if (rx_queue_.empty())
00410         {
00411             pollRead();            // This allows to use the socket not calling poll() explicitly.
00412             if (rx_queue_.empty())
00413             {
00414                 return 0;
00415             }
00416         }
00417         {
00418             const RxItem& rx = rx_queue_.front();
00419             out_frame        = rx.frame;
00420             out_ts_monotonic = rx.ts_mono;
00421             out_ts_utc       = rx.ts_utc;
00422             out_flags        = rx.flags;
00423         }
00424         rx_queue_.pop();
00425         return 1;
00426     }
00427 
00428     /**
00429      * Performs socket read/write.
00430      * @param read  Socket is readable
00431      * @param write Socket is writeable
00432      */
00433     void poll(bool read, bool write)
00434     {
00435         if (read)
00436         {
00437             pollRead();  // Read poll must be executed first because it may decrement frames_in_socket_tx_queue_
00438         }
00439         if (write)
00440         {
00441             pollWrite();
00442         }
00443     }
00444 
00445     bool hasReadyRx() const { return !rx_queue_.empty(); }
00446     bool hasReadyTx() const
00447     {
00448         return !tx_queue_.empty() && (frames_in_socket_tx_queue_ < max_frames_in_socket_tx_queue_);
00449     }
00450 
00451     std::int16_t configureFilters(const uavcan::CanFilterConfig* const filter_configs,
00452                                   const std::uint16_t num_configs) override
00453     {
00454         if (filter_configs == nullptr)
00455         {
00456             assert(0);
00457             return -1;
00458         }
00459         hw_filters_container_.clear();
00460         hw_filters_container_.resize(num_configs);
00461 
00462         for (unsigned i = 0; i < num_configs; i++)
00463         {
00464             const uavcan::CanFilterConfig& fc = filter_configs[i];
00465             hw_filters_container_[i].can_id   = fc.id   & uavcan::CanFrame::MaskExtID;
00466             hw_filters_container_[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
00467             if (fc.id & uavcan::CanFrame::FlagEFF)
00468             {
00469                 hw_filters_container_[i].can_id |= CAN_EFF_FLAG;
00470             }
00471             if (fc.id & uavcan::CanFrame::FlagRTR)
00472             {
00473                 hw_filters_container_[i].can_id |= CAN_RTR_FLAG;
00474             }
00475             if (fc.mask & uavcan::CanFrame::FlagEFF)
00476             {
00477                 hw_filters_container_[i].can_mask |= CAN_EFF_FLAG;
00478             }
00479             if (fc.mask & uavcan::CanFrame::FlagRTR)
00480             {
00481                 hw_filters_container_[i].can_mask |= CAN_RTR_FLAG;
00482             }
00483         }
00484 
00485         return 0;
00486     }
00487 
00488     /**
00489      * SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited.
00490      * This method returns a constant value.
00491      */
00492     static constexpr unsigned NumFilters = 8;
00493     std::uint16_t getNumFilters() const override { return NumFilters; }
00494 
00495 
00496     /**
00497      * Returns total number of errors of each kind detected since the object was created.
00498      */
00499     std::uint64_t getErrorCount() const override
00500     {
00501         std::uint64_t ec = 0;
00502         for (auto& kv : errors_) { ec += kv.second; }
00503         return ec;
00504     }
00505 
00506     /**
00507      * Returns number of errors of each kind in a map.
00508      */
00509     const decltype(errors_) & getErrors() const { return errors_; }
00510 
00511     int getFileDescriptor() const { return fd_; }
00512 
00513     /**
00514      * Open and configure a CAN socket on iface specified by name.
00515      * @param iface_name String containing iface name, e.g. "can0", "vcan1", "slcan0"
00516      * @return Socket descriptor or negative number on error.
00517      */
00518     static int openSocket(const std::string& iface_name)
00519     {
00520         errno = 0;
00521 
00522         const int s = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
00523         if (s < 0)
00524         {
00525             return s;
00526         }
00527 
00528         class RaiiCloser
00529         {
00530             int fd_;
00531         public:
00532             RaiiCloser(int filedesc) : fd_(filedesc)
00533             {
00534                 assert(fd_ >= 0);
00535             }
00536             ~RaiiCloser()
00537             {
00538                 if (fd_ >= 0)
00539                 {
00540                     UAVCAN_TRACE("SocketCAN", "RaiiCloser: Closing fd %d", fd_);
00541                     (void)::close(fd_);
00542                 }
00543             }
00544             void disarm() { fd_ = -1; }
00545         } raii_closer(s);
00546 
00547         // Detect the iface index
00548         auto ifr = ::ifreq();
00549         if (iface_name.length() >= IFNAMSIZ)
00550         {
00551             errno = ENAMETOOLONG;
00552             return -1;
00553         }
00554         (void)std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length());
00555         if (::ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0)
00556         {
00557             return -1;
00558         }
00559 
00560         // Bind to the specified CAN iface
00561         {
00562             auto addr = ::sockaddr_can();
00563             addr.can_family = AF_CAN;
00564             addr.can_ifindex = ifr.ifr_ifindex;
00565             if (::bind(s, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
00566             {
00567                 return -1;
00568             }
00569         }
00570 
00571         // Configure
00572         {
00573             const int on = 1;
00574             // Timestamping
00575             if (::setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0)
00576             {
00577                 return -1;
00578             }
00579             // Socket loopback
00580             if (::setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0)
00581             {
00582                 return -1;
00583             }
00584             // Non-blocking
00585             if (::fcntl(s, F_SETFL, O_NONBLOCK) < 0)
00586             {
00587                 return -1;
00588             }
00589         }
00590 
00591         // Validate the resulting socket
00592         {
00593             int socket_error = 0;
00594             ::socklen_t errlen = sizeof(socket_error);
00595             (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
00596             if (socket_error != 0)
00597             {
00598                 errno = socket_error;
00599                 return -1;
00600             }
00601         }
00602 
00603         raii_closer.disarm();
00604         return s;
00605     }
00606 };
00607 
00608 /**
00609  * Multiplexing container for multiple SocketCAN sockets.
00610  * Uses ppoll() for multiplexing.
00611  *
00612  * When an interface becomes down/disconnected while the node is running,
00613  * the driver will silently exclude it from the IO loop and continue to run on the remaining interfaces.
00614  * When all interfaces become down/disconnected, the driver will throw @ref AllIfacesDownException
00615  * from @ref SocketCanDriver::select().
00616  * Whether a certain interface is down can be checked with @ref SocketCanDriver::isIfaceDown().
00617  */
00618 class SocketCanDriver : public uavcan::ICanDriver
00619 {
00620     class IfaceWrapper : public SocketCanIface
00621     {
00622         bool down_ = false;
00623 
00624     public:
00625         IfaceWrapper(const SystemClock& clock, int fd) : SocketCanIface(clock, fd) { }
00626 
00627         void updateDownStatusFromPollResult(const ::pollfd& pfd)
00628         {
00629             assert(pfd.fd == this->getFileDescriptor());
00630             if (!down_ && (pfd.revents & POLLERR))
00631             {
00632                 int error = 0;
00633                 ::socklen_t errlen = sizeof(error);
00634                 (void)::getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&error), &errlen);
00635 
00636                 down_ = error == ENETDOWN || error == ENODEV;
00637 
00638                 UAVCAN_TRACE("SocketCAN", "Iface %d is dead; error %d", this->getFileDescriptor(), error);
00639             }
00640         }
00641 
00642         bool isDown() const { return down_; }
00643     };
00644 
00645     const SystemClock& clock_;
00646     std::vector<std::unique_ptr<IfaceWrapper>> ifaces_;
00647 
00648 public:
00649     /**
00650      * Reference to the clock object shall remain valid.
00651      */
00652     explicit SocketCanDriver(const SystemClock& clock)
00653         : clock_(clock)
00654     {
00655         ifaces_.reserve(uavcan::MaxCanIfaces);
00656     }
00657 
00658     /**
00659      * This function may return before deadline expiration even if no requested IO operations become possible.
00660      * This behavior makes implementation way simpler, and it is OK since libuavcan can properly handle such
00661      * early returns.
00662      * Also it can return more events than were originally requested by uavcan, which is also acceptable.
00663      */
00664     std::int16_t select(uavcan::CanSelectMasks& inout_masks,
00665                         const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
00666                         uavcan::MonotonicTime blocking_deadline) override
00667     {
00668         // Detecting whether we need to block at all
00669         bool need_block = (inout_masks.write == 0);    // Write queue is infinite
00670         for (unsigned i = 0; need_block && (i < ifaces_.size()); i++)
00671         {
00672             const bool need_read = inout_masks.read  & (1 << i);
00673             if (need_read && ifaces_[i]->hasReadyRx())
00674             {
00675                 need_block = false;
00676             }
00677         }
00678 
00679         if (need_block)
00680         {
00681             // Poll FD set setup
00682             ::pollfd pollfds[uavcan::MaxCanIfaces] = {};
00683             unsigned num_pollfds = 0;
00684             IfaceWrapper* pollfd_index_to_iface[uavcan::MaxCanIfaces] = { };
00685 
00686             for (unsigned i = 0; i < ifaces_.size(); i++)
00687             {
00688                 if (!ifaces_[i]->isDown())
00689                 {
00690                     pollfds[num_pollfds].fd = ifaces_[i]->getFileDescriptor();
00691                     pollfds[num_pollfds].events = POLLIN;
00692                     if (ifaces_[i]->hasReadyTx() || (inout_masks.write & (1U << i)))
00693                     {
00694                         pollfds[num_pollfds].events |= POLLOUT;
00695                     }
00696                     pollfd_index_to_iface[num_pollfds] = ifaces_[i].get();
00697                     num_pollfds++;
00698                 }
00699             }
00700 
00701             // This is where we abort when the last iface goes down
00702             if (num_pollfds == 0)
00703             {
00704                 throw AllIfacesDownException();
00705             }
00706 
00707             // Timeout conversion
00708             const std::int64_t timeout_usec = (blocking_deadline - clock_.getMonotonic()).toUSec();
00709             auto ts = ::timespec();
00710             if (timeout_usec > 0)
00711             {
00712                 ts.tv_sec = timeout_usec / 1000000LL;
00713                 ts.tv_nsec = (timeout_usec % 1000000LL) * 1000;
00714             }
00715 
00716             // Blocking here
00717             const int res = ::ppoll(pollfds, num_pollfds, &ts, nullptr);
00718             if (res < 0)
00719             {
00720                 return res;
00721             }
00722 
00723             // Handling poll output
00724             for (unsigned i = 0; i < num_pollfds; i++)
00725             {
00726                 pollfd_index_to_iface[i]->updateDownStatusFromPollResult(pollfds[i]);
00727 
00728                 const bool poll_read  = pollfds[i].revents & POLLIN;
00729                 const bool poll_write = pollfds[i].revents & POLLOUT;
00730                 pollfd_index_to_iface[i]->poll(poll_read, poll_write);
00731             }
00732         }
00733 
00734         // Writing the output masks
00735         inout_masks = uavcan::CanSelectMasks();
00736         for (unsigned i = 0; i < ifaces_.size(); i++)
00737         {
00738             if (!ifaces_[i]->isDown())
00739             {
00740                 inout_masks.write |= std::uint8_t(1U << i);     // Always ready to write if not down
00741             }
00742             if (ifaces_[i]->hasReadyRx())
00743             {
00744                 inout_masks.read |= std::uint8_t(1U << i);      // Readability depends only on RX buf, even if down
00745             }
00746         }
00747 
00748         // Return value is irrelevant as long as it's non-negative
00749         return ifaces_.size();
00750     }
00751 
00752     SocketCanIface* getIface(std::uint8_t iface_index) override
00753     {
00754         return (iface_index >= ifaces_.size()) ? nullptr : ifaces_[iface_index].get();
00755     }
00756 
00757     std::uint8_t getNumIfaces() const override { return ifaces_.size(); }
00758 
00759     /**
00760      * Adds one iface by name. Will fail if there are @ref MaxIfaces ifaces registered already.
00761      * @param iface_name E.g. "can0", "vcan1"
00762      * @return Negative on error, interface index on success.
00763      * @throws uavcan_linux::Exception.
00764      */
00765     int addIface(const std::string& iface_name)
00766     {
00767         if (ifaces_.size() >= uavcan::MaxCanIfaces)
00768         {
00769             return -1;
00770         }
00771 
00772         // Open the socket
00773         const int fd = SocketCanIface::openSocket(iface_name);
00774         if (fd < 0)
00775         {
00776             return fd;
00777         }
00778 
00779         // Construct the iface - upon successful construction the iface will take ownership of the fd.
00780         try
00781         {
00782             ifaces_.emplace_back(new IfaceWrapper(clock_, fd));
00783         }
00784         catch (...)
00785         {
00786             (void)::close(fd);
00787             throw;
00788         }
00789 
00790         UAVCAN_TRACE("SocketCAN", "New iface '%s' fd %d", iface_name.c_str(), fd);
00791 
00792         return ifaces_.size() - 1;
00793     }
00794 
00795     /**
00796      * Returns false if the specified interface is functioning, true if it became unavailable.
00797      */
00798     bool isIfaceDown(std::uint8_t iface_index) const
00799     {
00800         return ifaces_.at(iface_index)->isDown();
00801     }
00802 };
00803 
00804 }