Modified CAN driver API: Passing the next pending TX frames into the select() call to facilitate proper prioritization in the driver

This commit is contained in:
Pavel Kirienko
2015-07-05 18:46:21 +03:00
parent b4ba088e08
commit 8d923fee4c
10 changed files with 127 additions and 24 deletions
+13 -2
View File
@@ -13,6 +13,11 @@
namespace uavcan
{
/**
* This limit is defined by the specification.
*/
enum { MaxCanIfaces = 3 };
/**
* Raw CAN frame, as passed to/from the CAN driver.
*/
@@ -137,6 +142,9 @@ public:
/**
* Non-blocking transmission.
* If the frame wasn't transmitted upon TX deadline, the driver should discard it.
* Note that it is LIKELY that the library will want to send the frames that were passed into the select()
* method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new
* frames between the calls.
* @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
*/
virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) = 0;
@@ -202,12 +210,15 @@ public:
* Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO.
* Bit position in the masks defines interface index.
* Note that it is allowed to return from this method even if no requested events actually happened, or if
* there are events that were not requested by the lirary.
* there are events that were not requested by the library.
* @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO.
* @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next.
* @param [in] blocking_deadline Zero means non-blocking operation.
* @return Positive number of ready interfaces or negative error code.
*/
virtual int16_t select(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline) = 0;
virtual int16_t select(CanSelectMasks& inout_masks,
const CanFrame* (& pending_tx)[MaxCanIfaces],
MonotonicTime blocking_deadline) = 0;
};
}
@@ -21,8 +21,6 @@
namespace uavcan
{
enum { MaxCanIfaces = 3 };
struct UAVCAN_EXPORT CanRxFrame : public CanFrame
{
MonotonicTime ts_mono;
@@ -108,7 +106,9 @@ public:
Entry* peek(); // Modifier
void remove(Entry*& entry);
const CanFrame* getTopPriorityPendingFrame() const;
/// The 'or equal' condition is necessary to avoid frame reordering.
bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const;
uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }
@@ -154,7 +154,8 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
int sendFromTxQueue(uint8_t iface_index);
int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline);
int callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces],
MonotonicTime blocking_deadline);
public:
CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
+38 -5
View File
@@ -210,6 +210,11 @@ void CanTxQueue::remove(Entry*& entry)
Entry::destroy(entry, allocator_);
}
const CanFrame* CanTxQueue::getTopPriorityPendingFrame() const
{
return (queue_.get() == NULL) ? NULL : &queue_.get()->frame;
}
bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
{
const Entry* entry = queue_.get();
@@ -261,14 +266,17 @@ int CanIOManager::sendFromTxQueue(uint8_t iface_index)
return res;
}
int CanIOManager::callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline)
int CanIOManager::callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces],
MonotonicTime blocking_deadline)
{
const CanSelectMasks in_masks = inout_masks;
const int res = driver_.select(inout_masks, blocking_deadline);
const int res = driver_.select(inout_masks, pending_tx, blocking_deadline);
if (res < 0)
{
return -ErrDriver;
}
inout_masks.read &= in_masks.read; // Driver is not required to clean the masks
inout_masks.write &= in_masks.write;
return res;
@@ -341,16 +349,33 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
int retval = 0;
while (true)
while (true) // Somebody please refactor this.
{
if (iface_mask == 0)
{
break;
}
CanSelectMasks masks;
masks.write = iface_mask | makePendingTxMask();
{
const int select_res = callSelect(masks, blocking_deadline);
// Building the list of next pending frames per iface.
// The driver will give them a scrutinizing look before deciding whether he wants to accept them.
const CanFrame* pending_tx[MaxCanIfaces] = {};
for (int i = 0; i < num_ifaces; i++)
{
CanTxQueue& q = *tx_queues_[i];
if (iface_mask & (1 << i)) // I hate myself so much right now.
{
pending_tx[i] = q.topPriorityHigherOrEqual(frame) ? q.getTopPriorityPendingFrame() : &frame;
}
else
{
pending_tx[i] = q.getTopPriorityPendingFrame();
}
}
const int select_res = callSelect(masks, pending_tx, blocking_deadline);
if (select_res < 0)
{
return -ErrDriver;
@@ -422,7 +447,13 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
masks.write = makePendingTxMask();
masks.read = uint8_t((1 << num_ifaces) - 1);
{
const int select_res = callSelect(masks, blocking_deadline);
const CanFrame* pending_tx[MaxCanIfaces] = {};
for (int i = 0; i < num_ifaces; i++) // Dear compiler, kindly unroll this. Thanks.
{
pending_tx[i] = tx_queues_[i]->getTopPriorityPendingFrame();
}
const int select_res = callSelect(masks, pending_tx, blocking_deadline);
if (select_res < 0)
{
return -ErrDriver;
@@ -449,6 +480,7 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
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)
{
@@ -456,6 +488,7 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
continue;
}
out_frame.iface_index = i;
if ((res > 0) && !(out_flags & CanIOFlagLoopback))
{
counters_[i].frames_rx += 1;
+3 -1
View File
@@ -75,7 +75,9 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
virtual uavcan::uint8_t getNumIfaces() const { return 1; }
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline)
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline)
{
if (inout_masks.read == 1)
{
+20 -1
View File
@@ -54,6 +54,7 @@ public:
uint64_t num_errors;
uavcan::ISystemClock& iclock;
bool enable_utc_timestamping;
uavcan::CanFrame pending_tx;
CanIfaceMock(uavcan::ISystemClock& iclock)
: writeable(true)
@@ -88,6 +89,17 @@ public:
return (frame_time.frame == frame) && (frame_time.time == tx_deadline);
}
bool matchPendingTx(const uavcan::CanFrame& frame) const
{
if (pending_tx != frame)
{
std::cout << "Pending TX mismatch: \n"
<< " Expected: " << frame.toString(uavcan::CanFrame::StrAligned) << "\n"
<< " Actual: " << pending_tx.toString(uavcan::CanFrame::StrAligned) << std::endl;
}
return pending_tx == frame;
}
bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline_usec)
{
return matchAndPopTx(frame, uavcan::MonotonicTime::fromUSec(tx_deadline_usec));
@@ -196,11 +208,18 @@ public:
}
}
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime deadline)
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;
+10 -6
View File
@@ -13,13 +13,15 @@ TEST(CanDriverMock, Basic)
SystemClockMock clockmock;
CanDriverMock driver(3, clockmock);
const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { };
ASSERT_EQ(3, driver.getNumIfaces());
// All WR, no RD
CanSelectMasks masks;
masks.write = 7;
masks.read = 7;
EXPECT_LT(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(7, masks.write);
EXPECT_EQ(0, masks.read);
@@ -31,7 +33,7 @@ TEST(CanDriverMock, Basic)
// No WR, no RD
masks.write = 7;
masks.read = 7;
EXPECT_EQ(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(0, masks.write);
EXPECT_EQ(0, masks.read);
EXPECT_EQ(100, clockmock.monotonic);
@@ -42,7 +44,7 @@ TEST(CanDriverMock, Basic)
driver.ifaces.at(1).pushRx(fr1);
masks.write = 7;
masks.read = 6;
EXPECT_LT(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(0, masks.write);
EXPECT_EQ(2, masks.read);
CanFrame fr2;
@@ -60,7 +62,7 @@ TEST(CanDriverMock, Basic)
driver.select_failure = true;
masks.write = 1;
masks.read = 7;
EXPECT_EQ(-1, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(-1, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(1, masks.write); // Leaving masks unchanged - the library must ignore them
EXPECT_EQ(7, masks.read);
}
@@ -73,10 +75,12 @@ TEST(CanDriverMock, Loopback)
SystemClockMock clockmock;
CanDriverMock driver(1, clockmock);
const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { };
CanSelectMasks masks;
masks.write = 1;
masks.read = 1;
EXPECT_LT(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(1, masks.write);
EXPECT_EQ(0, masks.read);
@@ -88,7 +92,7 @@ TEST(CanDriverMock, Loopback)
masks.write = 0;
masks.read = 1;
EXPECT_LT(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(0, masks.write);
EXPECT_EQ(1, masks.read);
+27
View File
@@ -141,9 +141,13 @@ TEST(CanIOManager, Transmission)
EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent, flags)); // To #1 only
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame()));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
EXPECT_EQ(0, clockmock.monotonic);
EXPECT_EQ(0, clockmock.utc);
@@ -166,11 +170,15 @@ TEST(CanIOManager, Transmission)
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
EXPECT_EQ(1, pool.getNumUsedBlocks()); // One frame went into TX queue, and will expire soon
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // This one will persist
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // This will drop off on the second select()
// Sending to both, both blocked
driver.ifaces.at(1).writeable = false;
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Still 0
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); // 1!!
// Sending to #0, both blocked
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent, flags));
@@ -179,6 +187,8 @@ TEST(CanIOManager, Transmission)
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
EXPECT_EQ(4, pool.getNumUsedBlocks());
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
// At this time TX queues are containing the following data:
// iface 0: frames[0] (EXPIRED), frames[1], frames[2]
@@ -193,6 +203,8 @@ TEST(CanIOManager, Transmission)
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Expired but still will be reported
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
// Calling receive() to flush the rest two frames
uavcan::CanRxFrame dummy_rx_frame;
@@ -200,6 +212,8 @@ TEST(CanIOManager, Transmission)
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
ASSERT_EQ(0, flags);
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
// Final checks
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
@@ -220,6 +234,9 @@ TEST(CanIOManager, Transmission)
// One frame kicked here:
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
// State checks
EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full
EXPECT_EQ(1200, clockmock.monotonic);
@@ -241,12 +258,16 @@ TEST(CanIOManager, Transmission)
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
ASSERT_EQ(0, flags);
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags));
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE)
ASSERT_EQ(0, flags);
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[2]));
// State checks
EXPECT_EQ(0, pool.getNumUsedBlocks()); // TX queue is empty
@@ -268,6 +289,8 @@ TEST(CanIOManager, Transmission)
EXPECT_EQ(1200, clockmock.monotonic);
EXPECT_EQ(1200, clockmock.utc);
ASSERT_EQ(0, flags);
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
// Transmission failure
driver.select_failure = false;
@@ -277,6 +300,8 @@ TEST(CanIOManager, Transmission)
driver.ifaces.at(1).tx_failure = true;
// Non-blocking - return < 0
EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered
@@ -288,6 +313,8 @@ TEST(CanIOManager, Transmission)
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted
ASSERT_EQ(0, flags);
EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame())); // Last call will be receive-only,
EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // hence empty TX
/*
* Perf counters
@@ -310,7 +310,9 @@ class VirtualCanDriver : public uavcan::ICanDriver,
/**
* This and other methods of ICanDriver will be invoked by the sub-node thread.
*/
int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) override
int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override
{
bool need_block = (inout_masks.write == 0); // Write queue is infinite
for (unsigned i = 0; need_block && (i < num_ifaces_); i++)
+6 -4
View File
@@ -236,6 +236,8 @@ static void testDriver(const std::vector<std::string>& iface_names)
ENFORCE(nullptr == driver.getIface(255));
ENFORCE(nullptr == driver.getIface(driver.getNumIfaces()));
const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = {};
const unsigned AllIfacesMask = (1 << driver.getNumIfaces()) - 1;
/*
@@ -243,7 +245,7 @@ static void testDriver(const std::vector<std::string>& iface_names)
*/
std::cout << "select() 1" << std::endl;
uavcan::CanSelectMasks masks; // Driver provides masks for all available events
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == 0);
ENFORCE(masks.write == AllIfacesMask);
@@ -253,7 +255,7 @@ static void testDriver(const std::vector<std::string>& iface_names)
}
std::cout << "select() 2" << std::endl;
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == 0);
ENFORCE(masks.write == AllIfacesMask);
@@ -269,7 +271,7 @@ static void testDriver(const std::vector<std::string>& iface_names)
}
std::cout << "select() 3" << std::endl;
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == AllIfacesMask);
ENFORCE(masks.write == AllIfacesMask);
@@ -294,7 +296,7 @@ static void testDriver(const std::vector<std::string>& iface_names)
std::cout << "select() 4" << std::endl;
masks.write = 0;
ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
ENFORCE(masks.read == 0);
ENFORCE(masks.write == AllIfacesMask);
@@ -532,7 +532,9 @@ public:
* early returns.
* Also it can return more events than were originally requested by uavcan, which is also acceptable.
*/
std::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) override
std::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override
{
// Detecting whether we need to block at all
bool need_block = (inout_masks.write == 0); // Write queue is infinite