diff --git a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp
index 9e4816fa69..35de4619be 100644
--- a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp
+++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp
@@ -29,11 +29,20 @@ public:
      */
     static CanDriver& instance() { return self; }
 
+    /**
+     * Attempts to detect bit rate of the CAN bus.
+     * This function may block for up to X seconds, where X is the number of bit rates to try.
+     * This function is NOT guaranteed to reset the CAN controller upon return.
+     * @return On success: detected bit rate, in bits per second.
+     *         On failure: zero.
+     */
+    static uavcan::uint32_t detectBitRate(void (*idle_callback)() = nullptr);
+
     /**
      * Returns negative value if the requested baudrate can't be used.
      * Returns zero if OK.
      */
-    int init(uavcan::uint32_t baudrate);
+    int init(uavcan::uint32_t bitrate);
 
     bool hasReadyRx() const;
     bool hasEmptyTx() const;
@@ -44,26 +53,41 @@ public:
      */
     bool hadActivity();
 
-    virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
-                                 uavcan::CanIOFlags flags);
+    /**
+     * Returns the number of times the RX queue was overrun.
+     */
+    uavcan::uint32_t getRxQueueOverflowCount() const;
 
-    virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
-                                    uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
+    /**
+     * Whether the controller is currently in bus off state.
+     * Note that the driver recovers the CAN controller from the bus off state automatically!
+     * Therefore, this method serves only monitoring purposes and is not necessary to use.
+     */
+    bool isInBusOffState() const;
 
-    virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
-                                   const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
-                                   uavcan::MonotonicTime blocking_deadline);
+    uavcan::int16_t send(const uavcan::CanFrame& frame,
+                         uavcan::MonotonicTime tx_deadline,
+                         uavcan::CanIOFlags flags) override;
 
-    virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
-                                             uavcan::uint16_t num_configs);
+    uavcan::int16_t receive(uavcan::CanFrame& out_frame,
+                            uavcan::MonotonicTime& out_ts_monotonic,
+                            uavcan::UtcTime& out_ts_utc,
+                            uavcan::CanIOFlags& out_flags) override;
 
-    virtual uavcan::uint64_t getErrorCount() const;
+    uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
+                           const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
+                           uavcan::MonotonicTime blocking_deadline) override;
 
-    virtual uavcan::uint16_t getNumFilters() const;
+    uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
+                                     uavcan::uint16_t num_configs) override;
 
-    virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index);
+    uavcan::uint64_t getErrorCount() const override;
 
-    virtual uavcan::uint8_t getNumIfaces() const;
+    uavcan::uint16_t getNumFilters() const override;
+
+    uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) override;
+
+    uavcan::uint8_t getNumIfaces() const override;
 };
 
 }
diff --git a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp
index 3ff6b4d127..47f3b5fea5 100644
--- a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp
+++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp
@@ -51,9 +51,9 @@ class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
 
     SystemClock() { }
 
-    virtual uavcan::MonotonicTime getMonotonic()     const { return clock::getMonotonic(); }
-    virtual uavcan::UtcTime getUtc()                 const { return clock::getUtc(); }
-    virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
+    uavcan::MonotonicTime getMonotonic()     const override { return clock::getMonotonic(); }
+    uavcan::UtcTime getUtc()                 const override { return clock::getUtc(); }
+    void adjustUtc(uavcan::UtcDuration adjustment) override { clock::adjustUtc(adjustment); }
 
 public:
     /**
diff --git a/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp b/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp
new file mode 100644
index 0000000000..4882e65f41
--- /dev/null
+++ b/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp
@@ -0,0 +1,191 @@
+/*
+ * Bosch C_CAN controller API.
+ *
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+namespace uavcan_lpc11c24
+{
+namespace c_can
+{
+
+struct MsgIfaceType
+{
+    std::uint32_t CMDREQ;
+
+    union
+    {
+        std::uint32_t W;
+        std::uint32_t R;
+    } CMDMSK;
+
+    std::uint32_t MSK1;
+    std::uint32_t MSK2;
+
+    std::uint32_t ARB1;
+    std::uint32_t ARB2;
+
+    std::uint32_t MCTRL;
+
+    std::uint32_t DA1;
+    std::uint32_t DA2;
+    std::uint32_t DB1;
+    std::uint32_t DB2;
+
+    const std::uint32_t _skip[13];
+};
+
+static_assert(offsetof(MsgIfaceType, CMDMSK) == 0x04, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, MSK1)   == 0x08, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, ARB1)   == 0x10, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, MCTRL)  == 0x18, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, DA1)    == 0x1c, "C_CAN offset");
+static_assert(offsetof(MsgIfaceType, DB2)    == 0x28, "C_CAN offset");
+
+static_assert(sizeof(MsgIfaceType) == 96, "C_CAN size");
+
+
+struct Type
+{
+    std::uint32_t CNTL;
+    std::uint32_t STAT;
+    const std::uint32_t EC;
+    std::uint32_t BT;
+    const std::uint32_t INT;
+    std::uint32_t TEST;
+    std::uint32_t BRPE;
+
+    const std::uint32_t _skip_a[1];
+
+    MsgIfaceType IF[2];                 // [0] @ 0x020, [1] @ 0x080
+
+    const std::uint32_t _skip_b[8];
+
+    const std::uint32_t TXREQ[2];       // 0x100
+
+    const std::uint32_t _skip_c[6];
+
+    const std::uint32_t ND[2];          // 0x120
+
+    const std::uint32_t _skip_d[6];
+
+    const std::uint32_t IR[2];          // 0x140
+
+    const std::uint32_t _skip_e[6];
+
+    const std::uint32_t MSGV[2];        // 0x160
+
+    const std::uint32_t _skip_f[6];
+
+    std::uint32_t CLKDIV;               // 0x180
+};
+
+static_assert(offsetof(Type, CNTL)   == 0x000, "C_CAN offset");
+static_assert(offsetof(Type, STAT)   == 0x004, "C_CAN offset");
+static_assert(offsetof(Type, TEST)   == 0x014, "C_CAN offset");
+static_assert(offsetof(Type, BRPE)   == 0x018, "C_CAN offset");
+static_assert(offsetof(Type, IF[0])  == 0x020, "C_CAN offset");
+static_assert(offsetof(Type, IF[1])  == 0x080, "C_CAN offset");
+static_assert(offsetof(Type, TXREQ)  == 0x100, "C_CAN offset");
+static_assert(offsetof(Type, ND)     == 0x120, "C_CAN offset");
+static_assert(offsetof(Type, IR)     == 0x140, "C_CAN offset");
+static_assert(offsetof(Type, MSGV)   == 0x160, "C_CAN offset");
+static_assert(offsetof(Type, CLKDIV) == 0x180, "C_CAN offset");
+
+static_assert(offsetof(Type, IF[0].DB2) == 0x048, "C_CAN offset");
+static_assert(offsetof(Type, IF[1].DB2) == 0x0A8, "C_CAN offset");
+
+
+volatile Type& CAN = *reinterpret_cast<volatile Type*>(0x40050000);
+
+
+/*
+ * CNTL
+ */
+static constexpr std::uint32_t CNTL_TEST = 1 << 7;
+static constexpr std::uint32_t CNTL_CCE  = 1 << 6;
+static constexpr std::uint32_t CNTL_DAR  = 1 << 5;
+static constexpr std::uint32_t CNTL_EIE  = 1 << 3;
+static constexpr std::uint32_t CNTL_SIE  = 1 << 2;
+static constexpr std::uint32_t CNTL_IE   = 1 << 1;
+static constexpr std::uint32_t CNTL_INIT = 1 << 0;
+
+static constexpr std::uint32_t CNTL_IRQ_MASK = CNTL_EIE | CNTL_IE | CNTL_SIE;
+
+/*
+ * TEST
+ */
+static constexpr std::uint32_t TEST_RX       = 1 << 7;
+static constexpr std::uint32_t TEST_LBACK    = 1 << 4;
+static constexpr std::uint32_t TEST_SILENT   = 1 << 3;
+static constexpr std::uint32_t TEST_BASIC    = 1 << 2;
+static constexpr std::uint32_t TEST_TX_SHIFT = 5;
+
+enum class TestTx : std::uint32_t
+{
+    Controller      = 0,
+    SamplePoint     = 1,
+    LowDominant     = 2,
+    HighRecessive   = 3
+};
+
+/*
+ * STAT
+ */
+static constexpr std::uint32_t STAT_BOFF      = 1 << 7;
+static constexpr std::uint32_t STAT_EWARN     = 1 << 6;
+static constexpr std::uint32_t STAT_EPASS     = 1 << 5;
+static constexpr std::uint32_t STAT_RXOK      = 1 << 4;
+static constexpr std::uint32_t STAT_TXOK      = 1 << 3;
+static constexpr std::uint32_t STAT_LEC_MASK  = 7;
+static constexpr std::uint32_t STAT_LEC_SHIFT = 0;
+
+enum class StatLec : std::uint32_t
+{
+    NoError     = 0,
+    StuffError  = 1,
+    FormError   = 2,
+    AckError    = 3,
+    Bit1Error   = 4,
+    Bit0Error   = 5,
+    CRCError    = 6,
+    Unused      = 7
+};
+
+/*
+ * IF.CMDREQ
+ */
+static constexpr std::uint32_t IF_CMDREQ_BUSY = 1 << 15;
+
+/*
+ * IF.CMDMSK
+ */
+static constexpr std::uint32_t IF_CMDMSK_W_DATA_A = 1 << 0;
+static constexpr std::uint32_t IF_CMDMSK_W_DATA_B = 1 << 1;
+static constexpr std::uint32_t IF_CMDMSK_W_TXRQST = 1 << 2;
+static constexpr std::uint32_t IF_CMDMSK_W_CTRL   = 1 << 4;
+static constexpr std::uint32_t IF_CMDMSK_W_ARB    = 1 << 5;
+static constexpr std::uint32_t IF_CMDMSK_W_MASK   = 1 << 6;
+static constexpr std::uint32_t IF_CMDMSK_W_WR_RD  = 1 << 7;
+
+/*
+ * IF.MCTRL
+ */
+static constexpr std::uint32_t IF_MCTRL_NEWDAT   = 1 << 15;
+static constexpr std::uint32_t IF_MCTRL_MSGLST   = 1 << 14;
+static constexpr std::uint32_t IF_MCTRL_INTPND   = 1 << 13;
+static constexpr std::uint32_t IF_MCTRL_UMASK    = 1 << 12;
+static constexpr std::uint32_t IF_MCTRL_TXIE     = 1 << 11;
+static constexpr std::uint32_t IF_MCTRL_RXIE     = 1 << 10;
+static constexpr std::uint32_t IF_MCTRL_RMTEN    = 1 << 9;
+static constexpr std::uint32_t IF_MCTRL_TXRQST   = 1 << 8;
+static constexpr std::uint32_t IF_MCTRL_EOB      = 1 << 7;
+static constexpr std::uint32_t IF_MCTRL_DLC_MASK = 15;
+
+}
+}
diff --git a/libuavcan_drivers/lpc11c24/driver/src/can.cpp b/libuavcan_drivers/lpc11c24/driver/src/can.cpp
index fb1d80f63c..405588eb76 100644
--- a/libuavcan_drivers/lpc11c24/driver/src/can.cpp
+++ b/libuavcan_drivers/lpc11c24/driver/src/can.cpp
@@ -6,6 +6,7 @@
 #include <uavcan_lpc11c24/clock.hpp>
 #include <uavcan/util/templates.hpp>
 #include <chip.h>
