diff --git a/libuavcan/CMakeLists.txt b/libuavcan/CMakeLists.txt
index 3f09304379..05a7c1ec26 100644
--- a/libuavcan/CMakeLists.txt
+++ b/libuavcan/CMakeLists.txt
@@ -44,11 +44,12 @@ if (GTEST_FOUND)
     add_dependencies(libuavcan_test uavcan)
 
     set_target_properties(libuavcan_test PROPERTIES
-        COMPILE_FLAGS "-fno-rtti -fno-exceptions -Wno-unused-parameter -Wno-unused-function"
+        COMPILE_FLAGS "-fno-exceptions -Wno-unused-parameter -Wno-unused-function"
     )
 
     target_link_libraries(libuavcan_test ${GTEST_BOTH_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
     target_link_libraries(libuavcan_test ${CMAKE_BINARY_DIR}/libuavcan.so)
+    target_link_libraries(libuavcan_test rt)
 
     # DSDL compiler invocation
     add_custom_target(dsdlc dsdl_compiler/dsdlc.py -v
diff --git a/libuavcan/test/common.hpp b/libuavcan/test/common.hpp
index 2fb015cdca..5870b35d56 100644
--- a/libuavcan/test/common.hpp
+++ b/libuavcan/test/common.hpp
@@ -7,6 +7,8 @@
 #include <cassert>
 #include <uavcan/can_driver.hpp>
 #include <uavcan/system_clock.hpp>
+#include <time.h>
+#include <sys/time.h>
 
 class SystemClockMock : public uavcan::ISystemClock
 {
@@ -47,6 +49,41 @@ public:
     }
 };
 
+
+class SystemClockDriver : public uavcan::ISystemClock
+{
+public:
+    uint64_t getMonotonicMicroseconds() const
+    {
+        struct timespec ts;
+        const int ret = clock_gettime(CLOCK_MONOTONIC, &ts);
+        if (ret != 0)
+        {
+            assert(0);
+            return 0;
+        }
+        return uint64_t(ts.tv_sec) * 1000000UL + ts.tv_nsec / 1000UL;
+    }
+
+    uint64_t getUtcMicroseconds() const
+    {
+        struct timeval tv;
+        const int ret = gettimeofday(&tv, NULL);
+        if (ret != 0)
+        {
+            assert(0);
+            return 0;
+        }
+        return uint64_t(tv.tv_sec) * 1000000UL + tv.tv_usec;
+    }
+
+    void adjustUtcMicroseconds(uint64_t new_timestamp_usec, int64_t offset_usec)
+    {
+        assert(0);
+    }
+};
+
+
 enum FrameType { STD, EXT };
 static uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type)
 {
diff --git a/libuavcan/test/transport/can/iface_mock.hpp b/libuavcan/test/transport/can/iface_mock.hpp
index 3ec0a49860..27c9716070 100644
--- a/libuavcan/test/transport/can/iface_mock.hpp
+++ b/libuavcan/test/transport/can/iface_mock.hpp
@@ -32,19 +32,19 @@ public:
     bool tx_failure;
     bool rx_failure;
     uint64_t num_errors;
-    SystemClockMock& clockmock;
+    uavcan::ISystemClock& iclock;
 
-    CanIfaceMock(SystemClockMock& clockmock)
+    CanIfaceMock(uavcan::ISystemClock& iclock)
     : writeable(true)
     , tx_failure(false)
     , rx_failure(false)
     , num_errors(0)
-    , clockmock(clockmock)
+    , iclock(iclock)
     { }
 
     void pushRx(const uavcan::CanFrame& frame)
     {
-        rx.push(FrameWithTime(frame, clockmock.monotonic));
+        rx.push(FrameWithTime(frame, iclock.getMonotonicMicroseconds()));
     }
 
     void pushRx(const uavcan::RxFrame& frame)
@@ -74,7 +74,7 @@ public:
             return -1;
         if (!writeable)
             return 0;
-        const uint64_t monotonic_deadline = tx_timeout_usec + clockmock.monotonic;
+        const uint64_t monotonic_deadline = tx_timeout_usec + iclock.getMonotonicMicroseconds();
         tx.push(FrameWithTime(frame, monotonic_deadline));
         return 1;
     }
@@ -107,19 +107,19 @@ class CanDriverMock : public uavcan::ICanDriver
 {
 public:
     std::vector<CanIfaceMock> ifaces;
-    SystemClockMock& clockmock;
+    uavcan::ISystemClock& iclock;
     bool select_failure;
 
-    CanDriverMock(int num_ifaces, SystemClockMock& clockmock)
-    : ifaces(num_ifaces, CanIfaceMock(clockmock))
-    , clockmock(clockmock)
+    CanDriverMock(int num_ifaces, uavcan::ISystemClock& iclock)
+    : ifaces(num_ifaces, CanIfaceMock(iclock))
+    , iclock(iclock)
     , select_failure(false)
     { }
 
     int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uint64_t timeout_usec)
     {
         assert(this);
-        std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
+        //std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
 
         if (select_failure)
             return -1;
@@ -142,7 +142,11 @@ public:
         inout_read_iface_mask = out_read_mask;
         if ((out_write_mask | out_read_mask) == 0)
         {
-            clockmock.advance(timeout_usec);   // Emulating timeout
+            SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
+            if (mock)
+                mock->advance(timeout_usec);   // Emulating timeout
+            else
+                usleep(timeout_usec);
             return 0;
         }
         return 1;  // This value is not being checked anyway, it just has to be greater than zero