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);
}
}
}