+#include "c_can.hpp"
 #include "internal.hpp"
 
 /**
@@ -19,9 +20,9 @@
 # error UAVCAN_LPC11C24_RX_QUEUE_LEN is too large
 #endif
 
-extern "C" void canRxCallback(uint8_t msg_obj_num);
-extern "C" void canTxCallback(uint8_t msg_obj_num);
-extern "C" void canErrorCallback(uint32_t error_info);
+extern "C" void canRxCallback(std::uint8_t msg_obj_num);
+extern "C" void canTxCallback(std::uint8_t msg_obj_num);
+extern "C" void canErrorCallback(std::uint32_t error_info);
 
 namespace uavcan_lpc11c24
 {
@@ -34,25 +35,36 @@ namespace
  * TX priority is defined by the message object number, not by the CAN ID (chapter 16.7.3.5 of the user manual),
  * hence we can't use more than one object because that would cause priority inversion on long transfers.
  */
-const unsigned NumMsgObjects = 32;
+constexpr unsigned NumberOfMessageObjects   = 32;
+constexpr unsigned NumberOfTxMessageObjects = 1;
+constexpr unsigned NumberOfRxMessageObjects = NumberOfMessageObjects - NumberOfTxMessageObjects;
+constexpr unsigned TxMessageObjectNumber    = 1;
 
 /**
  * Total number of CAN errors.
  * Does not overflow.
  */
-uint32_t error_cnt;
+volatile std::uint32_t error_cnt = 0;
 
 /**
- * True if there's no pending TX frame, i.e. write is possible.
+ * False if there's no pending TX frame, i.e. write is possible.
  */
-bool tx_free = true;
+volatile bool tx_pending = false;
+
+/**
+ * Currently pending frame must be aborted on first error.
+ */
+volatile bool tx_abort_on_error = false;
 
 /**
  * Gets updated every time the CAN IRQ handler is being called.
  */
-uint64_t last_irq_utc_timestamp = 0;
+volatile std::uint64_t last_irq_utc_timestamp = 0;
 
-bool had_activity;
+/**
+ * Set by the driver on every successful TX or RX; reset by the application.
+ */
+volatile bool had_activity = false;
 
 /**
  * After a received message gets extracted from C_CAN, it will be stored in the RX queue until libuavcan
@@ -62,26 +74,18 @@ class RxQueue
 {
     struct Item
     {
-        uint64_t utc_usec;
+        std::uint64_t utc_usec = 0;
         uavcan::CanFrame frame;
-        Item() : utc_usec(0) { }
     };
 
     Item buf_[UAVCAN_LPC11C24_RX_QUEUE_LEN];
-    uint32_t overflow_cnt_;
-    uint8_t in_;
-    uint8_t out_;
-    uint8_t len_;
+    std::uint32_t overflow_cnt_ = 0;
+    std::uint8_t in_ = 0;
+    std::uint8_t out_ = 0;
+    std::uint8_t len_ = 0;
 
 public:
-    RxQueue()
-        : overflow_cnt_(0)
-        , in_(0)
-        , out_(0)
-        , len_(0)
-    { }
-
-    void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec)
+    void push(const uavcan::CanFrame& frame, const volatile std::uint64_t& utc_usec)
     {
         buf_[in_].frame    = frame;
         buf_[in_].utc_usec = utc_usec;
@@ -106,7 +110,7 @@ public:
         }
     }
 
-    void pop(uavcan::CanFrame& out_frame, uint64_t& out_utc_usec)
+    void pop(uavcan::CanFrame& out_frame, std::uint64_t& out_utc_usec)
     {
         if (len_ > 0)
         {
@@ -123,90 +127,200 @@ public:
 
     unsigned getLength() const { return len_; }
 
-    uint32_t getOverflowCount() const { return overflow_cnt_; }
+    std::uint32_t getOverflowCount() const { return overflow_cnt_; }
 };
 
 RxQueue rx_queue;
 
 
-int computeBaudrate(uint32_t baud_rate, uint32_t can_api_timing_cfg[2])
+struct BitTimingSettings
 {
-    const uint32_t pclk = Chip_Clock_GetMainClockRate();
-    const uint32_t clk_per_bit = pclk / baud_rate;
-    for (uint32_t div = 0; div <= 15; div++)
+    std::uint32_t canclkdiv;
+    std::uint32_t canbtr;
+
+    bool isValid() const { return canbtr != 0; }
+};
+
+/**
+ * http://www.bittiming.can-wiki.info
+ */
+BitTimingSettings computeBitTimings(std::uint32_t bitrate)
+{
+    if (Chip_Clock_GetSystemClockRate() == 48000000) // 48 MHz is optimal for CAN timings
     {
-        for (uint32_t quanta = 1; quanta <= 32; quanta++)
+        switch (bitrate)
         {
-            for (uint32_t segs = 3; segs <= 17; segs++)
-            {
-                if (clk_per_bit == (segs * quanta * (div + 1)))
-                {
-                    segs -= 3;
-                    const uint32_t seg1 = segs / 2;
-                    const uint32_t seg2 = segs - seg1;
-                    const uint32_t can_sjw = (seg1 > 3) ? 3 : seg1;
-                    can_api_timing_cfg[0] = div;
-                    can_api_timing_cfg[1] = ((quanta - 1) & 0x3F) |
-                                            (can_sjw & 0x03) << 6 |
-                                            (seg1 & 0x0F) << 8 |
-                                            (seg2 & 0x07) << 12;
-                    return 0;
-                }
-            }
+        case 1000000: return BitTimingSettings{ 0, 0x0505 }; // 8  quanta, 87.5%
+        case 500000:  return BitTimingSettings{ 0, 0x1c05 }; // 16 quanta, 87.5%
+        case 250000:  return BitTimingSettings{ 0, 0x1c0b }; // 16 quanta, 87.5%
+        case 125000:  return BitTimingSettings{ 0, 0x1c17 }; // 16 quanta, 87.5%
+        default:      return BitTimingSettings{ 0, 0 };
         }
     }
-    return -1;
+    else
+    {
+        return BitTimingSettings{ 0, 0 };
+    }
 }
 
 } // namespace
 
 CanDriver CanDriver::self;
 
