STM32: Partially implemented CAN driver (transmission only); added debug tracing macro and some stuff into internal.hpp

.gitignore

@@ -13,6 +13,7 @@ __pycache__
 .project
 .cproject
 .pydevproject
+.gdbinit
 
 # libuavcan DSDL compiler default output directory
 dsdlc_generated

libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp

@@ -4,27 +4,116 @@
 
 #pragma once
 
+#include <uavcan_stm32/thread.hpp>
+#include <uavcan/driver/can.hpp>
+
 #if UAVCAN_STM32_CHIBIOS
 # include <hal.h>
 #else
 # error "Unknown OS"
 #endif
 
-#include <uavcan/driver/can.hpp>
+#ifndef UAVCAN_STM32_NUM_IFACES
+# if defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F4XX)
+#  define UAVCAN_STM32_NUM_IFACES   2
+# else
+#  define UAVCAN_STM32_NUM_IFACES   1
+# endif
+#endif
+
+#if UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2
+# error UAVCAN_STM32_NUM_IFACES
+#endif
 
 namespace uavcan_stm32
 {
-
-enum { CanFiltersPerIface = 14 };
-
-
-class CanIface : public uavcan::ICanIface
+/**
+ * RX queue item
+ */
+struct CanRxItem
 {
-    CAN_TypeDef* can_;
+    uavcan::uint64_t utc_usec;
+    uavcan::CanFrame frame;
+    uavcan::CanIOFlags flags;
+    CanRxItem()
+        : utc_usec(0)
+        , flags(0)
+    { }
+};
 
+/**
+ * Single CAN iface.
+ */
+class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
+{
+    class RxQueue
+    {
+        CanRxItem* const buf_;
+        const uavcan::uint8_t capacity_;
+        uavcan::uint8_t in_;
+        uavcan::uint8_t out_;
+        uavcan::uint8_t len_;
+        uavcan::uint32_t overflow_cnt_;
+
+        void registerOverflow();
+
+    public:
+        RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
+            : buf_(buf)
+            , capacity_(capacity)
+            , in_(0)
+            , out_(0)
+            , len_(0)
+            , overflow_cnt_(0)
+        { }
+
+        void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
+        void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
+
+        unsigned getLength() const { return len_; }
+
+        uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; }
+    };
+
+    struct Timings
+    {
+        uavcan::uint16_t prescaler;
+        uavcan::uint8_t sjw;
+        uavcan::uint8_t bs1;
+        uavcan::uint8_t bs2;
+
+        Timings()
+            : prescaler(0)
+            , sjw(0)
+            , bs1(0)
+            , bs2(0)
+        { }
+    };
+
+    struct TxItem
+    {
+        uavcan::CanFrame frame;
+        uavcan::MonotonicTime deadline;
+        bool pending;
+        bool loopback;
+        TxItem()
+            : pending(false)
+            , loopback(false)
+        { }
+    };
+
+    enum { NumTxMailboxes = 3 };
+    enum { NumFilters = 14 };
+
+    RxQueue rx_queue_;
+    CAN_TypeDef* const can_;
     uavcan::uint64_t error_cnt_;
+    Event& update_event_;
+    TxItem pending_tx_[NumTxMailboxes];
+    uavcan::uint8_t last_hw_error_code_;
+
+    int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
 
-public:
     virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
                                  uavcan::CanIOFlags flags);
 
@@ -34,9 +123,95 @@ public:
     virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
                                              uavcan::uint16_t num_configs);
 
-    virtual uavcan::uint16_t getNumFilters() const { return CanFiltersPerIface; }
+    virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
 
