int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags) { const uint8_t num_ifaces = getNumIfaces(); while (true) { CanSelectMasks masks; masks.write = makePendingTxMask(); masks.read = uint8_t((1 << num_ifaces) - 1); { const int select_res = callSelect(masks, blocking_deadline); if (select_res < 0) { return -ErrDriver; } } // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call for (uint8_t i = 0; i < num_ifaces; i++) { if (masks.write & (1 << i)) { (void)sendFromTxQueue(i); // It may fail, we don't care. Requested operation was receive, not send. } } // Read for (uint8_t i = 0; i < num_ifaces; i++) { if (masks.read & (1 << i)) { ICanIface* const iface = driver_.getIface(i); if (iface == NULL) { UAVCAN_ASSERT(0); // Nonexistent interface continue; } const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags); if (res == 0) { UAVCAN_ASSERT(0); // select() reported that iface has pending RX frames, but receive() returned none continue; } out_frame.iface_index = i; if ((res > 0) && !(out_flags & CanIOFlagLoopback)) { counters_[i].frames_rx += 1; } return (res < 0) ? -ErrDriver : res; } } // Timeout checked in the last order - this way we can operate with expired deadline: if (sysclock_.getMonotonic() >= blocking_deadline) { break; } } return 0; }
void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) { // This method implements Rule B UAVCAN_ASSERT(preferred_node_id_.isValid()); if (isAllocationComplete()) { UAVCAN_ASSERT(0); terminate(); return; } // Filling the message protocol::dynamic_node_id::Allocation msg; msg.node_id = preferred_node_id_.get(); msg.first_part_of_unique_id = true; msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST); copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin()); UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)); // Broadcasting UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 1st stage: preferred ID: %d", static_cast<int>(preferred_node_id_.get())); const int res = dnida_pub_.broadcast(msg); if (res < 0) { dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); } }
ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key) { if (key.isEmpty()) { UAVCAN_ASSERT(0); return UAVCAN_NULLPTR; } remove(key); TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_); if (tbme == UAVCAN_NULLPTR) { return UAVCAN_NULLPTR; // Epic fail. } buffers_.insert(tbme); UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str()); if (tbme != UAVCAN_NULLPTR) { UAVCAN_ASSERT(tbme->isEmpty()); tbme->reset(key); } return tbme; }
int DataTypeInfoProvider::start() { int res = 0; res = cats_srv_.start( ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest)); if (res < 0) { goto fail; } res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); if (res < 0) { goto fail; } UAVCAN_ASSERT(res >= 0); return res; fail: UAVCAN_ASSERT(res < 0); cats_srv_.stop(); gdti_srv_.stop(); return res; }
int TransferBufferManagerEntry::read(unsigned offset, uint8_t* data, unsigned len) const { if (!data) { UAVCAN_ASSERT(0); return -ErrInvalidParam; } if (offset >= max_write_pos_) { return 0; } if ((offset + len) > max_write_pos_) { len = max_write_pos_ - offset; } UAVCAN_ASSERT((offset + len) <= max_write_pos_); // This shall be optimized. unsigned total_offset = 0; unsigned left_to_read = len; uint8_t* outptr = data; Block* p = blocks_.get(); while (p) { p->read(outptr, offset, total_offset, left_to_read); if (left_to_read == 0) { break; } p = p->getNextListNode(); } UAVCAN_ASSERT(left_to_read == 0); return int(len); }
virtual void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request) { UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); ServiceResponseDataStructure<ResponseType> response; if (coerceOrFallback<bool>(callback_, true)) { UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default callback_(request, response); } else { handleFatalError("Srv serv clbk"); } if (response.isResponseEnabled()) { publisher_.setPriority(request.getPriority()); // Responding at the same priority. const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), request.getTransferID()); if (res < 0) { UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res); publisher_.getNode().getDispatcher().getTransferPerfCounter().addError(); response_failure_count_++; } } else { UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application"); } }
/* * MultiFrameIncomingTransfer */ MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, const RxFrame& last_frame, TransferBufferAccessor& tba) : IncomingTransfer(ts_mono, ts_utc, last_frame.getPriority(), last_frame.getTransferType(), last_frame.getTransferID(), last_frame.getSrcNodeID(), last_frame.getIfaceIndex()) , buf_acc_(tba) { UAVCAN_ASSERT(last_frame.isValid()); UAVCAN_ASSERT(last_frame.isLast()); }
int NodeStatusProvider::publish() { const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_; UAVCAN_ASSERT(uptime.isPositive()); node_info_.status.uptime_sec = uptime.toMSec() / 1000; UAVCAN_ASSERT(node_info_.status.status_code <= protocol::NodeStatus::FieldTypes::status_code::max()); return node_status_pub_.broadcast(node_info_.status); }
void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame) { RxFrame frame; if (!frame.parse(can_frame)) { UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str()); UAVCAN_ASSERT(0); // No way! return; } UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID()); loopback_listeners_.invokeListeners(frame); }
OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id) : data_type_id_(data_type_id) , transfer_type_(transfer_type) , destination_node_id_(destination_node_id) { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast()); /* * Service response transfers must use the same Transfer ID as matching service request transfer, * so this registry is not applicable for service response transfers at all. */ UAVCAN_ASSERT(transfer_type != TransferTypeServiceResponse); }
DataTypeSignature GlobalDataTypeRegistry::computeAggregateSignature(DataTypeKind kind, DataTypeIDMask& inout_id_mask) const { UAVCAN_ASSERT(isFrozen()); // Computing the signature if the registry is not frozen is pointless const List* list = selectList(kind); if (!list) { UAVCAN_ASSERT(0); return DataTypeSignature(); } int prev_dtid = -1; DataTypeSignature signature; bool signature_initialized = false; Entry* p = list->get(); while (p) { const DataTypeDescriptor& desc = p->descriptor; const int dtid = desc.getID().get(); if (inout_id_mask[unsigned(dtid)]) { if (signature_initialized) { signature.extend(desc.getSignature()); } else { signature = DataTypeSignature(desc.getSignature()); } signature_initialized = true; } UAVCAN_ASSERT(prev_dtid < dtid); // Making sure that list is ordered properly prev_dtid++; while (prev_dtid < dtid) { inout_id_mask[unsigned(prev_dtid++)] = false; // Erasing bits for missing types } UAVCAN_ASSERT(prev_dtid == dtid); p = p->getNextListNode(); } prev_dtid++; while (prev_dtid <= DataTypeID::Max) { inout_id_mask[unsigned(prev_dtid++)] = false; } return signature; }
void TimerBase::startPeriodic(MonotonicDuration period) { UAVCAN_ASSERT(period < MonotonicDuration::getInfinite()); stop(); period_ = period; DeadlineHandler::startWithDelay(period); }
/** * Invokes storage IO. */ int setCurrentTerm(Term term) { if (term < current_term_) { UAVCAN_ASSERT(0); return -ErrInvalidParam; } tracer_.onEvent(TraceRaftCurrentTermUpdate, term); StorageMarshaller io(storage_); Term tmp = term; int res = io.setAndGetBack(getCurrentTermKey(), tmp); if (res < 0) { return res; } if (tmp != term) { return -ErrFailure; } current_term_ = term; return 0; }
GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd) { if (!dtd) { UAVCAN_ASSERT(0); return RegistResultInvalidParams; } if (isFrozen()) { return RegistResultFrozen; } List* list = selectList(dtd->descriptor.getKind()); if (!list) { return RegistResultInvalidParams; } list->remove(dtd); // If this call came from regist<>(), that would be enough Entry* p = list->get(); // But anyway while (p) { Entry* const next = p->getNextListNode(); if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName())) { list->remove(p); } p = next; } return RegistResultOk; }
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg) { if (!needToQuery(msg.getSrcNodeID())) { return; } NodeData* data = node_map_.access(msg.getSrcNodeID()); if (data == NULL) { trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get()); data = node_map_.insert(msg.getSrcNodeID(), NodeData()); if (data == NULL) { getNode().registerInternalFailure("NodeDiscoverer OOM"); return; } } UAVCAN_ASSERT(data != NULL); if (msg.uptime_sec < data->last_seen_uptime) { trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get()); data->num_get_node_info_attempts = 0; } data->last_seen_uptime = msg.uptime_sec; if (!isRunning()) { startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } }
NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const { out_at_least_one_request_needed = false; for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin { last_picked_node_++; if (last_picked_node_ > NodeID::Max) { last_picked_node_ = 1; } UAVCAN_ASSERT((last_picked_node_ >= 1) && (last_picked_node_ <= NodeID::Max)); const Entry& entry = getEntry(last_picked_node_); if (entry.request_needed) { out_at_least_one_request_needed = true; if (entry.updated_since_last_attempt && !get_node_info_client_.hasPendingCallToServer(last_picked_node_)) { UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_)); return NodeID(last_picked_node_); } } } return NodeID(); // No node could be found }
/** * Invokes storage IO. */ int setVotedFor(NodeID node_id) { if (!node_id.isValid()) { UAVCAN_ASSERT(0); return -ErrInvalidParam; } tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get()); StorageMarshaller io(storage_); uint32_t tmp = node_id.get(); int res = io.setAndGetBack(getVotedForKey(), tmp); if (res < 0) { return res; } if (node_id.get() != tmp) { return -ErrFailure; } voted_for_ = node_id; return 0; }
int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const { /* * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped? */ const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id); UAVCAN_ASSERT(!tx_deadline.isZero()); const MonotonicTime otr_deadline = tx_deadline + max(max_transfer_interval_ * 2, OutgoingTransferRegistry::MinEntryLifetime); TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); if (tid == UAVCAN_NULLPTR) { UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i", int(data_type_id_.get()), int(transfer_type)); return -ErrMemory; } const TransferID this_tid = tid->get(); tid->increment(); return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type, dst_node_id, this_tid); }
void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, protocol::GetNodeInfo::Response& rsp) { UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request"); UAVCAN_ASSERT(isNodeInfoInitialized()); rsp = node_info_; }
inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index) { UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); if (utc_usec > 0) { utc_usec--; } if (ifaces[iface_index] != NULL) { ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec); } else { UAVCAN_ASSERT(0); } }
Frame(DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false) : transfer_priority_(getDefaultPriorityForTransferType(transfer_type)) , transfer_type_(transfer_type) , data_type_id_(data_type_id) , payload_len_(0) , src_node_id_(src_node_id) , dst_node_id_(dst_node_id) , frame_index_(frame_index) , transfer_id_(transfer_id) , last_frame_(last_frame) { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type))); UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); UAVCAN_ASSERT(frame_index <= getMaxIndex()); }
void GlobalDataTypeRegistry::getDataTypeIDMask(DataTypeKind kind, DataTypeIDMask& mask) const { (void)mask.reset(); const List* list = selectList(kind); if (!list) { UAVCAN_ASSERT(0); return; } Entry* p = list->get(); while (p) { UAVCAN_ASSERT(p->descriptor.getKind() == kind); mask[p->descriptor.getID().get()] = true; p = p->getNextListNode(); } }
/* * SingleFrameIncomingTransfer */ SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm) : IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getPriority(), frm.getTransferType(), frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex()) , payload_(frm.getPayloadPtr()) , payload_len_(uint8_t(frm.getPayloadLen())) { UAVCAN_ASSERT(frm.isValid()); }
int GlobalTimeSyncMaster::IfaceMaster::publish(TransferID tid, MonotonicTime current_time) { UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; iface_prev_pub_mono_ = current_time; UAVCAN_ASSERT(since_prev_pub.isPositive()); const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; protocol::GlobalTimeSync msg; msg.prev_utc_usec = long_period ? 0 : prev_tx_utc_.toUSec(); prev_tx_utc_ = UtcTime(); UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", static_cast<unsigned long long>(msg.prev_utc_usec), int(iface_index_), int(tid.get())); return pub_.broadcast(msg, tid); }
ITransferBuffer* TransferBufferManager::access(const TransferBufferManagerKey& key) { if (key.isEmpty()) { UAVCAN_ASSERT(0); return UAVCAN_NULLPTR; } return findFirst(key); }
int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) { if (frame.getSrcNodeID() != getNodeID()) { UAVCAN_ASSERT(0); return -ErrLogic; } CanFrame can_frame; if (!frame.compile(can_frame)) { UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str()); UAVCAN_ASSERT(0); return -ErrLogic; } return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags); }
bool Dispatcher::registerServiceResponseListener(TransferListenerBase* listener) { if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService) { UAVCAN_ASSERT(0); return false; } return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv }
bool Dispatcher::registerMessageListener(TransferListenerBase* listener) { if (listener->getDataTypeDescriptor().getKind() != DataTypeKindMessage) { UAVCAN_ASSERT(0); return false; } return lmsg_.add(listener, ListenerRegistry::ManyListeners); // Multiple subscribers are OK }
bool Dispatcher::registerServiceRequestListener(TransferListenerBase* listener) { if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService) { UAVCAN_ASSERT(0); return false; } return lsrv_req_.add(listener, ListenerRegistry::UniqueListener); // Only one server per data type }
void handleBeginFirmwareUpdateResponse(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>& result) { if (!result.isSuccessful()) { UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry", int(result.getCallID().server_node_id.get())); return; } FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id); if (old_path == UAVCAN_NULLPTR) { // The entry has been removed, assuming that it's not needed anymore return; } const bool confirmed = result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK || result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; if (confirmed) { UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request", int(result.getCallID().server_node_id.get())); pending_nodes_.remove(result.getCallID().server_node_id); checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse()); } else { UAVCAN_ASSERT(old_path != UAVCAN_NULLPTR); UAVCAN_ASSERT(TimerBase::isRunning()); // We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer const bool update_needed = checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path); if (!update_needed) { UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry", int(result.getCallID().server_node_id.get())); pending_nodes_.remove(result.getCallID().server_node_id); } } }