test_socket.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include <vector>
00007 #include <cerrno>
00008 #include <uavcan_linux/uavcan_linux.hpp>
00009 #include "debug.hpp"
00010 
00011 static uavcan::CanFrame makeFrame(std::uint32_t id, const std::string& data)
00012 {
00013     return uavcan::CanFrame(id, reinterpret_cast<const std::uint8_t*>(data.c_str()), data.length());
00014 }
00015 
00016 static uavcan::MonotonicTime tsMonoOffsetMs(std::int64_t ms)
00017 {
00018     return uavcan_linux::SystemClock().getMonotonic() + uavcan::MonotonicDuration::fromMSec(ms);
00019 }
00020 
00021 static void testNonexistentIface()
00022 {
00023     const int sock1 = uavcan_linux::SocketCanIface::openSocket("noif9");
00024     ENFORCE(sock1 < 0);
00025     const int sock2 = uavcan_linux::SocketCanIface::openSocket("verylongifacenameverylongifacenameverylongifacename");
00026     ENFORCE(sock2 < 0);
00027 }
00028 
00029 static void testSocketRxTx(const std::string& iface_name)
00030 {
00031     const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
00032     const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
00033     ENFORCE(sock1 >= 0 && sock2 >= 0);
00034 
00035     /*
00036      * Clocks will have some offset from the true system time
00037      * SocketCAN driver must handle this correctly
00038      */
00039     uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
00040     clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(100000));
00041     const uavcan_linux::SystemClock& clock = clock_impl;
00042 
00043     uavcan_linux::SocketCanIface if1(clock, sock1);
00044     uavcan_linux::SocketCanIface if2(clock, sock2);
00045 
00046     /*
00047      * Sending two frames, one of which must be returned back
00048      */
00049     ENFORCE(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
00050     ENFORCE(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
00051     if1.poll(true, true);
00052     if1.poll(true, true);
00053     ENFORCE(0 == if1.getErrorCount());
00054     ENFORCE(!if1.hasReadyTx());
00055     ENFORCE(if1.hasReadyRx());       // Second loopback
00056 
00057     /*
00058      * Second iface, same thing
00059      */
00060     ENFORCE(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
00061     ENFORCE(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
00062     ENFORCE(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(-1), uavcan::CanIOFlagLoopback));  // Will timeout
00063     if2.poll(true, true);
00064     if2.poll(true, true);
00065     ENFORCE(1 == if2.getErrorCount());  // One timed out
00066     ENFORCE(!if2.hasReadyTx());
00067     ENFORCE(if2.hasReadyRx());
00068 
00069     /*
00070      * No-op
00071      */
00072     if1.poll(true, true);
00073     if2.poll(true, true);
00074 
00075     uavcan::CanFrame frame;
00076     uavcan::MonotonicTime ts_mono;
00077     uavcan::UtcTime ts_utc;
00078     uavcan::CanIOFlags flags = 0;
00079 
00080     /*
00081      * Read first
00082      */
00083     ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
00084     ENFORCE(frame == makeFrame(456, "if1-2"));
00085     ENFORCE(flags == uavcan::CanIOFlagLoopback);
00086     ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00087     ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00088 
00089     ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
00090     ENFORCE(frame == makeFrame(321, "if2-1"));
00091     ENFORCE(flags == 0);
00092     ENFORCE(!ts_mono.isZero());
00093     ENFORCE(!ts_utc.isZero());
00094     ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00095     ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00096 
00097     ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
00098     ENFORCE(frame == makeFrame(654, "if2-2"));
00099     ENFORCE(flags == 0);
00100     ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00101     ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00102 
00103     ENFORCE(0 == if1.receive(frame, ts_mono, ts_utc, flags));
00104     ENFORCE(!if1.hasReadyTx());
00105     ENFORCE(!if1.hasReadyRx());
00106 
00107     /*
00108      * Read second
00109      */
00110     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00111     ENFORCE(frame == makeFrame(123, "if1-1"));
00112     ENFORCE(flags == 0);
00113     ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00114     ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00115 
00116     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00117     ENFORCE(frame == makeFrame(456, "if1-2"));
00118     ENFORCE(flags == 0);
00119     ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00120     ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00121 
00122     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00123     ENFORCE(frame == makeFrame(654, "if2-2"));
00124     ENFORCE(flags == uavcan::CanIOFlagLoopback);
00125     ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00126     ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00127 
00128     ENFORCE(0 == if2.receive(frame, ts_mono, ts_utc, flags));
00129     ENFORCE(!if2.hasReadyTx());
00130     ENFORCE(!if2.hasReadyRx());
00131 }
00132 
00133 static void testSocketFilters(const std::string& iface_name)
00134 {
00135     using uavcan::CanFrame;
00136 
00137     const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
00138     const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
00139     ENFORCE(sock1 >= 0 && sock2 >= 0);
00140 
00141     /*
00142      * Clocks will have some offset from the true system time
00143      * SocketCAN driver must handle this correctly
00144      */
00145     uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
00146     clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(-1000));
00147     const uavcan_linux::SystemClock& clock = clock_impl;
00148 
00149     uavcan_linux::SocketCanIface if1(clock, sock1);
00150     uavcan_linux::SocketCanIface if2(clock, sock2);
00151 
00152     /*
00153      * Configuring filters
00154      */
00155     uavcan::CanFilterConfig fcs[3];
00156     // STD/EXT 123
00157     fcs[0].id = 123;
00158     fcs[0].mask = CanFrame::MaskExtID;
00159     // Only EXT 456789
00160     fcs[1].id = 456789 | CanFrame::FlagEFF;
00161     fcs[1].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
00162     // Only STD 0
00163     fcs[2].id = 0;
00164     fcs[2].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
00165 
00166     ENFORCE(0 == if2.configureFilters(fcs, 3));
00167 
00168     /*
00169      * Sending data from 1 to 2, making sure only filtered data will be accepted
00170      */
00171     const auto EFF = CanFrame::FlagEFF;
00172     ENFORCE(1 == if1.send(makeFrame(123,          "1"), tsMonoOffsetMs(100), 0)); // Accept 0
00173     ENFORCE(1 == if1.send(makeFrame(123 | EFF,    "2"), tsMonoOffsetMs(100), 0)); // Accept 0
00174     ENFORCE(1 == if1.send(makeFrame(456,          "3"), tsMonoOffsetMs(100), 0)); // Drop
00175     ENFORCE(1 == if1.send(makeFrame(456789,       "4"), tsMonoOffsetMs(100), 0)); // Drop
00176     ENFORCE(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
00177     ENFORCE(1 == if1.send(makeFrame(0,            "6"), tsMonoOffsetMs(100), 0)); // Accept 2
00178     ENFORCE(1 == if1.send(makeFrame(EFF,          "7"), tsMonoOffsetMs(100), 0)); // Drop
00179 
00180     for (int i = 0; i < 7; i++)
00181     {
00182         if1.poll(true, true);
00183         if2.poll(true, false);
00184     }
00185     ENFORCE(!if1.hasReadyTx());
00186     ENFORCE(!if1.hasReadyRx());
00187     ENFORCE(0 == if1.getErrorCount());
00188     ENFORCE(if2.hasReadyRx());
00189 
00190     /*
00191      * Checking RX on 2
00192      */
00193     uavcan::CanFrame frame;
00194     uavcan::MonotonicTime ts_mono;
00195     uavcan::UtcTime ts_utc;
00196     uavcan::CanIOFlags flags = 0;
00197 
00198     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00199     ENFORCE(frame == makeFrame(123, "1"));
00200 
00201     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00202     ENFORCE(frame == makeFrame(123 | EFF, "2"));
00203 
00204     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00205     ENFORCE(frame == makeFrame(456789 | EFF, "5"));
00206 
00207     ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
00208     ENFORCE(frame == makeFrame(0, "6"));
00209     ENFORCE(flags == 0);
00210 
00211     ENFORCE(!if2.hasReadyRx());
00212 }
00213 
00214 static void testDriver(const std::vector<std::string>& iface_names)
00215 {
00216     /*
00217      * Clocks will have some offset from the true system time
00218      * SocketCAN driver must handle this correctly
00219      */
00220     uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
00221     clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(9000000));
00222     const uavcan_linux::SystemClock& clock = clock_impl;
00223 
00224     uavcan_linux::SocketCanDriver driver(clock);
00225     for (auto ifn : iface_names)
00226     {
00227         std::cout << "Adding iface " << ifn << std::endl;
00228         ENFORCE(0 == driver.addIface(ifn));
00229     }
00230 
00231     ENFORCE(-1 == driver.addIface("noif9"));
00232     ENFORCE(-1 == driver.addIface("noif9"));
00233     ENFORCE(-1 == driver.addIface("noif9"));
00234 
00235     ENFORCE(driver.getNumIfaces() == iface_names.size());
00236     ENFORCE(nullptr == driver.getIface(255));
00237     ENFORCE(nullptr == driver.getIface(driver.getNumIfaces()));
00238 
00239     const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = {};
00240 
00241     const unsigned AllIfacesMask = (1 << driver.getNumIfaces()) - 1;
00242 
00243     /*
00244      * Send, no loopback
00245      */
00246     std::cout << "select() 1" << std::endl;
00247     uavcan::CanSelectMasks masks;        // Driver provides masks for all available events
00248     ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
00249     ENFORCE(masks.read == 0);
00250     ENFORCE(masks.write == AllIfacesMask);
00251 
00252     for (int i = 0; i < driver.getNumIfaces(); i++)
00253     {
00254         ENFORCE(1 == driver.getIface(i)->send(makeFrame(123, std::to_string(i)), tsMonoOffsetMs(10), 0));
00255     }
00256 
00257     std::cout << "select() 2" << std::endl;
00258     ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
00259     ENFORCE(masks.read == 0);
00260     ENFORCE(masks.write == AllIfacesMask);
00261 
00262     /*
00263      * Send with loopback
00264      */
00265     for (int i = 0; i < driver.getNumIfaces(); i++)
00266     {
00267         ENFORCE(1 == driver.getIface(i)->send(makeFrame(456, std::to_string(i)), tsMonoOffsetMs(10),
00268                                               uavcan::CanIOFlagLoopback));
00269         ENFORCE(1 == driver.getIface(i)->send(makeFrame(789, std::to_string(i)), tsMonoOffsetMs(-1), // Will timeout
00270                                               uavcan::CanIOFlagLoopback));
00271     }
00272 
00273     std::cout << "select() 3" << std::endl;
00274     ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
00275     ENFORCE(masks.read == AllIfacesMask);
00276     ENFORCE(masks.write == AllIfacesMask);
00277 
00278     /*
00279      * Receive loopback
00280      */
00281     for (int i = 0; i < driver.getNumIfaces(); i++)
00282     {
00283         uavcan::CanFrame frame;
00284         uavcan::MonotonicTime ts_mono;
00285         uavcan::UtcTime ts_utc;
00286         uavcan::CanIOFlags flags = 0;
00287         ENFORCE(1 == driver.getIface(i)->receive(frame, ts_mono, ts_utc, flags));
00288         ENFORCE(frame == makeFrame(456, std::to_string(i)));
00289         ENFORCE(flags == uavcan::CanIOFlagLoopback);
00290         ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
00291         ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
00292 
00293         ENFORCE(!driver.getIface(i)->hasReadyTx());
00294         ENFORCE(!driver.getIface(i)->hasReadyRx());
00295     }
00296 
00297     std::cout << "select() 4" << std::endl;
00298     masks.write = 0;
00299     ENFORCE(driver.getNumIfaces() == driver.select(masks, pending_tx, tsMonoOffsetMs(1000)));
00300     ENFORCE(masks.read == 0);
00301     ENFORCE(masks.write == AllIfacesMask);
00302 
00303     std::cout << "exit" << std::endl;
00304 
00305     /*
00306      * Error checks
00307      */
00308     for (int i = 0; i < driver.getNumIfaces(); i++)
00309     {
00310         for (auto kv : driver.getIface(i)->getErrors())
00311         {
00312             switch (kv.first)
00313             {
00314             case uavcan_linux::SocketCanError::SocketReadFailure:
00315             case uavcan_linux::SocketCanError::SocketWriteFailure:
00316             {
00317                 ENFORCE(kv.second == 0);
00318                 break;
00319             }
00320             case uavcan_linux::SocketCanError::TxTimeout:
00321             {
00322                 ENFORCE(kv.second == 1);  // One timed out frame from the above
00323                 break;
00324             }
00325             default:
00326             {
00327                 ENFORCE(false);
00328                 break;
00329             }
00330             }
00331         }
00332     }
00333 }
00334 
00335 int main(int argc, const char** argv)
00336 {
00337     try
00338     {
00339         if (argc < 2)
00340         {
00341             std::cerr << "Usage:\n\t" << argv[0] << " <can-iface-name-1> [can-iface-name-N...]" << std::endl;
00342             return 1;
00343         }
00344 
00345         std::vector<std::string> iface_names;
00346         for (int i = 1; i < argc; i++)
00347         {
00348             iface_names.emplace_back(argv[i]);
00349         }
00350 
00351         testNonexistentIface();
00352         testSocketRxTx(iface_names[0]);
00353         testSocketFilters(iface_names[0]);
00354 
00355         testDriver(iface_names);
00356 
00357         return 0;
00358     }
00359     catch (const std::exception& ex)
00360     {
00361         std::cerr << "Exception: " << ex.what() << std::endl;
00362         return 1;
00363     }
00364 }