-int CanDriver::init(uavcan::uint32_t baudrate)
+uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)())
 {
-    CriticalSectionLocker locker;
+    static constexpr uavcan::uint32_t BitRatesToTry[] =
+    {
+        1000000,
+        500000,
+        250000,
+        125000
+    };
 
-    error_cnt = 0;
-    tx_free = true;
-    last_irq_utc_timestamp = 0;
-    had_activity = false;
+    const auto ListeningDuration = uavcan::MonotonicDuration::fromMSec(1050);
+
+    NVIC_DisableIRQ(CAN_IRQn);
+    Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
+
+    for (auto bitrate : BitRatesToTry)
+    {
+        // Computing bit timings
+        const auto bit_timings = computeBitTimings(bitrate);
+        if (!bit_timings.isValid())
+        {
+            return 0;
+        }
+
+        // Configuring the CAN controller
+        {
+            CriticalSectionLocker locker;
+
+            LPC_SYSCTL->PRESETCTRL |= (1U << RESET_CAN0);
+
+            // Entering initialization mode
+            c_can::CAN.CNTL = c_can::CNTL_INIT | c_can::CNTL_CCE;
+
+            while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0)
+            {
+                ; // Nothing to do
+            }
+
+            // Configuring bit rate
+            c_can::CAN.CLKDIV = bit_timings.canclkdiv;
+            c_can::CAN.BT     = bit_timings.canbtr;
+            c_can::CAN.BRPE   = 0;
+
+            // Configuring silent mode
+            c_can::CAN.CNTL |= c_can::CNTL_TEST;
+            c_can::CAN.TEST = c_can::TEST_SILENT;
+
+            // Configuring status monitor
+            c_can::CAN.STAT = (unsigned(c_can::StatLec::Unused) << c_can::STAT_LEC_SHIFT);
+
+            // Leaving initialization mode
+            c_can::CAN.CNTL &= ~(c_can::CNTL_INIT | c_can::CNTL_CCE);
+
+            while ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0)
+            {
+                ; // Nothing to do
+            }
+        }
+
+        // Listening
+        const auto deadline = clock::getMonotonic() + ListeningDuration;
+        bool match_detected = false;
+        while (clock::getMonotonic() < deadline)
+        {
+            if (idle_callback != nullptr)
+            {
+                idle_callback();
+            }
+
+            const auto LastErrorCode = (c_can::CAN.STAT >> c_can::STAT_LEC_SHIFT) & c_can::STAT_LEC_MASK;
+
+            if (LastErrorCode == unsigned(c_can::StatLec::NoError))
+            {
+                match_detected = true;
+                break;
+            }
+        }
+
+        // De-configuring the CAN controller back to reset state
+        {
+            CriticalSectionLocker locker;
+
+            c_can::CAN.CNTL = c_can::CNTL_INIT;
+
+            while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0)
+            {
+                ; // Nothing to do
+            }
+
+            LPC_SYSCTL->PRESETCTRL &= ~(1U << RESET_CAN0);
+        }
+
+        // Termination condition
+        if (match_detected)
+        {
+            return bitrate;
+        }
+    }
+
+    return 0;   // No match
+}
+
+int CanDriver::init(uavcan::uint32_t bitrate)
+{
+    {
+        auto bit_timings = computeBitTimings(bitrate);
+        if (!bit_timings.isValid())
+        {
+            return -1;
+        }
+
+        CriticalSectionLocker locker;
+
+        error_cnt = 0;
+        tx_abort_on_error = false;
+        tx_pending = false;
+        last_irq_utc_timestamp = 0;
+        had_activity = false;
+
+        /*
+         * C_CAN init
+         */
+        Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
+
+        LPC_CCAN_API->init_can(reinterpret_cast<std::uint32_t*>(&bit_timings), true);
+
+        static CCAN_CALLBACKS_T ccan_callbacks =
+        {
+            canRxCallback,
+            canTxCallback,
+            canErrorCallback,
+            nullptr,
+            nullptr,
+            nullptr,
+            nullptr,
+            nullptr
+        };
+        LPC_CCAN_API->config_calb(&ccan_callbacks);
+
+        /*
+         * Interrupts
+         */
+        c_can::CAN.CNTL |= c_can::CNTL_SIE;         // This is necessary for transmission aborts on error
+
+        NVIC_EnableIRQ(CAN_IRQn);
+    }
 
     /*
-     * C_CAN init
+     * Applying default filter configuration (accept all)
      */
-    Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
-    static uint32_t can_api_init_table[2];
-    if (computeBaudrate(baudrate, can_api_init_table) != 0)
+    if (configureFilters(nullptr, 0) < 0)
     {
         return -1;
     }
-    LPC_CCAN_API->init_can(can_api_init_table, true);
-    static CCAN_CALLBACKS_T ccan_callbacks =
-    {
-        canRxCallback,
-        canTxCallback,
-        canErrorCallback,
-        0,
-        0,
-        0,
-        0,
-        0
-    };
-    LPC_CCAN_API->config_calb(&ccan_callbacks);
-    NVIC_EnableIRQ(CAN_IRQn);
-
-    /*
-     * Default RX msgobj config:
-     *  31 - all STD
-     *  32 - all EXT
-     * RTR ignored
-     */
-    CCAN_MSG_OBJ_T msg_obj = CCAN_MSG_OBJ_T();
-    msg_obj.msgobj = 31;
-    LPC_CCAN_API->config_rxmsgobj(&msg_obj);
-    msg_obj.mode_id = CAN_MSGOBJ_EXT;
-    msg_obj.msgobj = 32;
-    LPC_CCAN_API->config_rxmsgobj(&msg_obj);
 
     return 0;
 }
@@ -220,7 +334,7 @@ bool CanDriver::hasReadyRx() const
 bool CanDriver::hasEmptyTx() const
 {
     CriticalSectionLocker locker;
-    return tx_free;
+    return !tx_pending;
 }
 
 bool CanDriver::hadActivity()
@@ -231,12 +345,23 @@ bool CanDriver::hadActivity()
     return ret;
 }
 
+uavcan::uint32_t CanDriver::getRxQueueOverflowCount() const
+{
+    CriticalSectionLocker locker;
+    return rx_queue.getOverflowCount();
+}
+
+bool CanDriver::isInBusOffState() const
+{
+    return (c_can::CAN.STAT & c_can::STAT_BOFF) != 0;
+}
+
 uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
                                 uavcan::CanIOFlags flags)
 {
     if (frame.isErrorFrame() ||
         frame.dlc > 8 ||
-        flags != 0)      // Only plain IO is allowed. Loopback, TX timestamping are not supported by this driver.
+        (flags & uavcan::CanIOFlagLoopback) != 0)   // TX timestamping is not supported by this driver.
     {
         return -1;
     }
