/*
* TransferListenerBase
*/
bool TransferListenerBase::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const
{
TransferCRC crc = crc_base_;
unsigned offset = 0;
while (true)
{
uint8_t buf[16];
const int res = tbb.read(offset, buf, sizeof(buf));
if (res < 0)
{
UAVCAN_TRACE("TransferListenerBase", "Failed to check CRC: Buffer read failure %i", res);
return false;
}
if (res == 0)
{
break;
}
offset += unsigned(res);
crc.add(buf, unsigned(res));
}
if (crc.get() != compare_with)
{
UAVCAN_TRACE("TransferListenerBase", "CRC mismatch, expected=0x%04x, got=0x%04x",
int(compare_with), int(crc.get()));
return false;
}
return true;
}
int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
TransferID tid) const
{
Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, tid);
frame.setPriority(priority_);
frame.setStartOfTransfer(true);
UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str());
/*
* Checking if we're allowed to send.
* In passive mode we can send only anonymous transfers, if they are enabled.
*/
if (dispatcher_.isPassiveMode())
{
const bool allow = allow_anonymous_transfers_ &&
(transfer_type == TransferTypeMessageBroadcast) &&
(int(payload_len) <= frame.getPayloadCapacity());
if (!allow)
{
return -ErrPassiveMode;
}
}
dispatcher_.getTransferPerfCounter().addTxTransfer();
/*
* Sending frames
*/
if (frame.getPayloadCapacity() >= payload_len) // Single Frame Transfer
{
const int res = frame.setPayload(payload, payload_len);
if (res != int(payload_len))
{
UAVCAN_ASSERT(0);
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res);
registerError();
return (res < 0) ? res : -ErrLogic;
}
frame.setEndOfTransfer(true);
UAVCAN_ASSERT(frame.isStartOfTransfer() && frame.isEndOfTransfer() && !frame.getToggle());
const CanIOFlags flags = frame.getSrcNodeID().isUnicast() ? flags_ : (flags_ | CanIOFlagAbortOnError);
return dispatcher_.send(frame, tx_deadline, blocking_deadline, flags, iface_mask_);
}
else // Multi Frame Transfer
{
UAVCAN_ASSERT(!dispatcher_.isPassiveMode());
UAVCAN_ASSERT(frame.getSrcNodeID().isUnicast());
int offset = 0;
{
TransferCRC crc = crc_base_;
crc.add(payload, payload_len);
static const int BUFLEN = sizeof(static_cast<CanFrame*>(0)->data);
uint8_t buf[BUFLEN];
buf[0] = uint8_t(crc.get() & 0xFFU); // Transfer CRC, little endian
buf[1] = uint8_t((crc.get() >> 8) & 0xFF);
(void)copy(payload, payload + BUFLEN - 2, buf + 2);
const int write_res = frame.setPayload(buf, BUFLEN);
if (write_res < 2)
{
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
registerError();
return write_res;
}
offset = write_res - 2;
UAVCAN_ASSERT(int(payload_len) > offset);
}
int num_sent = 0;
while (true)
{
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, flags_, iface_mask_);
if (send_res < 0)
{
registerError();
return send_res;
}
num_sent++;
if (frame.isEndOfTransfer())
{
return num_sent; // Number of frames transmitted
}
frame.setStartOfTransfer(false);
frame.flipToggle();
UAVCAN_ASSERT(offset >= 0);
const int write_res = frame.setPayload(payload + offset, payload_len - unsigned(offset));
if (write_res < 0)
{
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
registerError();
return write_res;
}
offset += write_res;
UAVCAN_ASSERT(offset <= int(payload_len));
if (offset >= int(payload_len))
{
frame.setEndOfTransfer(true);
}
}
}
UAVCAN_ASSERT(0);
return -ErrLogic; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
}
