uc_dispatcher.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/transport/dispatcher.hpp>
00006 #include <uavcan/debug.hpp>
00007 #include <cassert>
00008 
00009 namespace uavcan
00010 {
00011 #if !UAVCAN_TINY
00012 /*
00013  * LoopbackFrameListenerBase
00014  */
00015 void LoopbackFrameListenerBase::startListening()
00016 {
00017     dispatcher_.getLoopbackFrameListenerRegistry().add(this);
00018 }
00019 
00020 void LoopbackFrameListenerBase::stopListening()
00021 {
00022     dispatcher_.getLoopbackFrameListenerRegistry().remove(this);
00023 }
00024 
00025 bool LoopbackFrameListenerBase::isListening() const
00026 {
00027     return dispatcher_.getLoopbackFrameListenerRegistry().doesExist(this);
00028 }
00029 
00030 /*
00031  * LoopbackFrameListenerRegistry
00032  */
00033 void LoopbackFrameListenerRegistry::add(LoopbackFrameListenerBase* listener)
00034 {
00035     UAVCAN_ASSERT(listener);
00036     listeners_.insert(listener);
00037 }
00038 
00039 void LoopbackFrameListenerRegistry::remove(LoopbackFrameListenerBase* listener)
00040 {
00041     UAVCAN_ASSERT(listener);
00042     listeners_.remove(listener);
00043 }
00044 
00045 bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* listener) const
00046 {
00047     UAVCAN_ASSERT(listener);
00048     const LoopbackFrameListenerBase* p = listeners_.get();
00049     while (p)
00050     {
00051         if (p == listener)
00052         {
00053             return true;
00054         }
00055         p = p->getNextListNode();
00056     }
00057     return false;
00058 }
00059 
00060 void LoopbackFrameListenerRegistry::invokeListeners(RxFrame& frame)
00061 {
00062     LoopbackFrameListenerBase* p = listeners_.get();
00063     while (p)
00064     {
00065         LoopbackFrameListenerBase* const next = p->getNextListNode();
00066         p->handleLoopbackFrame(frame);     // p may be modified
00067         p = next;
00068     }
00069 }
00070 #endif
00071 
00072 /*
00073  * Dispatcher::ListenerRegister
00074  */
00075 bool Dispatcher::ListenerRegistry::add(TransferListener* listener, Mode mode)
00076 {
00077     if (mode == UniqueListener)
00078     {
00079         TransferListener* p = list_.get();
00080         while (p)
00081         {
00082             if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID())
00083             {
00084                 return false;
00085             }
00086             p = p->getNextListNode();
00087         }
00088     }
00089     // Objective is to arrange entries by Data Type ID in ascending order from root.
00090     list_.insertBefore(listener, DataTypeIDInsertionComparator(listener->getDataTypeDescriptor().getID()));
00091     return true;
00092 }
00093 
00094 void Dispatcher::ListenerRegistry::remove(TransferListener* listener)
00095 {
00096     list_.remove(listener);
00097 }
00098 
00099 bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const
00100 {
00101     TransferListener* p = list_.get();
00102     while (p)
00103     {
00104         if (p->getDataTypeDescriptor().getID() == dtid)
00105         {
00106             return true;
00107         }
00108         p = p->getNextListNode();
00109     }
00110     return false;
00111 }
00112 
00113 void Dispatcher::ListenerRegistry::cleanup(MonotonicTime ts)
00114 {
00115     TransferListener* p = list_.get();
00116     while (p)
00117     {
00118         TransferListener* const next = p->getNextListNode();
00119         p->cleanup(ts); // p may be modified
00120         p = next;
00121     }
00122 }
00123 
00124 void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame)
00125 {
00126     TransferListener* p = list_.get();
00127     while (p)
00128     {
00129         TransferListener* const next = p->getNextListNode();
00130         if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
00131         {
00132             p->handleFrame(frame); // p may be modified
00133         }
00134         else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID())  // Listeners are ordered by data type id!
00135         {
00136             break;
00137         }
00138         else
00139         {
00140             ;  // Nothing to do with this one
00141         }
00142         p = next;
00143     }
00144 }
00145 
00146 /*
00147  * Dispatcher
00148  */
00149 void Dispatcher::handleFrame(const CanRxFrame& can_frame)
00150 {
00151     RxFrame frame;
00152     if (!frame.parse(can_frame))
00153     {
00154         // This is not counted as a transport error
00155         UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
00156         return;
00157     }
00158 
00159     if ((frame.getDstNodeID() != NodeID::Broadcast) &&
00160         (frame.getDstNodeID() != getNodeID()))
00161     {
00162         return;
00163     }
00164 
00165     switch (frame.getTransferType())
00166     {
00167     case TransferTypeMessageBroadcast:
00168     {
00169         lmsg_.handleFrame(frame);
00170         break;
00171     }
00172     case TransferTypeServiceRequest:
00173     {
00174         lsrv_req_.handleFrame(frame);
00175         break;
00176     }
00177     case TransferTypeServiceResponse:
00178     {
00179         lsrv_resp_.handleFrame(frame);
00180         break;
00181     }
00182     default:
00183     {
00184         UAVCAN_ASSERT(0);
00185         break;
00186     }
00187     }
00188 }
00189 
00190 #if UAVCAN_TINY
00191 void Dispatcher::handleLoopbackFrame(const CanRxFrame&)
00192 {
00193 }
00194 
00195 void Dispatcher::notifyRxFrameListener(const CanRxFrame&, CanIOFlags)
00196 {
00197 }
00198 #else
00199 void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame)
00200 {
00201     RxFrame frame;
00202     if (!frame.parse(can_frame))
00203     {
00204         UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str());
00205         UAVCAN_ASSERT(0);  // No way!
00206         return;
00207     }
00208     UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID());
00209     loopback_listeners_.invokeListeners(frame);
00210 }
00211 
00212 void Dispatcher::notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags)
00213 {
00214     if (rx_listener_ != UAVCAN_NULLPTR)
00215     {
00216         rx_listener_->handleRxFrame(can_frame, flags);
00217     }
00218 }
00219 #endif
00220 
00221 int Dispatcher::spin(MonotonicTime deadline)
00222 {
00223     int num_frames_processed = 0;
00224     do
00225     {
00226         CanIOFlags flags = 0;
00227         CanRxFrame frame;
00228         const int res = canio_.receive(frame, deadline, flags);
00229         if (res < 0)
00230         {
00231             return res;
00232         }
00233         if (res > 0)
00234         {
00235             if (flags & CanIOFlagLoopback)
00236             {
00237                 handleLoopbackFrame(frame);
00238             }
00239             else
00240             {
00241                 num_frames_processed++;
00242                 handleFrame(frame);
00243             }
00244             notifyRxFrameListener(frame, flags);
00245         }
00246     }
00247     while (sysclock_.getMonotonic() < deadline);
00248 
00249     return num_frames_processed;
00250 }
00251 
00252 int Dispatcher::spinOnce()
00253 {
00254     int num_frames_processed = 0;
00255 
00256     while (true)
00257     {
00258         CanIOFlags flags = 0;
00259         CanRxFrame frame;
00260         const int res = canio_.receive(frame, MonotonicTime(), flags);
00261         if (res < 0)
00262         {
00263             return res;
00264         }
00265         else if (res > 0)
00266         {
00267             if (flags & CanIOFlagLoopback)
00268             {
00269                 handleLoopbackFrame(frame);
00270             }
00271             else
00272             {
00273                 num_frames_processed++;
00274                 handleFrame(frame);
00275             }
00276             notifyRxFrameListener(frame, flags);
00277         }
00278         else
00279         {
00280             break;      // No frames left
00281         }
00282     }
00283 
00284     return num_frames_processed;
00285 }
00286 
00287 int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
00288                      CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask)
00289 {
00290     if (frame.getSrcNodeID() != getNodeID())
00291     {
00292         UAVCAN_ASSERT(0);
00293         return -ErrLogic;
00294     }
00295 
00296     CanFrame can_frame;
00297     if (!frame.compile(can_frame))
00298     {
00299         UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str());
00300         UAVCAN_ASSERT(0);
00301         return -ErrLogic;
00302     }
00303     return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags);
00304 }
00305 
00306 void Dispatcher::cleanup(MonotonicTime ts)
00307 {
00308     outgoing_transfer_reg_.cleanup(ts);
00309     lmsg_.cleanup(ts);
00310     lsrv_req_.cleanup(ts);
00311     lsrv_resp_.cleanup(ts);
00312 }
00313 
00314 bool Dispatcher::registerMessageListener(TransferListener* listener)
00315 {
00316     if (listener->getDataTypeDescriptor().getKind() != DataTypeKindMessage)
00317     {
00318         UAVCAN_ASSERT(0);
00319         return false;
00320     }
00321     return lmsg_.add(listener, ListenerRegistry::ManyListeners);       // Multiple subscribers are OK
00322 }
00323 
00324 bool Dispatcher::registerServiceRequestListener(TransferListener* listener)
00325 {
00326     if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService)
00327     {
00328         UAVCAN_ASSERT(0);
00329         return false;
00330     }
00331     return lsrv_req_.add(listener, ListenerRegistry::UniqueListener);  // Only one server per data type
00332 }
00333 
00334 bool Dispatcher::registerServiceResponseListener(TransferListener* listener)
00335 {
00336     if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService)
00337     {
00338         UAVCAN_ASSERT(0);
00339         return false;
00340     }
00341     return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners);  // Multiple callers may call same srv
00342 }
00343 
00344 void Dispatcher::unregisterMessageListener(TransferListener* listener)
00345 {
00346     lmsg_.remove(listener);
00347 }
00348 
00349 void Dispatcher::unregisterServiceRequestListener(TransferListener* listener)
00350 {
00351     lsrv_req_.remove(listener);
00352 }
00353 
00354 void Dispatcher::unregisterServiceResponseListener(TransferListener* listener)
00355 {
00356     lsrv_resp_.remove(listener);
00357 }
00358 
00359 bool Dispatcher::hasSubscriber(DataTypeID dtid) const
00360 {
00361     return lmsg_.exists(dtid);
00362 }
00363 
00364 bool Dispatcher::hasPublisher(DataTypeID dtid) const
00365 {
00366     return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast);
00367 }
00368 
00369 bool Dispatcher::hasServer(DataTypeID dtid) const
00370 {
00371     return lsrv_req_.exists(dtid);
00372 }
00373 
00374 bool Dispatcher::setNodeID(NodeID nid)
00375 {
00376     if (!self_node_id_is_set_)
00377     {
00378         self_node_id_ = nid;
00379         self_node_id_is_set_ = true;
00380         return true;
00381     }
00382     return false;
00383 }
00384 
00385 }