@@ -264,10 +389,11 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic
 
     CriticalSectionLocker locker;
 
-    if (tx_free)
+    if (!tx_pending)
     {
-        tx_free = false;   // Mark as pending - will be released in TX callback
-        msgobj.msgobj = 1;
+        tx_pending = true;   // Mark as pending - will be released in TX callback
+        tx_abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
+        msgobj.msgobj = TxMessageObjectNumber;
         LPC_CCAN_API->can_transmit(&msgobj);
         return 1;
     }
@@ -277,7 +403,7 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic
 uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
                                    uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
 {
-    out_ts_monotonic = uavcan_lpc11c24::clock::getMonotonic();
+    out_ts_monotonic = clock::getMonotonic();
     out_flags = 0;                                            // We don't support any IO flags
 
     CriticalSectionLocker locker;
@@ -285,7 +411,7 @@ uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::Monotoni
     {
         return 0;
     }
-    uint64_t ts_utc = 0;
+    std::uint64_t ts_utc = 0;
     rx_queue.pop(out_frame, ts_utc);
     out_ts_utc = uavcan::UtcTime::fromUSec(ts_utc);
     return 1;
@@ -295,6 +421,16 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
                                   const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
                                   uavcan::MonotonicTime blocking_deadline)
 {
+    const bool bus_off = isInBusOffState();
+    if (bus_off)                                // Recover automatically on bus-off
+    {
+        CriticalSectionLocker locker;
+        if ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0)
+        {
+            c_can::CAN.CNTL &= ~c_can::CNTL_INIT;
+        }
+    }
+
     const bool noblock = ((inout_masks.read  == 1) && hasReadyRx()) ||
                          ((inout_masks.write == 1) && hasEmptyTx());
 
@@ -322,32 +458,98 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
     }
 
     inout_masks.read  = hasReadyRx() ? 1 : 0;
-    inout_masks.write = hasEmptyTx() ? 1 : 0;
-    return 0;  // Return value doesn't matter as long as it is non-negative
+
+    inout_masks.write = (hasEmptyTx() && !bus_off) ? 1 : 0;     // Disable write while in bus-off
+
+    return 0;           // Return value doesn't matter as long as it is non-negative
 }
 
 uavcan::int16_t CanDriver::configureFilters(const uavcan::CanFilterConfig* filter_configs,
                                             uavcan::uint16_t num_configs)
 {
-    (void)filter_configs;
-    (void)num_configs;
-    return -1;
+    CriticalSectionLocker locker;
+
+    /*
+     * If C_CAN is active (INIT=0) and the CAN bus has intensive traffic, RX object configuration may fail.
+     * The solution is to disable the controller while configuration is in progress.
+     * The documentation, as always, doesn't bother to mention this detail. Shame on you, NXP.
+     */
+    struct RAIIDisabler
+    {
+        RAIIDisabler()
+        {
+            c_can::CAN.CNTL |= c_can::CNTL_INIT;
+        }
+        ~RAIIDisabler()
+        {
+            c_can::CAN.CNTL &= ~c_can::CNTL_INIT;
+        }
+    } can_disabler;    // Must be instantiated AFTER the critical section locker
+
+    if (num_configs == 0)
+    {
+        auto msg_obj = CCAN_MSG_OBJ_T();
+        msg_obj.msgobj = NumberOfTxMessageObjects + 1;
+        LPC_CCAN_API->config_rxmsgobj(&msg_obj);    // all STD frames
+
+        msg_obj.mode_id = CAN_MSGOBJ_EXT;
+        msg_obj.msgobj = NumberOfTxMessageObjects + 2;
+        LPC_CCAN_API->config_rxmsgobj(&msg_obj);    // all EXT frames
+    }
+    else if (num_configs <= NumberOfRxMessageObjects)
+    {
+        // Making sure the configs use only EXT frames; otherwise we can't accept them
+        for (unsigned i = 0; i < num_configs; i++)
+        {
+            auto& f = filter_configs[i];
+            if ((f.id & f.mask & uavcan::CanFrame::FlagEFF) == 0)
+            {
+                return -1;
+            }
+        }
+
+        // Installing the configuration
+        for (unsigned i = 0; i < NumberOfRxMessageObjects; i++)
+        {
+            auto msg_obj = CCAN_MSG_OBJ_T();
+            msg_obj.msgobj = std::uint8_t(i + 1U + NumberOfTxMessageObjects);   // Message objects are numbered from 1
+
+            if (i < num_configs)
+            {
+                msg_obj.mode_id = (filter_configs[i].id & uavcan::CanFrame::MaskExtID) | CAN_MSGOBJ_EXT; // Only EXT
+                msg_obj.mask    = filter_configs[i].mask & uavcan::CanFrame::MaskExtID;
+            }
+            else
+            {
+                msg_obj.mode_id = CAN_MSGOBJ_RTR;               // Using this configuration to disable the object
+                msg_obj.mask    = uavcan::CanFrame::MaskStdID;
+            }
+
+            LPC_CCAN_API->config_rxmsgobj(&msg_obj);
+        }
+    }
+    else
+    {
+        return -1;
+    }
+
+    return 0;
 }
 
 uavcan::uint64_t CanDriver::getErrorCount() const
 {
     CriticalSectionLocker locker;
-    return uint64_t(error_cnt) + uint64_t(rx_queue.getOverflowCount());
+    return std::uint64_t(error_cnt) + std::uint64_t(rx_queue.getOverflowCount());
 }
 
 uavcan::uint16_t CanDriver::getNumFilters() const
 {
-    return NumMsgObjects - 1;  // First msgobj is reserved for TX frame
+    return NumberOfRxMessageObjects;
 }
 
 uavcan::ICanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
 {
-    return (iface_index == 0) ? this : NULL;
+    return (iface_index == 0) ? this : nullptr;
 }
 
 uavcan::uint8_t CanDriver::getNumIfaces() const
@@ -363,9 +565,11 @@ uavcan::uint8_t CanDriver::getNumIfaces() const
 extern "C"
 {
 
-void canRxCallback(uint8_t msg_obj_num)
+void canRxCallback(std::uint8_t msg_obj_num)
 {
-    CCAN_MSG_OBJ_T msg_obj = CCAN_MSG_OBJ_T();
+    using namespace uavcan_lpc11c24;
+
+    auto msg_obj = CCAN_MSG_OBJ_T();
     msg_obj.msgobj = msg_obj_num;
     LPC_CCAN_API->can_receive(&msg_obj);
 
@@ -392,23 +596,40 @@ void canRxCallback(uint8_t msg_obj_num)
     frame.dlc = msg_obj.dlc;
     uavcan::copy(msg_obj.data, msg_obj.data + msg_obj.dlc, frame.data);
 
-    uavcan_lpc11c24::rx_queue.push(frame, uavcan_lpc11c24::last_irq_utc_timestamp);
-    uavcan_lpc11c24::had_activity = true;
+    rx_queue.push(frame, last_irq_utc_timestamp);
+    had_activity = true;
 }
 
-void canTxCallback(uint8_t msg_obj_num)
+void canTxCallback(std::uint8_t msg_obj_num)
 {
+    using namespace uavcan_lpc11c24;
+
     (void)msg_obj_num;
-    uavcan_lpc11c24::tx_free = true;
-    uavcan_lpc11c24::had_activity = true;
+
+    tx_pending = false;
+    had_activity = true;
 }
 
-void canErrorCallback(uint32_t error_info)
+void canErrorCallback(std::uint32_t error_info)
 {
-    (void)error_info;
-    if (uavcan_lpc11c24::error_cnt < 0xFFFFFFFF)
+    using namespace uavcan_lpc11c24;
+
+    // Updating the error counter
+    if ((error_info != 0) && (error_cnt < 0xFFFFFFFFUL))
     {
-        uavcan_lpc11c24::error_cnt++;
+        error_cnt++;
+    }
+
+    // Serving abort requests
+    if (tx_pending && tx_abort_on_error)
+    {
+        tx_pending = false;
+        tx_abort_on_error = false;
+
+        // Using the first interface, because this approach seems to be compliant with the BASIC mode (just in case)
+        c_can::CAN.IF[0].CMDREQ = TxMessageObjectNumber;
+        c_can::CAN.IF[0].CMDMSK.W = c_can::IF_CMDMSK_W_WR_RD;   // Clearing IF_CMDMSK_W_TXRQST
+        c_can::CAN.IF[0].MCTRL &= ~c_can::IF_MCTRL_TXRQST;      // Clearing IF_MCTRL_TXRQST
     }
 }
 
@@ -416,7 +637,10 @@ void CAN_IRQHandler();
 
 void CAN_IRQHandler()
 {
-    uavcan_lpc11c24::last_irq_utc_timestamp = uavcan_lpc11c24::clock::getUtcUSecFromCanInterrupt();
+    using namespace uavcan_lpc11c24;
+
+    last_irq_utc_timestamp = clock::getUtcUSecFromCanInterrupt();
+
     LPC_CCAN_API->isr();
 }
 
diff --git a/libuavcan_drivers/lpc11c24/driver/src/clock.cpp b/libuavcan_drivers/lpc11c24/driver/src/clock.cpp
index 2230635e8f..a9d299f063 100644
--- a/libuavcan_drivers/lpc11c24/driver/src/clock.cpp
+++ b/libuavcan_drivers/lpc11c24/driver/src/clock.cpp
@@ -17,18 +17,18 @@ namespace
 bool initialized = false;
 bool utc_set = false;
 
-int32_t utc_correction_usec_per_overflow_x16 = 0;
-int64_t prev_adjustment = 0;
+std::int32_t utc_correction_usec_per_overflow_x16 = 0;
+std::int64_t prev_adjustment = 0;
 
-uint64_t time_mono = 0;
-uint64_t time_utc = 0;
+std::uint64_t time_mono = 0;
+std::uint64_t time_utc = 0;
 
 /**
  * If this value is too large for the given core clock, reload value will be out of the 24-bit integer range.
  * This will be detected at run time during timer initialization - refer to SysTick_Config().
  */
-const uint32_t USecPerOverflow = 65536 * 2;
-const int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
+constexpr std::uint32_t USecPerOverflow = 65536 * 2;
+constexpr std::int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
 
 }
 