-    virtual uavcan::uint64_t getErrorCount() const { return error_cnt_; }
+    void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
+
+public:
+    enum { MaxRxQueueCapacity = 254 };
+
+    CanIface(CAN_TypeDef* can, Event& update_event, CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
+        : rx_queue_(rx_queue_buffer, rx_queue_capacity)
+        , can_(can)
+        , error_cnt_(0)
+        , update_event_(update_event)
+        , last_hw_error_code_(0)
+    { }
+
+    /**
+     * Assumes:
+     *   - Iface clock is enabled
+     *   - Iface has been resetted via RCC
+     *   - Interrupts are disabled
+     *   - Caller will configure NVIC by itself
+     */
+    int init(uavcan::uint32_t bitrate);
+
+    void handleTxInterrupt(uavcan::uint64_t utc_usec);
+    void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec);
+    void handleStatusInterrupt();
+
+    void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
+
+    bool isTxBufferFull() const;
+    bool isRxBufferEmpty() const { return rx_queue_.getLength() == 0; }
+
+    virtual uavcan::uint64_t getErrorCount() const;
+    uavcan::uint8_t yieldLastHardwareErrorCode();
+};
+
+/**
+ * CAN driver, incorporates all available CAN ifaces.
+ */
+class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
+{
+    Event update_event_;
+    CanIface if0_;
+#if UAVCAN_STM32_NUM_IFACES > 1
+    CanIface if1_;
+#endif
+
+    virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline);
+
+public:
+    template <unsigned RxQueueCapacity>
+    CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
+        : if0_(CAN1, update_event_, rx_queue_storage[0], RxQueueCapacity)
+#if UAVCAN_STM32_NUM_IFACES > 1
+        , if1_(CAN2, update_event_, rx_queue_storage[1], RxQueueCapacity)
+#endif
+    {
+        uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
+    }
+
+    int init(uavcan::uint32_t bitrate);
+
+    virtual CanIface* getIface(uavcan::uint8_t iface_index);
+
+    virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; }
+};
+
+/**
+ * Helper class.
+ * Normally only this class should be used by the application.
+ * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 16 RX slots * 145 usec --> 2320 usec before RX queue overruns.
+ */
+template <unsigned RxQueueCapacity = 16>
+class CanInitHelper
+{
+    CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
+
+public:
+    CanDriver driver;
+
+    CanInitHelper()
+        : driver(queue_storage_)
+    { }
+
+    int init(uavcan::uint32_t bitrate)
+    {
+        return driver.init(bitrate);
+    }
 };
 
 }

libuavcan_drivers/stm32/driver/src/internal.hpp

@@ -10,17 +10,32 @@
 # error "Unknown OS"
 #endif
 
+#if UAVCAN_STM32_DEBUG
+# include <cstdio>
+# include <cstdarg>
+#endif
+
 /**
  * IRQ handler macros
  */
 #if UAVCAN_STM32_CHIBIOS
+
 # define UAVCAN_STM32_IRQ_HANDLER(id)  CH_IRQ_HANDLER(id)
-# define UAVCAN_STM32_IRQ_PROLOGUE()   CH_IRQ_PROLOGUE()
-# define UAVCAN_STM32_IRQ_EPILOGUE()   CH_IRQ_EPILOGUE()
+
+# define UAVCAN_STM32_IRQ_PROLOGUE()    \
+    CH_IRQ_PROLOGUE(); \
+    chSysLockFromIsr()
+
+# define UAVCAN_STM32_IRQ_EPILOGUE()    \
+    chSysUnlockFromIsr(); \
+    CH_IRQ_EPILOGUE()
+
 #else
+
 # define UAVCAN_STM32_IRQ_HANDLER(id)  void id(void)
 # define UAVCAN_STM32_IRQ_PROLOGUE()
 # define UAVCAN_STM32_IRQ_EPILOGUE()
