diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
index f23e51e180..1bf3ab3f57 100644
--- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
+++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
@@ -159,7 +159,7 @@ public:
     void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
 
     bool isTxBufferFull() const;
-    bool isRxBufferEmpty() const { return rx_queue_.getLength() == 0; }
+    bool isRxBufferEmpty() const;
 
     virtual uavcan::uint64_t getErrorCount() const;
     uavcan::uint8_t yieldLastHardwareErrorCode();
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
index 4eda4d3195..d7116bcc0c 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
@@ -23,13 +23,13 @@ CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] =
 
 inline void handleTxInterrupt(uavcan::uint8_t iface_index)
 {
+    assert(iface_index < UAVCAN_STM32_NUM_IFACES);
     uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
     if (utc_usec > 0)
     {
         utc_usec--;
     }
-    if (iface_index < UAVCAN_STM32_NUM_IFACES &&
-        ifaces[iface_index] != NULL)
+    if (ifaces[iface_index] != NULL)
     {
         ifaces[iface_index]->handleTxInterrupt(utc_usec);
     }
@@ -41,13 +41,13 @@ inline void handleTxInterrupt(uavcan::uint8_t iface_index)
 
 inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index)
 {
+    assert(iface_index < UAVCAN_STM32_NUM_IFACES);
     uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
     if (utc_usec > 0)
     {
         utc_usec--;
     }
-    if (iface_index < UAVCAN_STM32_NUM_IFACES &&
-        ifaces[iface_index] != NULL)
+    if (ifaces[iface_index] != NULL)
     {
         ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec);
     }
@@ -59,8 +59,8 @@ inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_
 
 inline void handleStatusInterrupt(uavcan::uint8_t iface_index)
 {
-    if (iface_index < UAVCAN_STM32_NUM_IFACES &&
-        ifaces[iface_index] != NULL)
+    assert(iface_index < UAVCAN_STM32_NUM_IFACES);
+    if (ifaces[iface_index] != NULL)
     {
         ifaces[iface_index]->handleStatusInterrupt();
     }
@@ -183,11 +183,6 @@ int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out
     out_timings.sjw = 1;
     out_timings.bs1 = bs1 - 1;
     out_timings.bs2 = bs2 - 1;
-
-    UAVCAN_STM32_TRACE("CAN pclk=%lu bitrt=%lu presc=%u sjw=%u bs1=%u bs2=%u",
-                       static_cast<unsigned long>(pclk), static_cast<unsigned long>(target_bitrate),
-                       static_cast<unsigned>(out_timings.prescaler), static_cast<unsigned>(out_timings.sjw),
-                       static_cast<unsigned>(out_timings.bs1), static_cast<unsigned>(out_timings.bs2));
     return 0;
 }
 
@@ -504,7 +499,13 @@ void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
 
 bool CanIface::isTxBufferFull() const
 {
-    return (can_->TSR & CAN_TSR_TME) == 0;
+    return (can_->TSR & CAN_TSR_TME) == 0;  // Interrupts enabled
+}
+
+bool CanIface::isRxBufferEmpty() const
+{
+    CriticalSectionLock lock;
+    return rx_queue_.getLength() == 0;
 }
 
 uavcan::uint64_t CanIface::getErrorCount() const
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
index c7a8809758..da159ff4fc 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
@@ -47,21 +47,18 @@ const uavcan::int32_t MaxUtcSpeedCorrection = 500; // x / 65536
 
 void init()
 {
+    CriticalSectionLock lock;
     if (initialized)
     {
         return;
     }
     initialized = true;
 
-    chSysDisable();
-
     // Power-on and reset
     TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
     TIMX_RCC_RSTR |=  TIMX_RCC_RSTR_MASK;
     TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK;
 
-    chSysEnable();
-
     // Enable IRQ
     nvicEnableVector(TIMX_IRQn,  UAVCAN_STM32_IRQ_PRIORITY_MASK);
 
@@ -175,16 +172,18 @@ void adjustUtc(uavcan::UtcDuration adjustment)
      */
     if (adjustment.getAbs().toMSec() > 1 || !utc_set)
     {
-        if (adjustment.isNegative() &&
-            uavcan::uint64_t(adjustment.getAbs().toUSec()) > time_utc)
+        const uavcan::int64_t adj_usec = adjustment.toUSec();
+
         {
             CriticalSectionLock locker;
-            time_utc = 1;
-        }
-        else
-        {
-            CriticalSectionLock locker;
-            time_utc += adjustment.toUSec();
+            if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
+            {
+                time_utc = 1;
+            }
+            else
+            {
+                time_utc += adj_usec;
+            }
         }
 
         if (utc_set)