@@ -59,30 +59,34 @@ void init()
     }
 }
 
-static uint64_t sampleFromCriticalSection(const volatile uint64_t* const value)
+static std::uint64_t sampleFromCriticalSection(const volatile std::uint64_t* const value)
 {
-    const uint32_t reload = SysTick->LOAD + 1;  // SysTick counts downwards, hence the value subtracted from reload
+    const std::uint32_t reload = SysTick->LOAD + 1;  // SysTick counts downwards, hence the value subtracted from reload
 
-    volatile uint64_t time = *value;
-    volatile uint32_t cycles = reload - SysTick->VAL;
+    volatile std::uint64_t time = *value;
+    volatile std::uint32_t cycles = reload - SysTick->VAL;
 
     if ((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) == SCB_ICSR_PENDSTSET_Msk)
     {
         cycles = reload - SysTick->VAL;
         time += USecPerOverflow;
     }
-    const uint32_t cycles_per_usec = SystemCoreClock / 1000000;
+    const std::uint32_t cycles_per_usec = SystemCoreClock / 1000000;
     return time + (cycles / cycles_per_usec);
 }
 
-uint64_t getUtcUSecFromCanInterrupt()
+std::uint64_t getUtcUSecFromCanInterrupt()
 {
     return utc_set ? sampleFromCriticalSection(&time_utc) : 0;
 }
 
 uavcan::MonotonicTime getMonotonic()
 {
-    uint64_t usec = 0;
+    if (!initialized)
+    {
+        fail();
+    }
+    std::uint64_t usec = 0;
     {
         CriticalSectionLocker locker;
         usec = sampleFromCriticalSection(&time_mono);
@@ -92,7 +96,11 @@ uavcan::MonotonicTime getMonotonic()
 
 uavcan::UtcTime getUtc()
 {
-    uint64_t usec = 0;
+    if (!initialized)
+    {
+        fail();
+    }
+    std::uint64_t usec = 0;
     if (utc_set)
     {
         CriticalSectionLocker locker;
@@ -108,7 +116,7 @@ uavcan::UtcDuration getPrevUtcAdjustment()
 
 void adjustUtc(uavcan::UtcDuration adjustment)
 {
-    const int64_t adj_delta = adjustment.toUSec() - prev_adjustment;  // This is the P term
+    const std::int64_t adj_delta = adjustment.toUSec() - prev_adjustment;  // This is the P term
     prev_adjustment = adjustment.toUSec();
 
     utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
@@ -121,16 +129,16 @@ void adjustUtc(uavcan::UtcDuration adjustment)
 
     if (adjustment.getAbs().toMSec() > 9 || !utc_set)
     {
-        const int64_t adj_usec = adjustment.toUSec();
+        const std::int64_t adj_usec = adjustment.toUSec();
         {
             CriticalSectionLocker locker;
-            if ((adj_usec < 0) && uint64_t(-adj_usec) > time_utc)
+            if ((adj_usec < 0) && std::uint64_t(-adj_usec) > time_utc)
             {
                 time_utc = 1;
             }
             else
             {
-                time_utc = uint64_t(int64_t(time_utc) + adj_usec);
+                time_utc = std::uint64_t(std::int64_t(time_utc) + adj_usec);
             }
         }
         if (!utc_set)
@@ -170,7 +178,7 @@ void SysTick_Handler()
         if (utc_set)
         {
             // Values below 16 are ignored
-            time_utc += uint64_t(int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
+            time_utc += std::uint64_t(std::int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
         }
     }
     else
diff --git a/libuavcan_drivers/lpc11c24/driver/src/internal.hpp b/libuavcan_drivers/lpc11c24/driver/src/internal.hpp
index 3bcf0d82c8..80be9f1032 100644
--- a/libuavcan_drivers/lpc11c24/driver/src/internal.hpp
+++ b/libuavcan_drivers/lpc11c24/driver/src/internal.hpp
@@ -4,8 +4,19 @@
 
 #pragma once
 
+#include <cstdint>
 #include <chip.h>
 
+/*
+ * Compiler version check
+ */
+#ifdef __GNUC__
+# if (__GNUC__ * 10 + __GNUC_MINOR__) < 49
+#  error "Use GCC 4.9 or newer"
+# endif
+#endif
+
+
 namespace uavcan_lpc11c24
 {
 
@@ -31,7 +42,7 @@ struct CriticalSectionLocker
 namespace clock
 {
 
-uint64_t getUtcUSecFromCanInterrupt();
+std::uint64_t getUtcUSecFromCanInterrupt();
 
 }
 
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
index 818975feca..9a503bc4ef 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
@@ -49,10 +49,8 @@ DEPDIR = $(BUILDDIR)/dep
 
 DEF += -DNDEBUG -DCHIP_LPC11CXX -DCORE_M0 -DTHUMB_NO_INTERWORKING -U__STRICT_ANSI__
 
-# Removing -fconserve-stack reduces code size a little
 FLAGS = -mthumb -mcpu=cortex-m0 -mno-thumb-interwork -flto -Os -g3 -Wall -Wextra -Werror -Wundef -ffunction-sections \
         -fdata-sections -fno-common -fno-exceptions -fno-unwind-tables -fno-stack-protector -fomit-frame-pointer \
-        -ftracer -ftree-loop-distribute-patterns -frename-registers -freorder-blocks -fconserve-stack \
         -Wfloat-equal -Wconversion -Wsign-conversion -Wmissing-declarations
 
 C_CPP_FLAGS = $(FLAGS) -MD -MP -MF $(DEPDIR)/$(@F).d
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/uart_11xx.c b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/uart_11xx.c
new file mode 100644
index 0000000000..ae404b0dc0
--- /dev/null
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/uart_11xx.c
@@ -0,0 +1,289 @@
+/*
+ * @brief LPC11xx UART chip driver
+ *
+ * @note
+ * Copyright(C) NXP Semiconductors, 2012
+ * All rights reserved.
+ *
+ * @par
+ * Software that is described herein is for illustrative purposes only
+ * which provides customers with programming information regarding the
+ * LPC products.  This software is supplied "AS IS" without any warranties of
+ * any kind, and NXP Semiconductors and its licensor disclaim any and
+ * all warranties, express or implied, including all implied warranties of
+ * merchantability, fitness for a particular purpose and non-infringement of
+ * intellectual property rights.  NXP Semiconductors assumes no responsibility
+ * or liability for the use of the software, conveys no license or rights under any
+ * patent, copyright, mask work right, or any other intellectual property rights in
+ * or to any products. NXP Semiconductors reserves the right to make changes
+ * in the software without notification. NXP Semiconductors also makes no
+ * representation or warranty that such application will be suitable for the
+ * specified use without further testing or modification.
+ *
+ * @par
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation is hereby granted, under NXP Semiconductors' and its
+ * licensor's relevant copyrights in the software, without fee, provided that it
+ * is used in conjunction with NXP Semiconductors microcontrollers.  This
+ * copyright, permission, and disclaimer notice must appear in all copies of
+ * this code.
+ */
+
+#include "chip.h"
+
+#if __GNUC__
+# pragma GCC diagnostic ignored "-Wsign-conversion"
+# pragma GCC diagnostic ignored "-Wconversion"
+# pragma GCC diagnostic ignored "-Wmissing-declarations"
+# pragma GCC diagnostic ignored "-Wunused-parameter"
+#endif
+
+/*****************************************************************************
+ * Private types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Public types/enumerations/variables
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Private functions
+ ****************************************************************************/
+
+/*****************************************************************************
+ * Public functions
+ ****************************************************************************/
+
+/* Initializes the pUART peripheral */
+void Chip_UART_Init(LPC_USART_T *pUART)
+{
+	Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_UART0);
+	Chip_Clock_SetUARTClockDiv(1);
+
+	/* Enable FIFOs by default, reset them */
+	Chip_UART_SetupFIFOS(pUART, (UART_FCR_FIFO_EN | UART_FCR_RX_RS | UART_FCR_TX_RS));
+
+	/* Default 8N1, with DLAB disabled */
+	Chip_UART_ConfigData(pUART, (UART_LCR_WLEN8 | UART_LCR_SBS_1BIT | UART_LCR_PARITY_DIS));
+
+	/* Disable fractional divider */
+	pUART->FDR = 0x10;
+}
+
+/* De-initializes the pUART peripheral */
+void Chip_UART_DeInit(LPC_USART_T *pUART)
+{
+	Chip_Clock_DisablePeriphClock(SYSCTL_CLOCK_UART0);
+}
+
+/* Transmit a byte array through the UART peripheral (non-blocking) */
+int Chip_UART_Send(LPC_USART_T *pUART, const void *data, int numBytes)
+{
+	int sent = 0;
+	uint8_t *p8 = (uint8_t *) data;
+
+	/* Send until the transmit FIFO is full or out of bytes */
+	while ((sent < numBytes) &&
+		   ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_THRE) != 0)) {
+		Chip_UART_SendByte(pUART, *p8);
+		p8++;
+		sent++;
+	}
+
+	return sent;
+}
+
+/* Transmit a byte array through the UART peripheral (blocking) */
+int Chip_UART_SendBlocking(LPC_USART_T *pUART, const void *data, int numBytes)
+{
+	int pass, sent = 0;
+	uint8_t *p8 = (uint8_t *) data;
+
+	while (numBytes > 0) {
+		pass = Chip_UART_Send(pUART, p8, numBytes);
+		numBytes -= pass;
+		sent += pass;
+		p8 += pass;
+	}
+
+	return sent;
+}
+
+/* Read data through the UART peripheral (non-blocking) */
+int Chip_UART_Read(LPC_USART_T *pUART, void *data, int numBytes)
+{
+	int readBytes = 0;
+	uint8_t *p8 = (uint8_t *) data;
+
+	/* Send until the transmit FIFO is full or out of bytes */
+	while ((readBytes < numBytes) &&
+		   ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_RDR) != 0)) {
+		*p8 = Chip_UART_ReadByte(pUART);
+		p8++;
+		readBytes++;
+	}
+
+	return readBytes;
+}
+
+/* Read data through the UART peripheral (blocking) */
+int Chip_UART_ReadBlocking(LPC_USART_T *pUART, void *data, int numBytes)
+{
+	int pass, readBytes = 0;
+	uint8_t *p8 = (uint8_t *) data;
+
+	while (readBytes < numBytes) {
+		pass = Chip_UART_Read(pUART, p8, numBytes);
+		numBytes -= pass;
+		readBytes += pass;
+		p8 += pass;
+	}
+
+	return readBytes;
+}
+
+/* Determines and sets best dividers to get a target bit rate */
+uint32_t Chip_UART_SetBaud(LPC_USART_T *pUART, uint32_t baudrate)
+{
+	uint32_t div, divh, divl, clkin;
+
+	/* Determine UART clock in rate without FDR */
+	clkin = Chip_Clock_GetMainClockRate();
+	div = clkin / (baudrate * 16);
+
+	/* High and low halves of the divider */
+	divh = div / 256;
+	divl = div - (divh * 256);
+
+	Chip_UART_EnableDivisorAccess(pUART);
+	Chip_UART_SetDivisorLatches(pUART, divl, divh);
+	Chip_UART_DisableDivisorAccess(pUART);
+
+	/* Fractional FDR alreadt setup for 1 in UART init */
+
+	return clkin / div;
+}
+
+/* UART receive-only interrupt handler for ring buffers */
+void Chip_UART_RXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB)
+{
+	/* New data will be ignored if data not popped in time */
+	while (Chip_UART_ReadLineStatus(pUART) & UART_LSR_RDR) {
+		uint8_t ch = Chip_UART_ReadByte(pUART);
+		RingBuffer_Insert(pRB, &ch);
+	}
+}
+
+/* UART transmit-only interrupt handler for ring buffers */
+void Chip_UART_TXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB)
+{
+	uint8_t ch;
+
+	/* Fill FIFO until full or until TX ring buffer is empty */
+	while ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_THRE) != 0 &&
+		   RingBuffer_Pop(pRB, &ch)) {
+		Chip_UART_SendByte(pUART, ch);
+	}
+}
+
+/* Populate a transmit ring buffer and start UART transmit */
+uint32_t Chip_UART_SendRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, const void *data, int bytes)
+{
+	uint32_t ret;
+	uint8_t *p8 = (uint8_t *) data;
+
+	/* Don't let UART transmit ring buffer change in the UART IRQ handler */
+	Chip_UART_IntDisable(pUART, UART_IER_THREINT);
+
+	/* Move as much data as possible into transmit ring buffer */
+	ret = RingBuffer_InsertMult(pRB, p8, bytes);
+	Chip_UART_TXIntHandlerRB(pUART, pRB);
+
+	/* Add additional data to transmit ring buffer if possible */
+	ret += RingBuffer_InsertMult(pRB, (p8 + ret), (bytes - ret));
+
+	/* Enable UART transmit interrupt */
+	Chip_UART_IntEnable(pUART, UART_IER_THREINT);
+
+	return ret;
+}
+
+/* Copy data from a receive ring buffer */
+int Chip_UART_ReadRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, void *data, int bytes)
+{
+	(void) pUART;
+
+	return RingBuffer_PopMult(pRB, (uint8_t *) data, bytes);
+}
+
+/* UART receive/transmit interrupt handler for ring buffers */
+void Chip_UART_IRQRBHandler(LPC_USART_T *pUART, RINGBUFF_T *pRXRB, RINGBUFF_T *pTXRB)
+{
+	/* Handle transmit interrupt if enabled */
+	if (pUART->IER & UART_IER_THREINT) {
+		Chip_UART_TXIntHandlerRB(pUART, pTXRB);
+
+		/* Disable transmit interrupt if the ring buffer is empty */
+		if (RingBuffer_IsEmpty(pTXRB)) {
+			Chip_UART_IntDisable(pUART, UART_IER_THREINT);
+		}
+	}
+
+	/* Handle receive interrupt */
+	Chip_UART_RXIntHandlerRB(pUART, pRXRB);
+}
+
+/* Determines and sets best dividers to get a target baud rate */
+uint32_t Chip_UART_SetBaudFDR(LPC_USART_T *pUART, uint32_t baudrate)
+
+{
+    uint32_t uClk = 0;
+    uint32_t dval = 0;
+    uint32_t mval = 0;
+    uint32_t dl = 0;
+    uint32_t rate16 = 16 * baudrate;
+    uint32_t actualRate = 0;
+
+    /* Get Clock rate */
+    uClk = Chip_Clock_GetMainClockRate();
+
+    /* The fractional is calculated as (PCLK  % (16 * Baudrate)) / (16 * Baudrate)
+     * Let's make it to be the ratio DivVal / MulVal
+     */
+	dval = uClk % rate16;
+
+   /* The PCLK / (16 * Baudrate) is fractional
+    * => dval = pclk % rate16
+    * mval = rate16;
+    * now mormalize the ratio
+    * dval / mval = 1 / new_mval
+    * new_mval = mval / dval
+    * new_dval = 1
+    */
+    if (dval > 0) {
+        mval = rate16 / dval;
+        dval = 1;
+
+        /* In case mval still bigger then 4 bits
+        * no adjustment require
+        */
+        if (mval > 12) {
+         dval = 0;
+      }
+    }
+    dval &= 0xf;
+    mval &= 0xf;
+    dl = uClk / (rate16 + rate16 *dval / mval);
+
+    /* Update UART registers */
+    Chip_UART_EnableDivisorAccess(pUART);
+	Chip_UART_SetDivisorLatches(pUART, UART_LOAD_DLL(dl), UART_LOAD_DLM(dl));
+	Chip_UART_DisableDivisorAccess(pUART);
+
+	/* Set best fractional divider */
+	pUART->FDR = (UART_FDR_MULVAL(mval) | UART_FDR_DIVADDVAL(dval));
+
+	/* Return actual baud rate */
+	actualRate = uClk / (16 * dl + 16 * dl * dval / mval);
+	return actualRate;
+}
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
index 0f1ff0125c..9f81263c06 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
@@ -3,21 +3,74 @@
  */
 
 #include <cstdio>
