diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 6ef4909eb5..f23e51e180 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/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 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(); diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp index 2ed001a866..4eda4d3195 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp +++ b/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; - (void)out_frame; - (void)out_ts_monotonic; - (void)out_ts_utc; - (void)out_flags; - return -1; + out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps + uavcan::uint64_t utc_usec = 0; + { + CriticalSectionLock lock; + if (rx_queue_.getLength() == 0) + { + 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(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); - (void)fifo_index; - (void)utc_usec; + assert(fifo_index < 2); + + 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(); } diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp index 24c89074a0..263052df09 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp +++ b/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(app::can.driver.getIface(0))->send(frame, deadline, 0); + const int send_res = + static_cast(app::can.driver.getIface(0))->send(frame, deadline, + uavcan::CanIOFlagLoopback); printf("send_res=%i errcnt=%lu hwerr=%i\n", send_res, static_cast(app::can.driver.getIface(0)->getErrorCount()), static_cast(app::can.driver.getIface(0)->yieldLastHardwareErrorCode())); + + while (true) + { + uavcan::CanRxFrame rx_frame; + uavcan::CanIOFlags flags = 0; + + const int recv_res = + static_cast(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; + } + } } }