From f66338d329c2a4069d34474a788ced0a858c116c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 4 Apr 2014 23:28:34 +0400 Subject: [PATCH] STM32: Partially implemented CAN driver (transmission only); added debug tracing macro and some stuff into internal.hpp --- .gitignore | 1 + .../stm32/driver/include/uavcan_stm32/can.hpp | 195 +++++- .../stm32/driver/src/internal.hpp | 53 +- .../stm32/driver/src/uc_stm32_can.cpp | 597 ++++++++++++++++++ .../stm32/test_stm32f107/Makefile | 2 +- .../stm32/test_stm32f107/src/main.cpp | 17 +- 6 files changed, 851 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index 729f770ed8..30541c9aa3 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ __pycache__ .project .cproject .pydevproject +.gdbinit # libuavcan DSDL compiler default output directory dsdlc_generated diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 599aaa6e78..6ef4909eb5 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -4,27 +4,116 @@ #pragma once +#include +#include + #if UAVCAN_STM32_CHIBIOS # include #else # error "Unknown OS" #endif -#include +#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 + 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 +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); + } }; } diff --git a/libuavcan_drivers/stm32/driver/src/internal.hpp b/libuavcan_drivers/stm32/driver/src/internal.hpp index 6fc959aa5b..31cd62d7e8 100644 --- a/libuavcan_drivers/stm32/driver/src/internal.hpp +++ b/libuavcan_drivers/stm32/driver/src/internal.hpp @@ -10,17 +10,32 @@ # error "Unknown OS" #endif +#if UAVCAN_STM32_DEBUG +# include +# include +#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(); } +}; + +} diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp index d3902a7bcb..0fd3c6f4fc 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp @@ -2,4 +2,601 @@ * Copyright (C) 2014 Pavel Kirienko */ +#include +#include #include +#include +#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(pclk), static_cast(target_bitrate), + static_cast(out_timings.prescaler), static_cast(out_timings.sjw), + static_cast(out_timings.bs1), static_cast(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" diff --git a/libuavcan_drivers/stm32/test_stm32f107/Makefile b/libuavcan_drivers/stm32/test_stm32f107/Makefile index 47f5c8b07b..0f1e369556 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/Makefile +++ b/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 diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp index 37cb285c8e..24c89074a0 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp +++ b/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(app::can.driver.getIface(0))->send(frame, deadline, 0); + 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())); } }