+#include <cstdlib>
 #include <algorithm>
 #include <board.hpp>
 #include <chip.h>
 #include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
 #include <uavcan/protocol/logger.hpp>
 
+/*
+ * GCC 4.9 cannot generate a working binary with higher optimization levels, although
+ *         rest of the firmware can be compiled with -Os.
+ * GCC 4.8 and earlier don't work at all on this firmware.
+ */
+#if __GNUC__
+# pragma GCC optimize 1
+#endif
+
+/**
+ * This function re-defines the standard ::rand(), which is used by the class uavcan::DynamicNodeIDClient.
+ * Redefinition is normally not needed, but GCC 4.9 tends to generate broken binaries if it is not redefined.
+ */
+int rand()
+{
+    static int x = 1;
+    x = x * 48271 % 2147483647;
+    return x;
+}
+
 namespace
 {
 
-typedef uavcan::Node<2800> Node;
+static constexpr unsigned NodeMemoryPoolSize = 2800;
 
-Node& getNode()
+/**
+ * This is a compact, reentrant and thread-safe replacement to standard llto().
+ * It returns the string by value, no extra storage is needed.
+ */
+typename uavcan::MakeString<22>::Type intToString(long long n)
 {
-    static Node node(uavcan_lpc11c24::CanDriver::instance(), uavcan_lpc11c24::SystemClock::instance());
+    char buf[24] = {};
+    const short sign = (n < 0) ? -1 : 1;
+    if (sign < 0)
+    {
+        n = -n;
+    }
+    unsigned pos = 0;
+    do
+    {
+        buf[pos++] = char(n % 10 + '0');
+    }
+    while ((n /= 10) > 0);
+    if (sign < 0)
+    {
+        buf[pos++] = '-';
+    }
+    buf[pos] = '\0';
+    for (unsigned i = 0, j = pos - 1U; i < j; i++, j--)
+    {
+        std::swap(buf[i], buf[j]);
+    }
+    return static_cast<const char*>(buf);
+}
+
+uavcan::Node<NodeMemoryPoolSize>& getNode()
+{
+    static uavcan::Node<NodeMemoryPoolSize> node(uavcan_lpc11c24::CanDriver::instance(),
+                                                 uavcan_lpc11c24::SystemClock::instance());
     return node;
 }
 
@@ -33,29 +86,63 @@ uavcan::Logger& getLogger()
     return logger;
 }
 
