allocation_request_manager.hpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED
00007 
00008 #include <uavcan/build_config.hpp>
00009 #include <uavcan/debug.hpp>
00010 #include <uavcan/node/subscriber.hpp>
00011 #include <uavcan/node/publisher.hpp>
00012 #include <uavcan/util/method_binder.hpp>
00013 #include <uavcan/protocol/dynamic_node_id_server/types.hpp>
00014 #include <uavcan/protocol/dynamic_node_id_server/event.hpp>
00015 // UAVCAN types
00016 #include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
00017 
00018 namespace uavcan
00019 {
00020 namespace dynamic_node_id_server
00021 {
00022 /**
00023  * The main allocator must implement this interface.
00024  */
00025 class IAllocationRequestHandler
00026 {
00027 public:
00028     /**
00029      * Allocation request manager uses this method to detect if it is allowed to publish follow-up responses.
00030      */
00031     virtual bool canPublishFollowupAllocationResponse() const = 0;
00032 
00033     /**
00034      * This method will be invoked when a new allocation request is received.
00035      */
00036     virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0;
00037 
00038     virtual ~IAllocationRequestHandler() { }
00039 };
00040 
00041 /**
00042  * This class manages communication with allocation clients.
00043  * Three-stage unique ID exchange is implemented here, as well as response publication.
00044  */
00045 class AllocationRequestManager
00046 {
00047     typedef MethodBinder<AllocationRequestManager*,
00048                          void (AllocationRequestManager::*)(const ReceivedDataStructure<Allocation>&)>
00049         AllocationCallback;
00050 
00051     const MonotonicDuration stage_timeout_;
00052 
00053     MonotonicTime last_message_timestamp_;
00054     MonotonicTime last_activity_timestamp_;
00055     Allocation::FieldTypes::unique_id current_unique_id_;
00056 
00057     IAllocationRequestHandler& handler_;
00058     IEventTracer& tracer_;
00059 
00060     Subscriber<Allocation, AllocationCallback> allocation_sub_;
00061     Publisher<Allocation>  allocation_pub_;
00062 
00063     enum { InvalidStage = 0 };
00064 
00065     void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
00066 
00067     static uint8_t detectRequestStage(const Allocation& msg)
00068     {
00069         const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
00070 
00071         if ((msg.unique_id.size() != max_bytes_per_request) &&
00072             (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) &&
00073             (msg.unique_id.size() != msg.unique_id.capacity()))     // Future proofness for CAN FD
00074         {
00075             return InvalidStage;
00076         }
00077         if (msg.first_part_of_unique_id)
00078         {
00079             return 1;       // Note that CAN FD frames can deliver the unique ID in one stage!
00080         }
00081         if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
00082         {
00083             return 2;
00084         }
00085         if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
00086         {
00087             return 3;
00088         }
00089         return InvalidStage;
00090     }
00091 
00092     uint8_t getExpectedStage() const
00093     {
00094         if (current_unique_id_.empty())
00095         {
00096             return 1;
00097         }
00098         if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2))
00099         {
00100             return 3;
00101         }
00102         if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
00103         {
00104             return 2;
00105         }
00106         return InvalidStage;
00107     }
00108 
00109     void publishFollowupAllocationResponse()
00110     {
00111         Allocation msg;
00112         msg.unique_id = current_unique_id_;
00113         UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity());
00114 
00115         UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID",
00116                      unsigned(msg.unique_id.size()));
00117 
00118         trace(TraceAllocationFollowupResponse, msg.unique_id.size());
00119 
00120         const int res = allocation_pub_.broadcast(msg);
00121         if (res < 0)
00122         {
00123             trace(TraceError, res);
00124             allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast");
00125         }
00126     }
00127 
00128     void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
00129     {
00130         trace(TraceAllocationActivity, msg.getSrcNodeID().get());
00131         last_activity_timestamp_ = msg.getMonotonicTimestamp();
00132 
00133         if (!msg.isAnonymousTransfer())
00134         {
00135             return;         // This is a response from another allocator, ignore
00136         }
00137 
00138         /*
00139          * Reset the expected stage on timeout
00140          */
00141         if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_))
00142         {
00143             UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset");
00144             current_unique_id_.clear();
00145             trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec());
00146         }
00147 
00148         /*
00149          * Checking if request stage matches the expected stage
00150          */
00151         const uint8_t request_stage = detectRequestStage(msg);
00152         if (request_stage == InvalidStage)
00153         {
00154             trace(TraceAllocationBadRequest, msg.unique_id.size());
00155             return;             // Malformed request - ignore without resetting
00156         }
00157 
00158         const uint8_t expected_stage = getExpectedStage();
00159         if (expected_stage == InvalidStage)
00160         {
00161             UAVCAN_ASSERT(0);
00162             return;
00163         }
00164 
00165         if (request_stage != expected_stage)
00166         {
00167             trace(TraceAllocationUnexpectedStage, request_stage);
00168             return;             // Ignore - stage mismatch
00169         }
00170 
00171         const uint8_t max_expected_bytes =
00172             static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size());
00173         UAVCAN_ASSERT(max_expected_bytes > 0);
00174         if (msg.unique_id.size() > max_expected_bytes)
00175         {
00176             trace(TraceAllocationBadRequest, msg.unique_id.size());
00177             return;             // Malformed request
00178         }
00179 
00180         /*
00181          * Updating the local state
00182          */
00183         for (uint8_t i = 0; i < msg.unique_id.size(); i++)
00184         {
00185             current_unique_id_.push_back(msg.unique_id[i]);
00186         }
00187 
00188         trace(TraceAllocationRequestAccepted, current_unique_id_.size());
00189 
00190         /*
00191          * Proceeding with allocation if possible
00192          * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader.
00193          */
00194         if (current_unique_id_.size() == current_unique_id_.capacity())
00195         {
00196             UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d",
00197                          int(msg.node_id));
00198 
00199             UniqueID unique_id;
00200             copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin());
00201             current_unique_id_.clear();
00202 
00203             {
00204                 uint64_t event_agrument = 0;
00205                 for (uint8_t i = 0; i < 8; i++)
00206                 {
00207                     event_agrument |= static_cast<uint64_t>(unique_id[i]) << (56U - i * 8U);
00208                 }
00209                 trace(TraceAllocationExchangeComplete, static_cast<int64_t>(event_agrument));
00210             }
00211 
00212             handler_.handleAllocationRequest(unique_id, msg.node_id);
00213         }
00214         else
00215         {
00216             if (handler_.canPublishFollowupAllocationResponse())
00217             {
00218                 publishFollowupAllocationResponse();
00219             }
00220             else
00221             {
00222                 trace(TraceAllocationFollowupDenied, 0);
00223                 current_unique_id_.clear();
00224             }
00225         }
00226 
00227         /*
00228          * It is super important to update timestamp only if the request has been processed successfully.
00229          */
00230         last_message_timestamp_ = msg.getMonotonicTimestamp();
00231     }
00232 
00233 public:
00234     AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler)
00235         : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS))
00236         , handler_(handler)
00237         , tracer_(tracer)
00238         , allocation_sub_(node)
00239         , allocation_pub_(node)
00240     { }
00241 
00242     int init(const TransferPriority priority)
00243     {
00244         int res = allocation_pub_.init(priority);
00245         if (res < 0)
00246         {
00247             return res;
00248         }
00249         allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS));
00250 
00251         res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation));
00252         if (res < 0)
00253         {
00254             return res;
00255         }
00256         allocation_sub_.allowAnonymousTransfers();
00257 
00258         return 0;
00259     }
00260 
00261     int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id)
00262     {
00263         Allocation msg;
00264 
00265         msg.unique_id.resize(msg.unique_id.capacity());
00266         copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
00267 
00268         msg.node_id = allocated_node_id.get();
00269 
00270         trace(TraceAllocationResponse, msg.node_id);
00271         last_activity_timestamp_ = allocation_pub_.getNode().getMonotonicTime();
00272 
00273         return allocation_pub_.broadcast(msg);
00274     }
00275 
00276     /**
00277      * When the last allocation activity was registered.
00278      * This value can be used to heuristically determine whether there are any unallocated nodes left in the network.
00279      */
00280     MonotonicTime getTimeOfLastAllocationActivity() const { return last_activity_timestamp_; }
00281 };
00282 
00283 }
00284 }
00285 
00286 #endif // Include guard