bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
{
const uint8_t* const payload = frame.getPayloadPtr();
const unsigned payload_len = frame.getPayloadLen();
if (frame.isFirst()) // First frame contains CRC, we need to extract it now
{
if (frame.getPayloadLen() < TransferCRC::NumBytes)
{
return false; // Must have been validated earlier though. I think I'm paranoid.
}
this_transfer_crc_ = static_cast<uint16_t>(payload[0] & 0xFF);
this_transfer_crc_ |= static_cast<uint16_t>(static_cast<uint16_t>(payload[1] & 0xFF) << 8); // Little endian.
const unsigned effective_payload_len = payload_len - TransferCRC::NumBytes;
const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len);
const bool success = res == static_cast<int>(effective_payload_len);
if (success)
{
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len);
}
return success;
}
else
{
const int res = buf.write(buffer_write_pos_, payload, payload_len);
const bool success = res == static_cast<int>(payload_len);
if (success)
{
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len);
}
return success;
}
}
bool TransferReceiver::validate(const RxFrame& frame) const
{
if (iface_index_ != frame.getIfaceIndex())
{
return false;
}
if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes))
{
UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
registerError();
return false;
}
if ((frame.getIndex() == Frame::MaxIndex) && !frame.isLast())
{
UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str());
registerError();
return false;
}
if (frame.getIndex() != next_frame_index_)
{
UAVCAN_TRACE("TransferReceiver", "Unexpected frame index (not %i), %s",
int(next_frame_index_), frame.toString().c_str());
registerError();
return false;
}
if (getTidRelation(frame) != TidSame)
{
UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str());
registerError();
return false;
}
return true;
}
TEST(SingleFrameIncomingTransfer, Basic)
{
using uavcan::RxFrame;
using uavcan::SingleFrameIncomingTransfer;
const RxFrame frame = makeFrame();
SingleFrameIncomingTransfer it(frame);
ASSERT_TRUE(match(it, frame, frame.getPayloadPtr(), frame.getPayloadLen()));
}
/*
* 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());
}
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);
}
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
{
if ((frame.getMonotonicTimestamp().isZero()) ||
(frame.getMonotonicTimestamp() < prev_transfer_ts_) ||
(frame.getMonotonicTimestamp() < this_transfer_ts_))
{
return ResultNotComplete;
}
const bool not_initialized = !isInitialized();
const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp());
const bool same_iface = frame.getIfaceIndex() == iface_index_;
const bool first_fame = frame.isFirst();
const TidRelation tid_rel = getTidRelation(frame);
const bool iface_timed_out =
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_usec_) * 2);
// FSM, the hard way
const bool need_restart =
(not_initialized) ||
(receiver_timed_out) ||
(same_iface && first_fame && (tid_rel == TidFuture)) ||
(iface_timed_out && first_fame && (tid_rel == TidFuture));
if (need_restart)
{
const bool error = !not_initialized && !receiver_timed_out;
if (error)
{
registerError();
}
UAVCAN_TRACE("TransferReceiver",
"Restart [not_inited=%i, iface_timeout=%i, recv_timeout=%i, same_iface=%i, first_frame=%i, tid_rel=%i], %s",
int(not_initialized), int(iface_timed_out), int(receiver_timed_out), int(same_iface),
int(first_fame), int(tid_rel), frame.toString().c_str());
tba.remove();
iface_index_ = frame.getIfaceIndex();
tid_ = frame.getTransferID();
next_frame_index_ = 0;
buffer_write_pos_ = 0;
this_transfer_crc_ = 0;
if (!first_fame)
{
tid_.increment();
return ResultNotComplete;
}
}
if (!validate(frame))
{
return ResultNotComplete;
}
return receive(frame, tba);
}
/*
* 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());
}
/*
* Dispatcher
*/
void Dispatcher::handleFrame(const CanRxFrame& can_frame)
{
RxFrame frame;
if (!frame.parse(can_frame))
{
// This is not counted as a transport error
UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
return;
}
if ((frame.getDstNodeID() != NodeID::Broadcast) &&
(frame.getDstNodeID() != getNodeID()))
{
return;
}
switch (frame.getTransferType())
{
case TransferTypeMessageBroadcast:
case TransferTypeMessageUnicast:
{
lmsg_.handleFrame(frame);
break;
}
case TransferTypeServiceRequest:
{
lsrv_req_.handleFrame(frame);
break;
}
case TransferTypeServiceResponse:
{
lsrv_resp_.handleFrame(frame);
break;
}
default:
{
UAVCAN_ASSERT(0);
break;
}
}
}
TEST(MultiFrameIncomingTransfer, Basic)
{
using uavcan::RxFrame;
using uavcan::MultiFrameIncomingTransfer;
uavcan::PoolManager<1> poolmgr; // We don't need dynamic memory
uavcan::TransferBufferManager<256, 1> bufmgr(poolmgr);
const RxFrame frame = makeFrame();
uavcan::TransferBufferManagerKey bufmgr_key(frame.getSrcNodeID(), frame.getTransferType());
uavcan::TransferBufferAccessor tba(bufmgr, bufmgr_key);
MultiFrameIncomingTransfer it(frame.getMonotonicTimestamp(), frame.getUtcTimestamp(), frame, tba);
/*
* Empty read must fail
*/
uint8_t data_byte = 0;
ASSERT_GT(0, it.read(0, &data_byte, 1)); // Error - no such buffer
/*
* Filling the test data
*/
const std::string data = "123Hello world";
const uint8_t* const data_ptr = reinterpret_cast<const uint8_t*>(data.c_str());
ASSERT_FALSE(bufmgr.access(bufmgr_key));
ASSERT_TRUE(bufmgr.create(bufmgr_key));
ASSERT_EQ(data.length(), bufmgr.access(bufmgr_key)->write(0, data_ptr, data.length()));
/*
* Check
*/
ASSERT_TRUE(match(it, frame, data_ptr, data.length()));
/*
* Buffer release
*/
ASSERT_TRUE(bufmgr.access(bufmgr_key));
it.release();
ASSERT_FALSE(bufmgr.access(bufmgr_key));
}
void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame)
{
TransferListenerBase* p = list_.get();
while (p)
{
TransferListenerBase* const next = p->getNextListNode();
if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
{
p->handleFrame(frame); // p may be modified
}
else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id!
{
break;
}
else
{
; // Nothing to do with this one
}
p = next;
}
}
TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const
{
const int distance = tid_.computeForwardDistance(frame.getTransferID());
if (distance == 0)
{
return TidSame;
}
if (distance < ((1 << TransferID::BitLen) / 2))
{
return TidFuture;
}
return TidRepeat;
}
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
{
// Transfer timestamps are derived from the first frame
if (frame.isFirst())
{
this_transfer_ts_ = frame.getMonotonicTimestamp();
first_frame_ts_ = frame.getUtcTimestamp();
}
if (frame.isFirst() && frame.isLast())
{
tba.remove();
updateTransferTimings();
prepareForNextTransfer();
this_transfer_crc_ = 0; // SFT has no CRC
return ResultSingleFrame;
}
// Payload write
ITransferBuffer* buf = tba.access();
if (buf == NULL)
{
buf = tba.create();
}
if (buf == NULL)
{
UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
prepareForNextTransfer();
registerError();
return ResultNotComplete;
}
if (!writePayload(frame, *buf))
{
UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str());
tba.remove();
prepareForNextTransfer();
registerError();
return ResultNotComplete;
}
next_frame_index_++;
if (frame.isLast())
{
updateTransferTimings();
prepareForNextTransfer();
return ResultComplete;
}
return ResultNotComplete;
}
void TransferListenerBase::handleFrame(const RxFrame& frame)
{
if (frame.getSrcNodeID().isUnicast()) // Normal transfer
{
const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
TransferReceiver* recv = receivers_.access(key);
if (recv == NULL)
{
if (!frame.isFirst())
{
return;
}
TransferReceiver new_recv;
recv = receivers_.insert(key, new_recv);
if (recv == NULL)
{
UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
return;
}
}
TransferBufferAccessor tba(bufmgr_, key);
handleReception(*recv, frame, tba);
}
else if (frame.getSrcNodeID().isBroadcast() &&
frame.isFirst() &&
frame.isLast() &&
frame.getDstNodeID().isBroadcast()) // Anonymous transfer
{
handleAnonymousTransferReception(frame);
}
else
{
UAVCAN_TRACE("TransferListenerBase", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame
}
}
void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxFrame& frame,
TransferBufferAccessor& tba)
{
switch (receiver.addFrame(frame, tba))
{
case TransferReceiver::ResultNotComplete:
{
perf_.addErrors(receiver.yieldErrorCount());
break;
}
case TransferReceiver::ResultSingleFrame:
{
perf_.addRxTransfer();
SingleFrameIncomingTransfer it(frame);
handleIncomingTransfer(it);
break;
}
case TransferReceiver::ResultComplete:
{
perf_.addRxTransfer();
const ITransferBuffer* tbb = tba.access();
if (tbb == NULL)
{
UAVCAN_TRACE("TransferListenerBase", "Buffer access failure, last frame: %s", frame.toString().c_str());
break;
}
if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb))
{
UAVCAN_TRACE("TransferListenerBase", "CRC error, last frame: %s", frame.toString().c_str());
break;
}
MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(),
receiver.getLastTransferTimestampUtc(), frame, tba);
handleIncomingTransfer(it);
it.release();
break;
}
default:
{
UAVCAN_ASSERT(0);
break;
}
}
}
/*
* GlobalTimeSyncMaster
*/
void GlobalTimeSyncMaster::handleLoopbackFrame(const RxFrame& frame)
{
const uint8_t iface = frame.getIfaceIndex();
if (initialized_ && iface < MaxCanIfaces)
{
if (frame.getDataTypeID() == dtid_ &&
frame.getTransferType() == TransferTypeMessageBroadcast &&
frame.isLast() && frame.isFirst() &&
frame.getSrcNodeID() == node_.getNodeID())
{
iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp());
}
}
else
{
UAVCAN_ASSERT(0);
}
}