+
 #endif
 
 /**
@@ -39,9 +54,43 @@
 # error UAVCAN_STM32_TIMER_NUMBER
 #endif
 
+/**
+ * Driver debug output
+ */
+#if UAVCAN_STM32_DEBUG
+# if __GNUC__
+__attribute__ ((format(printf, 1, 2)))
+# endif
+static void UAVCAN_STM32_TRACE(const char* fmt, ...)
+{
+    (void)UAVCAN_STM32_TRACE;
+    va_list args;
+    (void)std::printf("UAVCAN: STM32: ");
+    va_start(args, fmt);
+    (void)std::vprintf(fmt, args);
+    va_end(args);
+    (void)std::puts("");
+}
+#else
+# define UAVCAN_STM32_TRACE(...)    ((void)0)
+#endif
 
+/**
+ * Glue macros
+ */
 #define UAVCAN_STM32_GLUE2_(A, B)       A##B
 #define UAVCAN_STM32_GLUE2(A, B)        UAVCAN_STM32_GLUE2_(A, B)
 
 #define UAVCAN_STM32_GLUE3_(A, B, C)    A##B##C
 #define UAVCAN_STM32_GLUE3(A, B, C)     UAVCAN_STM32_GLUE3_(A, B, C)
+
+namespace uavcan_stm32
+{
+
+struct CriticalSectionLock
+{
+    CriticalSectionLock() { chSysSuspend(); }
+    ~CriticalSectionLock() { chSysEnable(); }
+};
+
+}

libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp

@@ -2,4 +2,601 @@
  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
+#include <cassert>
+#include <cstring>
 #include <uavcan_stm32/can.hpp>
