11 #if UAVCAN_STM32_NUTTX 12 # include <nuttx/arch.h> 13 # include <nuttx/irq.h> 14 # include <arch/board/board.h> 19 #if UAVCAN_STM32_NUTTX 20 # if !defined(STM32_IRQ_CAN1TX) && !defined(STM32_IRQ_CAN1RX0) 21 # define STM32_IRQ_CAN1TX STM32_IRQ_USBHPCANTX 22 # define STM32_IRQ_CAN1RX0 STM32_IRQ_USBLPCANRX0 26 static int can1_irq(
const int irq,
void *,
void *);
27 #if UAVCAN_STM32_NUM_IFACES > 1 28 static int can2_irq(
const int irq,
void *,
void *);
34 #if defined(STM32F3XX) 35 #define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN 36 #define RCC_APB1RSTR_CAN1RST RCC_APB1RSTR_CANRST 37 #define CAN1_TX_IRQn CAN_TX_IRQn 38 #define CAN1_RX0_IRQn CAN_RX0_IRQn 39 #define CAN1_RX1_IRQn CAN_RX1_IRQn 48 CanIface *ifaces[UAVCAN_STM32_NUM_IFACES] = {
50 #if UAVCAN_STM32_NUM_IFACES > 1 55 inline void handleTxInterrupt(uavcan::uint8_t iface_index)
57 UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES);
64 if (ifaces[iface_index] != UAVCAN_NULLPTR) {
65 ifaces[iface_index]->handleTxInterrupt(utc_usec);
72 inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index)
74 UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES);
81 if (ifaces[iface_index] != UAVCAN_NULLPTR) {
82 ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec);
139 }
else { UAVCAN_ASSERT(0); }
161 if (target_bitrate < 1) {
168 #if UAVCAN_STM32_NUTTX 169 const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY;
174 static const int MaxBS1 = 16;
175 static const int MaxBS2 = 8;
188 const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
190 UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2));
192 static const int MaxSamplePointLocation = 900;
204 const uavcan::uint32_t prescaler_bs = pclk / target_bitrate;
209 uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1);
211 while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
212 if (bs1_bs2_sum <= 2) {
219 const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
221 if ((prescaler < 1U) || (prescaler > 1024U)) {
246 uavcan::uint16_t sample_point_permill;
251 sample_point_permill(0)
254 BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) :
256 bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)),
257 sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
259 UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1);
262 bool isValid()
const {
return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); }
266 BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8));
268 if (solution.sample_point_permill > MaxSamplePointLocation) {
270 solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8));
282 if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) {
288 int(1 + solution.bs1 + solution.bs2),
float(solution.sample_point_permill) / 10.F);
290 out_timings.
prescaler = uavcan::uint16_t(prescaler - 1U);
292 out_timings.
bs1 = uavcan::uint8_t(solution.bs1 - 1);
293 out_timings.
bs2 = uavcan::uint8_t(solution.bs2 - 1);
297 uavcan::int16_t
CanIface::send(
const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
298 uavcan::CanIOFlags flags)
300 if (frame.isErrorFrame() || frame.dlc > 8) {
319 CriticalSectionLocker
lock;
324 uavcan::uint8_t txmailbox = 0xFF;
346 if (frame.isExtended()) {
350 mb.
TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21);
353 if (frame.isRemoteTransmissionRequest()) {
359 mb.
TDHR = (uavcan::uint32_t(frame.data[7]) << 24) |
360 (uavcan::uint32_t(frame.data[6]) << 16) |
361 (uavcan::uint32_t(frame.data[5]) << 8) |
362 (uavcan::uint32_t(frame.data[4]) << 0);
363 mb.
TDLR = (uavcan::uint32_t(frame.data[3]) << 24) |
364 (uavcan::uint32_t(frame.data[2]) << 16) |
365 (uavcan::uint32_t(frame.data[1]) << 8) |
366 (uavcan::uint32_t(frame.data[0]) << 0);
376 txi.
loopback = (flags & uavcan::CanIOFlagLoopback) != 0;
377 txi.
abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
382 uavcan::int16_t
CanIface::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
383 uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags)
386 uavcan::uint64_t utc_usec = 0;
388 CriticalSectionLocker
lock;
396 out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec);
401 uavcan::uint16_t num_configs)
404 CriticalSectionLocker
lock;
418 if (num_configs == 0) {
422 can_->
FA1R |= 1U << filter_start_index;
426 if (i < num_configs) {
430 const uavcan::CanFilterConfig *
const cfg = filter_configs + i;
432 if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) {
433 id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3;
434 mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3;
438 id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21;
439 mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21;
442 if (cfg->id & uavcan::CanFrame::FlagRTR) {
446 if (cfg->mask & uavcan::CanFrame::FlagEFF) {
450 if (cfg->mask & uavcan::CanFrame::FlagRTR) {
457 can_->
FA1R |= (1 << (filter_start_index + i));
460 can_->
FA1R &= ~(1 << (filter_start_index + i));
475 #if UAVCAN_STM32_NUTTX 476 const unsigned Timeout = 1000;
478 const unsigned Timeout = 2000000;
481 for (
unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) {
484 if (state == target_state) {
488 #if UAVCAN_STM32_NUTTX 502 CriticalSectionLocker
lock;
532 if (timings_res < 0) {
538 unsigned(timings.
prescaler),
unsigned(timings.
sjw),
unsigned(timings.
bs1),
unsigned(timings.
bs2));
546 ((timings.
bs1 & 15U) << 16) |
547 ((timings.
bs2 & 7U) << 20) |
575 #if UAVCAN_STM32_NUM_IFACES > 1 639 UAVCAN_ASSERT(fifo_index < 2);
641 volatile uavcan::uint32_t *
const rfr_reg = (fifo_index == 0) ? &
can_->
RF0R : &
can_->
RF1R;
658 uavcan::CanFrame frame;
662 frame.id = uavcan::CanFrame::MaskStdID & (rf.
RIR >> 21);
665 frame.id = uavcan::CanFrame::MaskExtID & (rf.
RIR >> 3);
666 frame.id |= uavcan::CanFrame::FlagEFF;
670 frame.id |= uavcan::CanFrame::FlagRTR;
673 frame.dlc = rf.
RDTR & 15;
675 frame.data[0] = uavcan::uint8_t(0xFF & (rf.
RDLR >> 0));
676 frame.data[1] = uavcan::uint8_t(0xFF & (rf.
RDLR >> 8));
677 frame.data[2] = uavcan::uint8_t(0xFF & (rf.
RDLR >> 16));
678 frame.data[3] = uavcan::uint8_t(0xFF & (rf.
RDLR >> 24));
679 frame.data[4] = uavcan::uint8_t(0xFF & (rf.
RDHR >> 0));
680 frame.data[5] = uavcan::uint8_t(0xFF & (rf.
RDHR >> 8));
681 frame.data[6] = uavcan::uint8_t(0xFF & (rf.
RDHR >> 16));
682 frame.data[7] = uavcan::uint8_t(0xFF & (rf.
RDHR >> 24));
720 CriticalSectionLocker
lock;
742 const uavcan::uint32_t tme =
can_->
TSR & TME;
756 CriticalSectionLocker
lock;
769 CriticalSectionLocker
lock;
775 CriticalSectionLocker
lock;
781 CriticalSectionLocker
lock;
787 CriticalSectionLocker
lock;
798 uavcan::CanSelectMasks msk;
801 msk.read = if0_.isRxBufferEmpty() ? 0 : 1;
803 if (pending_tx[0] != UAVCAN_NULLPTR) {
804 msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0;
808 #if UAVCAN_STM32_NUM_IFACES > 1 810 if (!if1_.isRxBufferEmpty()) {
814 if (pending_tx[1] != UAVCAN_NULLPTR) {
815 if (if1_.canAcceptNewTxFrame(*pending_tx[1])) {
826 #if UAVCAN_STM32_NUM_IFACES == 1 827 return !if0_.isRxBufferEmpty();
828 #elif UAVCAN_STM32_NUM_IFACES == 2 829 return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty();
831 # error UAVCAN_STM32_NUM_IFACES 836 const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces],
837 const uavcan::MonotonicTime blocking_deadline)
839 const uavcan::CanSelectMasks in_masks = inout_masks;
842 if0_.discardTimedOutTxMailboxes(time);
844 CriticalSectionLocker cs_locker;
845 if0_.pollErrorFlagsFromISR();
848 #if UAVCAN_STM32_NUM_IFACES > 1 849 if1_.discardTimedOutTxMailboxes(time);
851 CriticalSectionLocker cs_locker;
852 if1_.pollErrorFlagsFromISR();
856 inout_masks = makeSelectMasks(pending_tx);
858 if ((inout_masks.read & in_masks.read) != 0 ||
859 (inout_masks.write & in_masks.write) != 0) {
864 inout_masks = makeSelectMasks(pending_tx);
875 CriticalSectionLocker
lock;
876 #if UAVCAN_STM32_NUTTX 877 modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN);
878 modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN1RST);
879 modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN1RST, 0);
880 # if UAVCAN_STM32_NUM_IFACES > 1 881 modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN);
882 modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST);
883 modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0);
886 RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
887 RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST;
888 RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
889 # if UAVCAN_STM32_NUM_IFACES > 1 890 RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
891 RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST;
892 RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
900 #if UAVCAN_STM32_NUTTX 901 # define IRQ_ATTACH(irq, handler) \ 903 const int res = irq_attach(irq, handler, NULL); \ 906 up_enable_irq(irq); \ 911 # if UAVCAN_STM32_NUM_IFACES > 1 924 UAVCAN_STM32_LOG(
"Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
926 static bool initialized_once =
false;
928 if (!initialized_once) {
929 initialized_once =
true;
939 res = if0_.init(bitrate, mode);
943 ifaces[0] = UAVCAN_NULLPTR;
950 #if UAVCAN_STM32_NUM_IFACES > 1 953 res = if1_.init(bitrate, mode);
957 ifaces[1] = UAVCAN_NULLPTR;
964 UAVCAN_ASSERT(res >= 0);
969 UAVCAN_ASSERT(res < 0);
975 if (iface_index < UAVCAN_STM32_NUM_IFACES) {
976 return ifaces[iface_index];
979 return UAVCAN_NULLPTR;
984 bool ret = if0_.hadActivity();
985 #if UAVCAN_STM32_NUM_IFACES > 1 986 ret |= if1_.hadActivity();
999 #if UAVCAN_STM32_NUTTX 1001 static int can1_irq(
const int irq,
void *,
void *)
1003 if (irq == STM32_IRQ_CAN1TX) {
1004 uavcan_stm32::handleTxInterrupt(0);
1006 }
else if (irq == STM32_IRQ_CAN1RX0) {
1007 uavcan_stm32::handleRxInterrupt(0, 0);
1009 }
else if (irq == STM32_IRQ_CAN1RX1) {
1010 uavcan_stm32::handleRxInterrupt(0, 1);
1019 # if UAVCAN_STM32_NUM_IFACES > 1 1021 static int can2_irq(
const int irq,
void *,
void *)
1023 if (irq == STM32_IRQ_CAN2TX) {
1024 uavcan_stm32::handleTxInterrupt(1);
1026 }
else if (irq == STM32_IRQ_CAN2RX0) {
1027 uavcan_stm32::handleRxInterrupt(1, 0);
1029 }
else if (irq == STM32_IRQ_CAN2RX1) {
1030 uavcan_stm32::handleRxInterrupt(1, 1);
1041 #else // UAVCAN_STM32_NUTTX 1043 #if !defined(CAN1_TX_IRQHandler) ||\ 1044 !defined(CAN1_RX0_IRQHandler) ||\ 1045 !defined(CAN1_RX1_IRQHandler) 1046 # error "Misconfigured build" 1052 UAVCAN_STM32_IRQ_PROLOGUE();
1053 uavcan_stm32::handleTxInterrupt(0);
1054 UAVCAN_STM32_IRQ_EPILOGUE();
1060 UAVCAN_STM32_IRQ_PROLOGUE();
1061 uavcan_stm32::handleRxInterrupt(0, 0);
1062 UAVCAN_STM32_IRQ_EPILOGUE();
1068 UAVCAN_STM32_IRQ_PROLOGUE();
1069 uavcan_stm32::handleRxInterrupt(0, 1);
1070 UAVCAN_STM32_IRQ_EPILOGUE();
1073 # if UAVCAN_STM32_NUM_IFACES > 1 1075 #if !defined(CAN2_TX_IRQHandler) ||\ 1076 !defined(CAN2_RX0_IRQHandler) ||\ 1077 !defined(CAN2_RX1_IRQHandler) 1078 # error "Misconfigured build" 1084 UAVCAN_STM32_IRQ_PROLOGUE();
1085 uavcan_stm32::handleTxInterrupt(1);
1086 UAVCAN_STM32_IRQ_EPILOGUE();
1092 UAVCAN_STM32_IRQ_PROLOGUE();
1093 uavcan_stm32::handleRxInterrupt(1, 0);
1094 UAVCAN_STM32_IRQ_EPILOGUE();
1100 UAVCAN_STM32_IRQ_PROLOGUE();
1101 uavcan_stm32::handleRxInterrupt(1, 1);
1102 UAVCAN_STM32_IRQ_EPILOGUE();
1106 #endif // UAVCAN_STM32_NUTTX uavcan::uint64_t utc_usec
constexpr unsigned long IER_FMPIE0
uavcan::uint32_t served_aborts_cnt_
constexpr unsigned long ESR_LEC_MASK
constexpr unsigned long MCR_AWUM
constexpr unsigned long TSR_ABRQ2
static const uavcan::int16_t ErrInvalidBitRate
Bit rate not supported.
constexpr unsigned long TSR_ABRQ1
static const uavcan::int16_t ErrMsrInakNotCleared
INAK bit of the MSR register is not 0.
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
Returns zero if OK.
RxMailboxType RxMailbox[2]
uavcan::uint64_t error_cnt_
uavcan::uint16_t prescaler
uavcan::uint64_t getUtcUSecFromCanInterrupt()
static const uavcan::int16_t ErrUnsupportedFrame
Frame not supported (e.g. RTR, CAN FD, etc)
constexpr unsigned long ESR_LEC_SHIFT
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs, uavcan::uint16_t num_configs)
void pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags)
constexpr unsigned long RIR_IDE
TxItem pending_tx_[NumTxMailboxes]
constexpr unsigned long TSR_TME1
static const uavcan::int16_t ErrMsrInakNotSet
INAK bit of the MSR register is not 1.
constexpr unsigned long TIR_RTR
constexpr unsigned long RFR_FOVR
int init(const uavcan::uint32_t bitrate, const OperatingMode mode)
Initializes the hardware CAN controller.
bool hadActivity()
Whether at least one iface had at least one successful IO since previous call of this method...
bool hasReadableInterfaces() const
Whether there's at least one interface where receive() would return a frame.
constexpr unsigned long MCR_RESET
virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags)
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame *(&pending_tx)[uavcan::MaxCanIfaces]) const
This function returns select masks indicating which interfaces are available for read/write.
virtual uavcan::uint64_t getErrorCount() const
Total number of hardware failures and other kinds of errors (e.g.
UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
constexpr unsigned long TSR_TXOK1
static const uavcan::int16_t ErrLogic
Internal logic error.
virtual CanIface * getIface(uavcan::uint8_t iface_index)
bool waitMsrINakBitStateChange(bool target_state)
bool isRxBufferEmpty() const
constexpr unsigned long TSR_TME2
FilterRegisterType FilterRegister[28]
constexpr unsigned long MCR_SLEEP
constexpr unsigned long IER_FMPIE1
constexpr unsigned long TSR_TXOK0
bool hadActivity()
Whether this iface had at least one successful IO since the previous call of this method...
TxMailboxType TxMailbox[3]
uavcan::MonotonicTime deadline
constexpr unsigned long IER_TMEIE
bxcan::CanType *const can_
uavcan::uint32_t overflow_cnt_
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]
void pollErrorFlagsFromISR()
This method is used to count errors and abort transmission on error if necessary. ...
virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags)
constexpr unsigned long MSR_INAK
constexpr unsigned long MCR_INRQ
constexpr unsigned long RFR_RFOM
constexpr unsigned long TSR_RQCP0
const uavcan::uint8_t capacity_
constexpr unsigned long TIR_IDE
unsigned getRxQueueLength() const
Returns the number of frames pending in the RX queue.
constexpr unsigned long BTR_SILM
constexpr _Tp max(_Tp a, _Tp b)
uavcan::uint8_t peak_tx_mailbox_index_
void handleTxInterrupt(uavcan::uint64_t utc_usec)
unsigned getLength() const
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec)
uavcan::MonotonicTime getMonotonic()
Returns current monotonic time since the moment when clock::init() was called.
void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
#define IRQ_ATTACH(irq, handler)
constexpr unsigned long TSR_RQCP1
uavcan::uint32_t getOverflowCount() const
int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings)
constexpr unsigned long TIR_TXRQ
constexpr unsigned long RFR_FULL
constexpr unsigned long RIR_RTR
constexpr unsigned long TSR_TME0
bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const
static const uavcan::int16_t ErrFilterNumConfigs
Number of filters is more than supported.
constexpr unsigned long FMR_FINIT
constexpr unsigned long RFR_FMP_MASK
virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks, const uavcan::CanFrame *(&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline)
constexpr unsigned long MCR_ABOM
constexpr unsigned long TSR_TXOK2
#define UAVCAN_STM32_LOG(...)
Debug output.
constexpr unsigned long TSR_RQCP2
constexpr unsigned long TSR_ABRQ0
void push(const uavcan::CanFrame &frame, const uint64_t &utc_usec, uavcan::CanIOFlags flags)
const uavcan::uint8_t self_index_