STM32: RX

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

@@ -111,6 +111,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
     Event& update_event_;
     TxItem pending_tx_[NumTxMailboxes];
     uavcan::uint8_t last_hw_error_code_;
+    const uavcan::uint8_t self_index_;
 
     int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
 
@@ -130,13 +131,17 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
 public:
     enum { MaxRxQueueCapacity = 254 };
 
-    CanIface(CAN_TypeDef* can, Event& update_event, CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
+    CanIface(CAN_TypeDef* can, Event& update_event, uavcan::uint8_t self_index,
+             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)
-    { }
+        , self_index_(self_index)
+    {
+        assert(self_index_ < UAVCAN_STM32_NUM_IFACES);
+    }
 
     /**
      * Assumes:
@@ -176,9 +181,9 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
 public:
     template <unsigned RxQueueCapacity>
     CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
-        : if0_(CAN1, update_event_, rx_queue_storage[0], RxQueueCapacity)
+        : if0_(CAN1, update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
 #if UAVCAN_STM32_NUM_IFACES > 1
-        , if1_(CAN2, update_event_, rx_queue_storage[1], RxQueueCapacity)
+        , if1_(CAN2, update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
 #endif
     {
         uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();

libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp

@@ -264,17 +264,24 @@ uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicT
 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;
+    out_ts_monotonic = clock::getMonotonic();  // High precision is not required for monotonic timestamps
-    (void)out_frame;
+    uavcan::uint64_t utc_usec = 0;
-    (void)out_ts_monotonic;
+    {
-    (void)out_ts_utc;
+        CriticalSectionLock lock;
-    (void)out_flags;
+        if (rx_queue_.getLength() == 0)
-    return -1;
+        {
+            return 0;
+        }
+        rx_queue_.pop(out_frame, utc_usec, out_flags);
+    }
+    out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec);
+    return 1;
 }
 
 uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs,
                                            uavcan::uint16_t num_configs)
 {
+    // TODO: Hardware filter support
     CriticalSectionLock lock;
     (void)filter_configs;
     (void)num_configs;
@@ -324,8 +331,6 @@ int CanIface::init(uavcan::uint32_t bitrate)
     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
 
@@ -344,6 +349,36 @@ int CanIface::init(uavcan::uint32_t bitrate)
         goto leave;
     }
 
+    /*
+     * Default filter configuration
+     */
+    if (self_index_ == 0)
+    {
+        can_->FMR |= CAN_FMR_FINIT;
+
+        can_->FMR &= 0xFFFFC0F1;
+        can_->FMR |= static_cast<uavcan::uint32_t>(NumFilters) << 8;  // Slave (CAN2) gets half of the filters
+
+        can_->FFA1R = 0;                           // All assigned to FIFO0 by default
+        can_->FM1R = 0;                            // Indentifier Mask mode
+
+#if UAVCAN_STM32_NUM_IFACES > 1
+        can_->FS1R = 0x7ffffff;                    // Single 32-bit for all
+        can_->sFilterRegister[0].FR1 = 0;          // CAN1 accepts everything
+        can_->sFilterRegister[0].FR2 = 0;
+        can_->sFilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything
+        can_->sFilterRegister[NumFilters].FR2 = 0;
+        can_->FA1R = 1 | (1 << NumFilters);        // One filter per each iface
+#else
+        can_->FS1R = 0x1fff;
+        can_->sFilterRegister[0].FR1 = 0;
+        can_->sFilterRegister[0].FR2 = 0;
+        can_->FA1R = 1;
+#endif
+
+        can_->FMR &= ~CAN_FMR_FINIT;
+    }
+
 leave:
     return res;
 }
@@ -385,9 +420,61 @@ void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec)
 
 void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
 {
-    assert(fifo_index == 0 || fifo_index == 1);
+    assert(fifo_index < 2);
-    (void)fifo_index;
+
-    (void)utc_usec;
+    volatile uavcan::uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
+    if ((*rfr_reg & CAN_RF0R_FMP0) == 0)
+    {
+        assert(0);  // Weird, IRQ is here but no data to read
+        return;
+    }
+
+    /*
+     * Register overflow as a hardware error
+     */
+    if ((*rfr_reg & CAN_RF0R_FOVR0) != 0)
+    {
+        error_cnt_++;
+    }
+
+    /*
+     * Read the frame contents
+     */
+    uavcan::CanFrame frame;
+    const CAN_FIFOMailBox_TypeDef& rf = can_->sFIFOMailBox[fifo_index];
+
+    if ((rf.RIR & CAN_RI0R_IDE) == 0)
+    {
+        frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21);
+    }
+    else
+    {
+        frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3);
+        frame.id |= uavcan::CanFrame::FlagEFF;
+    }
+
+    if ((rf.RIR & CAN_RI0R_RTR) != 0)
+    {
+        frame.id |= uavcan::CanFrame::FlagRTR;
+    }
+
+    frame.dlc = rf.RDTR & 15;
+
+    frame.data[0] = 0xFF & (rf.RDLR >> 0);
+    frame.data[1] = 0xFF & (rf.RDLR >> 8);
+    frame.data[2] = 0xFF & (rf.RDLR >> 16);
+    frame.data[3] = 0xFF & (rf.RDLR >> 24);
+    frame.data[4] = 0xFF & (rf.RDHR >> 0);
+    frame.data[5] = 0xFF & (rf.RDHR >> 8);
+    frame.data[6] = 0xFF & (rf.RDHR >> 16);
+    frame.data[7] = 0xFF & (rf.RDHR >> 24);
+
+    *rfr_reg = CAN_RF0R_RFOM0 | CAN_RF0R_FOVR0 | CAN_RF0R_FULL0;  // Release FIFO entry we just read
+
+    /*
+     * Store with timeout into the FIFO buffer and signal update event
+     */
+    rx_queue_.push(frame, utc_usec, 0);
     update_event_.signalFromInterrupt();
 }
 

libuavcan_drivers/stm32/test_stm32f107/src/main.cpp

@@ -98,9 +98,31 @@ int main()
         frame.data[0] = 42;
         frame.dlc = 2;
 
-        const int send_res = static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->send(frame, deadline, 0);
+        const int send_res =
+            static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->send(frame, deadline,
+                                                                               uavcan::CanIOFlagLoopback);
         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()));
+
+        while (true)
+        {
+            uavcan::CanRxFrame rx_frame;
+            uavcan::CanIOFlags flags = 0;
+
+            const int recv_res =
+                static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->receive(rx_frame, rx_frame.ts_mono,
+                                                                                      rx_frame.ts_utc, flags);
+
+            printf("recv_res=%i flg=%u canid=%lu dlc=%u data=[%02x %02x %02x %02x %02x %02x %02x %02x]\n",
+                   recv_res, unsigned(flags), rx_frame.id & uavcan::CanFrame::MaskExtID, int(rx_frame.dlc),
+                   int(rx_frame.data[0]), int(rx_frame.data[1]), int(rx_frame.data[2]), int(rx_frame.data[3]),
+                   int(rx_frame.data[4]), int(rx_frame.data[5]), int(rx_frame.data[6]), int(rx_frame.data[7]));
+
+            if (recv_res <= 0)
+            {
+                break;
+            }
+        }
     }
 }