+#include <uavcan_stm32/clock.hpp>
+#include "internal.hpp"
+
+namespace uavcan_stm32
+{
+namespace
+{
+
+CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] =
+{
+    NULL
+#if UAVCAN_STM32_NUM_IFACES > 1
+    , NULL
+#endif
+};
+
+inline void handleTxInterrupt(uavcan::uint8_t iface_index)
+{
+    uavcan::uint64_t utc_usec = clock::getUtcUSecFromInterrupt();
+    if (utc_usec > 0)
+    {
+        utc_usec--;
+    }
+    if (iface_index < UAVCAN_STM32_NUM_IFACES &&
+        ifaces[iface_index] != NULL)
+    {
+        ifaces[iface_index]->handleTxInterrupt(utc_usec);
+    }
+    else
+    {
+        assert(0);
+    }
+}
+
+inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index)
+{
+    uavcan::uint64_t utc_usec = clock::getUtcUSecFromInterrupt();
+    if (utc_usec > 0)
+    {
+        utc_usec--;
+    }
+    if (iface_index < UAVCAN_STM32_NUM_IFACES &&
+        ifaces[iface_index] != NULL)
+    {
+        ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec);
+    }
+    else
+    {
+        assert(0);
+    }
+}
+
+inline void handleStatusInterrupt(uavcan::uint8_t iface_index)
+{
+    if (iface_index < UAVCAN_STM32_NUM_IFACES &&
+        ifaces[iface_index] != NULL)
+    {
+        ifaces[iface_index]->handleStatusInterrupt();
+    }
+    else
+    {
+        assert(0);
+    }
+}
+
+} // namespace
+
+/*
+ * CanIface::RxQueue
+ */
+void CanIface::RxQueue::registerOverflow()
+{
+    if (overflow_cnt_ < 0xFFFFFFFF)
+    {
+        overflow_cnt_++;
+    }
+}
+
+void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags)
+{
+    buf_[in_].frame    = frame;
+    buf_[in_].utc_usec = utc_usec;
+    buf_[in_].flags    = flags;
+    in_++;
+    if (in_ >= capacity_)
+    {
+        in_ = 0;
+    }
+    len_++;
+    if (len_ >= capacity_)
+    {
+        len_ = capacity_;
+        registerOverflow();
+        out_++;
+        if (out_ >= capacity_)
+        {
+            out_ = 0;
+        }
+    }
+}
+
+void CanIface::RxQueue::pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags)
+{
+    if (len_ > 0)
+    {
+        out_frame    = buf_[out_].frame;
+        out_utc_usec = buf_[out_].utc_usec;
+        out_flags    = buf_[out_].flags;
+        out_++;
+        if (out_ >= capacity_)
+        {
+            out_ = 0;
+        }
+        len_--;
+    }
+    else { assert(0); }
+}
+
+/*
+ * CanIface
+ */
+int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings)
+{
+    /*
+     *   BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2))
+     *   BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2))
+     * let:
+     *   BS = 1 + BS1 + BS2
+     *   PRESCALER_BS = PRESCALER * BS
+     * ==>
+     *   PRESCALER_BS = PCLK / BITRATE
+     */
+    if (target_bitrate < 20000 || target_bitrate > 1000000)
+    {
+        return -1;
+    }
+
+    const uavcan::uint32_t pclk = STM32_PCLK1;
+    const uavcan::uint32_t prescaler_bs = pclk / target_bitrate;
+
+    // Initial guess:
+    uavcan::int8_t bs1 = 10;  // max 15
+    uavcan::int8_t bs2 = 5;   // max 8
+    uavcan::uint16_t prescaler = 0;
+
+    while (1)
+    {
+        prescaler = prescaler_bs / (1 + bs1 + bs2);
+        // Check result:
+        if ((prescaler >= 1) && (prescaler <= 1024))
+        {
+            const uavcan::uint32_t current_bitrate = pclk / (prescaler * (1 + bs1 + bs2));
+            if (current_bitrate == target_bitrate)
+            {
+                break;
+            }
+        }
+        if (bs1 > bs2)
+        {
+            bs1--;
+        }
+        else
+        {
+            bs2--;
+        }
+        if (bs1 <= 0 || bs2 <= 0)
+        {
+            return -1;
+        }
+    }
+    if (!((prescaler >= 1) && (prescaler <= 1024)))
+    {
+        return -1;
+    }
+    out_timings.prescaler = prescaler - 1;
+    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;
+}
+
+uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+                               uavcan::CanIOFlags flags)
+{
+    if (frame.isErrorFrame() || frame.dlc > 8)
+    {
+        return -1;  // WTF man how to handle that
+    }
+
+    CriticalSectionLock lock;
+
+    /*
+     * Seeking for an empty slot
+     */
+    uavcan::uint8_t txmailbox = 0xFF;
+    if ((can_->TSR & CAN_TSR_TME0) == CAN_TSR_TME0)
+    {
+        txmailbox = 0;
+    }
+    else if ((can_->TSR & CAN_TSR_TME1) == CAN_TSR_TME1)
+    {
+        txmailbox = 1;
+    }
+    else if ((can_->TSR & CAN_TSR_TME2) == CAN_TSR_TME2)
+    {
+        txmailbox = 2;
+    }
+    else { return 0; } // No way
+
+    /*
+     * Setting up the mailbox
+     */
+    CAN_TxMailBox_TypeDef& mb = can_->sTxMailBox[txmailbox];
+    if (frame.isExtended())
+    {
+        mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | CAN_TI0R_IDE;
+    }
+    else
+    {
+        mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21);
+    }
+
+    if (frame.isRemoteTransmissionRequest())
+    {
+        mb.TIR |= CAN_TI0R_RTR;
+    }
+
+    mb.TDTR = frame.dlc;
+
+    mb.TDHR = (uavcan::uint32_t(frame.data[7]) << 24) |
+              (uavcan::uint32_t(frame.data[6]) << 16) |
+              (uavcan::uint32_t(frame.data[5]) << 8)  |
+              (uavcan::uint32_t(frame.data[4]) << 0);
+    mb.TDLR = (uavcan::uint32_t(frame.data[3]) << 24) |
+              (uavcan::uint32_t(frame.data[2]) << 16) |
+              (uavcan::uint32_t(frame.data[1]) << 8)  |
+              (uavcan::uint32_t(frame.data[0]) << 0);
+
+    mb.TIR |= CAN_TI0R_TXRQ;  // Go.
+
+    /*
+     * Registering the pending transmission so we can track its deadline and loopback it as needed
+     */
+    TxItem& txi = pending_tx_[txmailbox];
+    txi.deadline = tx_deadline;
+    txi.frame    = frame;
+    txi.loopback = (flags & uavcan::CanIOFlagLoopback) == uavcan::CanIOFlagLoopback;
+    txi.pending  = true;
+    return 1;
+}
+
+uavcan::int16_t CanIface::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+                                  uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
+{
+    CriticalSectionLock lock;
+    (void)out_frame;
+    (void)out_ts_monotonic;
+    (void)out_ts_utc;
+    (void)out_flags;
+    return -1;
+}
+
+uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs,
+                                           uavcan::uint16_t num_configs)
+{
+    CriticalSectionLock lock;
+    (void)filter_configs;
+    (void)num_configs;
+    return -1;
+}
+
+int CanIface::init(uavcan::uint32_t bitrate)
+{
+    int res = 0;
+
+    /*
+     * CAN timings for this bitrate
+     */
+    Timings timings;
+    res = computeTimings(bitrate, timings);
+    if (res < 0)
+    {
+        goto leave;
+    }
+
+    /*
+     * Hardware initialization
+     */
+    can_->MCR &= ~CAN_MCR_SLEEP; // Exit sleep mode
+    can_->MCR |= CAN_MCR_INRQ;   // Request init
+
+    for (unsigned wait_ack = 0; wait_ack < 1000000; wait_ack++)
+    {
+        if ((can_->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)
+        {
+            break;
+        }
+    }
+    if ((can_->MSR & CAN_MSR_INAK) != CAN_MSR_INAK)
+    {
+        res = -1;
+        goto leave;
+    }
+
+    can_->MCR = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_INRQ;  // RM page 648
+
+    can_->BTR = ((timings.sjw & 3)  << 24) |
+                ((timings.bs1 & 15) << 16) |
+                ((timings.bs2 & 7)  << 20) |
+                (timings.prescaler & 1023);
+
+    can_->IER = CAN_IER_TMEIE |   // TX mailbox empty
+                CAN_IER_ERRIE |   // Error set in ESR register
+                CAN_IER_LECIE |   // LEC in ESR updated
+                CAN_IER_FOVIE0 |  // RX FIFO 0 overrun
+                CAN_IER_FOVIE1 |  // RX FIFO 1 overrun
+                CAN_IER_FMPIE0 |  // RX FIFO 0 is not empty
+                CAN_IER_FMPIE1;   // RX FIFO 1 is not empty
+
+    can_->MCR &= ~CAN_MCR_INRQ;  // Leave init mode
+
+    for (unsigned wait_ack = 0; wait_ack < 1000000; wait_ack++)
+    {
+        if ((can_->MSR & CAN_MSR_INAK) != CAN_MSR_INAK)
+        {
+            break;
+        }
+    }
+    if ((can_->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)
+    {
+        res = -1;
+        goto leave;
+    }
+
+leave:
+    return res;
+}
+
+void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec)
+{
+    assert(mailbox_index < NumTxMailboxes);
+    TxItem& txi = pending_tx_[mailbox_index];
+    if (txi.loopback && txok && txi.pending)
+    {
+        rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback);
+    }
+    txi.pending = false;
+}
+
+void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec)
+{
+    // TXOK == false means that there was a hardware failure
+    if (can_->TSR & CAN_TSR_RQCP0)
+    {
+        const bool txok = can_->TSR & CAN_TSR_TXOK0;
+        can_->TSR = CAN_TSR_RQCP0;
+        handleTxMailboxInterrupt(0, txok, utc_usec);
+    }
+    if (can_->TSR & CAN_TSR_RQCP1)
+    {
+        const bool txok = can_->TSR & CAN_TSR_TXOK1;
+        can_->TSR = CAN_TSR_RQCP1;
+        handleTxMailboxInterrupt(1, txok, utc_usec);
+    }
+    if (can_->TSR & CAN_TSR_RQCP2)
+    {
+        const bool txok = can_->TSR & CAN_TSR_TXOK2;
+        can_->TSR = CAN_TSR_RQCP2;
+        handleTxMailboxInterrupt(2, txok, utc_usec);
+    }
+    update_event_.signalFromInterrupt();
+}
+
+void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
+{
+    assert(fifo_index == 0 || fifo_index == 1);
+    (void)fifo_index;
+    (void)utc_usec;
+    update_event_.signalFromInterrupt();
+}
+
+void CanIface::handleStatusInterrupt()
+{
+    last_hw_error_code_ = (can_->ESR & CAN_ESR_LEC) >> 4;
+    can_->ESR = 0;
+    can_->MSR = CAN_MSR_ERRI | CAN_MSR_WKUI | CAN_MSR_SLAKI;
+    error_cnt_++;
+}
+
+void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
+{
+    static const uavcan::uint32_t AbortFlags[NumTxMailboxes] = { CAN_TSR_ABRQ0, CAN_TSR_ABRQ1, CAN_TSR_ABRQ2 };
+    CriticalSectionLock lock;
+    for (int i = 0; i < NumTxMailboxes; i++)
+    {
+        TxItem& txi = pending_tx_[i];
+        if (txi.pending && txi.deadline < current_time)
+        {
+            can_->TSR = AbortFlags[i];  // Goodnight sweet transmission
+            txi.pending = false;
+            error_cnt_++;
+        }
+    }
+}
+
+bool CanIface::isTxBufferFull() const
+{
+    return (can_->TSR & CAN_TSR_TME) == 0;
+}
+
+uavcan::uint64_t CanIface::getErrorCount() const
+{
+    CriticalSectionLock lock;
+    return error_cnt_ + rx_queue_.getOverflowCount();
+}
+
+uavcan::uint8_t CanIface::yieldLastHardwareErrorCode()
+{
+    CriticalSectionLock lock;
+    const uavcan::uint8_t val = last_hw_error_code_;
+    last_hw_error_code_ = 0;
+    return val;
+}
+
+/*
+ * CanDriver
+ */
+uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline)
+{
+    (void)inout_masks;
+    (void)blocking_deadline;
+    return -1;
+}
+
+int CanDriver::init(uavcan::uint32_t bitrate)
+{
+    int res = 0;
+
+    CriticalSectionLock lock;
+
+    /*
+     * CAN1
+     */
+    RCC->APB1ENR  |=  RCC_APB1ENR_CAN1EN;
+    RCC->APB1RSTR |=  RCC_APB1RSTR_CAN1RST;
+    RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
+
+    res = if0_.init(bitrate);
+    if (res < 0)
+    {
+        goto fail;
+    }
+    ifaces[0] = &if0_;
+
+    /*
+     * CAN2
+     */
+#if UAVCAN_STM32_NUM_IFACES > 1
+    RCC->APB1ENR  |=  RCC_APB1ENR_CAN2EN;
+    RCC->APB1RSTR |=  RCC_APB1RSTR_CAN2RST;
+    RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
+
+    res = if1_.init(bitrate);
+    if (res < 0)
+    {
+        goto fail;
+    }
+    ifaces[1] = &if1_;
+#endif
+
+    /*
+     * IRQ
+     */
+    nvicEnableVector(CAN1_TX_IRQn,  UAVCAN_STM32_IRQ_PRIORITY_MASK);
+    nvicEnableVector(CAN1_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+    nvicEnableVector(CAN1_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+    nvicEnableVector(CAN1_SCE_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+#if UAVCAN_STM32_NUM_IFACES > 1
+    nvicEnableVector(CAN2_TX_IRQn,  UAVCAN_STM32_IRQ_PRIORITY_MASK);
+    nvicEnableVector(CAN2_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+    nvicEnableVector(CAN2_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+    nvicEnableVector(CAN2_SCE_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
+#endif
+
+    assert(res >= 0);
+    return res;
+
+fail:
+    assert(res < 0);
+
+    RCC->APB1ENR &= ~RCC_APB1ENR_CAN1EN;
+
+#if UAVCAN_STM32_NUM_IFACES > 1
+    RCC->APB1ENR &= ~RCC_APB1ENR_CAN2EN;
+#endif
+    return res;
+}
+
+CanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
+{
+    if (iface_index < UAVCAN_STM32_NUM_IFACES)
+    {
+        return ifaces[iface_index];
+    }
+    return NULL;
+}
+
+} // namespace uavcan_stm32
+
+/*
+ * Interrupt handlers
+ */
+#if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F4XX))
+
+// IRQ numbers
+#define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn
+#define CAN1_TX_IRQn USB_HP_CAN1_TX_IRQn
+
+// IRQ vectors
+#if !defined(CAN1_RX0_IRQHandler) || !defined(CAN1_TX_IRQHandler)
+# define CAN1_TX_IRQHandler   USB_HP_CAN1_TX_IRQHandler
+# define CAN1_RX0_IRQHandler  USB_LP_CAN1_RX0_IRQHandler
+#endif
+
+#endif
+
+extern "C"
+{
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleTxInterrupt(0);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleRxInterrupt(0, 0);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleRxInterrupt(0, 1);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleStatusInterrupt(0);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+#if UAVCAN_STM32_NUM_IFACES > 1
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleTxInterrupt(1);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleRxInterrupt(1, 0);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleRxInterrupt(1, 1);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+UAVCAN_STM32_IRQ_HANDLER(CAN2_SCE_IRQHandler)
+{
+    UAVCAN_STM32_IRQ_PROLOGUE();
+    uavcan_stm32::handleStatusInterrupt(1);
+    UAVCAN_STM32_IRQ_EPILOGUE();
+}
+
+#endif
+
+} // extern "C"

libuavcan_drivers/stm32/test_stm32f107/Makefile

@@ -10,7 +10,7 @@ PROJECT = uavcan_test_stm32f107
 
 CPPSRC = src/main.cpp src/dummy.cpp
 
-UDEFS = -DUAVCAN_STM32_CHIBIOS=1
+UDEFS = -DUAVCAN_STM32_CHIBIOS=1 -DUAVCAN_STM32_DEBUG=1
 
 #
 # UAVCAN library

libuavcan_drivers/stm32/test_stm32f107/src/main.cpp

@@ -11,6 +11,8 @@ namespace app
 namespace
 {
 
+uavcan_stm32::CanInitHelper<> can;
+
 void ledSet(bool state)
 {
     palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state);
@@ -22,7 +24,7 @@ int init()
     chibios_rt::System::init();
     sdStart(&STDOUT_SD, NULL);
 
-    return 0;
+    return can.init(1000000);
 }
 
 __attribute__((noreturn))
@@ -87,5 +89,18 @@ int main()
         {
             uavcan_stm32::SystemClock::instance().adjustUtc(uavcan::UtcDuration::fromMSec(10000));
         }
+
+        uavcan::CanFrame frame;
+        const uavcan::MonotonicTime deadline =
+            uavcan_stm32::clock::getMonotonic() + uavcan::MonotonicDuration::fromMSec(10);
+
+        frame.id = 123 | uavcan::CanFrame::FlagEFF;
+        frame.data[0] = 42;
+        frame.dlc = 2;
+
+        const int send_res = static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->send(frame, deadline, 0);
+        printf("send_res=%i errcnt=%lu hwerr=%i\n", send_res,
+               static_cast<unsigned long>(app::can.driver.getIface(0)->getErrorCount()),
+               static_cast<int>(app::can.driver.getIface(0)->yieldLastHardwareErrorCode()));
     }
 }