virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime deadline)
{
assert(this);
//std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
for (unsigned i = 0; i < ifaces.size(); i++)
{
ifaces.at(i).pending_tx = (pending_tx[i] == NULL) ? uavcan::CanFrame() : *pending_tx[i];
}
if (select_failure)
{
return -1;
}
const uavcan::uint8_t valid_iface_mask = uavcan::uint8_t((1 << getNumIfaces()) - 1);
EXPECT_FALSE(inout_masks.write & ~valid_iface_mask);
EXPECT_FALSE(inout_masks.read & ~valid_iface_mask);
uavcan::uint8_t out_write_mask = 0;
uavcan::uint8_t out_read_mask = 0;
for (unsigned i = 0; i < getNumIfaces(); i++)
{
const uavcan::uint8_t mask = uavcan::uint8_t(1 << i);
if ((inout_masks.write & mask) && ifaces.at(i).writeable)
{
out_write_mask |= mask;
}
if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size()))
{
out_read_mask |= mask;
}
}
inout_masks.write = out_write_mask;
inout_masks.read = out_read_mask;
if ((out_write_mask | out_read_mask) == 0)
{
const uavcan::MonotonicTime ts = iclock.getMonotonic();
const uavcan::MonotonicDuration diff = deadline - ts;
SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
if (mock)
{
if (diff.isPositive())
{
mock->advance(uint64_t(diff.toUSec())); // Emulating timeout
}
}
else
{
if (diff.isPositive())
{
usleep(unsigned(diff.toUSec()));
}
}
return 0;
}
return 1; // This value is not being checked anyway, it just has to be greater than zero
}
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 pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
{
for (uint8_t i = 0; i < getNumIfaces(); i++)
{
ifaces.at(i).pushRx(can_frame);
}
}
uint8_t CanIOManager::makePendingTxMask() const
{
uint8_t write_mask = 0;
for (uint8_t i = 0; i < getNumIfaces(); i++)
{
if (!tx_queues_[i]->isEmpty())
{
write_mask |= uint8_t(1 << i);
}
}
return write_mask;
}
int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
{
const uint8_t num_ifaces = getNumIfaces();
const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1);
iface_mask &= all_ifaces_mask;
if (blocking_deadline > tx_deadline)
{
blocking_deadline = tx_deadline;
}
int retval = 0;
while (true)
{
if (iface_mask == 0)
{
break;
}
CanSelectMasks masks;
masks.write = iface_mask | makePendingTxMask();
{
const int select_res = callSelect(masks, blocking_deadline);
if (select_res < 0)
{
return -ErrDriver;
}
UAVCAN_ASSERT(masks.read == 0);
}
// Transmission
for (uint8_t i = 0; i < num_ifaces; i++)
{
if (masks.write & (1 << i))
{
int res = 0;
if (iface_mask & (1 << i))
{
if (tx_queues_[i]->topPriorityHigherOrEqual(frame))
{
res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired)
}
if (res <= 0)
{
res = sendToIface(i, frame, tx_deadline, flags);
if (res > 0)
{
iface_mask &= uint8_t(~(1 << i)); // Mark transmitted
}
}
}
else
{
res = sendFromTxQueue(i);
}
if (res > 0)
{
retval++;
}
}
}
// Timeout. Enqueue the frame if wasn't transmitted and leave.
const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline;
if (masks.write == 0 || timed_out)
{
if (!timed_out)
{
UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
continue;
}
for (uint8_t i = 0; i < num_ifaces; i++)
{
if (iface_mask & (1 << i))
{
tx_queues_[i]->push(frame, tx_deadline, qos, flags);
}
}
break;
}
}
return retval;
}
