io.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include "can.hpp"
00007 
00008 static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame,
00009                           uint64_t timestamp_usec, int iface_index)
00010 {
00011     if (static_cast<const uavcan::CanFrame&>(rxframe) != frame)
00012     {
00013         std::cout << "Frame mismatch:\n"
00014                   << "    " << rxframe.toString(uavcan::CanFrame::StrAligned) << "\n"
00015                   << "    " << frame.toString(uavcan::CanFrame::StrAligned) << std::endl;
00016     }
00017     return (static_cast<const uavcan::CanFrame&>(rxframe) == frame) &&
00018            (rxframe.ts_mono == uavcan::MonotonicTime::fromUSec(timestamp_usec)) &&
00019            (rxframe.iface_index == iface_index);
00020 }
00021 
00022 TEST(CanIOManager, Reception)
00023 {
00024     // Memory
00025     uavcan::PoolAllocator<sizeof(uavcan::CanTxQueue::Entry) * 4, sizeof(uavcan::CanTxQueue::Entry)> pool;
00026 
00027     // Platform interface
00028     SystemClockMock clockmock;
00029     CanDriverMock driver(2, clockmock);
00030 
00031     // IO Manager
00032     uavcan::CanIOManager iomgr(driver, pool, clockmock);
00033     ASSERT_EQ(2, iomgr.getNumIfaces());
00034 
00035     /*
00036      * Empty, will time out
00037      */
00038     uavcan::CanRxFrame frame;
00039     uavcan::CanIOFlags flags = uavcan::CanIOFlags();
00040     EXPECT_EQ(0, iomgr.receive(frame, tsMono(100), flags));
00041     EXPECT_EQ(0, flags);
00042     EXPECT_EQ(100, clockmock.monotonic);
00043     EXPECT_EQ(100, clockmock.utc);
00044 
00045     /*
00046      * Non empty from multiple ifaces
00047      */
00048     const uavcan::CanFrame frames[2][3] = {
00049         { makeCanFrame(1, "a0", EXT),    makeCanFrame(99, "a1", EXT),  makeCanFrame(803, "a2", STD) },
00050         { makeCanFrame(6341, "b0", EXT), makeCanFrame(196, "b1", STD), makeCanFrame(73, "b2", EXT) },
00051     };
00052 
00053     clockmock.advance(10);
00054     driver.ifaces.at(0).pushRx(frames[0][0]);  // Timestamp 110
00055     driver.ifaces.at(1).pushRx(frames[1][0]);
00056     clockmock.advance(10);
00057     driver.ifaces.at(0).pushRx(frames[0][1]);  // Timestamp 120
00058     driver.ifaces.at(1).pushRx(frames[1][1]);
00059     clockmock.advance(10);
00060     driver.ifaces.at(0).pushRx(frames[0][2]);  // Timestamp 130
00061     driver.ifaces.at(1).pushRx(frames[1][2]);
00062     clockmock.advance(10);
00063 
00064     EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00065     EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0));
00066     EXPECT_EQ(0, flags);
00067 
00068     EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00069     EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0));
00070     EXPECT_EQ(0, flags);
00071 
00072     EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00073     EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0));
00074     EXPECT_EQ(0, flags);
00075 
00076     EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00077     EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1));
00078     EXPECT_EQ(0, flags);
00079 
00080     EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00081     EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1));
00082     EXPECT_EQ(0, flags);
00083 
00084     EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00085     EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1));
00086     EXPECT_EQ(0, flags);
00087 
00088     EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime(), flags));  // Will time out
00089     EXPECT_EQ(0, flags);
00090 
00091     /*
00092      * Perf counters
00093      */
00094     driver.select_failure = true;
00095     EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00096 
00097     driver.select_failure = false;
00098     driver.ifaces.at(1).pushRx(frames[0][0]);
00099     driver.ifaces.at(1).rx_failure = true;
00100     EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
00101 
00102     driver.ifaces.at(0).num_errors = 9000;
00103     driver.ifaces.at(1).num_errors = 100500;
00104     EXPECT_EQ(9000, iomgr.getIfacePerfCounters(0).errors);
00105     EXPECT_EQ(100500, iomgr.getIfacePerfCounters(1).errors);
00106 
00107     EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_rx);
00108     EXPECT_EQ(3, iomgr.getIfacePerfCounters(1).frames_rx);
00109 
00110     EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_tx);
00111     EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx);
00112 }
00113 
00114 TEST(CanIOManager, Transmission)
00115 {
00116     using uavcan::CanIOManager;
00117     using uavcan::CanTxQueue;
00118 
00119     // Memory
00120     uavcan::PoolAllocator<sizeof(CanTxQueue::Entry) * 4, sizeof(CanTxQueue::Entry)> pool;
00121 
00122     // Platform interface
00123     SystemClockMock clockmock;
00124     CanDriverMock driver(2, clockmock);
00125 
00126     // IO Manager
00127     CanIOManager iomgr(driver, pool, clockmock, 9999);
00128     ASSERT_EQ(2, iomgr.getNumIfaces());
00129 
00130     const int ALL_IFACES_MASK = 3;
00131 
00132     const uavcan::CanFrame frames[] = {
00133         makeCanFrame(1, "a0", EXT),    makeCanFrame(99, "a1", EXT),  makeCanFrame(803, "a2", STD)
00134     };
00135 
00136     uavcan::CanIOFlags flags = uavcan::CanIOFlags();
00137 
00138     /*
00139      * Simple transmission
00140      */
00141     EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
00142     EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
00143     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
00144     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
00145     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
00146 
00147     EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent, flags));  // To #1 only
00148     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
00149     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame()));
00150     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
00151 
00152     EXPECT_EQ(0, clockmock.monotonic);
00153     EXPECT_EQ(0, clockmock.utc);
00154     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00155     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00156     EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).errors);
00157     EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors);
00158 
00159     /*
00160      * TX Queue basics
00161      */
00162     EXPECT_EQ(0, pool.getNumUsedBlocks());
00163 
00164     // Sending to both, #0 blocked
00165     driver.ifaces.at(0).writeable = false;
00166     EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
00167     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201));
00168     EXPECT_EQ(200, clockmock.monotonic);
00169     EXPECT_EQ(200, clockmock.utc);
00170     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00171     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00172     EXPECT_EQ(1, pool.getNumUsedBlocks());          // One frame went into TX queue, and will expire soon
00173     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));          // This one will persist
00174     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // This will drop off on the second select()
00175 
00176     // Sending to both, both blocked
00177     driver.ifaces.at(1).writeable = false;
00178     EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
00179     EXPECT_EQ(3, pool.getNumUsedBlocks());          // Total 3 frames in TX queue now
00180     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Still 0
00181     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); // 1!!
00182 
00183     // Sending to #0, both blocked
00184     EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent, flags));
00185     EXPECT_EQ(400, clockmock.monotonic);
00186     EXPECT_EQ(400, clockmock.utc);
00187     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00188     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00189     EXPECT_EQ(4, pool.getNumUsedBlocks());
00190     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
00191     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
00192 
00193     // At this time TX queues are containing the following data:
00194     // iface 0: frames[0] (EXPIRED), frames[1], frames[2]
00195     // iface 1: frames[1]
00196 
00197     // Sending to #1, both writeable
00198     driver.ifaces.at(0).writeable = true;
00199     driver.ifaces.at(1).writeable = true;
00200     // One frame per each iface will be sent:
00201     EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent, flags));
00202     EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777));   // Note that frame[0] on iface #0 has expired
00203     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999));   // In different order due to prioritization
00204     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00205     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00206     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));       // Expired but still will be reported
00207     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
00208 
00209     // Calling receive() to flush the rest two frames
00210     uavcan::CanRxFrame dummy_rx_frame;
00211     EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0), flags));
00212     EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
00213     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
00214     ASSERT_EQ(0, flags);
00215     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2]));
00216     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1]));
00217 
00218     // Final checks
00219     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00220     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00221     EXPECT_EQ(0, pool.getNumUsedBlocks());              // Make sure the memory was properly released
00222     EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors); // This is because of expired frame[0]
00223     EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors);
00224 
00225     /*
00226      * TX Queue updates from receive() call
00227      */
00228     driver.ifaces.at(0).writeable = false;
00229     driver.ifaces.at(1).writeable = false;
00230 
00231     // Sending 5 frames, one will be rejected
00232     EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
00233     EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent, flags));
00234     // One frame kicked here:
00235     EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
00236 
00237     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1]));
00238     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
00239 
00240     // State checks
00241     EXPECT_EQ(4, pool.getNumUsedBlocks());          // TX queue is full
00242     EXPECT_EQ(1200, clockmock.monotonic);
00243     EXPECT_EQ(1200, clockmock.utc);
00244     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00245     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00246 
00247     // Preparing the driver mock for receive() call
00248     driver.ifaces.at(0).writeable = true;
00249     driver.ifaces.at(1).writeable = true;
00250     const uavcan::CanFrame rx_frames[] = { makeCanFrame(123, "rx0", STD), makeCanFrame(321, "rx1", EXT) };
00251     driver.ifaces.at(0).pushRx(rx_frames[0]);
00252     driver.ifaces.at(1).pushRx(rx_frames[1]);
00253 
00254     // This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK)
00255     uavcan::CanRxFrame rx_frame;
00256     EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags));                         // Non-blocking
00257     EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0));
00258     EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
00259     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
00260     ASSERT_EQ(0, flags);
00261     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1]));
00262     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
00263 
00264     EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags));
00265     EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
00266     EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
00267     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222));  // Iface #1, frame[1] was rejected (VOLATILE)
00268     ASSERT_EQ(0, flags);
00269     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2]));
00270     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[2]));
00271 
00272     // State checks
00273     EXPECT_EQ(0, pool.getNumUsedBlocks());          // TX queue is empty
00274     EXPECT_EQ(1200, clockmock.monotonic);
00275     EXPECT_EQ(1200, clockmock.utc);
00276     EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
00277     EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
00278     EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors);
00279     EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).errors); // This is because of rejected frame[1]
00280 
00281     /*
00282      * Error handling
00283      */
00284     // Select failure
00285     driver.select_failure = true;
00286     EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(rx_frame, tsMono(2000), flags));
00287     EXPECT_EQ(-uavcan::ErrDriver,
00288               iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
00289     EXPECT_EQ(1200, clockmock.monotonic);
00290     EXPECT_EQ(1200, clockmock.utc);
00291     ASSERT_EQ(0, flags);
00292     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
00293     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
00294 
00295     // Transmission failure
00296     driver.select_failure = false;
00297     driver.ifaces.at(0).writeable = true;
00298     driver.ifaces.at(1).writeable = true;
00299     driver.ifaces.at(0).tx_failure = true;
00300     driver.ifaces.at(1).tx_failure = true;
00301     // Non-blocking - return < 0
00302     EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
00303     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0]));
00304     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0]));
00305 
00306     ASSERT_EQ(2, pool.getNumUsedBlocks());               // Untransmitted frames will be buffered
00307 
00308     // Failure removed - transmission shall proceed
00309     driver.ifaces.at(0).tx_failure = false;
00310     driver.ifaces.at(1).tx_failure = false;
00311     EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500), flags));
00312     EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200));
00313     EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
00314     EXPECT_EQ(0, pool.getNumUsedBlocks());               // All transmitted
00315     ASSERT_EQ(0, flags);
00316     EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame()));        // Last call will be receive-only,
00317     EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame()));        // hence empty TX
00318 
00319     /*
00320      * Perf counters
00321      */
00322     EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).frames_rx);
00323     EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).frames_rx);
00324 
00325     EXPECT_EQ(6, iomgr.getIfacePerfCounters(0).frames_tx);
00326     EXPECT_EQ(8, iomgr.getIfacePerfCounters(1).frames_tx);
00327 }
00328 
00329 TEST(CanIOManager, Loopback)
00330 {
00331     using uavcan::CanIOManager;
00332     using uavcan::CanTxQueue;
00333     using uavcan::CanFrame;
00334     using uavcan::CanRxFrame;
00335 
00336     // Memory
00337     uavcan::PoolAllocator<sizeof(CanTxQueue::Entry) * 4, sizeof(CanTxQueue::Entry)> pool;
00338 
00339     // Platform interface
00340     SystemClockMock clockmock;
00341     CanDriverMock driver(2, clockmock);
00342 
00343     // IO Manager
00344     CanIOManager iomgr(driver, pool, clockmock);
00345     ASSERT_EQ(2, iomgr.getNumIfaces());
00346 
00347     CanFrame fr1;
00348     fr1.id = 123 | CanFrame::FlagEFF;
00349 
00350     CanFrame fr2;
00351     fr2.id = 456 | CanFrame::FlagEFF;
00352 
00353     CanRxFrame rfr1;
00354     CanRxFrame rfr2;
00355 
00356     uavcan::CanIOFlags flags = 0;
00357     ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback));
00358     ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags));
00359     ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
00360     ASSERT_TRUE(rfr1 == fr1);
00361 
00362     flags = 0;
00363     ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback));
00364     ASSERT_EQ(1, iomgr.send(fr2, tsMono(1000), tsMono(0), 1, CanTxQueue::Persistent, uavcan::CanIOFlagLoopback));
00365     ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags));
00366     ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
00367     ASSERT_LE(0, iomgr.receive(rfr2, tsMono(100), flags));
00368     ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
00369     ASSERT_TRUE(rfr1 == fr1);
00370     ASSERT_TRUE(rfr2 == fr2);
00371 
00372     /*
00373      * Perf counters
00374      * Loopback frames are not registered as RX
00375      */
00376     EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_rx);
00377     EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_rx);
00378 
00379     EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_tx);
00380     EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx);
00381 }
00382 
00383 TEST(CanIOManager, Size)
00384 {
00385     std::cout << sizeof(uavcan::CanIOManager) << std::endl;
00386 }