-#if __GNUC__
-__attribute__((noreturn))
-#endif
-void die()
+uavcan::NodeID performDynamicNodeIDAllocation()
 {
-    while (true) { }
+    uavcan::DynamicNodeIDClient client(getNode());
+
+    const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
+    if (client_start_res < 0)
+    {
+        board::die();
+    }
+
+    while (!client.isAllocationComplete())
+    {
+        board::resetWatchdog();
+        (void)getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
+    }
+
+    return client.getAllocatedNodeID();
 }
 
-#if __GNUC__
-__attribute__((noinline))
-#endif
 void init()
 {
     board::resetWatchdog();
+    board::syslog("Boot\r\n");
 
-    if (uavcan_lpc11c24::CanDriver::instance().init(1000000) < 0)
+    board::setErrorLed(false);
+    board::setStatusLed(true);
+
+    /*
+     * Configuring the clock - this must be done before the CAN controller is initialized
+     */
+    uavcan_lpc11c24::clock::init();
+
+    /*
+     * Configuring the CAN controller
+     */
+    std::uint32_t bit_rate = 0;
+    while (bit_rate == 0)
     {
-        die();
+        board::syslog("CAN auto bitrate...\r\n");
+        bit_rate = uavcan_lpc11c24::CanDriver::detectBitRate(&board::resetWatchdog);
     }
+    board::syslog("Bitrate: ");
+    board::syslog(intToString(bit_rate).c_str());
+    board::syslog("\r\n");
+
+    if (uavcan_lpc11c24::CanDriver::instance().init(bit_rate) < 0)
+    {
+        board::die();
+    }
+
+    board::syslog("CAN init ok\r\n");
 
     board::resetWatchdog();
 
-    getNode().setNodeID(72);
+    /*
+     * Configuring the node
+     */
     getNode().setName("org.uavcan.lpc11c24_test");
 
     uavcan::protocol::SoftwareVersion swver;
@@ -73,55 +160,69 @@ void init()
 
     board::resetWatchdog();
 
-    while (getNode().start() < 0)
+    /*
+     * Starting the node and performing dynamic node ID allocation
+     */
+    if (getNode().start() < 0)
     {
+        board::die();
     }
 
+    board::syslog("Node ID allocation...\r\n");
+
+    getNode().setNodeID(performDynamicNodeIDAllocation());
+
+    board::syslog("Node ID ");
+    board::syslog(intToString(getNode().getNodeID().get()).c_str());
+    board::syslog("\r\n");
+
     board::resetWatchdog();
 
-    while (getTimeSyncSlave().start() < 0)
+    /*
+     * Example filter configuration.
+     * Can be removed safely.
+     */
     {
+        constexpr unsigned NumFilters = 3;
+        uavcan::CanFilterConfig filters[NumFilters];
+
+        // Acepting all service transfers addressed to us
+        filters[0].id   = (unsigned(getNode().getNodeID().get()) << 8) | (1U << 7) | uavcan::CanFrame::FlagEFF;
+        filters[0].mask = 0x7F80 | uavcan::CanFrame::FlagEFF;
+
+        // Accepting time sync messages
+        filters[1].id   = (4U << 8) | uavcan::CanFrame::FlagEFF;
+        filters[1].mask = 0xFFFF80 | uavcan::CanFrame::FlagEFF;
+
+        // Accepting zero CAN ID (just for the sake of testing)
+        filters[2].id   = 0 | uavcan::CanFrame::FlagEFF;
+        filters[2].mask = uavcan::CanFrame::MaskExtID | uavcan::CanFrame::FlagEFF;
+
+        if (uavcan_lpc11c24::CanDriver::instance().configureFilters(filters, NumFilters) < 0)
+        {
+            board::syslog("Filter init failed\r\n");
+            board::die();
+        }
     }
 
-    while (getLogger().init() < 0)
+    /*
+     * Initializing other libuavcan-related objects
+     */
+    if (getTimeSyncSlave().start() < 0)
     {
+        board::die();
     }
+
+    if (getLogger().init() < 0)
+    {
+        board::die();
+    }
+
     getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
 
     board::resetWatchdog();
 }
 
-void reverse(char* s)
-{
-    for (int i = 0, j = int(std::strlen(s)) - 1; i < j; i++, j--)
-    {
-        const char c = s[i];
-        s[i] = s[j];
-        s[j] = c;
-    }
-}
-
-void lltoa(long long n, char buf[24])
-{
-    const short sign = (n < 0) ? -1 : 1;
-    if (sign < 0)
-    {
-        n = -n;
-    }
-    unsigned i = 0;
-    do
-    {
-        buf[i++] = char(n % 10 + '0');
-    }
-    while ((n /= 10) > 0);
-    if (sign < 0)
-    {
-        buf[i++] = '-';
-    }
-    buf[i] = '\0';
-    reverse(buf);
-}
-
 }
 
 int main()
@@ -143,16 +244,31 @@ int main()
         {
             prev_log_at = ts;
 
-            // We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf()
-            char buf[24];
-            lltoa(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec(), buf);
-            buf[sizeof(buf) - 1] = '\0';
+            /*
+             * CAN bus off state monitoring
+             */
+            if (uavcan_lpc11c24::CanDriver::instance().isInBusOffState())
+            {
+                board::syslog("CAN BUS OFF\r\n");
+            }
 
-            // ...hence we need to construct the message manually:
+            /*
+             * CAN error counter, for debugging purposes
+             */
+            board::syslog("CAN errors: ");
+            board::syslog(intToString(static_cast<long long>(uavcan_lpc11c24::CanDriver::instance().getErrorCount())).c_str());
+            board::syslog(" ");
+            board::syslog(intToString(uavcan_lpc11c24::CanDriver::instance().getRxQueueOverflowCount()).c_str());
+            board::syslog("\r\n");
+
+            /*
+             * We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf(),
+             * so we need to construct the message manually:
+             */
             uavcan::protocol::debug::LogMessage logmsg;
             logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO;
             logmsg.source = "app";
-            logmsg.text = buf;
+            logmsg.text = intToString(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec()).c_str();
             (void)getLogger().log(logmsg);
         }
 
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
index 8a4125599c..26703c47ac 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
@@ -9,24 +9,26 @@
 #include <cstring>
 #include <numeric>
 
-#define PDRUNCFGUSEMASK 0x0000ED00
-#define PDRUNCFGMASKTMP 0x000000FF
+static constexpr unsigned long PDRUNCFGUSEMASK = 0x0000ED00U;
+static constexpr unsigned long PDRUNCFGMASKTMP = 0x000000FFU;
 
-const uint32_t OscRateIn = 12000000; ///< External crystal
-const uint32_t ExtRateIn = 0;
+const std::uint32_t OscRateIn = 12000000; ///< External crystal
+const std::uint32_t ExtRateIn = 0;
 
-uint32_t SystemCoreClock = 12000000; ///< Initialized to default clock value, will be changed on init
+std::uint32_t SystemCoreClock = 12000000; ///< Initialized to default clock value, will be changed on init
 
 namespace board
 {
 namespace
 {
 
-const unsigned ErrorLedPort = 1;
-const unsigned ErrorLedPin  = 10;
+constexpr unsigned TargetSystemCoreClock = 48000000;
 
-const unsigned StatusLedPort = 1;
-const unsigned StatusLedPin  = 11;
+constexpr unsigned ErrorLedPort = 1;
+constexpr unsigned ErrorLedPin  = 10;
+
+constexpr unsigned StatusLedPort = 1;
+constexpr unsigned StatusLedPin  = 11;
 
 struct PinMuxGroup
 {
@@ -34,10 +36,11 @@ struct PinMuxGroup
     unsigned modefunc : 24;
 };
 
-const PinMuxGroup pinmux[] =
+constexpr PinMuxGroup pinmux[] =
 {
-    { IOCON_PIO1_10, IOCON_FUNC0 | IOCON_MODE_INACT }, // Error LED
-    { IOCON_PIO1_11, IOCON_FUNC0 | IOCON_MODE_INACT }  // Status LED
+    { IOCON_PIO1_10, IOCON_FUNC0 | IOCON_MODE_INACT },                                          // Error LED
+    { IOCON_PIO1_11, IOCON_FUNC0 | IOCON_MODE_INACT },                                          // Status LED
+    { IOCON_PIO1_7,  IOCON_FUNC1 | IOCON_HYS_EN | IOCON_MODE_PULLUP },                          // UART_TXD
 };
 
 
@@ -92,7 +95,7 @@ void initClock()
 
     SystemCoreClock = Chip_Clock_GetSystemClockRate();
 
-    while (SystemCoreClock != 48000000) { }  // Loop forever if the clock failed to initialize properly
+    while (SystemCoreClock != TargetSystemCoreClock) { }  // Loop forever if the clock failed to initialize properly
 }
 
 void initGpio()
@@ -109,6 +112,13 @@ void initGpio()
     LPC_GPIO[StatusLedPort].DIR |= 1 << StatusLedPin;
 }
 
+void initUart()
+{
+    Chip_UART_Init(LPC_USART);
+    Chip_UART_SetBaud(LPC_USART, 115200);
+    Chip_UART_TXEnable(LPC_USART);
+}
+
 void init()
 {
     Chip_SYSCTL_SetBODLevels(SYSCTL_BODRSTLVL_2_06V, SYSCTL_BODINTVAL_RESERVED1);
@@ -117,16 +127,32 @@ void init()
     initWatchdog();
     initClock();
     initGpio();
+    initUart();
 
     resetWatchdog();
 }
 
 } // namespace
 
+void die()
+{
+    static const volatile unsigned& DHCSR = *reinterpret_cast<unsigned*>(0xE000EDF0U);
+
+    syslog("FATAL\r\n");
+
+    while (true)
+    {
+        if ((DHCSR & 1U) != 0)
+        {
+            __asm volatile ("bkpt #0\n");   // Break into the debugger
+        }
+    }
+}
+
 #if __GNUC__
 __attribute__((optimize(0)))     // Optimization must be disabled lest it hardfaults in the IAP call
 #endif
-void readUniqueID(uint8_t out_uid[UniqueIDSize])
+void readUniqueID(std::uint8_t out_uid[UniqueIDSize])
 {
     unsigned aligned_array[4] = {};  // out_uid may be unaligned, so we need to use temp array
     unsigned iap_command = 58;
@@ -149,6 +175,11 @@ void resetWatchdog()
     Chip_WWDT_Feed(LPC_WWDT);
 }
 
+void syslog(const char* msg)
+{
+    Chip_UART_SendBlocking(LPC_USART, msg, static_cast<int>(std::strlen(msg)));
+}
+
 } // namespace board
 
 extern "C"
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
index 0882f4b60e..21a64c4424 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
@@ -2,18 +2,31 @@
  * Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
  */
 
-#include <stdint.h>
+#include <cstdint>
 
 namespace board
 {
 
-static const unsigned UniqueIDSize = 16;
+#if __GNUC__
+__attribute__((noreturn))
+#endif
+void die();
 
-void readUniqueID(uint8_t out_uid[UniqueIDSize]);
+static constexpr unsigned UniqueIDSize = 16;
+
+/**
+ * Reads the globally unique 128-bit hardware ID from the MCU.
+ */
+void readUniqueID(std::uint8_t out_uid[UniqueIDSize]);
 
 void setStatusLed(bool state);
 void setErrorLed(bool state);
 
 void resetWatchdog();
 
+/**
+ * Sends the string to UART.
+ */
+void syslog(const char* msg);
+
 }
diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
index 888ef02ab1..ca63b91640 100644
--- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
+++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
@@ -13,13 +13,13 @@
 
 void* __dso_handle;
 
-void* operator new(size_t)
+void* operator new(std::size_t)
 {
     std::abort();
     return reinterpret_cast<void*>(0xFFFFFFFF);
 }
 
-void* operator new[](size_t)
+void* operator new[](std::size_t)
 {
     std::abort();
     return reinterpret_cast<void*>(0xFFFFFFFF);