diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 9301004de7..d28407df3b 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -11,7 +11,6 @@ exec find boards msg src platforms \ -path msg/templates/urtps -prune -o \ -path platforms/nuttx/NuttX -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ - -path src/drivers/uavcan/uavcan_drivers -prune -o \ -path src/lib/DriverFramework -prune -o \ -path src/lib/ecl -prune -o \ -path src/lib/matrix -prune -o \ diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp index 9fd49917d2..08f7dc3f68 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp @@ -32,16 +32,15 @@ static const uavcan::int16_t ErrMcrSOFTRSTNotCleared = 1011; ///< MCR_SOFTRST * RX queue item. * The application shall not use this directly. */ -struct CanRxItem -{ - uavcan::uint64_t utc_usec; - uavcan::CanFrame frame; - uavcan::CanIOFlags flags; - CanRxItem() - : utc_usec(0), - flags(0) - { - } +struct CanRxItem { + uavcan::uint64_t utc_usec; + uavcan::CanFrame frame; + uavcan::CanIOFlags flags; + CanRxItem() + : utc_usec(0), + flags(0) + { + } }; /** @@ -49,215 +48,218 @@ struct CanRxItem * The application shall not use this directly. */ class CanIface : public uavcan::ICanIface - , uavcan::Noncopyable + , uavcan::Noncopyable { - const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; - 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_; + const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; + 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(); + void registerOverflow(); - public: - RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) - : buf_(buf), - capacity_(capacity), - in_(0), - out_(0), - len_(0), - overflow_cnt_(0) - { - } + 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); + 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); - void reset(); - unsigned getLength() const { - return len_; - } + void reset(); + unsigned getLength() const + { + return len_; + } - uavcan::uint32_t getOverflowCount() const { - return overflow_cnt_; - } - }; + uavcan::uint32_t getOverflowCount() const + { + return overflow_cnt_; + } + }; - struct Timings - { - uavcan::uint8_t prescaler; - uavcan::uint8_t rjw; - uavcan::uint8_t pseg1; - uavcan::uint8_t pseg2; - uavcan::uint8_t propseg; + struct Timings { + uavcan::uint8_t prescaler; + uavcan::uint8_t rjw; + uavcan::uint8_t pseg1; + uavcan::uint8_t pseg2; + uavcan::uint8_t propseg; - Timings() - : prescaler(0), - rjw(0), - pseg1(0), - pseg2(0), - propseg(0) - { - } - }; + Timings() + : prescaler(0), + rjw(0), + pseg1(0), + pseg2(0), + propseg(0) + { + } + }; - struct TxItem - { - uavcan::MonotonicTime deadline; - uavcan::CanFrame frame; - enum {free = 0, busy, abort } pending; - bool loopback; - bool abort_on_error; + struct TxItem { + uavcan::MonotonicTime deadline; + uavcan::CanFrame frame; + enum {free = 0, busy, abort } pending; + bool loopback; + bool abort_on_error; - TxItem() - : pending(free), - loopback(false), - abort_on_error(false) - { - } - }; + TxItem() + : pending(free), + loopback(false), + abort_on_error(false) + { + } + }; - enum { NumTxMesgBuffers = 6 }; - enum { NumFilters = 16 }; + enum { NumTxMesgBuffers = 6 }; + enum { NumFilters = 16 }; - uavcan::uint8_t MaxMB; + uavcan::uint8_t MaxMB; - RxQueue rx_queue_; - flexcan::CanType* const can_; - uavcan::uint64_t error_cnt_; - uavcan::uint64_t fifo_warn_cnt_; - uavcan::uint32_t pending_aborts_; - uavcan::uint32_t served_aborts_cnt_; - BusEvent& update_event_; - TxItem pending_tx_[NumTxMesgBuffers]; - uavcan::uint8_t peak_tx_mailbox_index_; - const uavcan::uint8_t self_index_; - bool had_activity_; + RxQueue rx_queue_; + flexcan::CanType *const can_; + uavcan::uint64_t error_cnt_; + uavcan::uint64_t fifo_warn_cnt_; + uavcan::uint32_t pending_aborts_; + uavcan::uint32_t served_aborts_cnt_; + BusEvent &update_event_; + TxItem pending_tx_[NumTxMesgBuffers]; + uavcan::uint8_t peak_tx_mailbox_index_; + const uavcan::uint8_t self_index_; + bool had_activity_; - int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings); + int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings); - virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags); + virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags); - virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags); + virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags); - virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, - uavcan::uint16_t num_configs); + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs, + uavcan::uint16_t num_configs); - virtual uavcan::uint16_t getNumFilters() const { - return NumFilters; - } + virtual uavcan::uint16_t getNumFilters() const + { + return NumFilters; + } - void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec); + void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec); - bool waitMCRChange(uavcan::uint32_t mask, bool target_state); - void setMCR(uavcan::uint32_t mask, bool target_state); + bool waitMCRChange(uavcan::uint32_t mask, bool target_state); + void setMCR(uavcan::uint32_t mask, bool target_state); - bool setEnable(bool enable_true); - uavcan::int16_t doReset(Timings timings); - bool waitFreezeAckChange(bool target_state); - void setFreeze(bool freeze_true); + bool setEnable(bool enable_true); + uavcan::int16_t doReset(Timings timings); + bool waitFreezeAckChange(bool target_state); + void setFreeze(bool freeze_true); public: - enum { MaxRxQueueCapacity = 254 }; + enum { MaxRxQueueCapacity = 254 }; - enum OperatingMode - { - NormalMode, - SilentMode - }; + enum OperatingMode { + NormalMode, + SilentMode + }; - CanIface(flexcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, - CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) - : MaxMB(flexcan::HWMaxMB), - rx_queue_(rx_queue_buffer, rx_queue_capacity), - can_(can), - error_cnt_(0), - fifo_warn_cnt_(0), - pending_aborts_(0), - served_aborts_cnt_(0), - update_event_(update_event), - peak_tx_mailbox_index_(0), - self_index_(self_index), - had_activity_(false) - { - UAVCAN_ASSERT(self_index_ < UAVCAN_KINETIS_NUM_IFACES); - } + CanIface(flexcan::CanType *can, BusEvent &update_event, uavcan::uint8_t self_index, + CanRxItem *rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) + : MaxMB(flexcan::HWMaxMB), + rx_queue_(rx_queue_buffer, rx_queue_capacity), + can_(can), + error_cnt_(0), + fifo_warn_cnt_(0), + pending_aborts_(0), + served_aborts_cnt_(0), + update_event_(update_event), + peak_tx_mailbox_index_(0), + self_index_(self_index), + had_activity_(false) + { + UAVCAN_ASSERT(self_index_ < UAVCAN_KINETIS_NUM_IFACES); + } -/** - * Initializes the hardware CAN controller. - * Assumes: - * - Iface clock is enabled - * - Iface has been resetted via RCC - * - Caller will configure NVIC by itself - */ - int init(const uavcan::uint32_t bitrate, const OperatingMode mode); + /** + * Initializes the hardware CAN controller. + * Assumes: + * - Iface clock is enabled + * - Iface has been resetted via RCC + * - Caller will configure NVIC by itself + */ + int init(const uavcan::uint32_t bitrate, const OperatingMode mode); - void handleTxInterrupt(uavcan::uint32_t tx_iflags, uavcan::uint64_t utc_usec); - void handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec); + void handleTxInterrupt(uavcan::uint32_t tx_iflags, uavcan::uint64_t utc_usec); + void handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec); -/** - * This method is used to count errors and abort transmission on error if necessary. - * This functionality used to be implemented in the SCE interrupt handler, but that approach was - * generating too much processing overhead, especially on disconnected interfaces. - * - * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. - */ - void pollErrorFlagsFromISR(); + /** + * This method is used to count errors and abort transmission on error if necessary. + * This functionality used to be implemented in the SCE interrupt handler, but that approach was + * generating too much processing overhead, especially on disconnected interfaces. + * + * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. + */ + void pollErrorFlagsFromISR(); - void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); + void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); - bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; - bool isRxBufferEmpty() const; + bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const; + bool isRxBufferEmpty() const; -/** - * Number of RX frames lost due to queue overflow. - * This is an atomic read, it doesn't require a critical section. - */ - uavcan::uint32_t getRxQueueOverflowCount() const { - return rx_queue_.getOverflowCount(); - } + /** + * Number of RX frames lost due to queue overflow. + * This is an atomic read, it doesn't require a critical section. + */ + uavcan::uint32_t getRxQueueOverflowCount() const + { + return rx_queue_.getOverflowCount(); + } -/** - * Total number of hardware failures and other kinds of errors (e.g. queue overruns). - * May increase continuously if the interface is not connected to the bus. - */ - virtual uavcan::uint64_t getErrorCount() const; + /** + * Total number of hardware failures and other kinds of errors (e.g. queue overruns). + * May increase continuously if the interface is not connected to the bus. + */ + virtual uavcan::uint64_t getErrorCount() const; -/** - * Number of times the driver exercised library's requirement to abort transmission on first error. - * This is an atomic read, it doesn't require a critical section. - * See @ref uavcan::CanIOFlagAbortOnError. - */ - uavcan::uint32_t getVoluntaryTxAbortCount() const { - return served_aborts_cnt_; - } + /** + * Number of times the driver exercised library's requirement to abort transmission on first error. + * This is an atomic read, it doesn't require a critical section. + * See @ref uavcan::CanIOFlagAbortOnError. + */ + uavcan::uint32_t getVoluntaryTxAbortCount() const + { + return served_aborts_cnt_; + } -/** - * Returns the number of frames pending in the RX queue. - * This is intended for debug use only. - */ - unsigned getRxQueueLength() const; + /** + * Returns the number of frames pending in the RX queue. + * This is intended for debug use only. + */ + unsigned getRxQueueLength() const; -/** - * Whether this iface had at least one successful IO since the previous call of this method. - * This is designed for use with iface activity LEDs. - */ - bool hadActivity(); + /** + * Whether this iface had at least one successful IO since the previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); -/** - * Peak number of TX mailboxes used concurrently since initialization. - * Range is [1, 3]. - * Value of 3 suggests that priority inversion could be taking place. - */ - uavcan::uint8_t getPeakNumTxMailboxesUsed() const { - return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); - } + /** + * Peak number of TX mailboxes used concurrently since initialization. + * Range is [1, 3]. + * Value of 3 suggests that priority inversion could be taking place. + */ + uavcan::uint8_t getPeakNumTxMailboxesUsed() const + { + return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); + } }; /** @@ -265,61 +267,62 @@ public: * Please avoid direct use, prefer @ref CanInitHelper instead. */ class CanDriver : public uavcan::ICanDriver - , uavcan::Noncopyable + , uavcan::Noncopyable { - BusEvent update_event_; - CanIface if0_; + BusEvent update_event_; + CanIface if0_; #if UAVCAN_KINETIS_NUM_IFACES > 1 - CanIface if1_; + CanIface if1_; #endif - virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], - uavcan::MonotonicTime blocking_deadline); + virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline); - static void initOnce(); + static void initOnce(); public: - template - CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]) - : update_event_(*this), - if0_(flexcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) + template + CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]) + : update_event_(*this), + if0_(flexcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) #if UAVCAN_KINETIS_NUM_IFACES > 1 - , if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) + , if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) #endif - { - uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); - } + { + uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check(); + } -/** - * This function returns select masks indicating which interfaces are available for read/write. - */ - uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const; + /** + * This function returns select masks indicating which interfaces are available for read/write. + */ + uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces]) const; -/** - * Whether there's at least one interface where receive() would return a frame. - */ - bool hasReadableInterfaces() const; + /** + * Whether there's at least one interface where receive() would return a frame. + */ + bool hasReadableInterfaces() const; -/** - * Returns zero if OK. - * Returns negative value if failed (e.g. invalid bitrate). - */ - int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); + /** + * Returns zero if OK. + * Returns negative value if failed (e.g. invalid bitrate). + */ + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); - virtual CanIface* getIface(uavcan::uint8_t iface_index); + virtual CanIface *getIface(uavcan::uint8_t iface_index); - virtual uavcan::uint8_t getNumIfaces() const { - return UAVCAN_KINETIS_NUM_IFACES; - } + virtual uavcan::uint8_t getNumIfaces() const + { + return UAVCAN_KINETIS_NUM_IFACES; + } -/** - * Whether at least one iface had at least one successful IO since previous call of this method. - * This is designed for use with iface activity LEDs. - */ - bool hadActivity(); + /** + * Whether at least one iface had at least one successful IO since previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); - BusEvent& updateEvent() { return update_event_; } + BusEvent &updateEvent() { return update_event_; } }; /** @@ -330,92 +333,85 @@ public: template class CanInitHelper { - CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]; + CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]; public: - enum { BitRateAutoDetect = 0 }; + enum { BitRateAutoDetect = 0 }; - CanDriver driver; + CanDriver driver; - CanInitHelper() : - driver(queue_storage_) - { - } + CanInitHelper() : + driver(queue_storage_) + { + } -/** - * This overload simply configures the provided bitrate. - * Auto bit rate detection will not be performed. - * Bitrate value must be positive. - * @return Negative value on error; non-negative on success. Refer to constants Err*. - */ - int init(uavcan::uint32_t bitrate) - { - return driver.init(bitrate, CanIface::NormalMode); - } + /** + * This overload simply configures the provided bitrate. + * Auto bit rate detection will not be performed. + * Bitrate value must be positive. + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + int init(uavcan::uint32_t bitrate) + { + return driver.init(bitrate, CanIface::NormalMode); + } -/** - * This function can either initialize the driver at a fixed bit rate, or it can perform - * automatic bit rate detection. For theory please refer to the CiA application note #801. - * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. - * If auto detection was used, the function will update the argument - * with established bit rate. In case of an error the value will be undefined. - * - * @return Negative value on error; non-negative on success. Refer to constants Err*. - */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect) - { - if (inout_bitrate > 0) - { - return driver.init(inout_bitrate, CanIface::NormalMode); - } - else - { - static const uavcan::uint32_t StandardBitRates[] = - { - 1000000, - 500000, - 250000, - 125000 - }; + /** + * This function can either initialize the driver at a fixed bit rate, or it can perform + * automatic bit rate detection. For theory please refer to the CiA application note #801. + * + * @param delay_callable A callable entity that suspends execution for strictly more than one second. + * The callable entity will be invoked without arguments. + * @ref getRecommendedListeningDelay(). + * + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * If auto detection was used, the function will update the argument + * with established bit rate. In case of an error the value will be undefined. + * + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + template + int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + { + if (inout_bitrate > 0) { + return driver.init(inout_bitrate, CanIface::NormalMode); - for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) - { - inout_bitrate = StandardBitRates[br]; + } else { + static const uavcan::uint32_t StandardBitRates[] = { + 1000000, + 500000, + 250000, + 125000 + }; - const int res = driver.init(inout_bitrate, CanIface::SilentMode); + for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) { + inout_bitrate = StandardBitRates[br]; - delay_callable(); + const int res = driver.init(inout_bitrate, CanIface::SilentMode); - if (res >= 0) - { - for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) - { - if (!driver.getIface(iface)->isRxBufferEmpty()) - { - // Re-initializing in normal mode - return driver.init(inout_bitrate, CanIface::NormalMode); - } - } - } - } + delay_callable(); - return -ErrBitRateNotDetected; - } - } + if (res >= 0) { + for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { + if (!driver.getIface(iface)->isRxBufferEmpty()) { + // Re-initializing in normal mode + return driver.init(inout_bitrate, CanIface::NormalMode); + } + } + } + } -/** - * Use this value for listening delay during automatic bit rate detection. - */ - static uavcan::MonotonicDuration getRecommendedListeningDelay() - { - return uavcan::MonotonicDuration::fromMSec(1050); - } + return -ErrBitRateNotDetected; + } + } + + /** + * Use this value for listening delay during automatic bit rate detection. + */ + static uavcan::MonotonicDuration getRecommendedListeningDelay() + { + return uavcan::MonotonicDuration::fromMSec(1050); + } }; } diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp index 2dec34e3c4..f593feb60c 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp @@ -49,26 +49,25 @@ void adjustUtc(uavcan::UtcDuration adjustment); /** * UTC clock synchronization parameters */ -struct UtcSyncParams -{ - float offset_p; ///< PPM per one usec error - float rate_i; ///< PPM per one PPM error for second - float rate_error_corner_freq; - float max_rate_correction_ppm; - float lock_thres_rate_ppm; - uavcan::UtcDuration lock_thres_offset; - uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate +struct UtcSyncParams { + float offset_p; ///< PPM per one usec error + float rate_i; ///< PPM per one PPM error for second + float rate_error_corner_freq; + float max_rate_correction_ppm; + float lock_thres_rate_ppm; + uavcan::UtcDuration lock_thres_offset; + uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate - UtcSyncParams() - : offset_p(0.01F), - rate_i(0.02F), - rate_error_corner_freq(0.01F), - max_rate_correction_ppm(300.0F), - lock_thres_rate_ppm(2.0F), - lock_thres_offset(uavcan::UtcDuration::fromMSec(4)), - min_jump(uavcan::UtcDuration::fromMSec(10)) - { - } + UtcSyncParams() + : offset_p(0.01F), + rate_i(0.02F), + rate_error_corner_freq(0.01F), + max_rate_correction_ppm(300.0F), + lock_thres_rate_ppm(2.0F), + lock_thres_offset(uavcan::UtcDuration::fromMSec(4)), + min_jump(uavcan::UtcDuration::fromMSec(10)) + { + } }; /** @@ -96,7 +95,7 @@ bool isUtcLocked(); * Both functions are thread safe. */ UtcSyncParams getUtcSyncParams(); -void setUtcSyncParams(const UtcSyncParams& params); +void setUtcSyncParams(const UtcSyncParams ¶ms); } @@ -104,28 +103,32 @@ void setUtcSyncParams(const UtcSyncParams& params); * Adapter for uavcan::ISystemClock. */ class SystemClock : public uavcan::ISystemClock - , uavcan::Noncopyable + , uavcan::Noncopyable { - SystemClock() { - } + SystemClock() + { + } - virtual void adjustUtc(uavcan::UtcDuration adjustment) { - clock::adjustUtc(adjustment); - } + virtual void adjustUtc(uavcan::UtcDuration adjustment) + { + clock::adjustUtc(adjustment); + } public: - virtual uavcan::MonotonicTime getMonotonic() const { - return clock::getMonotonic(); - } - virtual uavcan::UtcTime getUtc() const { - return clock::getUtc(); - } + virtual uavcan::MonotonicTime getMonotonic() const + { + return clock::getMonotonic(); + } + virtual uavcan::UtcTime getUtc() const + { + return clock::getUtc(); + } -/** - * Calls clock::init() as needed. - * This function is thread safe. - */ - static SystemClock& instance(); + /** + * Calls clock::init() as needed. + * This function is thread safe. + */ + static SystemClock &instance(); }; } diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp index cc4f2d7acc..f521a4b606 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp @@ -26,159 +26,140 @@ namespace flexcan { enum { HWMaxMB = 16 }; -union MBcsType -{ - volatile uint32_t w; - struct - { - volatile uint32_t time_stamp : 16; - volatile uint32_t dlc : 4; - volatile uint32_t rtr : 1; - volatile uint32_t ide : 1; - volatile uint32_t srr : 1; - volatile uint32_t res : 1; - volatile uint32_t code : 4; - volatile uint32_t res2 : 4; - }; +union MBcsType { + volatile uint32_t w; + struct { + volatile uint32_t time_stamp : 16; + volatile uint32_t dlc : 4; + volatile uint32_t rtr : 1; + volatile uint32_t ide : 1; + volatile uint32_t srr : 1; + volatile uint32_t res : 1; + volatile uint32_t code : 4; + volatile uint32_t res2 : 4; + }; + }; + + union FIFOcsType { + volatile uint32_t cs; + struct { + volatile uint32_t time_stamp : 16; + volatile uint32_t dlc : 4; + volatile uint32_t rtr : 1; + volatile uint32_t ide : 1; + volatile uint32_t srr : 1; + volatile uint32_t res : 9; + }; + }; + + union IDType { + volatile uint32_t w; + struct { + volatile uint32_t ext : 29; + volatile uint32_t resex : 3; + }; + struct { + volatile uint32_t res : 18; + volatile uint32_t std : 11; + volatile uint32_t resstd : 3; + }; + }; + + union FilterType { + volatile uint32_t w; + struct { + volatile uint32_t res : 1; // Bit 0 - Reserved + volatile uint32_t ext : 29; // Bits 1 - 29 EID + }; + struct { + volatile uint32_t ress : 19; // Bits 0, 1-18 Reserved + volatile uint32_t std : 11; // StD ID + }; + struct { + volatile uint32_t resc : 30; // Bits 0 - 29 Reserved + volatile uint32_t ide : 1; // Bit 30 - EID + volatile uint32_t rtr : 1; // Bit 31 Remote + }; + }; + + union DataType { + volatile uint32_t l; + volatile uint32_t h; + struct { + volatile uint32_t b3 : 8; + volatile uint32_t b2 : 8; + volatile uint32_t b1 : 8; + volatile uint32_t b0 : 8; + volatile uint32_t b7 : 8; + volatile uint32_t b6 : 8; + volatile uint32_t b5 : 8; + volatile uint32_t b4 : 8; + }; + }; + + + struct MessageBufferType { + MBcsType CS; + IDType ID; + DataType data; }; -union FIFOcsType -{ - volatile uint32_t cs; - struct - { - volatile uint32_t time_stamp : 16; - volatile uint32_t dlc : 4; - volatile uint32_t rtr : 1; - volatile uint32_t ide : 1; - volatile uint32_t srr : 1; - volatile uint32_t res : 9; - }; +enum mb_code_tx { + TxMbInactive = 0x8, /*!< MB is not active.*/ + TxMbAbort = 0x9, /*!< MB is aborted.*/ + TxMbDataOrRemote = 0xC, /*!< MB is a TX Data Frame(when MB RTR = 0) or */ + /*!< MB is a TX Remote Request Frame (when MB RTR = 1).*/ + TxMbTanswer = 0xE, /*!< MB is a TX Response Request Frame from */ + /*! an incoming Remote Request Frame.*/ + TxMbNotUsed = 0xF, /*!< Not used.*/ }; -union IDType -{ - volatile uint32_t w; - struct - { - volatile uint32_t ext : 29; - volatile uint32_t resex : 3; - }; - struct - { - volatile uint32_t res : 18; - volatile uint32_t std : 11; - volatile uint32_t resstd : 3; - }; +struct RxFiFoType { + FIFOcsType CS; + IDType ID; + DataType data; }; -union FilterType -{ - volatile uint32_t w; - struct - { - volatile uint32_t res : 1; // Bit 0 - Reserved - volatile uint32_t ext : 29; // Bits 1 - 29 EID - }; - struct - { - volatile uint32_t ress : 19; // Bits 0, 1-18 Reserved - volatile uint32_t std : 11; // StD ID - }; - struct - { - volatile uint32_t resc : 30; // Bits 0 - 29 Reserved - volatile uint32_t ide : 1; // Bit 30 - EID - volatile uint32_t rtr : 1; // Bit 31 Remote - }; +enum mb_code_rx { + kRxMbInactive = 0x0, /*!< MB is not active.*/ + kRxMbFull = 0x2, /*!< MB is full.*/ + kRxMbEmpty = 0x4, /*!< MB is active and empty.*/ + kRxMbOverrun = 0x6, /*!< MB is overwritten into a full buffer.*/ + kRxMbBusy = 0x8, /*!< FlexCAN is updating the contents of the MB.*/ + /*! The CPU must not access the MB.*/ + kRxMbRanswer = 0xA, /*!< A frame was configured to recognize a Remote Request Frame */ + /*! and transmit a Response Frame in return.*/ + kRxMbNotUsed = 0xF, /*!< Not used.*/ }; -union DataType -{ - volatile uint32_t l; - volatile uint32_t h; - struct - { - volatile uint32_t b3 : 8; - volatile uint32_t b2 : 8; - volatile uint32_t b1 : 8; - volatile uint32_t b0 : 8; - volatile uint32_t b7 : 8; - volatile uint32_t b6 : 8; - volatile uint32_t b5 : 8; - volatile uint32_t b4 : 8; - }; -}; - - -struct MessageBufferType -{ - MBcsType CS; - IDType ID; - DataType data; -}; - -enum mb_code_tx -{ - TxMbInactive = 0x8, /*!< MB is not active.*/ - TxMbAbort = 0x9, /*!< MB is aborted.*/ - TxMbDataOrRemote = 0xC, /*!< MB is a TX Data Frame(when MB RTR = 0) or */ - /*!< MB is a TX Remote Request Frame (when MB RTR = 1).*/ - TxMbTanswer = 0xE, /*!< MB is a TX Response Request Frame from */ - /*! an incoming Remote Request Frame.*/ - TxMbNotUsed = 0xF, /*!< Not used.*/ -}; - -struct RxFiFoType -{ - FIFOcsType CS; - IDType ID; - DataType data; -}; - -enum mb_code_rx -{ - kRxMbInactive = 0x0, /*!< MB is not active.*/ - kRxMbFull = 0x2, /*!< MB is full.*/ - kRxMbEmpty = 0x4, /*!< MB is active and empty.*/ - kRxMbOverrun = 0x6, /*!< MB is overwritten into a full buffer.*/ - kRxMbBusy = 0x8, /*!< FlexCAN is updating the contents of the MB.*/ - /*! The CPU must not access the MB.*/ - kRxMbRanswer = 0xA, /*!< A frame was configured to recognize a Remote Request Frame */ - /*! and transmit a Response Frame in return.*/ - kRxMbNotUsed = 0xF, /*!< Not used.*/ -}; - -struct CanType -{ - volatile uint32_t MCR; /*!< Module Configuration Register, Address offset: 0x0000 */ - volatile uint32_t CTRL1; /*!< Control 1 Register, Address offset: 0x0004 */ - volatile uint32_t TIMER; /*!< Free Running Timer, Address offset: 0x0008 */ - uint32_t Reserved0; /*!< Reserved Address offset: 0x000c */ - volatile uint32_t RXMGMASK; /*!< Rx Mailboxes Global Mask Register, Address offset: 0x0010 */ - volatile uint32_t RX14MASK; /*!< Rx 14 Mask Register, Address offset: 0x0014 */ - volatile uint32_t RX15MASK; /*!< Rx 15 Mask Register, Address offset: 0x0018 */ - volatile uint32_t ECR; /*!< Error Counter, Address offset: 0x001c */ - volatile uint32_t ESR1; /*!< Error and Status 1 Register, Address offset: 0x0020 */ - uint32_t Reserved1; /*!< Reserved Address offset: 0x0024 */ - volatile uint32_t IMASK1; /*!< Interrupt Masks 1 Register, Address offset: 0x0028 */ - uint32_t Reserved2; /*!< Reserved Address offset: 0x002C */ - volatile uint32_t IFLAG1; /*!< Interrupt Flags 1 Register, Address offset: 0x0030 */ - volatile uint32_t CTRL2; /*!< Control 2 Register, Address offset: 0x0034 */ - volatile uint32_t ESR2; /*!< Error and Status 2 Register, Address offset: 0x0038 */ - uint32_t Reserved3; /*!< Reserved Address offset: 0x003c */ - uint32_t Reserved4; /*!< Reserved Address offset: 0x0040 */ - volatile uint32_t CRCR; /*!< CRC Register, Address offset: 0x0044 */ - volatile uint32_t RXFGMASK; /*!< Rx FIFO Global Mask Register, Address offset: 0x0048 */ - volatile uint32_t RXFIR; /*!< Rx FIFO Information Register, Address offset: 0x004c */ - uint32_t RESERVED5[12]; /*!< Reserved Address offset: 0x0050 */ - union - { - RxFiFoType fifo; - MessageBufferType mb; - } MB[HWMaxMB]; - uint32_t RESERVED6[448]; /*!< Reserved Address offset: 0x0180 */ - volatile FilterType RXIMR[HWMaxMB]; /*!< R0 Individual Mask Registers, Address offset: 0x0880 +struct CanType { + volatile uint32_t MCR; /*!< Module Configuration Register, Address offset: 0x0000 */ + volatile uint32_t CTRL1; /*!< Control 1 Register, Address offset: 0x0004 */ + volatile uint32_t TIMER; /*!< Free Running Timer, Address offset: 0x0008 */ + uint32_t Reserved0; /*!< Reserved Address offset: 0x000c */ + volatile uint32_t RXMGMASK; /*!< Rx Mailboxes Global Mask Register, Address offset: 0x0010 */ + volatile uint32_t RX14MASK; /*!< Rx 14 Mask Register, Address offset: 0x0014 */ + volatile uint32_t RX15MASK; /*!< Rx 15 Mask Register, Address offset: 0x0018 */ + volatile uint32_t ECR; /*!< Error Counter, Address offset: 0x001c */ + volatile uint32_t ESR1; /*!< Error and Status 1 Register, Address offset: 0x0020 */ + uint32_t Reserved1; /*!< Reserved Address offset: 0x0024 */ + volatile uint32_t IMASK1; /*!< Interrupt Masks 1 Register, Address offset: 0x0028 */ + uint32_t Reserved2; /*!< Reserved Address offset: 0x002C */ + volatile uint32_t IFLAG1; /*!< Interrupt Flags 1 Register, Address offset: 0x0030 */ + volatile uint32_t CTRL2; /*!< Control 2 Register, Address offset: 0x0034 */ + volatile uint32_t ESR2; /*!< Error and Status 2 Register, Address offset: 0x0038 */ + uint32_t Reserved3; /*!< Reserved Address offset: 0x003c */ + uint32_t Reserved4; /*!< Reserved Address offset: 0x0040 */ + volatile uint32_t CRCR; /*!< CRC Register, Address offset: 0x0044 */ + volatile uint32_t RXFGMASK; /*!< Rx FIFO Global Mask Register, Address offset: 0x0048 */ + volatile uint32_t RXFIR; /*!< Rx FIFO Information Register, Address offset: 0x004c */ + uint32_t RESERVED5[12]; /*!< Reserved Address offset: 0x0050 */ + union { + RxFiFoType fifo; + MessageBufferType mb; + } MB[HWMaxMB]; + uint32_t RESERVED6[448]; /*!< Reserved Address offset: 0x0180 */ + volatile FilterType RXIMR[HWMaxMB]; /*!< R0 Individual Mask Registers, Address offset: 0x0880 */ }; @@ -214,367 +195,366 @@ enum { TXMBMask = (0b111111 << NumMBinFiFoAndFilters) }; /* Address of the CAN registers */ -CanType* const Can[UAVCAN_KINETIS_NUM_IFACES] = -{ - reinterpret_cast(0x40024000) // CAN0 +CanType *const Can[UAVCAN_KINETIS_NUM_IFACES] = { + reinterpret_cast(0x40024000) // CAN0 #if UAVCAN_KINETIS_NUM_IFACES > 1 - , - reinterpret_cast(0x400A4000) // CAN1 + , + reinterpret_cast(0x400A4000) // CAN1 #endif }; /* Module Configuration Register */ -constexpr unsigned long MCR_MAXMB_SHIFT = (0); /* Bits 0-6: Number of the Last Message Buffer */ -constexpr unsigned long MCR_MAXMB_MASK = (0x7fU << MCR_MAXMB_SHIFT); +constexpr unsigned long MCR_MAXMB_SHIFT = (0); /* Bits 0-6: Number of the Last Message Buffer */ +constexpr unsigned long MCR_MAXMB_MASK = (0x7fU << MCR_MAXMB_SHIFT); /* Bit 7: Reserved */ -constexpr unsigned long MCR_IDAM_SHIFT = (8); /* Bits 8-9: ID Acceptance Mode */ -constexpr unsigned long MCR_IDAM_MASK = (3U << MCR_IDAM_SHIFT); +constexpr unsigned long MCR_IDAM_SHIFT = (8); /* Bits 8-9: ID Acceptance Mode */ +constexpr unsigned long MCR_IDAM_MASK = (3U << MCR_IDAM_SHIFT); constexpr unsigned long MCR_IDAM_FMTA(0U << MCR_IDAM_SHIFT); /* Format A: One full ID */ constexpr unsigned long CAN_MCR_IDAM_FMTB(1U << MCR_IDAM_SHIFT); /* Format B: Two full (or partial) IDs */ constexpr unsigned long MCR_IDAM_FMTC(2U << MCR_IDAM_SHIFT); /* Format C: Four partial IDs */ constexpr unsigned long MCR_IDAM_FMTD(3U << MCR_IDAM_SHIFT); /* Format D: All frames rejected */ /* Bits 10-11: Reserved */ -constexpr unsigned long MCR_AEN = (1U << 12); /* Bit 12: Abort Enable */ -constexpr unsigned long MCR_LPRIOEN = (1U << 13); /* Bit 13: Local Priority Enable */ - /* Bits 14-15: Reserved */ -constexpr unsigned long MCR_IRMQ = (1U << 16); /* Bit 16: Individual Rx Masking and Queue Enable */ -constexpr unsigned long MCR_SRXDIS = (1U << 17); /* Bit 17: Self Reception Disable */ - /* Bit 18: Reserved */ -constexpr unsigned long MCR_WAKSRC = (1U << 19); /* Bit 19: Wake up Source */ -constexpr unsigned long MCR_LPMACK = (1U << 20); /* Bit 20: Low Power Mode Acknowledge */ -constexpr unsigned long MCR_WRNEN = (1U << 21); /* Bit 21: Warning Interrupt Enable */ -constexpr unsigned long MCR_SLFWAK = (1U << 22); /* Bit 22: Self Wake Up */ -constexpr unsigned long MCR_SUPV = (1U << 23); /* Bit 23: Supervisor Mode */ -constexpr unsigned long MCR_FRZACK = (1U << 24); /* Bit 24: Freeze Mode Acknowledge */ -constexpr unsigned long MCR_SOFTRST = (1U << 25); /* Bit 25: Soft Reset */ -constexpr unsigned long MCR_WAKMSK = (1U << 26); /* Bit 26: Wake Up Interrupt Mask */ -constexpr unsigned long MCR_NOTRDY = (1U << 27); /* Bit 27: FlexCAN Not Ready */ -constexpr unsigned long MCR_HALT = (1U << 28); /* Bit 28: Halt FlexCAN */ -constexpr unsigned long MCR_RFEN = (1U << 29); /* Bit 29: Rx FIFO Enable */ -constexpr unsigned long MCR_FRZ = (1U << 30); /* Bit 30: Freeze Enable */ -constexpr unsigned long MCR_MDIS = (1U << 31); /* Bit 31: Module Disable */ +constexpr unsigned long MCR_AEN = (1U << 12); /* Bit 12: Abort Enable */ +constexpr unsigned long MCR_LPRIOEN = (1U << 13); /* Bit 13: Local Priority Enable */ +/* Bits 14-15: Reserved */ +constexpr unsigned long MCR_IRMQ = (1U << 16); /* Bit 16: Individual Rx Masking and Queue Enable */ +constexpr unsigned long MCR_SRXDIS = (1U << 17); /* Bit 17: Self Reception Disable */ +/* Bit 18: Reserved */ +constexpr unsigned long MCR_WAKSRC = (1U << 19); /* Bit 19: Wake up Source */ +constexpr unsigned long MCR_LPMACK = (1U << 20); /* Bit 20: Low Power Mode Acknowledge */ +constexpr unsigned long MCR_WRNEN = (1U << 21); /* Bit 21: Warning Interrupt Enable */ +constexpr unsigned long MCR_SLFWAK = (1U << 22); /* Bit 22: Self Wake Up */ +constexpr unsigned long MCR_SUPV = (1U << 23); /* Bit 23: Supervisor Mode */ +constexpr unsigned long MCR_FRZACK = (1U << 24); /* Bit 24: Freeze Mode Acknowledge */ +constexpr unsigned long MCR_SOFTRST = (1U << 25); /* Bit 25: Soft Reset */ +constexpr unsigned long MCR_WAKMSK = (1U << 26); /* Bit 26: Wake Up Interrupt Mask */ +constexpr unsigned long MCR_NOTRDY = (1U << 27); /* Bit 27: FlexCAN Not Ready */ +constexpr unsigned long MCR_HALT = (1U << 28); /* Bit 28: Halt FlexCAN */ +constexpr unsigned long MCR_RFEN = (1U << 29); /* Bit 29: Rx FIFO Enable */ +constexpr unsigned long MCR_FRZ = (1U << 30); /* Bit 30: Freeze Enable */ +constexpr unsigned long MCR_MDIS = (1U << 31); /* Bit 31: Module Disable */ /* Control 1 Register */ -constexpr unsigned long CTRL1_ROPSEG_SHIFT = (0); /* Bits 0-2: Propagation Segment */ -constexpr unsigned long CTRL1_ROPSEG_MASK = (7U << CTRL1_ROPSEG_SHIFT); -constexpr unsigned long CTRL1_LOM = (1U << 3); /* Bit 3: Listen-Only Mode */ -constexpr unsigned long CTRL1_LBUF = (1U << 4); /* Bit 4: Lowest Buffer Transmitted First */ -constexpr unsigned long CTRL1_TSYN = (1U << 5); /* Bit 5: Timer Sync */ -constexpr unsigned long CTRL1_BOFFREC = (1U << 6); /* Bit 6: Bus Off Recovery */ -constexpr unsigned long CTRL1_SMP = (1U << 7); /* Bit 7: CAN Bit Sampling */ - /* Bits 8-9: Reserved */ -constexpr unsigned long CTRL1_RWRNMSK = (1U << 10); /* Bit 10: Rx Warning Interrupt Mask */ -constexpr unsigned long CTRL1_TWRNMSK = (1U << 11); /* Bit 11: Tx Warning Interrupt Mask */ -constexpr unsigned long CTRL1_LPB = (1U << 12); /* Bit 12: Loop Back Mode */ -constexpr unsigned long CTRL1_CLKSRC = (1U << 13); /* Bit 13: CAN Engine Clock Source */ -constexpr unsigned long CTRL1_ERRMSK = (1U << 14); /* Bit 14: Error Mask */ -constexpr unsigned long CTRL1_BOFFMSK = (1U << 15); /* Bit 15: Bus Off Mask */ -constexpr unsigned long CTRL1_PSEG2_SHIFT = (16); /* Bits 16-18: Phase Segment 2 */ -constexpr unsigned long CTRL1_PSEG2_MASK = (7U << CTRL1_PSEG2_SHIFT); -constexpr unsigned long CTRL1_PSEG1_SHIFT = (19); /* Bits 19-21: Phase Segment 1 */ -constexpr unsigned long CTRL1_PSEG1_MASK = (7U << CTRL1_PSEG1_SHIFT); -constexpr unsigned long CTRL1_RJW_SHIFT = (22); /* Bits 22-23: Resync Jump Width */ -constexpr unsigned long CTRL1_RJW_MASK = (3U << CTRL1_RJW_SHIFT); +constexpr unsigned long CTRL1_ROPSEG_SHIFT = (0); /* Bits 0-2: Propagation Segment */ +constexpr unsigned long CTRL1_ROPSEG_MASK = (7U << CTRL1_ROPSEG_SHIFT); +constexpr unsigned long CTRL1_LOM = (1U << 3); /* Bit 3: Listen-Only Mode */ +constexpr unsigned long CTRL1_LBUF = (1U << 4); /* Bit 4: Lowest Buffer Transmitted First */ +constexpr unsigned long CTRL1_TSYN = (1U << 5); /* Bit 5: Timer Sync */ +constexpr unsigned long CTRL1_BOFFREC = (1U << 6); /* Bit 6: Bus Off Recovery */ +constexpr unsigned long CTRL1_SMP = (1U << 7); /* Bit 7: CAN Bit Sampling */ +/* Bits 8-9: Reserved */ +constexpr unsigned long CTRL1_RWRNMSK = (1U << 10); /* Bit 10: Rx Warning Interrupt Mask */ +constexpr unsigned long CTRL1_TWRNMSK = (1U << 11); /* Bit 11: Tx Warning Interrupt Mask */ +constexpr unsigned long CTRL1_LPB = (1U << 12); /* Bit 12: Loop Back Mode */ +constexpr unsigned long CTRL1_CLKSRC = (1U << 13); /* Bit 13: CAN Engine Clock Source */ +constexpr unsigned long CTRL1_ERRMSK = (1U << 14); /* Bit 14: Error Mask */ +constexpr unsigned long CTRL1_BOFFMSK = (1U << 15); /* Bit 15: Bus Off Mask */ +constexpr unsigned long CTRL1_PSEG2_SHIFT = (16); /* Bits 16-18: Phase Segment 2 */ +constexpr unsigned long CTRL1_PSEG2_MASK = (7U << CTRL1_PSEG2_SHIFT); +constexpr unsigned long CTRL1_PSEG1_SHIFT = (19); /* Bits 19-21: Phase Segment 1 */ +constexpr unsigned long CTRL1_PSEG1_MASK = (7U << CTRL1_PSEG1_SHIFT); +constexpr unsigned long CTRL1_RJW_SHIFT = (22); /* Bits 22-23: Resync Jump Width */ +constexpr unsigned long CTRL1_RJW_MASK = (3U << CTRL1_RJW_SHIFT); constexpr unsigned long CTRL1_PRESDIV_SHIFT = (24); /* Bits 24-31: Prescaler Division Factor */ -constexpr unsigned long CTRL1_PRESDIV_MASK = (0xff << CTRL1_PRESDIV_SHIFT); +constexpr unsigned long CTRL1_PRESDIV_MASK = (0xff << CTRL1_PRESDIV_SHIFT); /* Free Running Timer */ -constexpr unsigned long TIMER_SHIFT = (0U); /* Bits 0-15: Timer value */ -constexpr unsigned long TIMER_MASK = (0xffffU << TIMER_SHIFT); +constexpr unsigned long TIMER_SHIFT = (0U); /* Bits 0-15: Timer value */ +constexpr unsigned long TIMER_MASK = (0xffffU << TIMER_SHIFT); /* Bits 16-31: Reserved */ /* Rx Mailboxes Global Mask Register (32 Rx Mailboxes Global Mask Bits) */ -constexpr unsigned long RXMGMASK0 = (1U << 0); /* Bit 0: Rx Mailbox 0 Global Mask Bit */ -constexpr unsigned long RXMGMASK1 = (1U << 1); /* Bit 1: Rx Mailbox 1 Global Mask Bit */ -constexpr unsigned long RXMGMASK2 = (1U << 2); /* Bit 2: Rx Mailbox 2 Global Mask Bit */ -constexpr unsigned long RXMGMASK3 = (1U << 3); /* Bit 3: Rx Mailbox 3 Global Mask Bit */ -constexpr unsigned long RXMGMASK4 = (1U << 4); /* Bit 4: Rx Mailbox 4 Global Mask Bit */ -constexpr unsigned long RXMGMASK5 = (1U << 5); /* Bit 5: Rx Mailbox 5 Global Mask Bit */ -constexpr unsigned long RXMGMASK6 = (1U << 6); /* Bit 6: Rx Mailbox 6 Global Mask Bit */ -constexpr unsigned long RXMGMASK7 = (1U << 7); /* Bit 7: Rx Mailbox 7 Global Mask Bit */ -constexpr unsigned long RXMGMASK8 = (1U << 8); /* Bit 8: Rx Mailbox 8 Global Mask Bit */ -constexpr unsigned long RXMGMASK9 = (1U << 9); /* Bit 9: Rx Mailbox 9 Global Mask Bit */ -constexpr unsigned long RXMGMASK10 = (1U << 10); /* Bit 10: Rx Mailbox 10 Global Mask Bit */ -constexpr unsigned long RXMGMASK11 = (1U << 11); /* Bit 11: Rx Mailbox 11 Global Mask Bit */ -constexpr unsigned long RXMGMASK12 = (1U << 12); /* Bit 12: Rx Mailbox 12 Global Mask Bit */ -constexpr unsigned long RXMGMASK13 = (1U << 13); /* Bit 13: Rx Mailbox 13 Global Mask Bit */ -constexpr unsigned long RXMGMASK14 = (1U << 14); /* Bit 14: Rx Mailbox 14 Global Mask Bit */ -constexpr unsigned long RXMGMASK15 = (1U << 15); /* Bit 15: Rx Mailbox 15 Global Mask Bit */ -constexpr unsigned long RXMGMASK16 = (1U << 16); /* Bit 16: Rx Mailbox 16 Global Mask Bit */ -constexpr unsigned long RXMGMASK17 = (1U << 17); /* Bit 17: Rx Mailbox 17 Global Mask Bit */ -constexpr unsigned long RXMGMASK18 = (1U << 18); /* Bit 18: Rx Mailbox 18 Global Mask Bit */ -constexpr unsigned long RXMGMASK19 = (1U << 19); /* Bit 19: Rx Mailbox 19 Global Mask Bit */ -constexpr unsigned long RXMGMASK20 = (1U << 20); /* Bit 20: Rx Mailbox 20 Global Mask Bit */ -constexpr unsigned long RXMGMASK21 = (1U << 21); /* Bit 21: Rx Mailbox 21 Global Mask Bit */ -constexpr unsigned long RXMGMASK22 = (1U << 22); /* Bit 22: Rx Mailbox 22 Global Mask Bit */ -constexpr unsigned long RXMGMASK23 = (1U << 23); /* Bit 23: Rx Mailbox 23 Global Mask Bit */ -constexpr unsigned long RXMGMASK24 = (1U << 24); /* Bit 24: Rx Mailbox 24 Global Mask Bit */ -constexpr unsigned long RXMGMASK25 = (1U << 25); /* Bit 25: Rx Mailbox 25 Global Mask Bit */ -constexpr unsigned long RXMGMASK26 = (1U << 26); /* Bit 26: Rx Mailbox 26 Global Mask Bit */ -constexpr unsigned long RXMGMASK27 = (1U << 27); /* Bit 27: Rx Mailbox 27 Global Mask Bit */ -constexpr unsigned long RXMGMASK28 = (1U << 28); /* Bit 28: Rx Mailbox 28 Global Mask Bit */ -constexpr unsigned long RXMGMASK29 = (1U << 29); /* Bit 29: Rx Mailbox 29 Global Mask Bit */ -constexpr unsigned long RXMGMASK30 = (1U << 30); /* Bit 30: Rx Mailbox 30 Global Mask Bit */ -constexpr unsigned long RXMGMASK31 = (1U << 31); /* Bit 31: Rx Mailbox 31 Global Mask Bit */ +constexpr unsigned long RXMGMASK0 = (1U << 0); /* Bit 0: Rx Mailbox 0 Global Mask Bit */ +constexpr unsigned long RXMGMASK1 = (1U << 1); /* Bit 1: Rx Mailbox 1 Global Mask Bit */ +constexpr unsigned long RXMGMASK2 = (1U << 2); /* Bit 2: Rx Mailbox 2 Global Mask Bit */ +constexpr unsigned long RXMGMASK3 = (1U << 3); /* Bit 3: Rx Mailbox 3 Global Mask Bit */ +constexpr unsigned long RXMGMASK4 = (1U << 4); /* Bit 4: Rx Mailbox 4 Global Mask Bit */ +constexpr unsigned long RXMGMASK5 = (1U << 5); /* Bit 5: Rx Mailbox 5 Global Mask Bit */ +constexpr unsigned long RXMGMASK6 = (1U << 6); /* Bit 6: Rx Mailbox 6 Global Mask Bit */ +constexpr unsigned long RXMGMASK7 = (1U << 7); /* Bit 7: Rx Mailbox 7 Global Mask Bit */ +constexpr unsigned long RXMGMASK8 = (1U << 8); /* Bit 8: Rx Mailbox 8 Global Mask Bit */ +constexpr unsigned long RXMGMASK9 = (1U << 9); /* Bit 9: Rx Mailbox 9 Global Mask Bit */ +constexpr unsigned long RXMGMASK10 = (1U << 10); /* Bit 10: Rx Mailbox 10 Global Mask Bit */ +constexpr unsigned long RXMGMASK11 = (1U << 11); /* Bit 11: Rx Mailbox 11 Global Mask Bit */ +constexpr unsigned long RXMGMASK12 = (1U << 12); /* Bit 12: Rx Mailbox 12 Global Mask Bit */ +constexpr unsigned long RXMGMASK13 = (1U << 13); /* Bit 13: Rx Mailbox 13 Global Mask Bit */ +constexpr unsigned long RXMGMASK14 = (1U << 14); /* Bit 14: Rx Mailbox 14 Global Mask Bit */ +constexpr unsigned long RXMGMASK15 = (1U << 15); /* Bit 15: Rx Mailbox 15 Global Mask Bit */ +constexpr unsigned long RXMGMASK16 = (1U << 16); /* Bit 16: Rx Mailbox 16 Global Mask Bit */ +constexpr unsigned long RXMGMASK17 = (1U << 17); /* Bit 17: Rx Mailbox 17 Global Mask Bit */ +constexpr unsigned long RXMGMASK18 = (1U << 18); /* Bit 18: Rx Mailbox 18 Global Mask Bit */ +constexpr unsigned long RXMGMASK19 = (1U << 19); /* Bit 19: Rx Mailbox 19 Global Mask Bit */ +constexpr unsigned long RXMGMASK20 = (1U << 20); /* Bit 20: Rx Mailbox 20 Global Mask Bit */ +constexpr unsigned long RXMGMASK21 = (1U << 21); /* Bit 21: Rx Mailbox 21 Global Mask Bit */ +constexpr unsigned long RXMGMASK22 = (1U << 22); /* Bit 22: Rx Mailbox 22 Global Mask Bit */ +constexpr unsigned long RXMGMASK23 = (1U << 23); /* Bit 23: Rx Mailbox 23 Global Mask Bit */ +constexpr unsigned long RXMGMASK24 = (1U << 24); /* Bit 24: Rx Mailbox 24 Global Mask Bit */ +constexpr unsigned long RXMGMASK25 = (1U << 25); /* Bit 25: Rx Mailbox 25 Global Mask Bit */ +constexpr unsigned long RXMGMASK26 = (1U << 26); /* Bit 26: Rx Mailbox 26 Global Mask Bit */ +constexpr unsigned long RXMGMASK27 = (1U << 27); /* Bit 27: Rx Mailbox 27 Global Mask Bit */ +constexpr unsigned long RXMGMASK28 = (1U << 28); /* Bit 28: Rx Mailbox 28 Global Mask Bit */ +constexpr unsigned long RXMGMASK29 = (1U << 29); /* Bit 29: Rx Mailbox 29 Global Mask Bit */ +constexpr unsigned long RXMGMASK30 = (1U << 30); /* Bit 30: Rx Mailbox 30 Global Mask Bit */ +constexpr unsigned long RXMGMASK31 = (1U << 31); /* Bit 31: Rx Mailbox 31 Global Mask Bit */ /* Rx 14 Mask Register */ -constexpr unsigned long RXM14MASK0 = (1U << 0); /* Bit 0: Rx Buffer 14 Mask Bit 0 */ -constexpr unsigned long RXM14MASK1 = (1U << 1); /* Bit 1: Rx Buffer 14 Mask Bit 1 */ -constexpr unsigned long RXM14MASK2 = (1U << 2); /* Bit 2: Rx Buffer 14 Mask Bit 2 */ -constexpr unsigned long RXM14MASK3 = (1U << 3); /* Bit 3: Rx Buffer 14 Mask Bit 3 */ -constexpr unsigned long RXM14MASK4 = (1U << 4); /* Bit 4: Rx Buffer 14 Mask Bit 4 */ -constexpr unsigned long RXM14MASK5 = (1U << 5); /* Bit 5: Rx Buffer 14 Mask Bit 5 */ -constexpr unsigned long RXM14MASK6 = (1U << 6); /* Bit 6: Rx Buffer 14 Mask Bit 6 */ -constexpr unsigned long RXM14MASK7 = (1U << 7); /* Bit 7: Rx Buffer 14 Mask Bit 7 */ -constexpr unsigned long RXM14MASK8 = (1U << 8); /* Bit 8: Rx Buffer 14 Mask Bit 8 */ -constexpr unsigned long RXM14MASK9 = (1U << 9); /* Bit 9: Rx Buffer 14 Mask Bit 9 */ -constexpr unsigned long RXM14MASK10 = (1U << 10); /* Bit 10: Rx Buffer 14 Mask Bit 10 */ -constexpr unsigned long RXM14MASK11 = (1U << 11); /* Bit 11: Rx Buffer 14 Mask Bit 11 */ -constexpr unsigned long RXM14MASK12 = (1U << 12); /* Bit 12: Rx Buffer 14 Mask Bit 12 */ -constexpr unsigned long RXM14MASK13 = (1U << 13); /* Bit 13: Rx Buffer 14 Mask Bit 13 */ -constexpr unsigned long RXM14MASK14 = (1U << 14); /* Bit 14: Rx Buffer 14 Mask Bit 14 */ -constexpr unsigned long RXM14MASK15 = (1U << 15); /* Bit 15: Rx Buffer 14 Mask Bit 15 */ -constexpr unsigned long RXM14MASK16 = (1U << 16); /* Bit 16: Rx Buffer 14 Mask Bit 16 */ -constexpr unsigned long RXM14MASK17 = (1U << 17); /* Bit 17: Rx Buffer 14 Mask Bit 17 */ -constexpr unsigned long RXM14MASK18 = (1U << 18); /* Bit 18: Rx Buffer 14 Mask Bit 18 */ -constexpr unsigned long RXM14MASK19 = (1U << 19); /* Bit 19: Rx Buffer 14 Mask Bit 19 */ -constexpr unsigned long RXM14MASK20 = (1U << 20); /* Bit 20: Rx Buffer 14 Mask Bit 20 */ -constexpr unsigned long RXM14MASK21 = (1U << 21); /* Bit 21: Rx Buffer 14 Mask Bit 21 */ -constexpr unsigned long RXM14MASK22 = (1U << 22); /* Bit 22: Rx Buffer 14 Mask Bit 22 */ -constexpr unsigned long RXM14MASK23 = (1U << 23); /* Bit 23: Rx Buffer 14 Mask Bit 23 */ -constexpr unsigned long RXM14MASK24 = (1U << 24); /* Bit 24: Rx Buffer 14 Mask Bit 24 */ -constexpr unsigned long RXM14MASK25 = (1U << 25); /* Bit 25: Rx Buffer 14 Mask Bit 25 */ -constexpr unsigned long RXM14MASK26 = (1U << 26); /* Bit 26: Rx Buffer 14 Mask Bit 26 */ -constexpr unsigned long RXM14MASK27 = (1U << 27); /* Bit 27: Rx Buffer 14 Mask Bit 27 */ -constexpr unsigned long RXM14MASK28 = (1U << 28); /* Bit 28: Rx Buffer 14 Mask Bit 28 */ -constexpr unsigned long RXM14MASK29 = (1U << 29); /* Bit 29: Rx Buffer 14 Mask Bit 29 */ -constexpr unsigned long RXM14MASK30 = (1U << 30); /* Bit 30: Rx Buffer 14 Mask Bit 30 */ -constexpr unsigned long RXM14MASK31 = (1U << 31); /* Bit 31: Rx Buffer 14 Mask Bit 31 */ +constexpr unsigned long RXM14MASK0 = (1U << 0); /* Bit 0: Rx Buffer 14 Mask Bit 0 */ +constexpr unsigned long RXM14MASK1 = (1U << 1); /* Bit 1: Rx Buffer 14 Mask Bit 1 */ +constexpr unsigned long RXM14MASK2 = (1U << 2); /* Bit 2: Rx Buffer 14 Mask Bit 2 */ +constexpr unsigned long RXM14MASK3 = (1U << 3); /* Bit 3: Rx Buffer 14 Mask Bit 3 */ +constexpr unsigned long RXM14MASK4 = (1U << 4); /* Bit 4: Rx Buffer 14 Mask Bit 4 */ +constexpr unsigned long RXM14MASK5 = (1U << 5); /* Bit 5: Rx Buffer 14 Mask Bit 5 */ +constexpr unsigned long RXM14MASK6 = (1U << 6); /* Bit 6: Rx Buffer 14 Mask Bit 6 */ +constexpr unsigned long RXM14MASK7 = (1U << 7); /* Bit 7: Rx Buffer 14 Mask Bit 7 */ +constexpr unsigned long RXM14MASK8 = (1U << 8); /* Bit 8: Rx Buffer 14 Mask Bit 8 */ +constexpr unsigned long RXM14MASK9 = (1U << 9); /* Bit 9: Rx Buffer 14 Mask Bit 9 */ +constexpr unsigned long RXM14MASK10 = (1U << 10); /* Bit 10: Rx Buffer 14 Mask Bit 10 */ +constexpr unsigned long RXM14MASK11 = (1U << 11); /* Bit 11: Rx Buffer 14 Mask Bit 11 */ +constexpr unsigned long RXM14MASK12 = (1U << 12); /* Bit 12: Rx Buffer 14 Mask Bit 12 */ +constexpr unsigned long RXM14MASK13 = (1U << 13); /* Bit 13: Rx Buffer 14 Mask Bit 13 */ +constexpr unsigned long RXM14MASK14 = (1U << 14); /* Bit 14: Rx Buffer 14 Mask Bit 14 */ +constexpr unsigned long RXM14MASK15 = (1U << 15); /* Bit 15: Rx Buffer 14 Mask Bit 15 */ +constexpr unsigned long RXM14MASK16 = (1U << 16); /* Bit 16: Rx Buffer 14 Mask Bit 16 */ +constexpr unsigned long RXM14MASK17 = (1U << 17); /* Bit 17: Rx Buffer 14 Mask Bit 17 */ +constexpr unsigned long RXM14MASK18 = (1U << 18); /* Bit 18: Rx Buffer 14 Mask Bit 18 */ +constexpr unsigned long RXM14MASK19 = (1U << 19); /* Bit 19: Rx Buffer 14 Mask Bit 19 */ +constexpr unsigned long RXM14MASK20 = (1U << 20); /* Bit 20: Rx Buffer 14 Mask Bit 20 */ +constexpr unsigned long RXM14MASK21 = (1U << 21); /* Bit 21: Rx Buffer 14 Mask Bit 21 */ +constexpr unsigned long RXM14MASK22 = (1U << 22); /* Bit 22: Rx Buffer 14 Mask Bit 22 */ +constexpr unsigned long RXM14MASK23 = (1U << 23); /* Bit 23: Rx Buffer 14 Mask Bit 23 */ +constexpr unsigned long RXM14MASK24 = (1U << 24); /* Bit 24: Rx Buffer 14 Mask Bit 24 */ +constexpr unsigned long RXM14MASK25 = (1U << 25); /* Bit 25: Rx Buffer 14 Mask Bit 25 */ +constexpr unsigned long RXM14MASK26 = (1U << 26); /* Bit 26: Rx Buffer 14 Mask Bit 26 */ +constexpr unsigned long RXM14MASK27 = (1U << 27); /* Bit 27: Rx Buffer 14 Mask Bit 27 */ +constexpr unsigned long RXM14MASK28 = (1U << 28); /* Bit 28: Rx Buffer 14 Mask Bit 28 */ +constexpr unsigned long RXM14MASK29 = (1U << 29); /* Bit 29: Rx Buffer 14 Mask Bit 29 */ +constexpr unsigned long RXM14MASK30 = (1U << 30); /* Bit 30: Rx Buffer 14 Mask Bit 30 */ +constexpr unsigned long RXM14MASK31 = (1U << 31); /* Bit 31: Rx Buffer 14 Mask Bit 31 */ /* Rx 15 Mask Register */ -constexpr unsigned long RXM15MASK0 = (1U << 0); /* Bit 0: Rx Buffer 15 Mask Bit 0 */ -constexpr unsigned long RXM15MASK1 = (1U << 1); /* Bit 1: Rx Buffer 15 Mask Bit 1 */ -constexpr unsigned long RXM15MASK2 = (1U << 2); /* Bit 2: Rx Buffer 15 Mask Bit 2 */ -constexpr unsigned long RXM15MASK3 = (1U << 3); /* Bit 3: Rx Buffer 15 Mask Bit 3 */ -constexpr unsigned long RXM15MASK4 = (1U << 4); /* Bit 4: Rx Buffer 15 Mask Bit 4 */ -constexpr unsigned long RXM15MASK5 = (1U << 5); /* Bit 5: Rx Buffer 15 Mask Bit 5 */ -constexpr unsigned long RXM15MASK6 = (1U << 6); /* Bit 6: Rx Buffer 15 Mask Bit 6 */ -constexpr unsigned long RXM15MASK7 = (1U << 7); /* Bit 7: Rx Buffer 15 Mask Bit 7 */ -constexpr unsigned long RXM15MASK8 = (1U << 8); /* Bit 8: Rx Buffer 15 Mask Bit 8 */ -constexpr unsigned long RXM15MASK9 = (1U << 9); /* Bit 9: Rx Buffer 15 Mask Bit 9 */ -constexpr unsigned long RXM15MASK10 = (1U << 10); /* Bit 10: Rx Buffer 15 Mask Bit 10 */ -constexpr unsigned long RXM15MASK11 = (1U << 11); /* Bit 11: Rx Buffer 15 Mask Bit 11 */ -constexpr unsigned long RXM15MASK12 = (1U << 12); /* Bit 12: Rx Buffer 15 Mask Bit 12 */ -constexpr unsigned long RXM15MASK13 = (1U << 13); /* Bit 13: Rx Buffer 15 Mask Bit 13 */ -constexpr unsigned long RXM15MASK14 = (1U << 14); /* Bit 14: Rx Buffer 15 Mask Bit 14 */ -constexpr unsigned long RXM15MASK15 = (1U << 15); /* Bit 15: Rx Buffer 15 Mask Bit 15 */ -constexpr unsigned long RXM15MASK16 = (1U << 16); /* Bit 16: Rx Buffer 15 Mask Bit 16 */ -constexpr unsigned long RXM15MASK17 = (1U << 17); /* Bit 17: Rx Buffer 15 Mask Bit 17 */ -constexpr unsigned long RXM15MASK18 = (1U << 18); /* Bit 18: Rx Buffer 15 Mask Bit 18 */ -constexpr unsigned long RXM15MASK19 = (1U << 19); /* Bit 19: Rx Buffer 15 Mask Bit 19 */ -constexpr unsigned long RXM15MASK20 = (1U << 20); /* Bit 20: Rx Buffer 15 Mask Bit 20 */ -constexpr unsigned long RXM15MASK21 = (1U << 21); /* Bit 21: Rx Buffer 15 Mask Bit 21 */ -constexpr unsigned long RXM15MASK22 = (1U << 22); /* Bit 22: Rx Buffer 15 Mask Bit 22 */ -constexpr unsigned long RXM15MASK23 = (1U << 23); /* Bit 23: Rx Buffer 15 Mask Bit 23 */ -constexpr unsigned long RXM15MASK24 = (1U << 24); /* Bit 24: Rx Buffer 15 Mask Bit 24 */ -constexpr unsigned long RXM15MASK25 = (1U << 25); /* Bit 25: Rx Buffer 15 Mask Bit 25 */ -constexpr unsigned long RXM15MASK26 = (1U << 26); /* Bit 26: Rx Buffer 15 Mask Bit 26 */ -constexpr unsigned long RXM15MASK27 = (1U << 27); /* Bit 27: Rx Buffer 15 Mask Bit 27 */ -constexpr unsigned long RXM15MASK28 = (1U << 28); /* Bit 28: Rx Buffer 15 Mask Bit 28 */ -constexpr unsigned long RXM15MASK29 = (1U << 29); /* Bit 29: Rx Buffer 15 Mask Bit 29 */ -constexpr unsigned long RXM15MASK30 = (1U << 30); /* Bit 30: Rx Buffer 15 Mask Bit 30 */ -constexpr unsigned long RXM15MASK31 = (1U << 31); /* Bit 31: Rx Buffer 15 Mask Bit 31 */ +constexpr unsigned long RXM15MASK0 = (1U << 0); /* Bit 0: Rx Buffer 15 Mask Bit 0 */ +constexpr unsigned long RXM15MASK1 = (1U << 1); /* Bit 1: Rx Buffer 15 Mask Bit 1 */ +constexpr unsigned long RXM15MASK2 = (1U << 2); /* Bit 2: Rx Buffer 15 Mask Bit 2 */ +constexpr unsigned long RXM15MASK3 = (1U << 3); /* Bit 3: Rx Buffer 15 Mask Bit 3 */ +constexpr unsigned long RXM15MASK4 = (1U << 4); /* Bit 4: Rx Buffer 15 Mask Bit 4 */ +constexpr unsigned long RXM15MASK5 = (1U << 5); /* Bit 5: Rx Buffer 15 Mask Bit 5 */ +constexpr unsigned long RXM15MASK6 = (1U << 6); /* Bit 6: Rx Buffer 15 Mask Bit 6 */ +constexpr unsigned long RXM15MASK7 = (1U << 7); /* Bit 7: Rx Buffer 15 Mask Bit 7 */ +constexpr unsigned long RXM15MASK8 = (1U << 8); /* Bit 8: Rx Buffer 15 Mask Bit 8 */ +constexpr unsigned long RXM15MASK9 = (1U << 9); /* Bit 9: Rx Buffer 15 Mask Bit 9 */ +constexpr unsigned long RXM15MASK10 = (1U << 10); /* Bit 10: Rx Buffer 15 Mask Bit 10 */ +constexpr unsigned long RXM15MASK11 = (1U << 11); /* Bit 11: Rx Buffer 15 Mask Bit 11 */ +constexpr unsigned long RXM15MASK12 = (1U << 12); /* Bit 12: Rx Buffer 15 Mask Bit 12 */ +constexpr unsigned long RXM15MASK13 = (1U << 13); /* Bit 13: Rx Buffer 15 Mask Bit 13 */ +constexpr unsigned long RXM15MASK14 = (1U << 14); /* Bit 14: Rx Buffer 15 Mask Bit 14 */ +constexpr unsigned long RXM15MASK15 = (1U << 15); /* Bit 15: Rx Buffer 15 Mask Bit 15 */ +constexpr unsigned long RXM15MASK16 = (1U << 16); /* Bit 16: Rx Buffer 15 Mask Bit 16 */ +constexpr unsigned long RXM15MASK17 = (1U << 17); /* Bit 17: Rx Buffer 15 Mask Bit 17 */ +constexpr unsigned long RXM15MASK18 = (1U << 18); /* Bit 18: Rx Buffer 15 Mask Bit 18 */ +constexpr unsigned long RXM15MASK19 = (1U << 19); /* Bit 19: Rx Buffer 15 Mask Bit 19 */ +constexpr unsigned long RXM15MASK20 = (1U << 20); /* Bit 20: Rx Buffer 15 Mask Bit 20 */ +constexpr unsigned long RXM15MASK21 = (1U << 21); /* Bit 21: Rx Buffer 15 Mask Bit 21 */ +constexpr unsigned long RXM15MASK22 = (1U << 22); /* Bit 22: Rx Buffer 15 Mask Bit 22 */ +constexpr unsigned long RXM15MASK23 = (1U << 23); /* Bit 23: Rx Buffer 15 Mask Bit 23 */ +constexpr unsigned long RXM15MASK24 = (1U << 24); /* Bit 24: Rx Buffer 15 Mask Bit 24 */ +constexpr unsigned long RXM15MASK25 = (1U << 25); /* Bit 25: Rx Buffer 15 Mask Bit 25 */ +constexpr unsigned long RXM15MASK26 = (1U << 26); /* Bit 26: Rx Buffer 15 Mask Bit 26 */ +constexpr unsigned long RXM15MASK27 = (1U << 27); /* Bit 27: Rx Buffer 15 Mask Bit 27 */ +constexpr unsigned long RXM15MASK28 = (1U << 28); /* Bit 28: Rx Buffer 15 Mask Bit 28 */ +constexpr unsigned long RXM15MASK29 = (1U << 29); /* Bit 29: Rx Buffer 15 Mask Bit 29 */ +constexpr unsigned long RXM15MASK30 = (1U << 30); /* Bit 30: Rx Buffer 15 Mask Bit 30 */ +constexpr unsigned long RXM15MASK31 = (1U << 31); /* Bit 31: Rx Buffer 15 Mask Bit 31 */ /* Error Counter */ -constexpr unsigned long ECR_TXERRCNT_SHIFT = (0U); /* Bits 0-7: Transmit Error Counter */ -constexpr unsigned long ECR_TXERRCNT_MASK = (0xff << ECR_TXERRCNT_SHIFT); -constexpr unsigned long ECR_RXERRCNT_SHIFT = (8); /* Bits 8-15: Receive Error Counter */ -constexpr unsigned long ECR_RXERRCNT_MASK = (0xff << ECR_RXERRCNT_SHIFT); +constexpr unsigned long ECR_TXERRCNT_SHIFT = (0U); /* Bits 0-7: Transmit Error Counter */ +constexpr unsigned long ECR_TXERRCNT_MASK = (0xff << ECR_TXERRCNT_SHIFT); +constexpr unsigned long ECR_RXERRCNT_SHIFT = (8); /* Bits 8-15: Receive Error Counter */ +constexpr unsigned long ECR_RXERRCNT_MASK = (0xff << ECR_RXERRCNT_SHIFT); /* Bits 16-31: Reserved */ /* Error and Status 1 Register */ -constexpr unsigned long ESR1_WAKINT = (1U << 0); /* Bit 0: Wake-Up Interrupt */ -constexpr unsigned long ESR1_ERRINT = (1U << 1); /* Bit 1: Error Interrupt */ -constexpr unsigned long ESR1_BOFFINT = (1U << 2); /* Bit 2: 'Bus Off' Interrupt */ -constexpr unsigned long ESR1_RX = (1U << 3); /* Bit 3: FlexCAN in Reception */ -constexpr unsigned long ESR1_FLTCONF_SHIFT = (4U); /* Bits 4-5: Fault Confinement State */ -constexpr unsigned long ESR1_FLTCONF_MASK = (3U << ESR1_FLTCONF_SHIFT); -constexpr unsigned long ESR1_FLTCONF_ACTV = (0U << ESR1_FLTCONF_SHIFT); /* Error Active */ -constexpr unsigned long ESR1_FLTCONF_PASV = (1U << ESR1_FLTCONF_SHIFT); /* Error Passive */ -constexpr unsigned long ESR1_FLTCONF_OFF = (2U << ESR1_FLTCONF_SHIFT); /* Bus Off */ -constexpr unsigned long ESR1_TX = (1U << 6); /* Bit 6: FlexCAN in Transmission */ -constexpr unsigned long ESR1_IDLE = (1U << 7); /* Bit 7: CAN bus is in IDLE state */ -constexpr unsigned long ESR1_RXWRN = (1U << 8); /* Bit 8: Rx Error Warning */ -constexpr unsigned long ESR1_TXWRN = (1U << 9); /* Bit 9: TX Error Warning */ -constexpr unsigned long ESR1_STFERR = (1U << 10); /* Bit 10: Stuffing Error */ -constexpr unsigned long ESR1_FRMERR = (1U << 11); /* Bit 11: Form Error */ -constexpr unsigned long ESR1_CRCERR = (1U << 12); /* Bit 12: Cyclic Redundancy Check Error */ -constexpr unsigned long ESR1_ACKERR = (1U << 13); /* Bit 13: Acknowledge Error */ -constexpr unsigned long ESR1_BIT0ERR = (1U << 14); /* Bit 14: Bit0 Error */ -constexpr unsigned long ESR1_BIT1ERR = (1U << 15); /* Bit 15: Bit1 Error */ -constexpr unsigned long ESR1_RWRNINT = (1U << 16); /* Bit 16: Rx Warning Interrupt Flag */ -constexpr unsigned long ESR1_TWRNINT = (1U << 17); /* Bit 17: Tx Warning Interrupt Flag */ -constexpr unsigned long ESR1_SYNCH = (1U << 18); /* Bit 18: CAN Synchronization Status */ - /* Bits 19-31: Reserved */ +constexpr unsigned long ESR1_WAKINT = (1U << 0); /* Bit 0: Wake-Up Interrupt */ +constexpr unsigned long ESR1_ERRINT = (1U << 1); /* Bit 1: Error Interrupt */ +constexpr unsigned long ESR1_BOFFINT = (1U << 2); /* Bit 2: 'Bus Off' Interrupt */ +constexpr unsigned long ESR1_RX = (1U << 3); /* Bit 3: FlexCAN in Reception */ +constexpr unsigned long ESR1_FLTCONF_SHIFT = (4U); /* Bits 4-5: Fault Confinement State */ +constexpr unsigned long ESR1_FLTCONF_MASK = (3U << ESR1_FLTCONF_SHIFT); +constexpr unsigned long ESR1_FLTCONF_ACTV = (0U << ESR1_FLTCONF_SHIFT); /* Error Active */ +constexpr unsigned long ESR1_FLTCONF_PASV = (1U << ESR1_FLTCONF_SHIFT); /* Error Passive */ +constexpr unsigned long ESR1_FLTCONF_OFF = (2U << ESR1_FLTCONF_SHIFT); /* Bus Off */ +constexpr unsigned long ESR1_TX = (1U << 6); /* Bit 6: FlexCAN in Transmission */ +constexpr unsigned long ESR1_IDLE = (1U << 7); /* Bit 7: CAN bus is in IDLE state */ +constexpr unsigned long ESR1_RXWRN = (1U << 8); /* Bit 8: Rx Error Warning */ +constexpr unsigned long ESR1_TXWRN = (1U << 9); /* Bit 9: TX Error Warning */ +constexpr unsigned long ESR1_STFERR = (1U << 10); /* Bit 10: Stuffing Error */ +constexpr unsigned long ESR1_FRMERR = (1U << 11); /* Bit 11: Form Error */ +constexpr unsigned long ESR1_CRCERR = (1U << 12); /* Bit 12: Cyclic Redundancy Check Error */ +constexpr unsigned long ESR1_ACKERR = (1U << 13); /* Bit 13: Acknowledge Error */ +constexpr unsigned long ESR1_BIT0ERR = (1U << 14); /* Bit 14: Bit0 Error */ +constexpr unsigned long ESR1_BIT1ERR = (1U << 15); /* Bit 15: Bit1 Error */ +constexpr unsigned long ESR1_RWRNINT = (1U << 16); /* Bit 16: Rx Warning Interrupt Flag */ +constexpr unsigned long ESR1_TWRNINT = (1U << 17); /* Bit 17: Tx Warning Interrupt Flag */ +constexpr unsigned long ESR1_SYNCH = (1U << 18); /* Bit 18: CAN Synchronization Status */ +/* Bits 19-31: Reserved */ /* Interrupt Masks 2 Register */ -constexpr unsigned long CAN_IMASK2_0 = (1U << 0); /* Bit 0: Buffer MB0 Mask */ -constexpr unsigned long CAN_IMASK2_1 = (1U << 1); /* Bit 1: Buffer MB1 Mask */ -constexpr unsigned long CAN_IMASK2_2 = (1U << 2); /* Bit 2: Buffer MB2 Mask */ -constexpr unsigned long CAN_IMASK2_3 = (1U << 3); /* Bit 3: Buffer MB3 Mask */ -constexpr unsigned long CAN_IMASK2_4 = (1U << 4); /* Bit 4: Buffer MB4 Mask */ -constexpr unsigned long CAN_IMASK2_5 = (1U << 5); /* Bit 5: Buffer MB5 Mask */ -constexpr unsigned long CAN_IMASK2_6 = (1U << 6); /* Bit 6: Buffer MB6 Mask */ -constexpr unsigned long CAN_IMASK2_7 = (1U << 7); /* Bit 7: Buffer MB7 Mask */ -constexpr unsigned long CAN_IMASK2_8 = (1U << 8); /* Bit 8: Buffer MB8 Mask */ -constexpr unsigned long CAN_IMASK2_9 = (1U << 9); /* Bit 9: Buffer MB9 Mask */ -constexpr unsigned long CAN_IMASK2_10 = (1U << 10); /* Bit 10: Buffer MB10 Mask */ -constexpr unsigned long CAN_IMASK2_11 = (1U << 11); /* Bit 11: Buffer MB11 Mask */ -constexpr unsigned long CAN_IMASK2_12 = (1U << 12); /* Bit 12: Buffer MB12 Mask */ -constexpr unsigned long CAN_IMASK2_13 = (1U << 13); /* Bit 13: Buffer MB13 Mask */ -constexpr unsigned long CAN_IMASK2_14 = (1U << 14); /* Bit 14: Buffer MB14 Mask */ -constexpr unsigned long CAN_IMASK2_15 = (1U << 15); /* Bit 15: Buffer MB15 Mask */ -constexpr unsigned long CAN_IMASK2_16 = (1U << 16); /* Bit 16: Buffer MB16 Mask */ -constexpr unsigned long CAN_IMASK2_17 = (1U << 17); /* Bit 17: Buffer MB17 Mask */ -constexpr unsigned long CAN_IMASK2_18 = (1U << 18); /* Bit 18: Buffer MB18 Mask */ -constexpr unsigned long CAN_IMASK2_19 = (1U << 19); /* Bit 19: Buffer MB19 Mask */ -constexpr unsigned long CAN_IMASK2_20 = (1U << 20); /* Bit 20: Buffer MB20 Mask */ -constexpr unsigned long CAN_IMASK2_21 = (1U << 21); /* Bit 21: Buffer MB21 Mask */ -constexpr unsigned long CAN_IMASK2_22 = (1U << 22); /* Bit 22: Buffer MB22 Mask */ -constexpr unsigned long CAN_IMASK2_23 = (1U << 23); /* Bit 23: Buffer MB23 Mask */ -constexpr unsigned long CAN_IMASK2_24 = (1U << 24); /* Bit 24: Buffer MB24 Mask */ -constexpr unsigned long CAN_IMASK2_25 = (1U << 25); /* Bit 25: Buffer MB25 Mask */ -constexpr unsigned long CAN_IMASK2_26 = (1U << 26); /* Bit 26: Buffer MB26 Mask */ -constexpr unsigned long CAN_IMASK2_27 = (1U << 27); /* Bit 27: Buffer MB27 Mask */ -constexpr unsigned long CAN_IMASK2_28 = (1U << 28); /* Bit 28: Buffer MB28 Mask */ -constexpr unsigned long CAN_IMASK2_29 = (1U << 29); /* Bit 29: Buffer MB29 Mask */ -constexpr unsigned long CAN_IMASK2_30 = (1U << 30); /* Bit 30: Buffer MB30 Mask */ -constexpr unsigned long CAN_IMASK2_31 = (1U << 31); /* Bit 31: Buffer MB31 Mask */ +constexpr unsigned long CAN_IMASK2_0 = (1U << 0); /* Bit 0: Buffer MB0 Mask */ +constexpr unsigned long CAN_IMASK2_1 = (1U << 1); /* Bit 1: Buffer MB1 Mask */ +constexpr unsigned long CAN_IMASK2_2 = (1U << 2); /* Bit 2: Buffer MB2 Mask */ +constexpr unsigned long CAN_IMASK2_3 = (1U << 3); /* Bit 3: Buffer MB3 Mask */ +constexpr unsigned long CAN_IMASK2_4 = (1U << 4); /* Bit 4: Buffer MB4 Mask */ +constexpr unsigned long CAN_IMASK2_5 = (1U << 5); /* Bit 5: Buffer MB5 Mask */ +constexpr unsigned long CAN_IMASK2_6 = (1U << 6); /* Bit 6: Buffer MB6 Mask */ +constexpr unsigned long CAN_IMASK2_7 = (1U << 7); /* Bit 7: Buffer MB7 Mask */ +constexpr unsigned long CAN_IMASK2_8 = (1U << 8); /* Bit 8: Buffer MB8 Mask */ +constexpr unsigned long CAN_IMASK2_9 = (1U << 9); /* Bit 9: Buffer MB9 Mask */ +constexpr unsigned long CAN_IMASK2_10 = (1U << 10); /* Bit 10: Buffer MB10 Mask */ +constexpr unsigned long CAN_IMASK2_11 = (1U << 11); /* Bit 11: Buffer MB11 Mask */ +constexpr unsigned long CAN_IMASK2_12 = (1U << 12); /* Bit 12: Buffer MB12 Mask */ +constexpr unsigned long CAN_IMASK2_13 = (1U << 13); /* Bit 13: Buffer MB13 Mask */ +constexpr unsigned long CAN_IMASK2_14 = (1U << 14); /* Bit 14: Buffer MB14 Mask */ +constexpr unsigned long CAN_IMASK2_15 = (1U << 15); /* Bit 15: Buffer MB15 Mask */ +constexpr unsigned long CAN_IMASK2_16 = (1U << 16); /* Bit 16: Buffer MB16 Mask */ +constexpr unsigned long CAN_IMASK2_17 = (1U << 17); /* Bit 17: Buffer MB17 Mask */ +constexpr unsigned long CAN_IMASK2_18 = (1U << 18); /* Bit 18: Buffer MB18 Mask */ +constexpr unsigned long CAN_IMASK2_19 = (1U << 19); /* Bit 19: Buffer MB19 Mask */ +constexpr unsigned long CAN_IMASK2_20 = (1U << 20); /* Bit 20: Buffer MB20 Mask */ +constexpr unsigned long CAN_IMASK2_21 = (1U << 21); /* Bit 21: Buffer MB21 Mask */ +constexpr unsigned long CAN_IMASK2_22 = (1U << 22); /* Bit 22: Buffer MB22 Mask */ +constexpr unsigned long CAN_IMASK2_23 = (1U << 23); /* Bit 23: Buffer MB23 Mask */ +constexpr unsigned long CAN_IMASK2_24 = (1U << 24); /* Bit 24: Buffer MB24 Mask */ +constexpr unsigned long CAN_IMASK2_25 = (1U << 25); /* Bit 25: Buffer MB25 Mask */ +constexpr unsigned long CAN_IMASK2_26 = (1U << 26); /* Bit 26: Buffer MB26 Mask */ +constexpr unsigned long CAN_IMASK2_27 = (1U << 27); /* Bit 27: Buffer MB27 Mask */ +constexpr unsigned long CAN_IMASK2_28 = (1U << 28); /* Bit 28: Buffer MB28 Mask */ +constexpr unsigned long CAN_IMASK2_29 = (1U << 29); /* Bit 29: Buffer MB29 Mask */ +constexpr unsigned long CAN_IMASK2_30 = (1U << 30); /* Bit 30: Buffer MB30 Mask */ +constexpr unsigned long CAN_IMASK2_31 = (1U << 31); /* Bit 31: Buffer MB31 Mask */ /* Interrupt Masks 1 Register */ -constexpr unsigned long CAN_IMASK1_0 = (1U << 0); /* Bit 0: Buffer MB0 Mask */ -constexpr unsigned long CAN_IMASK1_1 = (1U << 1); /* Bit 1: Buffer MB1 Mask */ -constexpr unsigned long CAN_IMASK1_2 = (1U << 2); /* Bit 2: Buffer MB2 Mask */ -constexpr unsigned long CAN_IMASK1_3 = (1U << 3); /* Bit 3: Buffer MB3 Mask */ -constexpr unsigned long CAN_IMASK1_4 = (1U << 4); /* Bit 4: Buffer MB4 Mask */ -constexpr unsigned long CAN_IMASK1_5 = (1U << 5); /* Bit 5: Buffer MB5 Mask */ -constexpr unsigned long CAN_IMASK1_6 = (1U << 6); /* Bit 6: Buffer MB6 Mask */ -constexpr unsigned long CAN_IMASK1_7 = (1U << 7); /* Bit 7: Buffer MB7 Mask */ -constexpr unsigned long CAN_IMASK1_8 = (1U << 8); /* Bit 8: Buffer MB8 Mask */ -constexpr unsigned long CAN_IMASK1_9 = (1U << 9); /* Bit 9: Buffer MB9 Mask */ -constexpr unsigned long CAN_IMASK1_10 = (1U << 10); /* Bit 10: Buffer MB10 Mask */ -constexpr unsigned long CAN_IMASK1_11 = (1U << 11); /* Bit 11: Buffer MB11 Mask */ -constexpr unsigned long CAN_IMASK1_12 = (1U << 12); /* Bit 12: Buffer MB12 Mask */ -constexpr unsigned long CAN_IMASK1_13 = (1U << 13); /* Bit 13: Buffer MB13 Mask */ -constexpr unsigned long CAN_IMASK1_14 = (1U << 14); /* Bit 14: Buffer MB14 Mask */ -constexpr unsigned long CAN_IMASK1_15 = (1U << 15); /* Bit 15: Buffer MB15 Mask */ -constexpr unsigned long CAN_IMASK1_16 = (1U << 16); /* Bit 16: Buffer MB16 Mask */ -constexpr unsigned long CAN_IMASK1_17 = (1U << 17); /* Bit 17: Buffer MB17 Mask */ -constexpr unsigned long CAN_IMASK1_18 = (1U << 18); /* Bit 18: Buffer MB18 Mask */ -constexpr unsigned long CAN_IMASK1_19 = (1U << 19); /* Bit 19: Buffer MB19 Mask */ -constexpr unsigned long CAN_IMASK1_20 = (1U << 20); /* Bit 20: Buffer MB20 Mask */ -constexpr unsigned long CAN_IMASK1_21 = (1U << 21); /* Bit 21: Buffer MB21 Mask */ -constexpr unsigned long CAN_IMASK1_22 = (1U << 22); /* Bit 22: Buffer MB22 Mask */ -constexpr unsigned long CAN_IMASK1_23 = (1U << 23); /* Bit 23: Buffer MB23 Mask */ -constexpr unsigned long CAN_IMASK1_24 = (1U << 24); /* Bit 24: Buffer MB24 Mask */ -constexpr unsigned long CAN_IMASK1_25 = (1U << 25); /* Bit 25: Buffer MB25 Mask */ -constexpr unsigned long CAN_IMASK1_26 = (1U << 26); /* Bit 26: Buffer MB26 Mask */ -constexpr unsigned long CAN_IMASK1_27 = (1U << 27); /* Bit 27: Buffer MB27 Mask */ -constexpr unsigned long CAN_IMASK1_28 = (1U << 28); /* Bit 28: Buffer MB28 Mask */ -constexpr unsigned long CAN_IMASK1_29 = (1U << 29); /* Bit 29: Buffer MB29 Mask */ -constexpr unsigned long CAN_IMASK1_30 = (1U << 30); /* Bit 30: Buffer MB30 Mask */ -constexpr unsigned long CAN_IMASK1_31 = (1U << 31); /* Bit 31: Buffer MB31 Mask */ +constexpr unsigned long CAN_IMASK1_0 = (1U << 0); /* Bit 0: Buffer MB0 Mask */ +constexpr unsigned long CAN_IMASK1_1 = (1U << 1); /* Bit 1: Buffer MB1 Mask */ +constexpr unsigned long CAN_IMASK1_2 = (1U << 2); /* Bit 2: Buffer MB2 Mask */ +constexpr unsigned long CAN_IMASK1_3 = (1U << 3); /* Bit 3: Buffer MB3 Mask */ +constexpr unsigned long CAN_IMASK1_4 = (1U << 4); /* Bit 4: Buffer MB4 Mask */ +constexpr unsigned long CAN_IMASK1_5 = (1U << 5); /* Bit 5: Buffer MB5 Mask */ +constexpr unsigned long CAN_IMASK1_6 = (1U << 6); /* Bit 6: Buffer MB6 Mask */ +constexpr unsigned long CAN_IMASK1_7 = (1U << 7); /* Bit 7: Buffer MB7 Mask */ +constexpr unsigned long CAN_IMASK1_8 = (1U << 8); /* Bit 8: Buffer MB8 Mask */ +constexpr unsigned long CAN_IMASK1_9 = (1U << 9); /* Bit 9: Buffer MB9 Mask */ +constexpr unsigned long CAN_IMASK1_10 = (1U << 10); /* Bit 10: Buffer MB10 Mask */ +constexpr unsigned long CAN_IMASK1_11 = (1U << 11); /* Bit 11: Buffer MB11 Mask */ +constexpr unsigned long CAN_IMASK1_12 = (1U << 12); /* Bit 12: Buffer MB12 Mask */ +constexpr unsigned long CAN_IMASK1_13 = (1U << 13); /* Bit 13: Buffer MB13 Mask */ +constexpr unsigned long CAN_IMASK1_14 = (1U << 14); /* Bit 14: Buffer MB14 Mask */ +constexpr unsigned long CAN_IMASK1_15 = (1U << 15); /* Bit 15: Buffer MB15 Mask */ +constexpr unsigned long CAN_IMASK1_16 = (1U << 16); /* Bit 16: Buffer MB16 Mask */ +constexpr unsigned long CAN_IMASK1_17 = (1U << 17); /* Bit 17: Buffer MB17 Mask */ +constexpr unsigned long CAN_IMASK1_18 = (1U << 18); /* Bit 18: Buffer MB18 Mask */ +constexpr unsigned long CAN_IMASK1_19 = (1U << 19); /* Bit 19: Buffer MB19 Mask */ +constexpr unsigned long CAN_IMASK1_20 = (1U << 20); /* Bit 20: Buffer MB20 Mask */ +constexpr unsigned long CAN_IMASK1_21 = (1U << 21); /* Bit 21: Buffer MB21 Mask */ +constexpr unsigned long CAN_IMASK1_22 = (1U << 22); /* Bit 22: Buffer MB22 Mask */ +constexpr unsigned long CAN_IMASK1_23 = (1U << 23); /* Bit 23: Buffer MB23 Mask */ +constexpr unsigned long CAN_IMASK1_24 = (1U << 24); /* Bit 24: Buffer MB24 Mask */ +constexpr unsigned long CAN_IMASK1_25 = (1U << 25); /* Bit 25: Buffer MB25 Mask */ +constexpr unsigned long CAN_IMASK1_26 = (1U << 26); /* Bit 26: Buffer MB26 Mask */ +constexpr unsigned long CAN_IMASK1_27 = (1U << 27); /* Bit 27: Buffer MB27 Mask */ +constexpr unsigned long CAN_IMASK1_28 = (1U << 28); /* Bit 28: Buffer MB28 Mask */ +constexpr unsigned long CAN_IMASK1_29 = (1U << 29); /* Bit 29: Buffer MB29 Mask */ +constexpr unsigned long CAN_IMASK1_30 = (1U << 30); /* Bit 30: Buffer MB30 Mask */ +constexpr unsigned long CAN_IMASK1_31 = (1U << 31); /* Bit 31: Buffer MB31 Mask */ /* Interrupt Flags 2 Register */ -constexpr unsigned long CAN_IFLAG2_0 = (1U << 0); /* Bit 0: Buffer MB0 Interrupt */ -constexpr unsigned long CAN_IFLAG2_1 = (1U << 1); /* Bit 1: Buffer MB1 Interrupt */ -constexpr unsigned long CAN_IFLAG2_2 = (1U << 2); /* Bit 2: Buffer MB2 Interrupt */ -constexpr unsigned long CAN_IFLAG2_3 = (1U << 3); /* Bit 3: Buffer MB3 Interrupt */ -constexpr unsigned long CAN_IFLAG2_4 = (1U << 4); /* Bit 4: Buffer MB4 Interrupt */ -constexpr unsigned long CAN_IFLAG2_5 = (1U << 5); /* Bit 5: Buffer MB5 Interrupt */ -constexpr unsigned long CAN_IFLAG2_6 = (1U << 6); /* Bit 6: Buffer MB6 Interrupt */ -constexpr unsigned long CAN_IFLAG2_7 = (1U << 7); /* Bit 7: Buffer MB7 Interrupt */ -constexpr unsigned long CAN_IFLAG2_8 = (1U << 8); /* Bit 8: Buffer MB8 Interrupt */ -constexpr unsigned long CAN_IFLAG2_9 = (1U << 9); /* Bit 9: Buffer MB9 Interrupt */ -constexpr unsigned long CAN_IFLAG2_10 = (1U << 10); /* Bit 10: Buffer MB10 Interrupt */ -constexpr unsigned long CAN_IFLAG2_11 = (1U << 11); /* Bit 11: Buffer MB11 Interrupt */ -constexpr unsigned long CAN_IFLAG2_12 = (1U << 12); /* Bit 12: Buffer MB12 Interrupt */ -constexpr unsigned long CAN_IFLAG2_13 = (1U << 13); /* Bit 13: Buffer MB13 Interrupt */ -constexpr unsigned long CAN_IFLAG2_14 = (1U << 14); /* Bit 14: Buffer MB14 Interrupt */ -constexpr unsigned long CAN_IFLAG2_15 = (1U << 15); /* Bit 15: Buffer MB15 Interrupt */ -constexpr unsigned long CAN_IFLAG2_16 = (1U << 16); /* Bit 16: Buffer MB16 Interrupt */ -constexpr unsigned long CAN_IFLAG2_17 = (1U << 17); /* Bit 17: Buffer MB17 Interrupt */ -constexpr unsigned long CAN_IFLAG2_18 = (1U << 18); /* Bit 18: Buffer MB18 Interrupt */ -constexpr unsigned long CAN_IFLAG2_19 = (1U << 19); /* Bit 19: Buffer MB19 Interrupt */ -constexpr unsigned long CAN_IFLAG2_20 = (1U << 20); /* Bit 20: Buffer MB20 Interrupt */ -constexpr unsigned long CAN_IFLAG2_21 = (1U << 21); /* Bit 21: Buffer MB21 Interrupt */ -constexpr unsigned long CAN_IFLAG2_22 = (1U << 22); /* Bit 22: Buffer MB22 Interrupt */ -constexpr unsigned long CAN_IFLAG2_23 = (1U << 23); /* Bit 23: Buffer MB23 Interrupt */ -constexpr unsigned long CAN_IFLAG2_24 = (1U << 24); /* Bit 24: Buffer MB24 Interrupt */ -constexpr unsigned long CAN_IFLAG2_25 = (1U << 25); /* Bit 25: Buffer MB25 Interrupt */ -constexpr unsigned long CAN_IFLAG2_26 = (1U << 26); /* Bit 26: Buffer MB26 Interrupt */ -constexpr unsigned long CAN_IFLAG2_27 = (1U << 27); /* Bit 27: Buffer MB27 Interrupt */ -constexpr unsigned long CAN_IFLAG2_28 = (1U << 28); /* Bit 28: Buffer MB28 Interrupt */ -constexpr unsigned long CAN_IFLAG2_29 = (1U << 29); /* Bit 29: Buffer MB29 Interrupt */ -constexpr unsigned long CAN_IFLAG2_30 = (1U << 30); /* Bit 30: Buffer MB30 Interrupt */ -constexpr unsigned long CAN_IFLAG2_31 = (1U << 31); /* Bit 31: Buffer MB31 Interrupt */ +constexpr unsigned long CAN_IFLAG2_0 = (1U << 0); /* Bit 0: Buffer MB0 Interrupt */ +constexpr unsigned long CAN_IFLAG2_1 = (1U << 1); /* Bit 1: Buffer MB1 Interrupt */ +constexpr unsigned long CAN_IFLAG2_2 = (1U << 2); /* Bit 2: Buffer MB2 Interrupt */ +constexpr unsigned long CAN_IFLAG2_3 = (1U << 3); /* Bit 3: Buffer MB3 Interrupt */ +constexpr unsigned long CAN_IFLAG2_4 = (1U << 4); /* Bit 4: Buffer MB4 Interrupt */ +constexpr unsigned long CAN_IFLAG2_5 = (1U << 5); /* Bit 5: Buffer MB5 Interrupt */ +constexpr unsigned long CAN_IFLAG2_6 = (1U << 6); /* Bit 6: Buffer MB6 Interrupt */ +constexpr unsigned long CAN_IFLAG2_7 = (1U << 7); /* Bit 7: Buffer MB7 Interrupt */ +constexpr unsigned long CAN_IFLAG2_8 = (1U << 8); /* Bit 8: Buffer MB8 Interrupt */ +constexpr unsigned long CAN_IFLAG2_9 = (1U << 9); /* Bit 9: Buffer MB9 Interrupt */ +constexpr unsigned long CAN_IFLAG2_10 = (1U << 10); /* Bit 10: Buffer MB10 Interrupt */ +constexpr unsigned long CAN_IFLAG2_11 = (1U << 11); /* Bit 11: Buffer MB11 Interrupt */ +constexpr unsigned long CAN_IFLAG2_12 = (1U << 12); /* Bit 12: Buffer MB12 Interrupt */ +constexpr unsigned long CAN_IFLAG2_13 = (1U << 13); /* Bit 13: Buffer MB13 Interrupt */ +constexpr unsigned long CAN_IFLAG2_14 = (1U << 14); /* Bit 14: Buffer MB14 Interrupt */ +constexpr unsigned long CAN_IFLAG2_15 = (1U << 15); /* Bit 15: Buffer MB15 Interrupt */ +constexpr unsigned long CAN_IFLAG2_16 = (1U << 16); /* Bit 16: Buffer MB16 Interrupt */ +constexpr unsigned long CAN_IFLAG2_17 = (1U << 17); /* Bit 17: Buffer MB17 Interrupt */ +constexpr unsigned long CAN_IFLAG2_18 = (1U << 18); /* Bit 18: Buffer MB18 Interrupt */ +constexpr unsigned long CAN_IFLAG2_19 = (1U << 19); /* Bit 19: Buffer MB19 Interrupt */ +constexpr unsigned long CAN_IFLAG2_20 = (1U << 20); /* Bit 20: Buffer MB20 Interrupt */ +constexpr unsigned long CAN_IFLAG2_21 = (1U << 21); /* Bit 21: Buffer MB21 Interrupt */ +constexpr unsigned long CAN_IFLAG2_22 = (1U << 22); /* Bit 22: Buffer MB22 Interrupt */ +constexpr unsigned long CAN_IFLAG2_23 = (1U << 23); /* Bit 23: Buffer MB23 Interrupt */ +constexpr unsigned long CAN_IFLAG2_24 = (1U << 24); /* Bit 24: Buffer MB24 Interrupt */ +constexpr unsigned long CAN_IFLAG2_25 = (1U << 25); /* Bit 25: Buffer MB25 Interrupt */ +constexpr unsigned long CAN_IFLAG2_26 = (1U << 26); /* Bit 26: Buffer MB26 Interrupt */ +constexpr unsigned long CAN_IFLAG2_27 = (1U << 27); /* Bit 27: Buffer MB27 Interrupt */ +constexpr unsigned long CAN_IFLAG2_28 = (1U << 28); /* Bit 28: Buffer MB28 Interrupt */ +constexpr unsigned long CAN_IFLAG2_29 = (1U << 29); /* Bit 29: Buffer MB29 Interrupt */ +constexpr unsigned long CAN_IFLAG2_30 = (1U << 30); /* Bit 30: Buffer MB30 Interrupt */ +constexpr unsigned long CAN_IFLAG2_31 = (1U << 31); /* Bit 31: Buffer MB31 Interrupt */ /* Interrupt Flags 1 Register */ -constexpr unsigned long CAN_IFLAG1_0 = (1U << 0); /* Bit 0: Buffer MB0 Interrupt */ -constexpr unsigned long CAN_IFLAG1_1 = (1U << 1); /* Bit 1: Buffer MB1 Interrupt */ -constexpr unsigned long CAN_IFLAG1_2 = (1U << 2); /* Bit 2: Buffer MB2 Interrupt */ -constexpr unsigned long CAN_IFLAG1_3 = (1U << 3); /* Bit 3: Buffer MB3 Interrupt */ -constexpr unsigned long CAN_IFLAG1_4 = (1U << 4); /* Bit 4: Buffer MB4 Interrupt */ -constexpr unsigned long CAN_IFLAG1_5 = (1U << 5); /* Bit 5: Buffer MB5 Interrupt */ +constexpr unsigned long CAN_IFLAG1_0 = (1U << 0); /* Bit 0: Buffer MB0 Interrupt */ +constexpr unsigned long CAN_IFLAG1_1 = (1U << 1); /* Bit 1: Buffer MB1 Interrupt */ +constexpr unsigned long CAN_IFLAG1_2 = (1U << 2); /* Bit 2: Buffer MB2 Interrupt */ +constexpr unsigned long CAN_IFLAG1_3 = (1U << 3); /* Bit 3: Buffer MB3 Interrupt */ +constexpr unsigned long CAN_IFLAG1_4 = (1U << 4); /* Bit 4: Buffer MB4 Interrupt */ +constexpr unsigned long CAN_IFLAG1_5 = (1U << 5); /* Bit 5: Buffer MB5 Interrupt */ constexpr unsigned long CAN_FIFO_NE = CAN_IFLAG1_5; /* Bit 5: Fifo almost Not empty Interrupt */ -constexpr unsigned long CAN_IFLAG1_6 = (1U << 6); /* Bit 6: Buffer MB6 Interrupt */ +constexpr unsigned long CAN_IFLAG1_6 = (1U << 6); /* Bit 6: Buffer MB6 Interrupt */ constexpr unsigned long CAN_FIFO_WARN = CAN_IFLAG1_6; /* Bit 6: Fifo almost full Interrupt */ -constexpr unsigned long CAN_IFLAG1_7 = (1U << 7); /* Bit 7: Buffer MB7 Interrupt */ +constexpr unsigned long CAN_IFLAG1_7 = (1U << 7); /* Bit 7: Buffer MB7 Interrupt */ constexpr unsigned long CAN_FIFO_OV = CAN_IFLAG1_7; /* Bit 7: Fifo Overflowed Interrupt */ -constexpr unsigned long CAN_IFLAG1_8 = (1U << 8); /* Bit 8: Buffer MB8 Interrupt */ -constexpr unsigned long CAN_IFLAG1_9 = (1U << 9); /* Bit 9: Buffer MB9 Interrupt */ -constexpr unsigned long CAN_IFLAG1_10 = (1U << 10); /* Bit 10: Buffer MB10 Interrupt */ -constexpr unsigned long CAN_IFLAG1_11 = (1U << 11); /* Bit 11: Buffer MB11 Interrupt */ -constexpr unsigned long CAN_IFLAG1_12 = (1U << 12); /* Bit 12: Buffer MB12 Interrupt */ -constexpr unsigned long CAN_IFLAG1_13 = (1U << 13); /* Bit 13: Buffer MB13 Interrupt */ -constexpr unsigned long CAN_IFLAG1_14 = (1U << 14); /* Bit 14: Buffer MB14 Interrupt */ -constexpr unsigned long CAN_IFLAG1_15 = (1U << 15); /* Bit 15: Buffer MB15 Interrupt */ -constexpr unsigned long CAN_IFLAG1_16 = (1U << 16); /* Bit 16: Buffer MB16 Interrupt */ -constexpr unsigned long CAN_IFLAG1_17 = (1U << 17); /* Bit 17: Buffer MB17 Interrupt */ -constexpr unsigned long CAN_IFLAG1_18 = (1U << 18); /* Bit 18: Buffer MB18 Interrupt */ -constexpr unsigned long CAN_IFLAG1_19 = (1U << 19); /* Bit 19: Buffer MB19 Interrupt */ -constexpr unsigned long CAN_IFLAG1_20 = (1U << 20); /* Bit 20: Buffer MB20 Interrupt */ -constexpr unsigned long CAN_IFLAG1_21 = (1U << 21); /* Bit 21: Buffer MB21 Interrupt */ -constexpr unsigned long CAN_IFLAG1_22 = (1U << 22); /* Bit 22: Buffer MB22 Interrupt */ -constexpr unsigned long CAN_IFLAG1_23 = (1U << 23); /* Bit 23: Buffer MB23 Interrupt */ -constexpr unsigned long CAN_IFLAG1_24 = (1U << 24); /* Bit 24: Buffer MB24 Interrupt */ -constexpr unsigned long CAN_IFLAG1_25 = (1U << 25); /* Bit 25: Buffer MB25 Interrupt */ -constexpr unsigned long CAN_IFLAG1_26 = (1U << 26); /* Bit 26: Buffer MB26 Interrupt */ -constexpr unsigned long CAN_IFLAG1_27 = (1U << 27); /* Bit 27: Buffer MB27 Interrupt */ -constexpr unsigned long CAN_IFLAG1_28 = (1U << 28); /* Bit 28: Buffer MB28 Interrupt */ -constexpr unsigned long CAN_IFLAG1_29 = (1U << 29); /* Bit 29: Buffer MB29 Interrupt */ -constexpr unsigned long CAN_IFLAG1_30 = (1U << 30); /* Bit 30: Buffer MB30 Interrupt */ -constexpr unsigned long CAN_IFLAG1_31 = (1U << 31); /* Bit 31: Buffer MB31 Interrupt */ +constexpr unsigned long CAN_IFLAG1_8 = (1U << 8); /* Bit 8: Buffer MB8 Interrupt */ +constexpr unsigned long CAN_IFLAG1_9 = (1U << 9); /* Bit 9: Buffer MB9 Interrupt */ +constexpr unsigned long CAN_IFLAG1_10 = (1U << 10); /* Bit 10: Buffer MB10 Interrupt */ +constexpr unsigned long CAN_IFLAG1_11 = (1U << 11); /* Bit 11: Buffer MB11 Interrupt */ +constexpr unsigned long CAN_IFLAG1_12 = (1U << 12); /* Bit 12: Buffer MB12 Interrupt */ +constexpr unsigned long CAN_IFLAG1_13 = (1U << 13); /* Bit 13: Buffer MB13 Interrupt */ +constexpr unsigned long CAN_IFLAG1_14 = (1U << 14); /* Bit 14: Buffer MB14 Interrupt */ +constexpr unsigned long CAN_IFLAG1_15 = (1U << 15); /* Bit 15: Buffer MB15 Interrupt */ +constexpr unsigned long CAN_IFLAG1_16 = (1U << 16); /* Bit 16: Buffer MB16 Interrupt */ +constexpr unsigned long CAN_IFLAG1_17 = (1U << 17); /* Bit 17: Buffer MB17 Interrupt */ +constexpr unsigned long CAN_IFLAG1_18 = (1U << 18); /* Bit 18: Buffer MB18 Interrupt */ +constexpr unsigned long CAN_IFLAG1_19 = (1U << 19); /* Bit 19: Buffer MB19 Interrupt */ +constexpr unsigned long CAN_IFLAG1_20 = (1U << 20); /* Bit 20: Buffer MB20 Interrupt */ +constexpr unsigned long CAN_IFLAG1_21 = (1U << 21); /* Bit 21: Buffer MB21 Interrupt */ +constexpr unsigned long CAN_IFLAG1_22 = (1U << 22); /* Bit 22: Buffer MB22 Interrupt */ +constexpr unsigned long CAN_IFLAG1_23 = (1U << 23); /* Bit 23: Buffer MB23 Interrupt */ +constexpr unsigned long CAN_IFLAG1_24 = (1U << 24); /* Bit 24: Buffer MB24 Interrupt */ +constexpr unsigned long CAN_IFLAG1_25 = (1U << 25); /* Bit 25: Buffer MB25 Interrupt */ +constexpr unsigned long CAN_IFLAG1_26 = (1U << 26); /* Bit 26: Buffer MB26 Interrupt */ +constexpr unsigned long CAN_IFLAG1_27 = (1U << 27); /* Bit 27: Buffer MB27 Interrupt */ +constexpr unsigned long CAN_IFLAG1_28 = (1U << 28); /* Bit 28: Buffer MB28 Interrupt */ +constexpr unsigned long CAN_IFLAG1_29 = (1U << 29); /* Bit 29: Buffer MB29 Interrupt */ +constexpr unsigned long CAN_IFLAG1_30 = (1U << 30); /* Bit 30: Buffer MB30 Interrupt */ +constexpr unsigned long CAN_IFLAG1_31 = (1U << 31); /* Bit 31: Buffer MB31 Interrupt */ /* Control 2 Register */ /* Bits 0-15: Reserved */ -constexpr unsigned long CTRL2_EACEN = (1U << 16); /* Bit 16: Entire Frame Arbitration Field Comparison +constexpr unsigned long CTRL2_EACEN = (1U << 16); /* Bit 16: Entire Frame Arbitration Field Comparison Enable (Rx) */ -constexpr unsigned long CTRL2_RRS = (1U << 17); /* Bit 17: Remote Request Storing */ -constexpr unsigned long CTRL2_MRP = (1U << 18); /* Bit 18: Mailboxes Reception Priority */ -constexpr unsigned long CTRL2_TASD_SHIFT = (19); /* Bits 19-23: Tx Arbitration Start Delay */ -constexpr unsigned long CTRL2_TASD_MASK = (31U << CTRL2_TASD_SHIFT); -constexpr unsigned long CTRL2_RFFN_SHIFT = (24); /* Bits 24-27: Number of Rx FIFO Filters */ +constexpr unsigned long CTRL2_RRS = (1U << 17); /* Bit 17: Remote Request Storing */ +constexpr unsigned long CTRL2_MRP = (1U << 18); /* Bit 18: Mailboxes Reception Priority */ +constexpr unsigned long CTRL2_TASD_SHIFT = (19); /* Bits 19-23: Tx Arbitration Start Delay */ +constexpr unsigned long CTRL2_TASD_MASK = (31U << CTRL2_TASD_SHIFT); +constexpr unsigned long CTRL2_RFFN_SHIFT = (24); /* Bits 24-27: Number of Rx FIFO Filters */ constexpr unsigned long CTRL2_RFFN_MASK(15U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_8MB(0U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_16MB(1U << CTRL2_RFFN_SHIFT); @@ -592,67 +572,67 @@ constexpr unsigned long CTRL2_RFFN_104MB(12U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_112MB(13U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_120MB(14U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_128MB(15U << CTRL2_RFFN_SHIFT); -constexpr unsigned long CTRL2_WRMFRZ = (1U << 28U); /* Bit 28: Write-Access to Memory in Freeze mode */ - /* Bits 29-31: Reserved */ +constexpr unsigned long CTRL2_WRMFRZ = (1U << 28U); /* Bit 28: Write-Access to Memory in Freeze mode */ +/* Bits 29-31: Reserved */ /* Error and Status 2 Register */ /* Bits 0-12: Reserved */ -constexpr unsigned long ESR2_IMB = (1U << 13); /* Bit 13: Inactive Mailbox */ -constexpr unsigned long ESR2_VPS = (1U << 14); /* Bit 14: Valid Priority Status */ - /* Bit 15: Reserved */ -constexpr unsigned long ESR2_LPTM_SHIFT = (16); /* Bits 16-22: Lowest Priority Tx Mailbox */ -constexpr unsigned long ESR2_LPTM_MASK = (0x7fU << ESR2_LPTM_SHIFT); +constexpr unsigned long ESR2_IMB = (1U << 13); /* Bit 13: Inactive Mailbox */ +constexpr unsigned long ESR2_VPS = (1U << 14); /* Bit 14: Valid Priority Status */ +/* Bit 15: Reserved */ +constexpr unsigned long ESR2_LPTM_SHIFT = (16); /* Bits 16-22: Lowest Priority Tx Mailbox */ +constexpr unsigned long ESR2_LPTM_MASK = (0x7fU << ESR2_LPTM_SHIFT); /* Bits 23-31: Reserved */ /* CRC Register */ -constexpr unsigned long CRCR_TXCRC_SHIFT = (0U); /* Bits 0-14: CRC Transmitted */ -constexpr unsigned long CRCR_TXCRC_MASK = (0x7fffU << CRCR_TXCRC_SHIFT); /* Rx FIFO Global Mask Register (32 Rx +constexpr unsigned long CRCR_TXCRC_SHIFT = (0U); /* Bits 0-14: CRC Transmitted */ +constexpr unsigned long CRCR_TXCRC_MASK = (0x7fffU << CRCR_TXCRC_SHIFT); /* Rx FIFO Global Mask Register (32 Rx FIFO Global Mask Bits) */ /* Bits 23-31: Reserved */ -constexpr unsigned long CRCR_MBCRC_SHIFT = (16); /* Bits 16-22: CRC Mailbox */ -constexpr unsigned long CRCR_MBCRC_MASK = (0x7fU << CRCR_MBCRC_SHIFT); +constexpr unsigned long CRCR_MBCRC_SHIFT = (16); /* Bits 16-22: CRC Mailbox */ +constexpr unsigned long CRCR_MBCRC_MASK = (0x7fU << CRCR_MBCRC_SHIFT); /* Bit 15: Reserved */ /* Rx FIFO Information Register */ /* Bits 9-31: Reserved */ -constexpr unsigned long RXFIR_IDHIT_SHIFT = (0); /* Bits 0-8: Identifier Acceptance Filter Hit Indicator +constexpr unsigned long RXFIR_IDHIT_SHIFT = (0); /* Bits 0-8: Identifier Acceptance Filter Hit Indicator */ -constexpr unsigned long RXFIR_IDHIT_MASK = (0x1ffU << RXFIR_IDHIT_SHIFT); +constexpr unsigned long RXFIR_IDHIT_MASK = (0x1ffU << RXFIR_IDHIT_SHIFT); /* Rn Individual Mask Registers */ -constexpr unsigned long RXIMR0 = (1U << 0); /* Bit 0: Individual Mask Bits */ -constexpr unsigned long RXIMR1 = (1U << 1); /* Bit 1: Individual Mask Bits */ -constexpr unsigned long RXIMR2 = (1U << 2); /* Bit 2: Individual Mask Bits */ -constexpr unsigned long RXIMR3 = (1U << 3); /* Bit 3: Individual Mask Bits */ -constexpr unsigned long RXIMR4 = (1U << 4); /* Bit 4: Individual Mask Bits */ -constexpr unsigned long RXIMR5 = (1U << 5); /* Bit 5: Individual Mask Bits */ -constexpr unsigned long RXIMR6 = (1U << 6); /* Bit 6: Individual Mask Bits */ -constexpr unsigned long RXIMR7 = (1U << 7); /* Bit 7: Individual Mask Bits */ -constexpr unsigned long RXIMR8 = (1U << 8); /* Bit 8: Individual Mask Bits */ -constexpr unsigned long RXIMR9 = (1U << 9); /* Bit 9: Individual Mask Bits */ -constexpr unsigned long RXIMR10 = (1U << 10); /* Bit 10: Individual Mask Bits */ -constexpr unsigned long RXIMR11 = (1U << 11); /* Bit 11: Individual Mask Bits */ -constexpr unsigned long RXIMR12 = (1U << 12); /* Bit 12: Individual Mask Bits */ -constexpr unsigned long RXIMR13 = (1U << 13); /* Bit 13: Individual Mask Bits */ -constexpr unsigned long RXIMR14 = (1U << 14); /* Bit 14: Individual Mask Bits */ -constexpr unsigned long RXIMR15 = (1U << 15); /* Bit 15: Individual Mask Bits */ -constexpr unsigned long RXIMR16 = (1U << 16); /* Bit 16: Individual Mask Bits */ -constexpr unsigned long RXIMR17 = (1U << 17); /* Bit 17: Individual Mask Bits */ -constexpr unsigned long RXIMR18 = (1U << 18); /* Bit 18: Individual Mask Bits */ -constexpr unsigned long RXIMR19 = (1U << 19); /* Bit 19: Individual Mask Bits */ -constexpr unsigned long RXIMR20 = (1U << 20); /* Bit 20: Individual Mask Bits */ -constexpr unsigned long RXIMR21 = (1U << 21); /* Bit 21: Individual Mask Bits */ -constexpr unsigned long RXIMR22 = (1U << 22); /* Bit 22: Individual Mask Bits */ -constexpr unsigned long RXIMR23 = (1U << 23); /* Bit 23: Individual Mask Bits */ -constexpr unsigned long RXIMR24 = (1U << 24); /* Bit 24: Individual Mask Bits */ -constexpr unsigned long RXIMR25 = (1U << 25); /* Bit 25: Individual Mask Bits */ -constexpr unsigned long RXIMR26 = (1U << 26); /* Bit 26: Individual Mask Bits */ -constexpr unsigned long RXIMR27 = (1U << 27); /* Bit 27: Individual Mask Bits */ -constexpr unsigned long RXIMR28 = (1U << 28); /* Bit 28: Individual Mask Bits */ -constexpr unsigned long RXIMR29 = (1U << 29); /* Bit 29: Individual Mask Bits */ -constexpr unsigned long RXIMR30 = (1U << 30); /* Bit 30: Individual Mask Bits */ -constexpr unsigned long RXIMR31 = (1U << 31); /* Bit 31: Individual Mask Bits */ +constexpr unsigned long RXIMR0 = (1U << 0); /* Bit 0: Individual Mask Bits */ +constexpr unsigned long RXIMR1 = (1U << 1); /* Bit 1: Individual Mask Bits */ +constexpr unsigned long RXIMR2 = (1U << 2); /* Bit 2: Individual Mask Bits */ +constexpr unsigned long RXIMR3 = (1U << 3); /* Bit 3: Individual Mask Bits */ +constexpr unsigned long RXIMR4 = (1U << 4); /* Bit 4: Individual Mask Bits */ +constexpr unsigned long RXIMR5 = (1U << 5); /* Bit 5: Individual Mask Bits */ +constexpr unsigned long RXIMR6 = (1U << 6); /* Bit 6: Individual Mask Bits */ +constexpr unsigned long RXIMR7 = (1U << 7); /* Bit 7: Individual Mask Bits */ +constexpr unsigned long RXIMR8 = (1U << 8); /* Bit 8: Individual Mask Bits */ +constexpr unsigned long RXIMR9 = (1U << 9); /* Bit 9: Individual Mask Bits */ +constexpr unsigned long RXIMR10 = (1U << 10); /* Bit 10: Individual Mask Bits */ +constexpr unsigned long RXIMR11 = (1U << 11); /* Bit 11: Individual Mask Bits */ +constexpr unsigned long RXIMR12 = (1U << 12); /* Bit 12: Individual Mask Bits */ +constexpr unsigned long RXIMR13 = (1U << 13); /* Bit 13: Individual Mask Bits */ +constexpr unsigned long RXIMR14 = (1U << 14); /* Bit 14: Individual Mask Bits */ +constexpr unsigned long RXIMR15 = (1U << 15); /* Bit 15: Individual Mask Bits */ +constexpr unsigned long RXIMR16 = (1U << 16); /* Bit 16: Individual Mask Bits */ +constexpr unsigned long RXIMR17 = (1U << 17); /* Bit 17: Individual Mask Bits */ +constexpr unsigned long RXIMR18 = (1U << 18); /* Bit 18: Individual Mask Bits */ +constexpr unsigned long RXIMR19 = (1U << 19); /* Bit 19: Individual Mask Bits */ +constexpr unsigned long RXIMR20 = (1U << 20); /* Bit 20: Individual Mask Bits */ +constexpr unsigned long RXIMR21 = (1U << 21); /* Bit 21: Individual Mask Bits */ +constexpr unsigned long RXIMR22 = (1U << 22); /* Bit 22: Individual Mask Bits */ +constexpr unsigned long RXIMR23 = (1U << 23); /* Bit 23: Individual Mask Bits */ +constexpr unsigned long RXIMR24 = (1U << 24); /* Bit 24: Individual Mask Bits */ +constexpr unsigned long RXIMR25 = (1U << 25); /* Bit 25: Individual Mask Bits */ +constexpr unsigned long RXIMR26 = (1U << 26); /* Bit 26: Individual Mask Bits */ +constexpr unsigned long RXIMR27 = (1U << 27); /* Bit 27: Individual Mask Bits */ +constexpr unsigned long RXIMR28 = (1U << 28); /* Bit 28: Individual Mask Bits */ +constexpr unsigned long RXIMR29 = (1U << 29); /* Bit 29: Individual Mask Bits */ +constexpr unsigned long RXIMR30 = (1U << 30); /* Bit 30: Individual Mask Bits */ +constexpr unsigned long RXIMR31 = (1U << 31); /* Bit 31: Individual Mask Bits */ } } diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp index 9515f93cca..6da90e8b91 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp @@ -28,68 +28,68 @@ class CanDriver; */ class BusEvent : uavcan::Noncopyable { - using SignalCallbackHandler = void(*)(); + using SignalCallbackHandler = void(*)(); - SignalCallbackHandler signal_cb_{nullptr}; - sem_t sem_; + SignalCallbackHandler signal_cb_{nullptr}; + sem_t sem_; public: - BusEvent(CanDriver& can_driver); - ~BusEvent(); + BusEvent(CanDriver &can_driver); + ~BusEvent(); - void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } + void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } - bool wait(uavcan::MonotonicDuration duration); + bool wait(uavcan::MonotonicDuration duration); - void signalFromInterrupt(); + void signalFromInterrupt(); }; class Mutex { - pthread_mutex_t mutex_; + pthread_mutex_t mutex_; public: - Mutex() - { - init(); - } + Mutex() + { + init(); + } - int init() - { - return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); - } + int init() + { + return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); + } - int deinit() - { - return pthread_mutex_destroy(&mutex_); - } + int deinit() + { + return pthread_mutex_destroy(&mutex_); + } - void lock() - { - (void)pthread_mutex_lock(&mutex_); - } + void lock() + { + (void)pthread_mutex_lock(&mutex_); + } - void unlock() - { - (void)pthread_mutex_unlock(&mutex_); - } + void unlock() + { + (void)pthread_mutex_unlock(&mutex_); + } }; class MutexLocker { - Mutex& mutex_; + Mutex &mutex_; public: - MutexLocker(Mutex& mutex) - : mutex_(mutex) - { - mutex_.lock(); - } - ~MutexLocker() - { - mutex_.unlock(); - } + MutexLocker(Mutex &mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } }; } diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp index 64a98032a4..3e5bf18a44 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp @@ -48,19 +48,18 @@ namespace uavcan_kinetis { #if UAVCAN_KINETIS_NUTTX -struct CriticalSectionLocker -{ - const irqstate_t flags_; +struct CriticalSectionLocker { + const irqstate_t flags_; - CriticalSectionLocker() - : flags_(enter_critical_section()) - { - } + CriticalSectionLocker() + : flags_(enter_critical_section()) + { + } - ~CriticalSectionLocker() - { - leave_critical_section(flags_); - } + ~CriticalSectionLocker() + { + leave_critical_section(flags_); + } }; #endif diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp index a3abdc5535..fd57f8b39f 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp @@ -66,167 +66,166 @@ uavcan::uint64_t time_utc = 0; void init() { - CriticalSectionLocker lock; - if (initialized) - { - return; - } - initialized = true; + CriticalSectionLocker lock; - // Attach IRQ - irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL); + if (initialized) { + return; + } - // Power-on Clock - modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT); + initialized = true; - // Enable module - putreg32(0, KINETIS_PIT_MCR); + // Attach IRQ + irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL); - // Start the timer + // Power-on Clock + modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT); - putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET)); - putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start + // Enable module + putreg32(0, KINETIS_PIT_MCR); - // Prioritize and Enable IRQ + // Start the timer + + putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET)); + putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start + + // Prioritize and Enable IRQ #if 0 - // This has to be off or uses the default priority - // Without the ability to point the vector - // Directly to this ISR this will reenter the - // exception_common and cause the interrupt - // Stack pointer to be reset - up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); + // This has to be off or uses the default priority + // Without the ability to point the vector + // Directly to this ISR this will reenter the + // exception_common and cause the interrupt + // Stack pointer to be reset + up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); #endif - up_enable_irq(TIMX_IRQn); + up_enable_irq(TIMX_IRQn); } void setUtc(uavcan::UtcTime time) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); - { - CriticalSectionLocker locker; - time_utc = time.toUSec(); - } + { + CriticalSectionLocker locker; + time_utc = time.toUSec(); + } - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; } static uavcan::uint64_t sampleUtcFromCriticalSection() { - UAVCAN_ASSERT(initialized); - UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE); + UAVCAN_ASSERT(initialized); + UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE); - volatile uavcan::uint64_t time = time_utc; - volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); + volatile uavcan::uint64_t time = time_utc; + volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); - if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) - { - cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); - const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + - (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; - time = uavcan::uint64_t(uavcan::int64_t(time) + add); - } - return time + (cnt / CountsPerUs); + if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) { + cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); + const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + + (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; + time = uavcan::uint64_t(uavcan::int64_t(time) + add); + } + + return time + (cnt / CountsPerUs); } uavcan::uint64_t getUtcUSecFromCanInterrupt() { - return utc_set ? sampleUtcFromCriticalSection() : 0; + return utc_set ? sampleUtcFromCriticalSection() : 0; } uavcan::MonotonicTime getMonotonic() { - uavcan::uint64_t usec = 0; - // Scope Critical section - { - CriticalSectionLocker locker; + uavcan::uint64_t usec = 0; + // Scope Critical section + { + CriticalSectionLocker locker; - volatile uavcan::uint64_t time = time_mono; - volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); + volatile uavcan::uint64_t time = time_mono; + volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); - if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) - { - cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); - time += USecPerOverflow; - } - usec = time + (cnt / CountsPerUs); + if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) { + cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); + time += USecPerOverflow; + } - } // End Scope Critical section + usec = time + (cnt / CountsPerUs); - return uavcan::MonotonicTime::fromUSec(usec); + } // End Scope Critical section + + return uavcan::MonotonicTime::fromUSec(usec); } uavcan::UtcTime getUtc() { - if (utc_set) - { - uavcan::uint64_t usec = 0; - { - CriticalSectionLocker locker; - usec = sampleUtcFromCriticalSection(); - } - return uavcan::UtcTime::fromUSec(usec); - } - return uavcan::UtcTime(); + if (utc_set) { + uavcan::uint64_t usec = 0; + { + CriticalSectionLocker locker; + usec = sampleUtcFromCriticalSection(); + } + return uavcan::UtcTime::fromUSec(usec); + } + + return uavcan::UtcTime(); } static float lowpass(float xold, float xnew, float corner, float dt) { - const float tau = 1.F / corner; - return (dt * xnew + tau * xold) / (dt + tau); + const float tau = 1.F / corner; + return (dt * xnew + tau * xold) / (dt + tau); } static void updateRatePID(uavcan::UtcDuration adjustment) { - const uavcan::MonotonicTime ts = getMonotonic(); - const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F; - prev_utc_adj_at = ts; - const float adj_usec = float(adjustment.toUSec()); + const uavcan::MonotonicTime ts = getMonotonic(); + const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F; + prev_utc_adj_at = ts; + const float adj_usec = float(adjustment.toUSec()); - /* - * Target relative rate in PPM - * Positive to go faster - */ - const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p; + /* + * Target relative rate in PPM + * Positive to go faster + */ + const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p; - /* - * Current relative rate in PPM - * Positive if the local clock is faster - */ - const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM - utc_prev_adj = adj_usec; - utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt); + /* + * Current relative rate in PPM + * Positive if the local clock is faster + */ + const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM + utc_prev_adj = adj_usec; + utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt); - const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; + const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; - if (dt > 10) - { - utc_rel_rate_error_integral = 0; - } - else - { - utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; - utc_rel_rate_error_integral = - uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); - utc_rel_rate_error_integral = - uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm); - } + if (dt > 10) { + utc_rel_rate_error_integral = 0; - /* - * Rate controller - */ - float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral; - total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); - total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); + } else { + utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; + utc_rel_rate_error_integral = + uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); + utc_rel_rate_error_integral = + uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm); + } - utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F)); + /* + * Rate controller + */ + float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral; + total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); + total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); + + utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F)); // syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n", // adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm, @@ -235,96 +234,91 @@ static void updateRatePID(uavcan::UtcDuration adjustment) void adjustUtc(uavcan::UtcDuration adjustment) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); - if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) - { - const uavcan::int64_t adj_usec = adjustment.toUSec(); + if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { + const uavcan::int64_t adj_usec = adjustment.toUSec(); - { - CriticalSectionLocker locker; - if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) - { - time_utc = 1; - } - else - { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); - } - } + { + CriticalSectionLocker locker; - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; - } - else - { - updateRatePID(adjustment); + if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) { + time_utc = 1; - if (!utc_locked) - { - utc_locked = - (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && - (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); - } - } + } else { + time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); + } + } + + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; + + } else { + updateRatePID(adjustment); + + if (!utc_locked) { + utc_locked = + (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && + (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); + } + } } float getUtcRateCorrectionPPM() { - MutexLocker mlocker(mutex); - const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); - return 1e6F * rate_correction_mult; + MutexLocker mlocker(mutex); + const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); + return 1e6F * rate_correction_mult; } uavcan::uint32_t getUtcJumpCount() { - MutexLocker mlocker(mutex); - return utc_jump_cnt; + MutexLocker mlocker(mutex); + return utc_jump_cnt; } bool isUtcLocked() { - MutexLocker mlocker(mutex); - return utc_locked; + MutexLocker mlocker(mutex); + return utc_locked; } UtcSyncParams getUtcSyncParams() { - MutexLocker mlocker(mutex); - return utc_sync_params; + MutexLocker mlocker(mutex); + return utc_sync_params; } -void setUtcSyncParams(const UtcSyncParams& params) +void setUtcSyncParams(const UtcSyncParams ¶ms) { - MutexLocker mlocker(mutex); - // Add some sanity check - utc_sync_params = params; + MutexLocker mlocker(mutex); + // Add some sanity check + utc_sync_params = params; } } // namespace clock -SystemClock& SystemClock::instance() +SystemClock &SystemClock::instance() { - static union SystemClockStorage - { - uavcan::uint8_t buffer[sizeof(SystemClock)]; - long long _aligner_1; - long double _aligner_2; - } storage; + static union SystemClockStorage { + uavcan::uint8_t buffer[sizeof(SystemClock)]; + long long _aligner_1; + long double _aligner_2; + } storage; - SystemClock* const ptr = reinterpret_cast(storage.buffer); + SystemClock *const ptr = reinterpret_cast(storage.buffer); - if (!clock::initialized) - { - MutexLocker mlocker(clock::mutex); - clock::init(); - new (ptr)SystemClock(); - } - return *ptr; + if (!clock::initialized) { + MutexLocker mlocker(clock::mutex); + clock::init(); + new (ptr)SystemClock(); + } + + return *ptr; } } // namespace uavcan_kinetis @@ -337,38 +331,35 @@ SystemClock& SystemClock::instance() extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler) { - putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET)); + putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET)); - using namespace uavcan_kinetis::clock; - UAVCAN_ASSERT(initialized); + using namespace uavcan_kinetis::clock; + UAVCAN_ASSERT(initialized); - time_mono += USecPerOverflow; + time_mono += USecPerOverflow; - if (utc_set) - { - time_utc += USecPerOverflow; - utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; - if (std::abs(utc_accumulated_correction_nsec) >= 1000) - { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000); - utc_accumulated_correction_nsec %= 1000; - } + if (utc_set) { + time_utc += USecPerOverflow; + utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; - // Correction decay - 1 nsec per 65536 usec - if (utc_correction_nsec_per_overflow > 0) - { - utc_correction_nsec_per_overflow--; - } - else if (utc_correction_nsec_per_overflow < 0) - { - utc_correction_nsec_per_overflow++; - } - else - { - ; // Zero - } - } - return 0; + if (std::abs(utc_accumulated_correction_nsec) >= 1000) { + time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000); + utc_accumulated_correction_nsec %= 1000; + } + + // Correction decay - 1 nsec per 65536 usec + if (utc_correction_nsec_per_overflow > 0) { + utc_correction_nsec_per_overflow--; + + } else if (utc_correction_nsec_per_overflow < 0) { + utc_correction_nsec_per_overflow++; + + } else { + ; // Zero + } + } + + return 0; } #endif diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp index 25c0716739..727efabbeb 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp @@ -20,12 +20,11 @@ namespace uavcan_kinetis { -extern "C" -{ -static int can0_irq(const int irq, void*, void* args); - #if UAVCAN_KINETIS_NUM_IFACES > 1 -static int can1_irq(const int irq, void*, void* args); - #endif +extern "C" { + static int can0_irq(const int irq, void *, void *args); +#if UAVCAN_KINETIS_NUM_IFACES > 1 + static int can1_irq(const int irq, void *, void *args); +#endif } namespace flexcan { @@ -39,33 +38,34 @@ const uavcan::uint32_t numberFIFOFilters = flexcan::CTRL2_RFFN_16MB; // 16 R namespace { -CanIface* ifaces[UAVCAN_KINETIS_NUM_IFACES] = -{ - UAVCAN_NULLPTR +CanIface *ifaces[UAVCAN_KINETIS_NUM_IFACES] = { + UAVCAN_NULLPTR #if UAVCAN_KINETIS_NUM_IFACES > 1 - , UAVCAN_NULLPTR + , UAVCAN_NULLPTR #endif }; -inline void handleTxInterrupt(CanIface* can, uavcan::uint32_t iflags1) +inline void handleTxInterrupt(CanIface *can, uavcan::uint32_t iflags1) { - UAVCAN_ASSERT(iface_index < UAVCAN_KINETIS_NUM_IFACES); - uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); - if (utc_usec > 0) - { - utc_usec--; - } - can->handleTxInterrupt(iflags1, utc_usec); + UAVCAN_ASSERT(iface_index < UAVCAN_KINETIS_NUM_IFACES); + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + if (utc_usec > 0) { + utc_usec--; + } + + can->handleTxInterrupt(iflags1, utc_usec); } -inline void handleRxInterrupt(CanIface* can, uavcan::uint32_t iflags1) +inline void handleRxInterrupt(CanIface *can, uavcan::uint32_t iflags1) { - uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); - if (utc_usec > 0) - { - utc_usec--; - } - can->handleRxInterrupt(iflags1, utc_usec); + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + if (utc_usec > 0) { + utc_usec--; + } + + can->handleRxInterrupt(iflags1, utc_usec); } } // namespace @@ -75,944 +75,916 @@ inline void handleRxInterrupt(CanIface* can, uavcan::uint32_t iflags1) */ void CanIface::RxQueue::registerOverflow() { - if (overflow_cnt_ < 0xFFFFFFFF) - { - overflow_cnt_++; - } + if (overflow_cnt_ < 0xFFFFFFFF) { + overflow_cnt_++; + } } -void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags) +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; - } - } + 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) +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 - { - UAVCAN_ASSERT(0); - } + 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 { + UAVCAN_ASSERT(0); + } } void CanIface::RxQueue::reset() { - in_ = 0; - out_ = 0; - len_ = 0; - overflow_cnt_ = 0; + in_ = 0; + out_ = 0; + len_ = 0; + overflow_cnt_ = 0; } /* * CanIface */ -int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings) +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings &out_timings) { - if (target_bitrate < 1) - { - return -ErrInvalidBitRate; - } + if (target_bitrate < 1) { + return -ErrInvalidBitRate; + } - /* - * From FlexCAN Bit Timing Calculation by: Petr Stancik TIC - * buadrate = 1 / tNBT -The tNBT represents a period of the Nominal Bit Time (NBT). - * The NBT is separated into four non-overlaping segments, - * SYNC_SEG, PROP_SEG,PHASE_SEG1 and PHASE_SEG2. Each of - * these segments is an integer multiple of Time Quantum tQ - * tNBT= tQ+PROP_SEG* tQ + PHASE_SEG1* tQ + PHASE_SEG2* tQ = NBT * tQ - * tQ = 1/bitrate = 1/[canclk /(PRESDIV+1)]. - * NBT is 8..25 - * SYNC_SEG = 1 tQ - * PROP_SEG = 1..8 tQ - * PHASE1_SEG = 1,2..8 tQ 1..8 for 1 Sample per bit (Spb), 2..8 for 3 Spb - * PHASE2_SEG = 1..8 - * - */ - /* - * Hardware configuration - */ + /* + * From FlexCAN Bit Timing Calculation by: Petr Stancik TIC + * buadrate = 1 / tNBT -The tNBT represents a period of the Nominal Bit Time (NBT). + * The NBT is separated into four non-overlaping segments, + * SYNC_SEG, PROP_SEG,PHASE_SEG1 and PHASE_SEG2. Each of + * these segments is an integer multiple of Time Quantum tQ + * tNBT= tQ+PROP_SEG* tQ + PHASE_SEG1* tQ + PHASE_SEG2* tQ = NBT * tQ + * tQ = 1/bitrate = 1/[canclk /(PRESDIV+1)]. + * NBT is 8..25 + * SYNC_SEG = 1 tQ + * PROP_SEG = 1..8 tQ + * PHASE1_SEG = 1,2..8 tQ 1..8 for 1 Sample per bit (Spb), 2..8 for 3 Spb + * PHASE2_SEG = 1..8 + * + */ + /* + * Hardware configuration + */ - /* Maximize NBT - * NBT * Prescaler = canclk / baud rate. - * - */ + /* Maximize NBT + * NBT * Prescaler = canclk / baud rate. + * + */ - const uavcan::uint32_t nbt_prescaler = flexcan::canclk / target_bitrate; - const int max_quanta_per_bit = 17; + const uavcan::uint32_t nbt_prescaler = flexcan::canclk / target_bitrate; + const int max_quanta_per_bit = 17; - /* - * Searching for such prescaler value so that the number of quanta per bit is highest. - */ + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ - /* tNBT - tQ = PROP_SEG* tQ + PHASE_SEG1* tQ + PHASE_SEG2* tQ = NBT * tQ - tQ */ + /* tNBT - tQ = PROP_SEG* tQ + PHASE_SEG1* tQ + PHASE_SEG2* tQ = NBT * tQ - tQ */ - for (uavcan::uint32_t prescaler = 1; prescaler < 256; prescaler++) - { - if (prescaler > nbt_prescaler) - { - return -ErrInvalidBitRate; // No solution - } + for (uavcan::uint32_t prescaler = 1; prescaler < 256; prescaler++) { + if (prescaler > nbt_prescaler) { + return -ErrInvalidBitRate; // No solution + } - if ((0 == nbt_prescaler % prescaler) && (nbt_prescaler / prescaler) < max_quanta_per_bit) - { - out_timings.prescaler = prescaler; - break; - } - } - const uavcan::uint32_t NBT = nbt_prescaler / out_timings.prescaler; + if ((0 == nbt_prescaler % prescaler) && (nbt_prescaler / prescaler) < max_quanta_per_bit) { + out_timings.prescaler = prescaler; + break; + } + } - /* Choose a reasonable and some what arbitrary value for Propseg */ + const uavcan::uint32_t NBT = nbt_prescaler / out_timings.prescaler; - out_timings.propseg = 5; + /* Choose a reasonable and some what arbitrary value for Propseg */ + + out_timings.propseg = 5; - /* Ideal sampling point = 87.5% given by (SYNC_SEG + PROP_SEG + PHASE_SEG1) / (PHASE_SEG2) */ + /* Ideal sampling point = 87.5% given by (SYNC_SEG + PROP_SEG + PHASE_SEG1) / (PHASE_SEG2) */ - uavcan::uint32_t sp = (7 * NBT) / 8; + uavcan::uint32_t sp = (7 * NBT) / 8; - out_timings.pseg1 = sp - 1 - out_timings.propseg; - out_timings.pseg2 = NBT - (1 + out_timings.pseg1 + out_timings.propseg); - out_timings.rjw = uavcan::min((uavcan::uint8_t) 4, out_timings.pseg2); + out_timings.pseg1 = sp - 1 - out_timings.propseg; + out_timings.pseg2 = NBT - (1 + out_timings.pseg1 + out_timings.propseg); + out_timings.rjw = uavcan::min((uavcan::uint8_t) 4, out_timings.pseg2); - return ((out_timings.pseg1 <= 8) && (out_timings.pseg2 <= 8) && (out_timings.propseg <= 8)) ? 0 : - -ErrInvalidBitRate; + return ((out_timings.pseg1 <= 8) && (out_timings.pseg2 <= 8) && (out_timings.propseg <= 8)) ? 0 : + -ErrInvalidBitRate; } -uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) +uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) { - if (frame.isErrorFrame() || frame.dlc > 8) - { - return -ErrUnsupportedFrame; - } + if (frame.isErrorFrame() || frame.dlc > 8) { + return -ErrUnsupportedFrame; + } - /* - * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because - * it is possible that the highest-priority frame between select() and send() could have been - * replaced with a lower priority one due to TX timeout. But we don't do this check because: - * - * - It is a highly unlikely scenario. - * - * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new - * frame can only have higher priority, which doesn't break the logic. - * - * - If high-priority frames are timing out in the TX queue, there's probably a lot of other - * issues to take care of before this one becomes relevant. - * - * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. - */ - CriticalSectionLocker lock; + /* + * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because + * it is possible that the highest-priority frame between select() and send() could have been + * replaced with a lower priority one due to TX timeout. But we don't do this check because: + * + * - It is a highly unlikely scenario. + * + * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new + * frame can only have higher priority, which doesn't break the logic. + * + * - If high-priority frames are timing out in the TX queue, there's probably a lot of other + * issues to take care of before this one becomes relevant. + * + * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. + */ + CriticalSectionLocker lock; - /* - * Seeking for an empty slot - */ + /* + * Seeking for an empty slot + */ - uavcan::uint32_t mbi = 0; - if ((can_->ESR2 & (flexcan::ESR2_IMB | flexcan::ESR2_VPS)) == (flexcan::ESR2_IMB | flexcan::ESR2_VPS)) - { - mbi = (can_->ESR2 & flexcan::ESR2_LPTM_MASK) >> flexcan::ESR2_LPTM_SHIFT; - mbi -= flexcan::NumMBinFiFoAndFilters; - } + uavcan::uint32_t mbi = 0; - uavcan::uint32_t mb_bit = 1 << (flexcan::NumMBinFiFoAndFilters + mbi); + if ((can_->ESR2 & (flexcan::ESR2_IMB | flexcan::ESR2_VPS)) == (flexcan::ESR2_IMB | flexcan::ESR2_VPS)) { + mbi = (can_->ESR2 & flexcan::ESR2_LPTM_MASK) >> flexcan::ESR2_LPTM_SHIFT; + mbi -= flexcan::NumMBinFiFoAndFilters; + } - while (mbi < NumTxMesgBuffers) - { - if (can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code != flexcan::TxMbDataOrRemote) - { - can_->IFLAG1 = mb_bit; - break; - } - mb_bit <<= 1; - mbi++; - } + uavcan::uint32_t mb_bit = 1 << (flexcan::NumMBinFiFoAndFilters + mbi); - if (mbi == NumTxMesgBuffers) - { - return 0; // No transmission for you! - } + while (mbi < NumTxMesgBuffers) { + if (can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code != flexcan::TxMbDataOrRemote) { + can_->IFLAG1 = mb_bit; + break; + } - peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, (uavcan::uint8_t) mbi); // Statistics + mb_bit <<= 1; + mbi++; + } - flexcan::MBcsType cs; - cs.code = flexcan::TxMbDataOrRemote; - flexcan::MessageBufferType& mb = can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb; - mb.CS.code = flexcan::TxMbInactive; - if (frame.isExtended()) - { - cs.ide = 1; - mb.ID.ext = frame.id & uavcan::CanFrame::MaskExtID; - } - else - { - mb.ID.std = frame.id & uavcan::CanFrame::MaskStdID; - } + if (mbi == NumTxMesgBuffers) { + return 0; // No transmission for you! + } - cs.rtr = frame.isRemoteTransmissionRequest(); + peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, (uavcan::uint8_t) mbi); // Statistics - cs.dlc = frame.dlc; - mb.data.b0 = frame.data[0]; - mb.data.b1 = frame.data[1]; - mb.data.b2 = frame.data[2]; - mb.data.b3 = frame.data[3]; - mb.data.b4 = frame.data[4]; - mb.data.b5 = frame.data[5]; - mb.data.b6 = frame.data[6]; - mb.data.b7 = frame.data[7]; + flexcan::MBcsType cs; + cs.code = flexcan::TxMbDataOrRemote; + flexcan::MessageBufferType &mb = can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb; + mb.CS.code = flexcan::TxMbInactive; - /* - * Registering the pending transmission so we can track its deadline and loopback it as needed - */ - TxItem& txi = pending_tx_[mbi]; - txi.deadline = tx_deadline; - txi.frame = frame; - txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0; - txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; - txi.pending = TxItem::busy; + if (frame.isExtended()) { + cs.ide = 1; + mb.ID.ext = frame.id & uavcan::CanFrame::MaskExtID; - mb.CS = cs; // Go. - can_->IMASK1 |= mb_bit; - return 1; + } else { + mb.ID.std = frame.id & uavcan::CanFrame::MaskStdID; + } + + cs.rtr = frame.isRemoteTransmissionRequest(); + + cs.dlc = frame.dlc; + mb.data.b0 = frame.data[0]; + mb.data.b1 = frame.data[1]; + mb.data.b2 = frame.data[2]; + mb.data.b3 = frame.data[3]; + mb.data.b4 = frame.data[4]; + mb.data.b5 = frame.data[5]; + mb.data.b6 = frame.data[6]; + mb.data.b7 = frame.data[7]; + + /* + * Registering the pending transmission so we can track its deadline and loopback it as needed + */ + TxItem &txi = pending_tx_[mbi]; + txi.deadline = tx_deadline; + txi.frame = frame; + txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0; + txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; + txi.pending = TxItem::busy; + + mb.CS = cs; // Go. + can_->IMASK1 |= mb_bit; + 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) +uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) { - out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps - uavcan::uint64_t utc_usec = 0; - { - CriticalSectionLocker 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; + out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps + uavcan::uint64_t utc_usec = 0; + { + CriticalSectionLocker 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) +uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs, + uavcan::uint16_t num_configs) { - if (num_configs <= NumFilters) - { - CriticalSectionLocker lock; - setFreeze(true); - if (!waitFreezeAckChange(true)) - { - return -ErrMcrFRZACKAckNotSet; - } + if (num_configs <= NumFilters) { + CriticalSectionLocker lock; + setFreeze(true); - volatile flexcan::FilterType* filterBase = reinterpret_cast(&can_->MB[flexcan:: - FirstFilter].mb); + if (!waitFreezeAckChange(true)) { + return -ErrMcrFRZACKAckNotSet; + } - if (num_configs == 0) - { - // No filters except all - for (uint8_t i = 0; i < NumFilters; i++) - { - volatile flexcan::FilterType& filter = filterBase[i]; - filter.w = 0; // All Do not care - can_->RXIMR[i].w = 0;// All Do not care - } - can_->RXFGMASK = 0; // All Do not care - } - else - { - for (uint8_t i = 0; i < NumFilters; i++) - { - volatile flexcan::FilterType& filter = filterBase[i]; - volatile flexcan::FilterType& mask = can_->RXIMR[i]; + volatile flexcan::FilterType *filterBase = reinterpret_cast(&can_->MB[flexcan:: + FirstFilter].mb); - if (i < num_configs) - { + if (num_configs == 0) { + // No filters except all + for (uint8_t i = 0; i < NumFilters; i++) { + volatile flexcan::FilterType &filter = filterBase[i]; + filter.w = 0; // All Do not care + can_->RXIMR[i].w = 0;// All Do not care + } - const uavcan::CanFilterConfig* const cfg = filter_configs + i; + can_->RXFGMASK = 0; // All Do not care - if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) - { - filter.ide = 1; - filter.ext = cfg->id & uavcan::CanFrame::MaskExtID; - mask.ext = cfg->mask & uavcan::CanFrame::MaskExtID; - } - else - { - filter.ide = 0; - filter.std = cfg->id & uavcan::CanFrame::MaskStdID; - mask.std = cfg->mask & uavcan::CanFrame::MaskStdID; - } + } else { + for (uint8_t i = 0; i < NumFilters; i++) { + volatile flexcan::FilterType &filter = filterBase[i]; + volatile flexcan::FilterType &mask = can_->RXIMR[i]; - filter.rtr = cfg->id & uavcan::CanFrame::FlagRTR ? 1 : 0; + if (i < num_configs) { - mask.rtr = cfg->mask & uavcan::CanFrame::FlagRTR ? 1 : 0; - mask.ide = cfg->mask & uavcan::CanFrame::FlagEFF ? 1 : 0; - } - else - { - /* We can not really disable, with out effecting the memory map - * of the mail boxes, so set all bits to 1's and all to care - */ - filter.w = 0xffffffff; - mask.w = 0xffffffff; - } - } - } - setFreeze(false); - return waitFreezeAckChange(false) ? 0 : -ErrMcrFRZACKAckNotCleared; - } + const uavcan::CanFilterConfig *const cfg = filter_configs + i; - return -ErrFilterNumConfigs; + if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) { + filter.ide = 1; + filter.ext = cfg->id & uavcan::CanFrame::MaskExtID; + mask.ext = cfg->mask & uavcan::CanFrame::MaskExtID; + + } else { + filter.ide = 0; + filter.std = cfg->id & uavcan::CanFrame::MaskStdID; + mask.std = cfg->mask & uavcan::CanFrame::MaskStdID; + } + + filter.rtr = cfg->id & uavcan::CanFrame::FlagRTR ? 1 : 0; + + mask.rtr = cfg->mask & uavcan::CanFrame::FlagRTR ? 1 : 0; + mask.ide = cfg->mask & uavcan::CanFrame::FlagEFF ? 1 : 0; + + } else { + /* We can not really disable, with out effecting the memory map + * of the mail boxes, so set all bits to 1's and all to care + */ + filter.w = 0xffffffff; + mask.w = 0xffffffff; + } + } + } + + setFreeze(false); + return waitFreezeAckChange(false) ? 0 : -ErrMcrFRZACKAckNotCleared; + } + + return -ErrFilterNumConfigs; } bool CanIface::waitMCRChange(uavcan::uint32_t mask, bool target_state) { - const unsigned Timeout = 1000; - for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) - { - const bool state = (can_->MCR & mask) != 0; - if (state == target_state) - { - return true; - } - ::up_udelay(10); - } - return false; + const unsigned Timeout = 1000; + + for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) { + const bool state = (can_->MCR & mask) != 0; + + if (state == target_state) { + return true; + } + + ::up_udelay(10); + } + + return false; } void CanIface::setMCR(uavcan::uint32_t mask, bool target_state) { - if (target_state) - { - can_->MCR |= mask; - } - else - { - can_->MCR &= ~mask; - } + if (target_state) { + can_->MCR |= mask; + + } else { + can_->MCR &= ~mask; + } } bool CanIface::waitFreezeAckChange(bool target_state) { - return waitMCRChange(flexcan::MCR_FRZACK, target_state); + return waitMCRChange(flexcan::MCR_FRZACK, target_state); } void CanIface::setFreeze(bool freeze_true) { - { - CriticalSectionLocker lock; - if(freeze_true) - { - can_->MCR |= flexcan::MCR_FRZ; - can_->MCR |= flexcan::MCR_HALT; - } - else - { - can_->MCR &= ~flexcan::MCR_HALT; - can_->MCR &= ~flexcan::MCR_FRZ; + { + CriticalSectionLocker lock; - } - } + if (freeze_true) { + can_->MCR |= flexcan::MCR_FRZ; + can_->MCR |= flexcan::MCR_HALT; + + } else { + can_->MCR &= ~flexcan::MCR_HALT; + can_->MCR &= ~flexcan::MCR_FRZ; + + } + } } bool CanIface::setEnable(bool enable_true) { - setMCR(flexcan::MCR_MDIS, !enable_true); - return waitMCRChange(flexcan::MCR_LPMACK, true); + setMCR(flexcan::MCR_MDIS, !enable_true); + return waitMCRChange(flexcan::MCR_LPMACK, true); } uavcan::int16_t CanIface::doReset(Timings timings) { - setMCR(flexcan::MCR_SOFTRST, true); - if (!waitMCRChange(flexcan::MCR_SOFTRST, false)) - { - return -ErrMcrSOFTRSTNotCleared; - } + setMCR(flexcan::MCR_SOFTRST, true); - uavcan::uint8_t tasd = 25 - (flexcan::canclk * (flexcan::HWMaxMB + 3 - - (flexcan::numberFIFOFilters*8) - - (flexcan::useFIFO * flexcan::numberFIFOFilters * 2)) * 2) / \ - (flexcan::busclck * (1 + timings.pseg1 + timings.pseg2 + timings.propseg) - * timings.prescaler); + if (!waitMCRChange(flexcan::MCR_SOFTRST, false)) { + return -ErrMcrSOFTRSTNotCleared; + } - setMCR(flexcan::MCR_SUPV, false); + uavcan::uint8_t tasd = 25 - (flexcan::canclk * (flexcan::HWMaxMB + 3 - + (flexcan::numberFIFOFilters * 8) - + (flexcan::useFIFO * flexcan::numberFIFOFilters * 2)) * 2) / \ + (flexcan::busclck * (1 + timings.pseg1 + timings.pseg2 + timings.propseg) + * timings.prescaler); - for (int i = 0; i < flexcan::HWMaxMB; i++) - { - can_->MB[i].mb.CS.w = 0x0; - can_->MB[i].mb.ID.w = 0x0; - can_->MB[i].mb.data.l = 0x0; - can_->MB[i].mb.data.h = 0x0; - } + setMCR(flexcan::MCR_SUPV, false); - setMCR((flexcan::useFIFO ? flexcan::MCR_RFEN : 0) | flexcan::MCR_SLFWAK | flexcan::MCR_WRNEN | flexcan::MCR_WAKSRC - | flexcan::MCR_SRXDIS | flexcan::MCR_IRMQ | flexcan::MCR_AEN | - (((flexcan::HWMaxMB - 1) << flexcan::MCR_MAXMB_SHIFT) & flexcan::MCR_MAXMB_MASK), true); - can_->CTRL2 = flexcan::numberFIFOFilters | - ((tasd << flexcan::CTRL2_TASD_SHIFT) & flexcan::CTRL2_TASD_MASK) | - flexcan::CTRL2_RRS | - flexcan::CTRL2_EACEN; + for (int i = 0; i < flexcan::HWMaxMB; i++) { + can_->MB[i].mb.CS.w = 0x0; + can_->MB[i].mb.ID.w = 0x0; + can_->MB[i].mb.data.l = 0x0; + can_->MB[i].mb.data.h = 0x0; + } - for (int i = 0; i < flexcan::HWMaxMB; i++) - { - can_->RXIMR[i].w = 0x0; - } - can_->RX14MASK = 0x3FFFFFFF; - can_->RX15MASK = 0x3FFFFFFF; - can_->RXMGMASK = 0x3FFFFFFF; - can_->RXFGMASK = 0x0; - return 0; + setMCR((flexcan::useFIFO ? flexcan::MCR_RFEN : 0) | flexcan::MCR_SLFWAK | flexcan::MCR_WRNEN | flexcan::MCR_WAKSRC + | flexcan::MCR_SRXDIS | flexcan::MCR_IRMQ | flexcan::MCR_AEN | + (((flexcan::HWMaxMB - 1) << flexcan::MCR_MAXMB_SHIFT) & flexcan::MCR_MAXMB_MASK), true); + can_->CTRL2 = flexcan::numberFIFOFilters | + ((tasd << flexcan::CTRL2_TASD_SHIFT) & flexcan::CTRL2_TASD_MASK) | + flexcan::CTRL2_RRS | + flexcan::CTRL2_EACEN; + + for (int i = 0; i < flexcan::HWMaxMB; i++) { + can_->RXIMR[i].w = 0x0; + } + + can_->RX14MASK = 0x3FFFFFFF; + can_->RX15MASK = 0x3FFFFFFF; + can_->RXMGMASK = 0x3FFFFFFF; + can_->RXFGMASK = 0x0; + return 0; } int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) { - /* Set the module disabled */ + /* Set the module disabled */ - if (!setEnable(false)) - { - UAVCAN_KINETIS_LOG("MCR MCR_LPMACK not set"); - return -ErrMcrLPMAckNotSet; - } + if (!setEnable(false)) { + UAVCAN_KINETIS_LOG("MCR MCR_LPMACK not set"); + return -ErrMcrLPMAckNotSet; + } - /* Set the Clock to OSCERCLK or ) */ + /* Set the Clock to OSCERCLK or ) */ - can_->CTRL1 &= ~flexcan::CTRL1_CLKSRC; - can_->CTRL1 |= flexcan::CLOCKSEL; + can_->CTRL1 &= ~flexcan::CTRL1_CLKSRC; + can_->CTRL1 |= flexcan::CLOCKSEL; - /* Set the module enabled */ + /* Set the module enabled */ - if (!setEnable(true)) - { - UAVCAN_KINETIS_LOG("MCR MCR_LPMACK not cleared"); - return -ErrMcrLPMAckNotCleared; - } + if (!setEnable(true)) { + UAVCAN_KINETIS_LOG("MCR MCR_LPMACK not cleared"); + return -ErrMcrLPMAckNotCleared; + } - /* - * Object state - interrupts are disabled, so it's safe to modify it now - */ + /* + * Object state - interrupts are disabled, so it's safe to modify it now + */ - rx_queue_.reset(); - error_cnt_ = 0; - served_aborts_cnt_ = 0; - uavcan::fill_n(pending_tx_, NumTxMesgBuffers, TxItem()); - peak_tx_mailbox_index_ = 0; - had_activity_ = false; + rx_queue_.reset(); + error_cnt_ = 0; + served_aborts_cnt_ = 0; + uavcan::fill_n(pending_tx_, NumTxMesgBuffers, TxItem()); + peak_tx_mailbox_index_ = 0; + had_activity_ = false; - /* - * CAN timings for this bitrate - */ - Timings timings; - const int timings_res = computeTimings(bitrate, timings); - if (timings_res < 0) - { - setEnable(false); - return timings_res; - } - UAVCAN_KINETIS_LOG("Timings: prescaler=%u rjw=%u propseg=%u pseg1=%u pseg2=%u", - unsigned(timings.prescaler), unsigned(timings.rjw), - unsigned(timings.propseg), unsigned(timings.pseg1), - unsigned(timings.pseg2)); - /* - * Hardware initialization from reset state - */ + /* + * CAN timings for this bitrate + */ + Timings timings; + const int timings_res = computeTimings(bitrate, timings); + + if (timings_res < 0) { + setEnable(false); + return timings_res; + } + + UAVCAN_KINETIS_LOG("Timings: prescaler=%u rjw=%u propseg=%u pseg1=%u pseg2=%u", + unsigned(timings.prescaler), unsigned(timings.rjw), + unsigned(timings.propseg), unsigned(timings.pseg1), + unsigned(timings.pseg2)); + /* + * Hardware initialization from reset state + */ - uavcan::int16_t rv = doReset(timings); - if (rv != 0) - { - UAVCAN_KINETIS_LOG("doReset returned %d", rv); - return -rv; - } + uavcan::int16_t rv = doReset(timings); - uavcan::uint32_t ctl1 = can_->CTRL1; - ctl1 |= (timings.prescaler - 1) << flexcan::CTRL1_PRESDIV_SHIFT; - ctl1 |= (timings.rjw - 1) << flexcan::CTRL1_RJW_SHIFT; - ctl1 |= (timings.pseg1 - 1) << flexcan::CTRL1_PSEG1_SHIFT; - ctl1 |= (timings.pseg2 - 1) << flexcan::CTRL1_PSEG2_SHIFT; - ctl1 |= flexcan::CTRL1_ERRMSK; - ctl1 |= flexcan::CTRL1_TWRNMSK; - ctl1 |= flexcan::CTRL1_RWRNMSK; - ctl1 |= ((mode == SilentMode) ? flexcan::CTRL1_LOM : 0); - ctl1 |= ((timings.propseg - 1) << flexcan::CTRL1_ROPSEG_SHIFT); - can_->CTRL1 = ctl1; + if (rv != 0) { + UAVCAN_KINETIS_LOG("doReset returned %d", rv); + return -rv; + } - /* - * Default filter configuration - */ - volatile flexcan::FilterType* filterBase = reinterpret_cast(&can_->MB[flexcan::FirstFilter]. - mb); - for (uavcan::uint32_t i = 0; i < flexcan::NumHWFilters; i++) - { - volatile flexcan::FilterType& filter = filterBase[i]; - filter.w = 0; // All bits do not care - } - can_->RXFGMASK = 0; // All bits do not care + uavcan::uint32_t ctl1 = can_->CTRL1; + ctl1 |= (timings.prescaler - 1) << flexcan::CTRL1_PRESDIV_SHIFT; + ctl1 |= (timings.rjw - 1) << flexcan::CTRL1_RJW_SHIFT; + ctl1 |= (timings.pseg1 - 1) << flexcan::CTRL1_PSEG1_SHIFT; + ctl1 |= (timings.pseg2 - 1) << flexcan::CTRL1_PSEG2_SHIFT; + ctl1 |= flexcan::CTRL1_ERRMSK; + ctl1 |= flexcan::CTRL1_TWRNMSK; + ctl1 |= flexcan::CTRL1_RWRNMSK; + ctl1 |= ((mode == SilentMode) ? flexcan::CTRL1_LOM : 0); + ctl1 |= ((timings.propseg - 1) << flexcan::CTRL1_ROPSEG_SHIFT); + can_->CTRL1 = ctl1; - for (uavcan::uint32_t mb = 0; mb < flexcan::HWMaxMB; mb++) - { - can_->RXIMR[mb].w = 0; // All bits do not care - } + /* + * Default filter configuration + */ + volatile flexcan::FilterType *filterBase = reinterpret_cast(&can_->MB[flexcan::FirstFilter]. + mb); - can_->IFLAG1 = FIFO_IFLAG1 | flexcan::TXMBMask; - can_->IMASK1 = FIFO_IFLAG1; + for (uavcan::uint32_t i = 0; i < flexcan::NumHWFilters; i++) { + volatile flexcan::FilterType &filter = filterBase[i]; + filter.w = 0; // All bits do not care + } - setFreeze(false); - return waitFreezeAckChange(false) ? 0 : -ErrMcrFRZACKAckNotCleared; + can_->RXFGMASK = 0; // All bits do not care + + for (uavcan::uint32_t mb = 0; mb < flexcan::HWMaxMB; mb++) { + can_->RXIMR[mb].w = 0; // All bits do not care + } + + can_->IFLAG1 = FIFO_IFLAG1 | flexcan::TXMBMask; + can_->IMASK1 = FIFO_IFLAG1; + + setFreeze(false); + return waitFreezeAckChange(false) ? 0 : -ErrMcrFRZACKAckNotCleared; } void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec) { - UAVCAN_ASSERT(mailbox_index < NumTxMesgBuffers); + UAVCAN_ASSERT(mailbox_index < NumTxMesgBuffers); - had_activity_ = had_activity_ || txok; + had_activity_ = had_activity_ || txok; - TxItem& txi = pending_tx_[mailbox_index]; + TxItem &txi = pending_tx_[mailbox_index]; - if (txi.loopback && txok && txi.pending == TxItem::busy) - { - rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); - } + if (txi.loopback && txok && txi.pending == TxItem::busy) { + rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); + } - txi.pending = TxItem::free; + txi.pending = TxItem::free; } void CanIface::handleTxInterrupt(uavcan::uint32_t tx_iflags, const uavcan::uint64_t utc_usec) { - /* First Process aborts */ + /* First Process aborts */ - uavcan::uint32_t aborts = pending_aborts_ & tx_iflags; - if (aborts) - { - uavcan::uint32_t moreAborts = aborts; - uavcan::uint32_t bit = 1 << flexcan::NumMBinFiFoAndFilters; - for (uavcan::uint32_t mb = 0; moreAborts && mb < NumTxMesgBuffers; mb++ ) - { - if (moreAborts & bit) - { - moreAborts &= ~bit; - can_->IFLAG1 = bit; - pending_tx_[mb].pending = TxItem::free; - served_aborts_cnt_++; - } - bit <<= 1; - } - tx_iflags &= ~aborts; - pending_aborts_ &= ~aborts; + uavcan::uint32_t aborts = pending_aborts_ & tx_iflags; - } + if (aborts) { + uavcan::uint32_t moreAborts = aborts; + uavcan::uint32_t bit = 1 << flexcan::NumMBinFiFoAndFilters; - /* Now Process TX completions */ + for (uavcan::uint32_t mb = 0; moreAborts && mb < NumTxMesgBuffers; mb++) { + if (moreAborts & bit) { + moreAborts &= ~bit; + can_->IFLAG1 = bit; + pending_tx_[mb].pending = TxItem::free; + served_aborts_cnt_++; + } - uavcan::uint32_t mbBit = 1 << flexcan::NumMBinFiFoAndFilters; - for (uavcan::uint32_t mbi = 0; tx_iflags && mbi < NumTxMesgBuffers; mbi++ ) - { - if (tx_iflags & mbBit) - { - can_->IFLAG1 = mbBit; - tx_iflags &= ~mbBit; - const bool txok = can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code != flexcan::TxMbAbort; - handleTxMailboxInterrupt(mbi, txok, utc_usec); - } - mbBit <<= 1; - } + bit <<= 1; + } - update_event_.signalFromInterrupt(); + tx_iflags &= ~aborts; + pending_aborts_ &= ~aborts; - pollErrorFlagsFromISR(); + } + + /* Now Process TX completions */ + + uavcan::uint32_t mbBit = 1 << flexcan::NumMBinFiFoAndFilters; + + for (uavcan::uint32_t mbi = 0; tx_iflags && mbi < NumTxMesgBuffers; mbi++) { + if (tx_iflags & mbBit) { + can_->IFLAG1 = mbBit; + tx_iflags &= ~mbBit; + const bool txok = can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code != flexcan::TxMbAbort; + handleTxMailboxInterrupt(mbi, txok, utc_usec); + } + + mbBit <<= 1; + } + + update_event_.signalFromInterrupt(); + + pollErrorFlagsFromISR(); } void CanIface::handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec) { - UAVCAN_ASSERT(fifo_index < 2); + UAVCAN_ASSERT(fifo_index < 2); - uavcan::CanFrame frame; + uavcan::CanFrame frame; - if ((rx_iflags & FIFO_IFLAG1) == 0) - { - UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read - return; - } + if ((rx_iflags & FIFO_IFLAG1) == 0) { + UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read + return; + } - if (rx_iflags & flexcan::CAN_FIFO_OV) - { - error_cnt_++; - can_->IFLAG1 = flexcan::CAN_FIFO_OV; - } + if (rx_iflags & flexcan::CAN_FIFO_OV) { + error_cnt_++; + can_->IFLAG1 = flexcan::CAN_FIFO_OV; + } - if (rx_iflags & flexcan::CAN_FIFO_WARN) - { - fifo_warn_cnt_++; - can_->IFLAG1 = flexcan::CAN_FIFO_WARN; - } + if (rx_iflags & flexcan::CAN_FIFO_WARN) { + fifo_warn_cnt_++; + can_->IFLAG1 = flexcan::CAN_FIFO_WARN; + } - if (rx_iflags & flexcan::CAN_FIFO_NE) - { - const flexcan::RxFiFoType &rf = can_->MB[flexcan::FiFo].fifo; + if (rx_iflags & flexcan::CAN_FIFO_NE) { + const flexcan::RxFiFoType &rf = can_->MB[flexcan::FiFo].fifo; - /* - * Read the frame contents - */ + /* + * Read the frame contents + */ - if (rf.CS.ide) - { - frame.id = uavcan::CanFrame::MaskExtID & rf.ID.ext; - frame.id |= uavcan::CanFrame::FlagEFF; - } - else - { - frame.id = uavcan::CanFrame::MaskStdID & rf.ID.std; - } + if (rf.CS.ide) { + frame.id = uavcan::CanFrame::MaskExtID & rf.ID.ext; + frame.id |= uavcan::CanFrame::FlagEFF; - if (rf.CS.rtr) - { - frame.id |= uavcan::CanFrame::FlagRTR; - } + } else { + frame.id = uavcan::CanFrame::MaskStdID & rf.ID.std; + } - frame.dlc = rf.CS.dlc; + if (rf.CS.rtr) { + frame.id |= uavcan::CanFrame::FlagRTR; + } - frame.data[0] = rf.data.b0; - frame.data[1] = rf.data.b1; - frame.data[2] = rf.data.b2; - frame.data[3] = rf.data.b3; - frame.data[4] = rf.data.b4; - frame.data[5] = rf.data.b5; - frame.data[6] = rf.data.b6; - frame.data[7] = rf.data.b7; + frame.dlc = rf.CS.dlc; - volatile uavcan::uint32_t idhit = can_->RXFIR; - UNUSED(idhit); - (void)can_->TIMER; - can_->IFLAG1 = flexcan::CAN_FIFO_NE; + frame.data[0] = rf.data.b0; + frame.data[1] = rf.data.b1; + frame.data[2] = rf.data.b2; + frame.data[3] = rf.data.b3; + frame.data[4] = rf.data.b4; + frame.data[5] = rf.data.b5; + frame.data[6] = rf.data.b6; + frame.data[7] = rf.data.b7; - /* - * Store with timeout into the FIFO buffer and signal update event - */ - rx_queue_.push(frame, utc_usec, 0); - had_activity_ = true; - update_event_.signalFromInterrupt(); + volatile uavcan::uint32_t idhit = can_->RXFIR; + UNUSED(idhit); + (void)can_->TIMER; + can_->IFLAG1 = flexcan::CAN_FIFO_NE; - } + /* + * Store with timeout into the FIFO buffer and signal update event + */ + rx_queue_.push(frame, utc_usec, 0); + had_activity_ = true; + update_event_.signalFromInterrupt(); - pollErrorFlagsFromISR(); + } + + pollErrorFlagsFromISR(); } void CanIface::pollErrorFlagsFromISR() { - volatile uavcan::uint32_t esr1 = can_->ESR1 & (flexcan::ESR1_STFERR | flexcan::ESR1_FRMERR | - flexcan::ESR1_CRCERR | flexcan::ESR1_ACKERR | - flexcan::ESR1_BIT0ERR | flexcan::ESR1_BIT1ERR); - if (esr1 != 0) - { - can_->ESR1 = esr1; - error_cnt_++; - uavcan::uint32_t pending_aborts = 0; + volatile uavcan::uint32_t esr1 = can_->ESR1 & (flexcan::ESR1_STFERR | flexcan::ESR1_FRMERR | + flexcan::ESR1_CRCERR | flexcan::ESR1_ACKERR | + flexcan::ESR1_BIT0ERR | flexcan::ESR1_BIT1ERR); - // Begin abort requests + if (esr1 != 0) { + can_->ESR1 = esr1; + error_cnt_++; + uavcan::uint32_t pending_aborts = 0; - for (int i = 0; i < NumTxMesgBuffers; i++) - { - TxItem& txi = pending_tx_[i]; - uavcan::uint32_t mbi = flexcan::NumMBinFiFoAndFilters + i; - uavcan::uint32_t iflag1 = 1 << mbi; - if (txi.pending == TxItem::busy && txi.abort_on_error) - { - txi.pending = TxItem::abort; - can_->IFLAG1 = iflag1; - pending_aborts |= iflag1;; - can_->MB[mbi].mb.CS.code = flexcan::TxMbAbort; - } - } - pending_aborts_ = pending_aborts; - } + // Begin abort requests + + for (int i = 0; i < NumTxMesgBuffers; i++) { + TxItem &txi = pending_tx_[i]; + uavcan::uint32_t mbi = flexcan::NumMBinFiFoAndFilters + i; + uavcan::uint32_t iflag1 = 1 << mbi; + + if (txi.pending == TxItem::busy && txi.abort_on_error) { + txi.pending = TxItem::abort; + can_->IFLAG1 = iflag1; + pending_aborts |= iflag1;; + can_->MB[mbi].mb.CS.code = flexcan::TxMbAbort; + } + } + + pending_aborts_ = pending_aborts; + } } void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) { - CriticalSectionLocker lock; - for (int mbi = 0; mbi < NumTxMesgBuffers; mbi++) - { - TxItem& txi = pending_tx_[mbi]; - if (txi.pending == TxItem::busy && txi.deadline < current_time) - { - can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code = flexcan::TxMbAbort; - txi.pending = TxItem::abort; - error_cnt_++; - } - } + CriticalSectionLocker lock; + + for (int mbi = 0; mbi < NumTxMesgBuffers; mbi++) { + TxItem &txi = pending_tx_[mbi]; + + if (txi.pending == TxItem::busy && txi.deadline < current_time) { + can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code = flexcan::TxMbAbort; + txi.pending = TxItem::abort; + error_cnt_++; + } + } } -bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame& frame) const +bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame &frame) const { - /* - * We can accept more frames only if the following conditions are satisfied: - * - There is at least one TX mailbox free (obvious enough); - * - The priority of the new frame is higher than priority of all TX mailboxes. - */ - { - if ((can_->ESR2 & (flexcan::ESR2_IMB | flexcan::ESR2_VPS)) == flexcan::ESR2_VPS) - { - // No Free - return false; - } + /* + * We can accept more frames only if the following conditions are satisfied: + * - There is at least one TX mailbox free (obvious enough); + * - The priority of the new frame is higher than priority of all TX mailboxes. + */ + { + if ((can_->ESR2 & (flexcan::ESR2_IMB | flexcan::ESR2_VPS)) == flexcan::ESR2_VPS) { + // No Free + return false; + } - } + } - /* - * The second condition requires a critical section. - */ - CriticalSectionLocker lock; + /* + * The second condition requires a critical section. + */ + CriticalSectionLocker lock; - for (int mbi = 0; mbi < NumTxMesgBuffers; mbi++) - { - if (pending_tx_[mbi].pending == TxItem::busy && !frame.priorityHigherThan(pending_tx_[mbi].frame)) - { - return false; // There's a mailbox whose priority is higher or equal the priority of the new frame. - } - } + for (int mbi = 0; mbi < NumTxMesgBuffers; mbi++) { + if (pending_tx_[mbi].pending == TxItem::busy && !frame.priorityHigherThan(pending_tx_[mbi].frame)) { + return false; // There's a mailbox whose priority is higher or equal the priority of the new frame. + } + } - return true; // This new frame will be added to a free TX mailbox in the next @ref send(). + return true; // This new frame will be added to a free TX mailbox in the next @ref send(). } bool CanIface::isRxBufferEmpty() const { - CriticalSectionLocker lock; - return rx_queue_.getLength() == 0; + CriticalSectionLocker lock; + return rx_queue_.getLength() == 0; } uavcan::uint64_t CanIface::getErrorCount() const { - CriticalSectionLocker lock; - return error_cnt_ + rx_queue_.getOverflowCount(); + CriticalSectionLocker lock; + return error_cnt_ + rx_queue_.getOverflowCount(); } unsigned CanIface::getRxQueueLength() const { - CriticalSectionLocker lock; - return rx_queue_.getLength(); + CriticalSectionLocker lock; + return rx_queue_.getLength(); } bool CanIface::hadActivity() { - CriticalSectionLocker lock; - const bool ret = had_activity_; - had_activity_ = false; - return ret; + CriticalSectionLocker lock; + const bool ret = had_activity_; + had_activity_ = false; + return ret; } /* * CanDriver */ -uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const +uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces]) const { - uavcan::CanSelectMasks msk; + uavcan::CanSelectMasks msk; - // Iface 0 - msk.read = if0_.isRxBufferEmpty() ? 0 : 1; + // Iface 0 + msk.read = if0_.isRxBufferEmpty() ? 0 : 1; - if (pending_tx[0] != UAVCAN_NULLPTR) - { - msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0; - } + if (pending_tx[0] != UAVCAN_NULLPTR) { + msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0; + } - // Iface 1 + // Iface 1 #if UAVCAN_KINETIS_NUM_IFACES > 1 - if (!if1_.isRxBufferEmpty()) - { - msk.read |= 1 << 1; - } - if (pending_tx[1] != UAVCAN_NULLPTR) - { - if (if1_.canAcceptNewTxFrame(*pending_tx[1])) - { - msk.write |= 1 << 1; - } - } + if (!if1_.isRxBufferEmpty()) { + msk.read |= 1 << 1; + } + + if (pending_tx[1] != UAVCAN_NULLPTR) { + if (if1_.canAcceptNewTxFrame(*pending_tx[1])) { + msk.write |= 1 << 1; + } + } + #endif - return msk; + return msk; } bool CanDriver::hasReadableInterfaces() const { #if UAVCAN_KINETIS_NUM_IFACES == 1 - return !if0_.isRxBufferEmpty(); + return !if0_.isRxBufferEmpty(); #elif UAVCAN_KINETIS_NUM_IFACES == 2 - return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty(); + return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty(); #else # error UAVCAN_KINETIS_NUM_IFACES #endif } -uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], - const uavcan::MonotonicTime blocking_deadline) +uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces], + const uavcan::MonotonicTime blocking_deadline) { - const uavcan::CanSelectMasks in_masks = inout_masks; - const uavcan::MonotonicTime time = clock::getMonotonic(); + const uavcan::CanSelectMasks in_masks = inout_masks; + const uavcan::MonotonicTime time = clock::getMonotonic(); - if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots - { - CriticalSectionLocker cs_locker; - if0_.pollErrorFlagsFromISR(); - } + if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots + { + CriticalSectionLocker cs_locker; + if0_.pollErrorFlagsFromISR(); + } #if UAVCAN_KINETIS_NUM_IFACES > 1 - if1_.discardTimedOutTxMailboxes(time); - { - CriticalSectionLocker cs_locker; - if1_.pollErrorFlagsFromISR(); - } + if1_.discardTimedOutTxMailboxes(time); + { + CriticalSectionLocker cs_locker; + if1_.pollErrorFlagsFromISR(); + } #endif - inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events - if ((inout_masks.read & in_masks.read) != 0 || - (inout_masks.write & in_masks.write) != 0) - { - return 1; - } + inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events - (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates - inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set - return 1; // Return value doesn't matter as long as it is non-negative + if ((inout_masks.read & in_masks.read) != 0 || + (inout_masks.write & in_masks.write) != 0) { + return 1; + } + + (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates + inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set + return 1; // Return value doesn't matter as long as it is non-negative } void CanDriver::initOnce() { - /* - * CAN1, CAN2 - */ - { - CriticalSectionLocker lock; - modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_FLEXCAN0); + /* + * CAN1, CAN2 + */ + { + CriticalSectionLocker lock; + modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_FLEXCAN0); #if UAVCAN_KINETIS_NUM_IFACES > 1 - modifyreg32(KINETIS_SIM_SCGC3, 0, SIM_SCGC3_FLEXCAN1); + modifyreg32(KINETIS_SIM_SCGC3, 0, SIM_SCGC3_FLEXCAN1); #endif - } + } - /* - * IRQ - */ + /* + * IRQ + */ #define IRQ_ATTACH(irq, handler) \ - { \ - const int res = irq_attach(irq, handler, NULL); \ - (void)res; \ - assert(res >= 0); \ - up_enable_irq(irq); \ - } - IRQ_ATTACH(KINETIS_IRQ_CAN0MB, can0_irq); + { \ + const int res = irq_attach(irq, handler, NULL); \ + (void)res; \ + assert(res >= 0); \ + up_enable_irq(irq); \ + } + IRQ_ATTACH(KINETIS_IRQ_CAN0MB, can0_irq); #if UAVCAN_KINETIS_NUM_IFACES > 1 - IRQ_ATTACH(KINETIS_IRQ_CAN1MB, can1_irq); + IRQ_ATTACH(KINETIS_IRQ_CAN1MB, can1_irq); #endif #undef IRQ_ATTACH } int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) { - int res = 0; + int res = 0; - UAVCAN_KINETIS_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + UAVCAN_KINETIS_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); - static bool initialized_once = false; - if (!initialized_once) - { - initialized_once = true; - UAVCAN_KINETIS_LOG("First initialization"); - initOnce(); - } + static bool initialized_once = false; - /* - * CAN1 - */ - UAVCAN_KINETIS_LOG("Initing iface 0..."); - ifaces[0] = &if0_; // This link must be initialized first, - res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; - if (res < 0) // a typical race condition. - { - UAVCAN_KINETIS_LOG("Iface 0 init failed %i", res); - ifaces[0] = UAVCAN_NULLPTR; - goto fail; - } + if (!initialized_once) { + initialized_once = true; + UAVCAN_KINETIS_LOG("First initialization"); + initOnce(); + } - /* - * CAN2 - */ + /* + * CAN1 + */ + UAVCAN_KINETIS_LOG("Initing iface 0..."); + ifaces[0] = &if0_; // This link must be initialized first, + res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; + + if (res < 0) { // a typical race condition. + UAVCAN_KINETIS_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + + /* + * CAN2 + */ #if UAVCAN_KINETIS_NUM_IFACES > 1 - UAVCAN_KINETIS_LOG("Initing iface 1..."); - ifaces[1] = &if1_; // Same thing here. - res = if1_.init(bitrate, mode); - if (res < 0) - { - UAVCAN_KINETIS_LOG("Iface 1 init failed %i", res); - ifaces[1] = UAVCAN_NULLPTR; - goto fail; - } + UAVCAN_KINETIS_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + + if (res < 0) { + UAVCAN_KINETIS_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } + #endif - UAVCAN_KINETIS_LOG("CAN drv init OK"); - UAVCAN_ASSERT(res >= 0); - return res; + UAVCAN_KINETIS_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; fail: - UAVCAN_KINETIS_LOG("CAN drv init failed %i", res); - UAVCAN_ASSERT(res < 0); - return res; + UAVCAN_KINETIS_LOG("CAN drv init failed %i", res); + UAVCAN_ASSERT(res < 0); + return res; } -CanIface* CanDriver::getIface(uavcan::uint8_t iface_index) +CanIface *CanDriver::getIface(uavcan::uint8_t iface_index) { - if (iface_index < UAVCAN_KINETIS_NUM_IFACES) - { - return ifaces[iface_index]; - } - return UAVCAN_NULLPTR; + if (iface_index < UAVCAN_KINETIS_NUM_IFACES) { + return ifaces[iface_index]; + } + + return UAVCAN_NULLPTR; } bool CanDriver::hadActivity() { - bool ret = if0_.hadActivity(); + bool ret = if0_.hadActivity(); #if UAVCAN_KINETIS_NUM_IFACES > 1 - ret |= if1_.hadActivity(); + ret |= if1_.hadActivity(); #endif - return ret; + return ret; } @@ -1022,54 +994,56 @@ bool CanDriver::hadActivity() extern "C" { -static int can0_irq(const int irq, void*, void* args) -{ + static int can0_irq(const int irq, void *, void *args) + { - CanIface * cif = ifaces[0]; - flexcan::CanType* flex = flexcan::Can[0]; + CanIface *cif = ifaces[0]; + flexcan::CanType *flex = flexcan::Can[0]; - if (cif != UAVCAN_NULLPTR) - { - const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; - uavcan::uint32_t flags = flex->IFLAG1 & FIFO_IFLAG1; - if (flags) - { - handleRxInterrupt(cif, flags); - } - const uavcan::uint32_t MB_IFLAG1 = flexcan::TXMBMask; - flags = flex->IFLAG1 & MB_IFLAG1; - if (flags) - { - handleTxInterrupt(cif, flags); - } - } - return 0; -} + if (cif != UAVCAN_NULLPTR) { + const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; + uavcan::uint32_t flags = flex->IFLAG1 & FIFO_IFLAG1; + + if (flags) { + handleRxInterrupt(cif, flags); + } + + const uavcan::uint32_t MB_IFLAG1 = flexcan::TXMBMask; + flags = flex->IFLAG1 & MB_IFLAG1; + + if (flags) { + handleTxInterrupt(cif, flags); + } + } + + return 0; + } #if UAVCAN_KINETIS_NUM_IFACES > 1 -static int can1_irq(const int irq, void*, void*) -{ - CanIface * cif = ifaces[1]; - flexcan::CanType* flex = flexcan::Can[1]; + static int can1_irq(const int irq, void *, void *) + { + CanIface *cif = ifaces[1]; + flexcan::CanType *flex = flexcan::Can[1]; - if (cif != UAVCAN_NULLPTR) - { - const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; - uavcan::uint32_t flags = flex->IFLAG1 & FIFO_IFLAG1; - if (flags) - { - handleRxInterrupt(cif, flags); - } - const uavcan::uint32_t MB_IFLAG1 = flexcan::TXMBMask; - flags = flex->IFLAG1 & MB_IFLAG1; - if (flags) - { - handleTxInterrupt(cif, flags); - } - } - return 0; -} + if (cif != UAVCAN_NULLPTR) { + const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; + uavcan::uint32_t flags = flex->IFLAG1 & FIFO_IFLAG1; + + if (flags) { + handleRxInterrupt(cif, flags); + } + + const uavcan::uint32_t MB_IFLAG1 = flexcan::TXMBMask; + flags = flex->IFLAG1 & MB_IFLAG1; + + if (flags) { + handleTxInterrupt(cif, flags); + } + } + + return 0; + } #endif } // extern "C" diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp index 071044b5f3..a64826801c 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp @@ -11,50 +11,53 @@ namespace uavcan_kinetis { -BusEvent::BusEvent(CanDriver& can_driver) +BusEvent::BusEvent(CanDriver &can_driver) { - sem_init(&sem_, 0, 0); - sem_setprotocol(&sem_, SEM_PRIO_NONE); + sem_init(&sem_, 0, 0); + sem_setprotocol(&sem_, SEM_PRIO_NONE); } BusEvent::~BusEvent() { - sem_destroy(&sem_); + sem_destroy(&sem_); } bool BusEvent::wait(uavcan::MonotonicDuration duration) { - if (duration.isPositive()) { - timespec abstime; + if (duration.isPositive()) { + timespec abstime; - if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { - const unsigned billion = 1000 * 1000 * 1000; - uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; - abstime.tv_sec += nsecs / billion; - nsecs -= (nsecs / billion) * billion; - abstime.tv_nsec = nsecs; + if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { + const unsigned billion = 1000 * 1000 * 1000; + uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; + abstime.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + abstime.tv_nsec = nsecs; - int ret; - while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); - if (ret == -1) { // timed out or error - return false; - } - return true; - } - } - return false; + int ret; + + while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); + + if (ret == -1) { // timed out or error + return false; + } + + return true; + } + } + + return false; } void BusEvent::signalFromInterrupt() { - if (sem_.semcount <= 0) - { - (void)sem_post(&sem_); - } - if (signal_cb_) - { - signal_cb_(); - } + if (sem_.semcount <= 0) { + (void)sem_post(&sem_); + } + + if (signal_cb_) { + signal_cb_(); + } } } diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp index 7e4679443a..c4933bf644 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp @@ -24,262 +24,257 @@ namespace uavcan_stm32 namespace bxcan { -struct TxMailboxType -{ - volatile uint32_t TIR; - volatile uint32_t TDTR; - volatile uint32_t TDLR; - volatile uint32_t TDHR; +struct TxMailboxType { + volatile uint32_t TIR; + volatile uint32_t TDTR; + volatile uint32_t TDLR; + volatile uint32_t TDHR; }; -struct RxMailboxType -{ - volatile uint32_t RIR; - volatile uint32_t RDTR; - volatile uint32_t RDLR; - volatile uint32_t RDHR; +struct RxMailboxType { + volatile uint32_t RIR; + volatile uint32_t RDTR; + volatile uint32_t RDLR; + volatile uint32_t RDHR; }; -struct FilterRegisterType -{ - volatile uint32_t FR1; - volatile uint32_t FR2; +struct FilterRegisterType { + volatile uint32_t FR1; + volatile uint32_t FR2; }; -struct CanType -{ - volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */ - volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */ - volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */ - volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */ - volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */ - volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */ - volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */ - volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */ - uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */ - TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */ - RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */ - uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */ - volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */ - volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */ - uint32_t RESERVED2; /*!< Reserved, 0x208 */ - volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */ - uint32_t RESERVED3; /*!< Reserved, 0x210 */ - volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */ - uint32_t RESERVED4; /*!< Reserved, 0x218 */ - volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */ - uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */ - FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */ +struct CanType { + volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */ + volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */ + volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */ + volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */ + volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */ + volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */ + volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */ + volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */ + uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */ + TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */ + RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */ + uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */ + volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */ + volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */ + uint32_t RESERVED2; /*!< Reserved, 0x208 */ + volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */ + uint32_t RESERVED3; /*!< Reserved, 0x210 */ + volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */ + uint32_t RESERVED4; /*!< Reserved, 0x218 */ + volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */ + uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */ + FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */ }; /** * CANx register sets */ -CanType* const Can[UAVCAN_STM32_NUM_IFACES] = -{ - reinterpret_cast(0x40006400) +CanType *const Can[UAVCAN_STM32_NUM_IFACES] = { + reinterpret_cast(0x40006400) #if UAVCAN_STM32_NUM_IFACES > 1 - , - reinterpret_cast(0x40006800) + , + reinterpret_cast(0x40006800) #endif }; /* CAN master control register */ -constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */ -constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */ -constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */ -constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */ -constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */ -constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */ -constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */ -constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */ -constexpr unsigned long MCR_RESET = (1U << 15);/* Bit 15: bxCAN software master reset */ -constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */ +constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */ +constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */ +constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */ +constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */ +constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */ +constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */ +constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */ +constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */ +constexpr unsigned long MCR_RESET = (1U << 15); /* Bit 15: bxCAN software master reset */ +constexpr unsigned long MCR_DBF = (1U << 16); /* Bit 16: Debug freeze */ /* CAN master status register */ -constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */ -constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */ -constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */ -constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */ -constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */ -constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */ -constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */ -constexpr unsigned long MSR_SAMP = (1U << 10);/* Bit 10: Last Sample Point */ -constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */ +constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */ +constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */ +constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */ +constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */ +constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */ +constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */ +constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */ +constexpr unsigned long MSR_SAMP = (1U << 10); /* Bit 10: Last Sample Point */ +constexpr unsigned long MSR_RX = (1U << 11); /* Bit 11: CAN Rx Signal */ /* CAN transmit status register */ -constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */ -constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */ -constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */ -constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */ -constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */ -constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */ -constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */ -constexpr unsigned long TSR_ALST1 = (1U << 10);/* Bit 10 : Arbitration Lost for Mailbox 1 */ -constexpr unsigned long TSR_TERR1 = (1U << 11);/* Bit 11 : Transmission Error of Mailbox 1 */ -constexpr unsigned long TSR_ABRQ1 = (1U << 15);/* Bit 15 : Abort Request for Mailbox 1 */ -constexpr unsigned long TSR_RQCP2 = (1U << 16);/* Bit 16 : Request Completed Mailbox 2 */ -constexpr unsigned long TSR_TXOK2 = (1U << 17);/* Bit 17 : Transmission OK of Mailbox 2 */ -constexpr unsigned long TSR_ALST2 = (1U << 18);/* Bit 18: Arbitration Lost for Mailbox 2 */ -constexpr unsigned long TSR_TERR2 = (1U << 19);/* Bit 19: Transmission Error of Mailbox 2 */ -constexpr unsigned long TSR_ABRQ2 = (1U << 23);/* Bit 23: Abort Request for Mailbox 2 */ -constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */ -constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT); -constexpr unsigned long TSR_TME0 = (1U << 26);/* Bit 26: Transmit Mailbox 0 Empty */ -constexpr unsigned long TSR_TME1 = (1U << 27);/* Bit 27: Transmit Mailbox 1 Empty */ -constexpr unsigned long TSR_TME2 = (1U << 28);/* Bit 28: Transmit Mailbox 2 Empty */ -constexpr unsigned long TSR_LOW0 = (1U << 29);/* Bit 29: Lowest Priority Flag for Mailbox 0 */ -constexpr unsigned long TSR_LOW1 = (1U << 30);/* Bit 30: Lowest Priority Flag for Mailbox 1 */ -constexpr unsigned long TSR_LOW2 = (1U << 31);/* Bit 31: Lowest Priority Flag for Mailbox 2 */ +constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */ +constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */ +constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */ +constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */ +constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */ +constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */ +constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */ +constexpr unsigned long TSR_ALST1 = (1U << 10); /* Bit 10 : Arbitration Lost for Mailbox 1 */ +constexpr unsigned long TSR_TERR1 = (1U << 11); /* Bit 11 : Transmission Error of Mailbox 1 */ +constexpr unsigned long TSR_ABRQ1 = (1U << 15); /* Bit 15 : Abort Request for Mailbox 1 */ +constexpr unsigned long TSR_RQCP2 = (1U << 16); /* Bit 16 : Request Completed Mailbox 2 */ +constexpr unsigned long TSR_TXOK2 = (1U << 17); /* Bit 17 : Transmission OK of Mailbox 2 */ +constexpr unsigned long TSR_ALST2 = (1U << 18); /* Bit 18: Arbitration Lost for Mailbox 2 */ +constexpr unsigned long TSR_TERR2 = (1U << 19); /* Bit 19: Transmission Error of Mailbox 2 */ +constexpr unsigned long TSR_ABRQ2 = (1U << 23); /* Bit 23: Abort Request for Mailbox 2 */ +constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */ +constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT); +constexpr unsigned long TSR_TME0 = (1U << 26); /* Bit 26: Transmit Mailbox 0 Empty */ +constexpr unsigned long TSR_TME1 = (1U << 27); /* Bit 27: Transmit Mailbox 1 Empty */ +constexpr unsigned long TSR_TME2 = (1U << 28); /* Bit 28: Transmit Mailbox 2 Empty */ +constexpr unsigned long TSR_LOW0 = (1U << 29); /* Bit 29: Lowest Priority Flag for Mailbox 0 */ +constexpr unsigned long TSR_LOW1 = (1U << 30); /* Bit 30: Lowest Priority Flag for Mailbox 1 */ +constexpr unsigned long TSR_LOW2 = (1U << 31); /* Bit 31: Lowest Priority Flag for Mailbox 2 */ /* CAN receive FIFO 0/1 registers */ -constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */ -constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT); -constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */ -constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */ -constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */ +constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */ +constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT); +constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */ +constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */ +constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */ /* CAN interrupt enable register */ -constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */ -constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */ -constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */ -constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */ -constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */ -constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */ -constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */ -constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */ -constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */ -constexpr unsigned long IER_BOFIE = (1U << 10);/* Bit 10: Bus-Off Interrupt Enable */ -constexpr unsigned long IER_LECIE = (1U << 11);/* Bit 11: Last Error Code Interrupt Enable */ -constexpr unsigned long IER_ERRIE = (1U << 15);/* Bit 15: Error Interrupt Enable */ -constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup Interrupt Enable */ -constexpr unsigned long IER_SLKIE = (1U << 17);/* Bit 17: Sleep Interrupt Enable */ +constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */ +constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */ +constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */ +constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */ +constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */ +constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */ +constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */ +constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */ +constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */ +constexpr unsigned long IER_BOFIE = (1U << 10); /* Bit 10: Bus-Off Interrupt Enable */ +constexpr unsigned long IER_LECIE = (1U << 11); /* Bit 11: Last Error Code Interrupt Enable */ +constexpr unsigned long IER_ERRIE = (1U << 15); /* Bit 15: Error Interrupt Enable */ +constexpr unsigned long IER_WKUIE = (1U << 16); /* Bit 16: Wakeup Interrupt Enable */ +constexpr unsigned long IER_SLKIE = (1U << 17); /* Bit 17: Sleep Interrupt Enable */ /* CAN error status register */ -constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */ -constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */ -constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */ -constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */ -constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT); -constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT);/* 000: No Error */ -constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff Error */ -constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT);/* 010: Form Error */ -constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment Error */ -constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT);/* 100: Bit recessive Error */ -constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT);/* 101: Bit dominant Error */ -constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT);/* 110: CRC Error */ -constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT);/* 111: Set by software */ -constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */ -constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT); -constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */ -constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT); +constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */ +constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */ +constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */ +constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */ +constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT); +constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT); /* 000: No Error */ +constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT); /* 001: Stuff Error */ +constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT); /* 010: Form Error */ +constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT); /* 011: Acknowledgment Error */ +constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT); /* 100: Bit recessive Error */ +constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT); /* 101: Bit dominant Error */ +constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT); /* 110: CRC Error */ +constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT); /* 111: Set by software */ +constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */ +constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT); +constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */ +constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT); /* CAN bit timing register */ -constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */ -constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT); -constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */ -constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT); -constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */ -constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT); -constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */ -constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT); -constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/ -constexpr unsigned long BTR_SILM = (1U << 31);/* Bit 31: Silent Mode (Debug);*/ +constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */ +constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT); +constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */ +constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT); +constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */ +constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT); +constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */ +constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT); +constexpr unsigned long BTR_LBKM = (1U << 30); /* Bit 30: Loop Back Mode (Debug);*/ +constexpr unsigned long BTR_SILM = (1U << 31); /* Bit 31: Silent Mode (Debug);*/ -constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/ -constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/ -constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/ +constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/ +constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/ +constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/ /* TX mailbox identifier register */ -constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */ -constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ -constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ -constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ -constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT); -constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ -constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT); +constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */ +constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ +constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ +constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ +constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT); +constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ +constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT); /* Mailbox data length control and time stamp register */ -constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */ -constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT); -constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */ -constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */ -constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT); +constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */ +constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT); +constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */ +constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */ +constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT); /* Mailbox data low register */ -constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ -constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT); -constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ -constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT); -constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ -constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT); -constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ -constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT); +constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ +constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT); +constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ +constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT); +constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ +constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT); +constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ +constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT); /* Mailbox data high register */ -constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ -constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT); -constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ -constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT); -constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ -constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT); -constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ -constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT); +constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ +constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT); +constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ +constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT); +constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ +constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT); +constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ +constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT); /* Rx FIFO mailbox identifier register */ -constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ -constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ -constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ -constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT); -constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ -constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT); +constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ +constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ +constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ +constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT); +constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ +constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT); /* Receive FIFO mailbox data length control and time stamp register */ -constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */ -constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT); -constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */ -constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT); -constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */ -constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT); +constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */ +constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT); +constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */ +constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT); +constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */ +constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT); /* Receive FIFO mailbox data low register */ -constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ -constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT); -constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ -constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT); -constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ -constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT); -constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ -constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT); +constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ +constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT); +constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ +constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT); +constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ +constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT); +constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ +constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT); /* Receive FIFO mailbox data high register */ -constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ -constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT); -constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ -constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT); -constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ -constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT); -constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ -constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT); +constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ +constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT); +constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ +constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT); +constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ +constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT); +constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ +constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT); /* CAN filter master register */ -constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */ +constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */ } } diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 1fd21e829a..10ed09840b 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -29,15 +29,14 @@ static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of f * RX queue item. * The application shall not use this directly. */ -struct CanRxItem -{ - uavcan::uint64_t utc_usec; - uavcan::CanFrame frame; - uavcan::CanIOFlags flags; - CanRxItem() - : utc_usec(0) - , flags(0) - { } +struct CanRxItem { + uavcan::uint64_t utc_usec; + uavcan::CanFrame frame; + uavcan::CanIOFlags flags; + CanRxItem() + : utc_usec(0) + , flags(0) + { } }; /** @@ -46,185 +45,182 @@ struct CanRxItem */ 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_; + 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(); + void registerOverflow(); - public: - RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) - : buf_(buf) - , capacity_(capacity) - , in_(0) - , out_(0) - , len_(0) - , overflow_cnt_(0) - { } + 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); + 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); - void reset(); + void reset(); - unsigned getLength() const { return len_; } + unsigned getLength() const { return len_; } - uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; } - }; + 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; + 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) - { } - }; + Timings() + : prescaler(0) + , sjw(0) + , bs1(0) + , bs2(0) + { } + }; - struct TxItem - { - uavcan::MonotonicTime deadline; - uavcan::CanFrame frame; - bool pending; - bool loopback; - bool abort_on_error; + struct TxItem { + uavcan::MonotonicTime deadline; + uavcan::CanFrame frame; + bool pending; + bool loopback; + bool abort_on_error; - TxItem() - : pending(false) - , loopback(false) - , abort_on_error(false) - { } - }; + TxItem() + : pending(false) + , loopback(false) + , abort_on_error(false) + { } + }; - enum { NumTxMailboxes = 3 }; - enum { NumFilters = 14 }; + enum { NumTxMailboxes = 3 }; + enum { NumFilters = 14 }; - static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]; + static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]; - RxQueue rx_queue_; - bxcan::CanType* const can_; - uavcan::uint64_t error_cnt_; - uavcan::uint32_t served_aborts_cnt_; - BusEvent& update_event_; - TxItem pending_tx_[NumTxMailboxes]; - uavcan::uint8_t peak_tx_mailbox_index_; - const uavcan::uint8_t self_index_; - bool had_activity_; + RxQueue rx_queue_; + bxcan::CanType *const can_; + uavcan::uint64_t error_cnt_; + uavcan::uint32_t served_aborts_cnt_; + BusEvent &update_event_; + TxItem pending_tx_[NumTxMailboxes]; + uavcan::uint8_t peak_tx_mailbox_index_; + const uavcan::uint8_t self_index_; + bool had_activity_; - int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings); + int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings); - virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags); + virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags); - virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags); + virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags); - virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, - uavcan::uint16_t num_configs); + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs, + uavcan::uint16_t num_configs); - virtual uavcan::uint16_t getNumFilters() const { return NumFilters; } + virtual uavcan::uint16_t getNumFilters() const { return NumFilters; } - void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec); + void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec); - bool waitMsrINakBitStateChange(bool target_state); + bool waitMsrINakBitStateChange(bool target_state); public: - enum { MaxRxQueueCapacity = 254 }; + enum { MaxRxQueueCapacity = 254 }; - enum OperatingMode - { - NormalMode, - SilentMode - }; + enum OperatingMode { + NormalMode, + SilentMode + }; - CanIface(bxcan::CanType* can, BusEvent& 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) - , served_aborts_cnt_(0) - , update_event_(update_event) - , peak_tx_mailbox_index_(0) - , self_index_(self_index) - , had_activity_(false) - { - UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES); - } + CanIface(bxcan::CanType *can, BusEvent &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) + , served_aborts_cnt_(0) + , update_event_(update_event) + , peak_tx_mailbox_index_(0) + , self_index_(self_index) + , had_activity_(false) + { + UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES); + } - /** - * Initializes the hardware CAN controller. - * Assumes: - * - Iface clock is enabled - * - Iface has been resetted via RCC - * - Caller will configure NVIC by itself - */ - int init(const uavcan::uint32_t bitrate, const OperatingMode mode); + /** + * Initializes the hardware CAN controller. + * Assumes: + * - Iface clock is enabled + * - Iface has been resetted via RCC + * - Caller will configure NVIC by itself + */ + int init(const uavcan::uint32_t bitrate, const OperatingMode mode); - void handleTxInterrupt(uavcan::uint64_t utc_usec); - void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec); + void handleTxInterrupt(uavcan::uint64_t utc_usec); + void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec); - /** - * This method is used to count errors and abort transmission on error if necessary. - * This functionality used to be implemented in the SCE interrupt handler, but that approach was - * generating too much processing overhead, especially on disconnected interfaces. - * - * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. - */ - void pollErrorFlagsFromISR(); + /** + * This method is used to count errors and abort transmission on error if necessary. + * This functionality used to be implemented in the SCE interrupt handler, but that approach was + * generating too much processing overhead, especially on disconnected interfaces. + * + * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. + */ + void pollErrorFlagsFromISR(); - void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); + void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); - bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; - bool isRxBufferEmpty() const; + bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const; + bool isRxBufferEmpty() const; - /** - * Number of RX frames lost due to queue overflow. - * This is an atomic read, it doesn't require a critical section. - */ - uavcan::uint32_t getRxQueueOverflowCount() const { return rx_queue_.getOverflowCount(); } + /** + * Number of RX frames lost due to queue overflow. + * This is an atomic read, it doesn't require a critical section. + */ + uavcan::uint32_t getRxQueueOverflowCount() const { return rx_queue_.getOverflowCount(); } - /** - * Total number of hardware failures and other kinds of errors (e.g. queue overruns). - * May increase continuously if the interface is not connected to the bus. - */ - virtual uavcan::uint64_t getErrorCount() const; + /** + * Total number of hardware failures and other kinds of errors (e.g. queue overruns). + * May increase continuously if the interface is not connected to the bus. + */ + virtual uavcan::uint64_t getErrorCount() const; - /** - * Number of times the driver exercised library's requirement to abort transmission on first error. - * This is an atomic read, it doesn't require a critical section. - * See @ref uavcan::CanIOFlagAbortOnError. - */ - uavcan::uint32_t getVoluntaryTxAbortCount() const { return served_aborts_cnt_; } + /** + * Number of times the driver exercised library's requirement to abort transmission on first error. + * This is an atomic read, it doesn't require a critical section. + * See @ref uavcan::CanIOFlagAbortOnError. + */ + uavcan::uint32_t getVoluntaryTxAbortCount() const { return served_aborts_cnt_; } - /** - * Returns the number of frames pending in the RX queue. - * This is intended for debug use only. - */ - unsigned getRxQueueLength() const; + /** + * Returns the number of frames pending in the RX queue. + * This is intended for debug use only. + */ + unsigned getRxQueueLength() const; - /** - * Whether this iface had at least one successful IO since the previous call of this method. - * This is designed for use with iface activity LEDs. - */ - bool hadActivity(); + /** + * Whether this iface had at least one successful IO since the previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); - /** - * Peak number of TX mailboxes used concurrently since initialization. - * Range is [1, 3]. - * Value of 3 suggests that priority inversion could be taking place. - */ - uavcan::uint8_t getPeakNumTxMailboxesUsed() const { return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); } + /** + * Peak number of TX mailboxes used concurrently since initialization. + * Range is [1, 3]. + * Value of 3 suggests that priority inversion could be taking place. + */ + uavcan::uint8_t getPeakNumTxMailboxesUsed() const { return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); } }; /** @@ -233,57 +229,57 @@ public: */ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable { - BusEvent update_event_; - CanIface if0_; + BusEvent update_event_; + CanIface if0_; #if UAVCAN_STM32_NUM_IFACES > 1 - CanIface if1_; + CanIface if1_; #endif - virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], - uavcan::MonotonicTime blocking_deadline); + virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline); - static void initOnce(); + static void initOnce(); public: - template - CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) - : update_event_(*this) - , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) + template + CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) + : update_event_(*this) + , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) #if UAVCAN_STM32_NUM_IFACES > 1 - , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) + , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) #endif - { - uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); - } + { + uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check(); + } - /** - * This function returns select masks indicating which interfaces are available for read/write. - */ - uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const; + /** + * This function returns select masks indicating which interfaces are available for read/write. + */ + uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces]) const; - /** - * Whether there's at least one interface where receive() would return a frame. - */ - bool hasReadableInterfaces() const; + /** + * Whether there's at least one interface where receive() would return a frame. + */ + bool hasReadableInterfaces() const; - /** - * Returns zero if OK. - * Returns negative value if failed (e.g. invalid bitrate). - */ - int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); + /** + * Returns zero if OK. + * Returns negative value if failed (e.g. invalid bitrate). + */ + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); - virtual CanIface* getIface(uavcan::uint8_t iface_index); + virtual CanIface *getIface(uavcan::uint8_t iface_index); - virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; } + virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; } - /** - * Whether at least one iface had at least one successful IO since previous call of this method. - * This is designed for use with iface activity LEDs. - */ - bool hadActivity(); + /** + * Whether at least one iface had at least one successful IO since previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); - BusEvent& updateEvent() { return update_event_; } + BusEvent &updateEvent() { return update_event_; } }; /** @@ -294,91 +290,84 @@ public: template class CanInitHelper { - CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]; + CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]; public: - enum { BitRateAutoDetect = 0 }; + enum { BitRateAutoDetect = 0 }; - CanDriver driver; + CanDriver driver; - CanInitHelper() : - driver(queue_storage_) - { } + CanInitHelper() : + driver(queue_storage_) + { } - /** - * This overload simply configures the provided bitrate. - * Auto bit rate detection will not be performed. - * Bitrate value must be positive. - * @return Negative value on error; non-negative on success. Refer to constants Err*. - */ - int init(uavcan::uint32_t bitrate) - { - return driver.init(bitrate, CanIface::NormalMode); - } + /** + * This overload simply configures the provided bitrate. + * Auto bit rate detection will not be performed. + * Bitrate value must be positive. + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + int init(uavcan::uint32_t bitrate) + { + return driver.init(bitrate, CanIface::NormalMode); + } - /** - * This function can either initialize the driver at a fixed bit rate, or it can perform - * automatic bit rate detection. For theory please refer to the CiA application note #801. - * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. - * If auto detection was used, the function will update the argument - * with established bit rate. In case of an error the value will be undefined. - * - * @return Negative value on error; non-negative on success. Refer to constants Err*. - */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect) - { - if (inout_bitrate > 0) - { - return driver.init(inout_bitrate, CanIface::NormalMode); - } - else - { - static const uavcan::uint32_t StandardBitRates[] = - { - 1000000, - 500000, - 250000, - 125000 - }; + /** + * This function can either initialize the driver at a fixed bit rate, or it can perform + * automatic bit rate detection. For theory please refer to the CiA application note #801. + * + * @param delay_callable A callable entity that suspends execution for strictly more than one second. + * The callable entity will be invoked without arguments. + * @ref getRecommendedListeningDelay(). + * + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * If auto detection was used, the function will update the argument + * with established bit rate. In case of an error the value will be undefined. + * + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + template + int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + { + if (inout_bitrate > 0) { + return driver.init(inout_bitrate, CanIface::NormalMode); - for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) - { - inout_bitrate = StandardBitRates[br]; + } else { + static const uavcan::uint32_t StandardBitRates[] = { + 1000000, + 500000, + 250000, + 125000 + }; - const int res = driver.init(inout_bitrate, CanIface::SilentMode); + for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) { + inout_bitrate = StandardBitRates[br]; - delay_callable(); + const int res = driver.init(inout_bitrate, CanIface::SilentMode); - if (res >= 0) - { - for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) - { - if (!driver.getIface(iface)->isRxBufferEmpty()) - { - // Re-initializing in normal mode - return driver.init(inout_bitrate, CanIface::NormalMode); - } - } - } - } + delay_callable(); - return -ErrBitRateNotDetected; - } - } + if (res >= 0) { + for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { + if (!driver.getIface(iface)->isRxBufferEmpty()) { + // Re-initializing in normal mode + return driver.init(inout_bitrate, CanIface::NormalMode); + } + } + } + } - /** - * Use this value for listening delay during automatic bit rate detection. - */ - static uavcan::MonotonicDuration getRecommendedListeningDelay() - { - return uavcan::MonotonicDuration::fromMSec(1050); - } + return -ErrBitRateNotDetected; + } + } + + /** + * Use this value for listening delay during automatic bit rate detection. + */ + static uavcan::MonotonicDuration getRecommendedListeningDelay() + { + return uavcan::MonotonicDuration::fromMSec(1050); + } }; } diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp index 1f422f02dd..fcfff84da2 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -48,25 +48,24 @@ void adjustUtc(uavcan::UtcDuration adjustment); /** * UTC clock synchronization parameters */ -struct UtcSyncParams -{ - float offset_p; ///< PPM per one usec error - float rate_i; ///< PPM per one PPM error for second - float rate_error_corner_freq; - float max_rate_correction_ppm; - float lock_thres_rate_ppm; - uavcan::UtcDuration lock_thres_offset; - uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate +struct UtcSyncParams { + float offset_p; ///< PPM per one usec error + float rate_i; ///< PPM per one PPM error for second + float rate_error_corner_freq; + float max_rate_correction_ppm; + float lock_thres_rate_ppm; + uavcan::UtcDuration lock_thres_offset; + uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate - UtcSyncParams() - : offset_p(0.01F) - , rate_i(0.02F) - , rate_error_corner_freq(0.01F) - , max_rate_correction_ppm(300.0F) - , lock_thres_rate_ppm(2.0F) - , lock_thres_offset(uavcan::UtcDuration::fromMSec(4)) - , min_jump(uavcan::UtcDuration::fromMSec(10)) - { } + UtcSyncParams() + : offset_p(0.01F) + , rate_i(0.02F) + , rate_error_corner_freq(0.01F) + , max_rate_correction_ppm(300.0F) + , lock_thres_rate_ppm(2.0F) + , lock_thres_offset(uavcan::UtcDuration::fromMSec(4)) + , min_jump(uavcan::UtcDuration::fromMSec(10)) + { } }; /** @@ -94,7 +93,7 @@ bool isUtcLocked(); * Both functions are thread safe. */ UtcSyncParams getUtcSyncParams(); -void setUtcSyncParams(const UtcSyncParams& params); +void setUtcSyncParams(const UtcSyncParams ¶ms); } @@ -103,19 +102,19 @@ void setUtcSyncParams(const UtcSyncParams& params); */ class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable { - SystemClock() { } + SystemClock() { } - virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); } + virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); } public: - virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); } - virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); } + virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); } + virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); } - /** - * Calls clock::init() as needed. - * This function is thread safe. - */ - static SystemClock& instance(); + /** + * Calls clock::init() as needed. + * This function is thread safe. + */ + static SystemClock &instance(); }; } diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index 96bd0fdee4..18234ae1d4 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -32,69 +32,69 @@ class CanDriver; */ class BusEvent : uavcan::Noncopyable { - using SignalCallbackHandler = void(*)(); + using SignalCallbackHandler = void(*)(); - SignalCallbackHandler signal_cb_{nullptr}; - sem_t sem_; + SignalCallbackHandler signal_cb_{nullptr}; + sem_t sem_; public: - BusEvent(CanDriver& can_driver); - ~BusEvent(); + BusEvent(CanDriver &can_driver); + ~BusEvent(); - void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } + void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } - bool wait(uavcan::MonotonicDuration duration); + bool wait(uavcan::MonotonicDuration duration); - void signalFromInterrupt(); + void signalFromInterrupt(); }; class Mutex { - pthread_mutex_t mutex_; + pthread_mutex_t mutex_; public: - Mutex() - { - init(); - } + Mutex() + { + init(); + } - int init() - { - return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); - } + int init() + { + return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); + } - int deinit() - { - return pthread_mutex_destroy(&mutex_); - } + int deinit() + { + return pthread_mutex_destroy(&mutex_); + } - void lock() - { - (void)pthread_mutex_lock(&mutex_); - } + void lock() + { + (void)pthread_mutex_lock(&mutex_); + } - void unlock() - { - (void)pthread_mutex_unlock(&mutex_); - } + void unlock() + { + (void)pthread_mutex_unlock(&mutex_); + } }; #endif class MutexLocker { - Mutex& mutex_; + Mutex &mutex_; public: - MutexLocker(Mutex& mutex) - : mutex_(mutex) - { - mutex_.lock(); - } - ~MutexLocker() - { - mutex_.unlock(); - } + MutexLocker(Mutex &mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } }; } diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp index a5cfe6af1b..bc3350ccbd 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp @@ -50,18 +50,17 @@ namespace uavcan_stm32 { #if UAVCAN_STM32_NUTTX -struct CriticalSectionLocker -{ - const irqstate_t flags_; +struct CriticalSectionLocker { + const irqstate_t flags_; - CriticalSectionLocker() - : flags_(enter_critical_section()) - { } + CriticalSectionLocker() + : flags_(enter_critical_section()) + { } - ~CriticalSectionLocker() - { - leave_critical_section(flags_); - } + ~CriticalSectionLocker() + { + leave_critical_section(flags_); + } }; #endif diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp index 1b4216b1d8..2a5785cb11 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp @@ -23,9 +23,9 @@ # endif extern "C" { -static int can1_irq(const int irq, void*, void*); + static int can1_irq(const int irq, void *, void *); #if UAVCAN_STM32_NUM_IFACES > 1 -static int can2_irq(const int irq, void*, void*); + static int can2_irq(const int irq, void *, void *); #endif } #endif @@ -45,48 +45,45 @@ namespace uavcan_stm32 namespace { -CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] = -{ - UAVCAN_NULLPTR +CanIface *ifaces[UAVCAN_STM32_NUM_IFACES] = { + UAVCAN_NULLPTR #if UAVCAN_STM32_NUM_IFACES > 1 - , UAVCAN_NULLPTR + , UAVCAN_NULLPTR #endif }; inline void handleTxInterrupt(uavcan::uint8_t iface_index) { - UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); - uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); - if (utc_usec > 0) - { - utc_usec--; - } - if (ifaces[iface_index] != UAVCAN_NULLPTR) - { - ifaces[iface_index]->handleTxInterrupt(utc_usec); - } - else - { - UAVCAN_ASSERT(0); - } + UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + if (utc_usec > 0) { + utc_usec--; + } + + if (ifaces[iface_index] != UAVCAN_NULLPTR) { + ifaces[iface_index]->handleTxInterrupt(utc_usec); + + } else { + UAVCAN_ASSERT(0); + } } inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index) { - UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); - uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); - if (utc_usec > 0) - { - utc_usec--; - } - if (ifaces[iface_index] != UAVCAN_NULLPTR) - { - ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec); - } - else - { - UAVCAN_ASSERT(0); - } + UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + if (utc_usec > 0) { + utc_usec--; + } + + if (ifaces[iface_index] != UAVCAN_NULLPTR) { + ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec); + + } else { + UAVCAN_ASSERT(0); + } } } // namespace @@ -96,766 +93,732 @@ inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_ */ void CanIface::RxQueue::registerOverflow() { - if (overflow_cnt_ < 0xFFFFFFFF) - { - overflow_cnt_++; - } + if (overflow_cnt_ < 0xFFFFFFFF) { + overflow_cnt_++; + } } -void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags) +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; - } - } + 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) +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 { UAVCAN_ASSERT(0); } + 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 { UAVCAN_ASSERT(0); } } void CanIface::RxQueue::reset() { - in_ = 0; - out_ = 0; - len_ = 0; - overflow_cnt_ = 0; + in_ = 0; + out_ = 0; + len_ = 0; + overflow_cnt_ = 0; } /* * CanIface */ -const uavcan::uint32_t CanIface::TSR_ABRQx[CanIface::NumTxMailboxes] = -{ - bxcan::TSR_ABRQ0, - bxcan::TSR_ABRQ1, - bxcan::TSR_ABRQ2 +const uavcan::uint32_t CanIface::TSR_ABRQx[CanIface::NumTxMailboxes] = { + bxcan::TSR_ABRQ0, + bxcan::TSR_ABRQ1, + bxcan::TSR_ABRQ2 }; -int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings) +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings &out_timings) { - if (target_bitrate < 1) - { - return -ErrInvalidBitRate; - } + if (target_bitrate < 1) { + return -ErrInvalidBitRate; + } - /* - * Hardware configuration - */ + /* + * Hardware configuration + */ #if UAVCAN_STM32_NUTTX - const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY; + const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY; #else # error "Unknown OS" #endif - static const int MaxBS1 = 16; - static const int MaxBS2 = 8; + static const int MaxBS1 = 16; + static const int MaxBS2 = 8; - /* - * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG - * CAN in Automation, 2003 - * - * According to the source, optimal quanta per bit are: - * Bitrate Optimal Maximum - * 1000 kbps 8 10 - * 500 kbps 16 17 - * 250 kbps 16 17 - * 125 kbps 16 17 - */ - const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + /* + * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + * CAN in Automation, 2003 + * + * According to the source, optimal quanta per bit are: + * Bitrate Optimal Maximum + * 1000 kbps 8 10 + * 500 kbps 16 17 + * 250 kbps 16 17 + * 125 kbps 16 17 + */ + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; - UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); + UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); - static const int MaxSamplePointLocation = 900; + static const int MaxSamplePointLocation = 900; - /* - * Computing (prescaler * BS): - * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual - * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified - * let: - * BS = 1 + BS1 + BS2 -- Number of time quanta per bit - * PRESCALER_BS = PRESCALER * BS - * ==> - * PRESCALER_BS = PCLK / BITRATE - */ - const uavcan::uint32_t prescaler_bs = pclk / target_bitrate; + /* + * Computing (prescaler * BS): + * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + * let: + * BS = 1 + BS1 + BS2 -- Number of time quanta per bit + * PRESCALER_BS = PRESCALER * BS + * ==> + * PRESCALER_BS = PCLK / BITRATE + */ + const uavcan::uint32_t prescaler_bs = pclk / target_bitrate; - /* - * Searching for such prescaler value so that the number of quanta per bit is highest. - */ - uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1); + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ + uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1); - while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) - { - if (bs1_bs2_sum <= 2) - { - return -ErrInvalidBitRate; // No solution - } - bs1_bs2_sum--; - } + while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { + if (bs1_bs2_sum <= 2) { + return -ErrInvalidBitRate; // No solution + } - const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - if ((prescaler < 1U) || (prescaler > 1024U)) - { - return -ErrInvalidBitRate; // No solution - } + bs1_bs2_sum--; + } - /* - * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. - * We need to find the values so that the sample point is as close as possible to the optimal value. - * - * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) - * {{bs2 -> (1 + bs1)/7}} - * - * Hence: - * bs2 = (1 + bs1) / 7 - * bs1 = (7 * bs1_bs2_sum - 1) / 8 - * - * Sample point location can be computed as follows: - * Sample point location = (1 + bs1) / (1 + bs1 + bs2) - * - * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: - * - With rounding to nearest - * - With rounding to zero - */ - struct BsPair - { - uavcan::uint8_t bs1; - uavcan::uint8_t bs2; - uavcan::uint16_t sample_point_permill; + const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - BsPair() : - bs1(0), - bs2(0), - sample_point_permill(0) - { } + if ((prescaler < 1U) || (prescaler > 1024U)) { + return -ErrInvalidBitRate; // No solution + } - BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) : - bs1(arg_bs1), - bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)), - sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) - { - UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1); - } + /* + * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + * We need to find the values so that the sample point is as close as possible to the optimal value. + * + * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + * {{bs2 -> (1 + bs1)/7}} + * + * Hence: + * bs2 = (1 + bs1) / 7 + * bs1 = (7 * bs1_bs2_sum - 1) / 8 + * + * Sample point location can be computed as follows: + * Sample point location = (1 + bs1) / (1 + bs1 + bs2) + * + * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + * - With rounding to nearest + * - With rounding to zero + */ + struct BsPair { + uavcan::uint8_t bs1; + uavcan::uint8_t bs2; + uavcan::uint16_t sample_point_permill; - bool isValid() const { return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); } - }; + BsPair() : + bs1(0), + bs2(0), + sample_point_permill(0) + { } - // First attempt with rounding to nearest - BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); + BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) : + bs1(arg_bs1), + bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)), + sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) + { + UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1); + } - if (solution.sample_point_permill > MaxSamplePointLocation) - { - // Second attempt with rounding to zero - solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8)); - } + bool isValid() const { return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); } + }; - /* - * Final validation - * Helpful Python: - * def sample_point_from_btr(x): - * assert 0b0011110010000000111111000000000 & x == 0 - * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 - * return (1+ts1+1)/(1+ts1+1+ts2+1) - * - */ - if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) - { - UAVCAN_ASSERT(0); - return -ErrLogic; - } + // First attempt with rounding to nearest + BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); - UAVCAN_STM32_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%", - int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + if (solution.sample_point_permill > MaxSamplePointLocation) { + // Second attempt with rounding to zero + solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8)); + } - out_timings.prescaler = uavcan::uint16_t(prescaler - 1U); - out_timings.sjw = 0; // Which means one - out_timings.bs1 = uavcan::uint8_t(solution.bs1 - 1); - out_timings.bs2 = uavcan::uint8_t(solution.bs2 - 1); - return 0; + /* + * Final validation + * Helpful Python: + * def sample_point_from_btr(x): + * assert 0b0011110010000000111111000000000 & x == 0 + * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 + * return (1+ts1+1)/(1+ts1+1+ts2+1) + * + */ + if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + UAVCAN_STM32_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%", + int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + + out_timings.prescaler = uavcan::uint16_t(prescaler - 1U); + out_timings.sjw = 0; // Which means one + out_timings.bs1 = uavcan::uint8_t(solution.bs1 - 1); + out_timings.bs2 = uavcan::uint8_t(solution.bs2 - 1); + return 0; } -uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) +uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) { - if (frame.isErrorFrame() || frame.dlc > 8) - { - return -ErrUnsupportedFrame; - } + if (frame.isErrorFrame() || frame.dlc > 8) { + return -ErrUnsupportedFrame; + } - /* - * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because - * it is possible that the highest-priority frame between select() and send() could have been - * replaced with a lower priority one due to TX timeout. But we don't do this check because: - * - * - It is a highly unlikely scenario. - * - * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new - * frame can only have higher priority, which doesn't break the logic. - * - * - If high-priority frames are timing out in the TX queue, there's probably a lot of other - * issues to take care of before this one becomes relevant. - * - * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. - */ - CriticalSectionLocker lock; + /* + * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because + * it is possible that the highest-priority frame between select() and send() could have been + * replaced with a lower priority one due to TX timeout. But we don't do this check because: + * + * - It is a highly unlikely scenario. + * + * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new + * frame can only have higher priority, which doesn't break the logic. + * + * - If high-priority frames are timing out in the TX queue, there's probably a lot of other + * issues to take care of before this one becomes relevant. + * + * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. + */ + CriticalSectionLocker lock; - /* - * Seeking for an empty slot - */ - uavcan::uint8_t txmailbox = 0xFF; - if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) - { - txmailbox = 0; - } - else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) - { - txmailbox = 1; - } - else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) - { - txmailbox = 2; - } - else - { - return 0; // No transmission for you. - } + /* + * Seeking for an empty slot + */ + uavcan::uint8_t txmailbox = 0xFF; - peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics + if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) { + txmailbox = 0; - /* - * Setting up the mailbox - */ - bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox]; - if (frame.isExtended()) - { - mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE; - } - else - { - mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21); - } + } else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) { + txmailbox = 1; - if (frame.isRemoteTransmissionRequest()) - { - mb.TIR |= bxcan::TIR_RTR; - } + } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) { + txmailbox = 2; - mb.TDTR = frame.dlc; + } else { + return 0; // No transmission for you. + } - 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); + peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics - mb.TIR |= bxcan::TIR_TXRQ; // Go. + /* + * Setting up the mailbox + */ + bxcan::TxMailboxType &mb = can_->TxMailbox[txmailbox]; - /* - * 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) != 0; - txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; - txi.pending = true; - return 1; + if (frame.isExtended()) { + mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE; + + } else { + mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21); + } + + if (frame.isRemoteTransmissionRequest()) { + mb.TIR |= bxcan::TIR_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 |= bxcan::TIR_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) != 0; + txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; + 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) +uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) { - out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps - uavcan::uint64_t utc_usec = 0; - { - CriticalSectionLocker 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; + out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps + uavcan::uint64_t utc_usec = 0; + { + CriticalSectionLocker 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) +uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs, + uavcan::uint16_t num_configs) { - if (num_configs <= NumFilters) - { - CriticalSectionLocker lock; + if (num_configs <= NumFilters) { + CriticalSectionLocker lock; - can_->FMR |= bxcan::FMR_FINIT; + can_->FMR |= bxcan::FMR_FINIT; - // Slave (CAN2) gets half of the filters - can_->FMR &= ~0x00003F00UL; - can_->FMR |= static_cast(NumFilters) << 8; + // Slave (CAN2) gets half of the filters + can_->FMR &= ~0x00003F00UL; + can_->FMR |= static_cast(NumFilters) << 8; - can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters - can_->FM1R = 0; // Identifier Mask mode - can_->FS1R = 0x7ffffff; // Single 32-bit for all + can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters + can_->FM1R = 0; // Identifier Mask mode + can_->FS1R = 0x7ffffff; // Single 32-bit for all - const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters; + const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters; - if (num_configs == 0) - { - can_->FilterRegister[filter_start_index].FR1 = 0; - can_->FilterRegister[filter_start_index].FR2 = 0; - // We can't directly overwrite FA1R because that breaks the other CAN interface - can_->FA1R |= 1U << filter_start_index; // Other filters may still be enabled, we don't care - } - else - { - for (uint8_t i = 0; i < NumFilters; i++) - { - if (i < num_configs) - { - uint32_t id = 0; - uint32_t mask = 0; + if (num_configs == 0) { + can_->FilterRegister[filter_start_index].FR1 = 0; + can_->FilterRegister[filter_start_index].FR2 = 0; + // We can't directly overwrite FA1R because that breaks the other CAN interface + can_->FA1R |= 1U << filter_start_index; // Other filters may still be enabled, we don't care - const uavcan::CanFilterConfig* const cfg = filter_configs + i; + } else { + for (uint8_t i = 0; i < NumFilters; i++) { + if (i < num_configs) { + uint32_t id = 0; + uint32_t mask = 0; - if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) - { - id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3; - mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3; - id |= bxcan::RIR_IDE; - } - else - { - id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21; // Regular std frames, nothing fancy. - mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21; // Boring. - } + const uavcan::CanFilterConfig *const cfg = filter_configs + i; - if (cfg->id & uavcan::CanFrame::FlagRTR) - { - id |= bxcan::RIR_RTR; - } + if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) { + id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3; + mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3; + id |= bxcan::RIR_IDE; - if (cfg->mask & uavcan::CanFrame::FlagEFF) - { - mask |= bxcan::RIR_IDE; - } + } else { + id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21; // Regular std frames, nothing fancy. + mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21; // Boring. + } - if (cfg->mask & uavcan::CanFrame::FlagRTR) - { - mask |= bxcan::RIR_RTR; - } + if (cfg->id & uavcan::CanFrame::FlagRTR) { + id |= bxcan::RIR_RTR; + } - can_->FilterRegister[filter_start_index + i].FR1 = id; - can_->FilterRegister[filter_start_index + i].FR2 = mask; + if (cfg->mask & uavcan::CanFrame::FlagEFF) { + mask |= bxcan::RIR_IDE; + } - can_->FA1R |= (1 << (filter_start_index + i)); - } - else - { - can_->FA1R &= ~(1 << (filter_start_index + i)); - } - } - } + if (cfg->mask & uavcan::CanFrame::FlagRTR) { + mask |= bxcan::RIR_RTR; + } - can_->FMR &= ~bxcan::FMR_FINIT; + can_->FilterRegister[filter_start_index + i].FR1 = id; + can_->FilterRegister[filter_start_index + i].FR2 = mask; - return 0; - } + can_->FA1R |= (1 << (filter_start_index + i)); - return -ErrFilterNumConfigs; + } else { + can_->FA1R &= ~(1 << (filter_start_index + i)); + } + } + } + + can_->FMR &= ~bxcan::FMR_FINIT; + + return 0; + } + + return -ErrFilterNumConfigs; } bool CanIface::waitMsrINakBitStateChange(bool target_state) { #if UAVCAN_STM32_NUTTX - const unsigned Timeout = 1000; + const unsigned Timeout = 1000; #else - const unsigned Timeout = 2000000; + const unsigned Timeout = 2000000; #endif - for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) - { - const bool state = (can_->MSR & bxcan::MSR_INAK) != 0; - if (state == target_state) - { - return true; - } + + for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) { + const bool state = (can_->MSR & bxcan::MSR_INAK) != 0; + + if (state == target_state) { + return true; + } + #if UAVCAN_STM32_NUTTX - ::usleep(1000); + ::usleep(1000); #endif - } - return false; + } + + return false; } int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) { - /* - * We need to silence the controller in the first order, otherwise it may interfere with the following operations. - */ - { - CriticalSectionLocker lock; + /* + * We need to silence the controller in the first order, otherwise it may interfere with the following operations. + */ + { + CriticalSectionLocker lock; - can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode - can_->MCR |= bxcan::MCR_INRQ; // Request init + can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode + can_->MCR |= bxcan::MCR_INRQ; // Request init - can_->IER = 0; // Disable interrupts while initialization is in progress - } + can_->IER = 0; // Disable interrupts while initialization is in progress + } - if (!waitMsrINakBitStateChange(true)) - { - UAVCAN_STM32_LOG("MSR INAK not set"); - can_->MCR = bxcan::MCR_RESET; - return -ErrMsrInakNotSet; - } + if (!waitMsrINakBitStateChange(true)) { + UAVCAN_STM32_LOG("MSR INAK not set"); + can_->MCR = bxcan::MCR_RESET; + return -ErrMsrInakNotSet; + } - /* - * Object state - interrupts are disabled, so it's safe to modify it now - */ - rx_queue_.reset(); - error_cnt_ = 0; - served_aborts_cnt_ = 0; - uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem()); - peak_tx_mailbox_index_ = 0; - had_activity_ = false; + /* + * Object state - interrupts are disabled, so it's safe to modify it now + */ + rx_queue_.reset(); + error_cnt_ = 0; + served_aborts_cnt_ = 0; + uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem()); + peak_tx_mailbox_index_ = 0; + had_activity_ = false; - /* - * CAN timings for this bitrate - */ - Timings timings; - const int timings_res = computeTimings(bitrate, timings); - if (timings_res < 0) - { - can_->MCR = bxcan::MCR_RESET; - return timings_res; - } - UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u", - unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); + /* + * CAN timings for this bitrate + */ + Timings timings; + const int timings_res = computeTimings(bitrate, timings); - /* - * Hardware initialization (the hardware has already confirmed initialization mode, see above) - */ - can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648 + if (timings_res < 0) { + can_->MCR = bxcan::MCR_RESET; + return timings_res; + } - can_->BTR = ((timings.sjw & 3U) << 24) | - ((timings.bs1 & 15U) << 16) | - ((timings.bs2 & 7U) << 20) | - (timings.prescaler & 1023U) | - ((mode == SilentMode) ? bxcan::BTR_SILM : 0); + UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u", + unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); - can_->IER = bxcan::IER_TMEIE | // TX mailbox empty - bxcan::IER_FMPIE0 | // RX FIFO 0 is not empty - bxcan::IER_FMPIE1; // RX FIFO 1 is not empty + /* + * Hardware initialization (the hardware has already confirmed initialization mode, see above) + */ + can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648 - can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode + can_->BTR = ((timings.sjw & 3U) << 24) | + ((timings.bs1 & 15U) << 16) | + ((timings.bs2 & 7U) << 20) | + (timings.prescaler & 1023U) | + ((mode == SilentMode) ? bxcan::BTR_SILM : 0); - if (!waitMsrINakBitStateChange(false)) - { - UAVCAN_STM32_LOG("MSR INAK not cleared"); - can_->MCR = bxcan::MCR_RESET; - return -ErrMsrInakNotCleared; - } + can_->IER = bxcan::IER_TMEIE | // TX mailbox empty + bxcan::IER_FMPIE0 | // RX FIFO 0 is not empty + bxcan::IER_FMPIE1; // RX FIFO 1 is not empty - /* - * Default filter configuration - */ - if (self_index_ == 0) - { - can_->FMR |= bxcan::FMR_FINIT; + can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode - can_->FMR &= 0xFFFFC0F1; - can_->FMR |= static_cast(NumFilters) << 8; // Slave (CAN2) gets half of the filters + if (!waitMsrINakBitStateChange(false)) { + UAVCAN_STM32_LOG("MSR INAK not cleared"); + can_->MCR = bxcan::MCR_RESET; + return -ErrMsrInakNotCleared; + } - can_->FFA1R = 0; // All assigned to FIFO0 by default - can_->FM1R = 0; // Indentifier Mask mode + /* + * Default filter configuration + */ + if (self_index_ == 0) { + can_->FMR |= bxcan::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_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything - can_->FilterRegister[0].FR2 = 0; - can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything - can_->FilterRegister[NumFilters].FR2 = 0; - can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface + can_->FS1R = 0x7ffffff; // Single 32-bit for all + can_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything + can_->FilterRegister[0].FR2 = 0; + can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything + can_->FilterRegister[NumFilters].FR2 = 0; + can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface #else - can_->FS1R = 0x1fff; - can_->FilterRegister[0].FR1 = 0; - can_->FilterRegister[0].FR2 = 0; - can_->FA1R = 1; + can_->FS1R = 0x1fff; + can_->FilterRegister[0].FR1 = 0; + can_->FilterRegister[0].FR2 = 0; + can_->FA1R = 1; #endif - can_->FMR &= ~bxcan::FMR_FINIT; - } + can_->FMR &= ~bxcan::FMR_FINIT; + } - return 0; + return 0; } void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec) { - UAVCAN_ASSERT(mailbox_index < NumTxMailboxes); + UAVCAN_ASSERT(mailbox_index < NumTxMailboxes); - had_activity_ = had_activity_ || txok; + had_activity_ = had_activity_ || txok; - TxItem& txi = pending_tx_[mailbox_index]; + TxItem &txi = pending_tx_[mailbox_index]; - if (txi.loopback && txok && txi.pending) - { - rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); - } + if (txi.loopback && txok && txi.pending) { + rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); + } - txi.pending = false; + txi.pending = false; } void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) { - // TXOK == false means that there was a hardware failure - if (can_->TSR & bxcan::TSR_RQCP0) - { - const bool txok = can_->TSR & bxcan::TSR_TXOK0; - can_->TSR = bxcan::TSR_RQCP0; - handleTxMailboxInterrupt(0, txok, utc_usec); - } - if (can_->TSR & bxcan::TSR_RQCP1) - { - const bool txok = can_->TSR & bxcan::TSR_TXOK1; - can_->TSR = bxcan::TSR_RQCP1; - handleTxMailboxInterrupt(1, txok, utc_usec); - } - if (can_->TSR & bxcan::TSR_RQCP2) - { - const bool txok = can_->TSR & bxcan::TSR_TXOK2; - can_->TSR = bxcan::TSR_RQCP2; - handleTxMailboxInterrupt(2, txok, utc_usec); - } - update_event_.signalFromInterrupt(); + // TXOK == false means that there was a hardware failure + if (can_->TSR & bxcan::TSR_RQCP0) { + const bool txok = can_->TSR & bxcan::TSR_TXOK0; + can_->TSR = bxcan::TSR_RQCP0; + handleTxMailboxInterrupt(0, txok, utc_usec); + } - pollErrorFlagsFromISR(); + if (can_->TSR & bxcan::TSR_RQCP1) { + const bool txok = can_->TSR & bxcan::TSR_TXOK1; + can_->TSR = bxcan::TSR_RQCP1; + handleTxMailboxInterrupt(1, txok, utc_usec); + } + + if (can_->TSR & bxcan::TSR_RQCP2) { + const bool txok = can_->TSR & bxcan::TSR_TXOK2; + can_->TSR = bxcan::TSR_RQCP2; + handleTxMailboxInterrupt(2, txok, utc_usec); + } + + update_event_.signalFromInterrupt(); + + pollErrorFlagsFromISR(); } void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec) { - UAVCAN_ASSERT(fifo_index < 2); + UAVCAN_ASSERT(fifo_index < 2); - volatile uavcan::uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R; - if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) - { - UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read - return; - } + volatile uavcan::uint32_t *const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R; - /* - * Register overflow as a hardware error - */ - if ((*rfr_reg & bxcan::RFR_FOVR) != 0) - { - error_cnt_++; - } + if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) { + UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read + return; + } - /* - * Read the frame contents - */ - uavcan::CanFrame frame; - const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index]; + /* + * Register overflow as a hardware error + */ + if ((*rfr_reg & bxcan::RFR_FOVR) != 0) { + error_cnt_++; + } - if ((rf.RIR & bxcan::RIR_IDE) == 0) - { - frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21); - } - else - { - frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3); - frame.id |= uavcan::CanFrame::FlagEFF; - } + /* + * Read the frame contents + */ + uavcan::CanFrame frame; + const bxcan::RxMailboxType &rf = can_->RxMailbox[fifo_index]; - if ((rf.RIR & bxcan::RIR_RTR) != 0) - { - frame.id |= uavcan::CanFrame::FlagRTR; - } + if ((rf.RIR & bxcan::RIR_IDE) == 0) { + frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21); - frame.dlc = rf.RDTR & 15; + } else { + frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3); + frame.id |= uavcan::CanFrame::FlagEFF; + } - frame.data[0] = uavcan::uint8_t(0xFF & (rf.RDLR >> 0)); - frame.data[1] = uavcan::uint8_t(0xFF & (rf.RDLR >> 8)); - frame.data[2] = uavcan::uint8_t(0xFF & (rf.RDLR >> 16)); - frame.data[3] = uavcan::uint8_t(0xFF & (rf.RDLR >> 24)); - frame.data[4] = uavcan::uint8_t(0xFF & (rf.RDHR >> 0)); - frame.data[5] = uavcan::uint8_t(0xFF & (rf.RDHR >> 8)); - frame.data[6] = uavcan::uint8_t(0xFF & (rf.RDHR >> 16)); - frame.data[7] = uavcan::uint8_t(0xFF & (rf.RDHR >> 24)); + if ((rf.RIR & bxcan::RIR_RTR) != 0) { + frame.id |= uavcan::CanFrame::FlagRTR; + } - *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read + frame.dlc = rf.RDTR & 15; - /* - * Store with timeout into the FIFO buffer and signal update event - */ - rx_queue_.push(frame, utc_usec, 0); - had_activity_ = true; - update_event_.signalFromInterrupt(); + frame.data[0] = uavcan::uint8_t(0xFF & (rf.RDLR >> 0)); + frame.data[1] = uavcan::uint8_t(0xFF & (rf.RDLR >> 8)); + frame.data[2] = uavcan::uint8_t(0xFF & (rf.RDLR >> 16)); + frame.data[3] = uavcan::uint8_t(0xFF & (rf.RDLR >> 24)); + frame.data[4] = uavcan::uint8_t(0xFF & (rf.RDHR >> 0)); + frame.data[5] = uavcan::uint8_t(0xFF & (rf.RDHR >> 8)); + frame.data[6] = uavcan::uint8_t(0xFF & (rf.RDHR >> 16)); + frame.data[7] = uavcan::uint8_t(0xFF & (rf.RDHR >> 24)); - pollErrorFlagsFromISR(); + *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read + + /* + * Store with timeout into the FIFO buffer and signal update event + */ + rx_queue_.push(frame, utc_usec, 0); + had_activity_ = true; + update_event_.signalFromInterrupt(); + + pollErrorFlagsFromISR(); } void CanIface::pollErrorFlagsFromISR() { - const uavcan::uint8_t lec = uavcan::uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT); - if (lec != 0) - { - can_->ESR = 0; - error_cnt_++; + const uavcan::uint8_t lec = uavcan::uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT); - // Serving abort requests - for (int i = 0; i < NumTxMailboxes; i++) // Dear compiler, may I suggest you to unroll this loop please. - { - TxItem& txi = pending_tx_[i]; - if (txi.pending && txi.abort_on_error) - { - can_->TSR = TSR_ABRQx[i]; - txi.pending = false; - served_aborts_cnt_++; - } - } - } + if (lec != 0) { + can_->ESR = 0; + error_cnt_++; + + // Serving abort requests + for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please. + TxItem &txi = pending_tx_[i]; + + if (txi.pending && txi.abort_on_error) { + can_->TSR = TSR_ABRQx[i]; + txi.pending = false; + served_aborts_cnt_++; + } + } + } } void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) { - CriticalSectionLocker lock; - for (int i = 0; i < NumTxMailboxes; i++) - { - TxItem& txi = pending_tx_[i]; - if (txi.pending && txi.deadline < current_time) - { - can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission - txi.pending = false; - error_cnt_++; - } - } + CriticalSectionLocker lock; + + for (int i = 0; i < NumTxMailboxes; i++) { + TxItem &txi = pending_tx_[i]; + + if (txi.pending && txi.deadline < current_time) { + can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission + txi.pending = false; + error_cnt_++; + } + } } -bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame& frame) const +bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame &frame) const { - /* - * We can accept more frames only if the following conditions are satisfied: - * - There is at least one TX mailbox free (obvious enough); - * - The priority of the new frame is higher than priority of all TX mailboxes. - */ - { - static const uavcan::uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2; - const uavcan::uint32_t tme = can_->TSR & TME; + /* + * We can accept more frames only if the following conditions are satisfied: + * - There is at least one TX mailbox free (obvious enough); + * - The priority of the new frame is higher than priority of all TX mailboxes. + */ + { + static const uavcan::uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2; + const uavcan::uint32_t tme = can_->TSR & TME; - if (tme == TME) // All TX mailboxes are free (as in freedom). - { - return true; - } + if (tme == TME) { // All TX mailboxes are free (as in freedom). + return true; + } - if (tme == 0) // All TX mailboxes are busy transmitting. - { - return false; - } - } + if (tme == 0) { // All TX mailboxes are busy transmitting. + return false; + } + } - /* - * The second condition requires a critical section. - */ - CriticalSectionLocker lock; + /* + * The second condition requires a critical section. + */ + CriticalSectionLocker lock; - for (int mbx = 0; mbx < NumTxMailboxes; mbx++) - { - if (pending_tx_[mbx].pending && !frame.priorityHigherThan(pending_tx_[mbx].frame)) - { - return false; // There's a mailbox whose priority is higher or equal the priority of the new frame. - } - } + for (int mbx = 0; mbx < NumTxMailboxes; mbx++) { + if (pending_tx_[mbx].pending && !frame.priorityHigherThan(pending_tx_[mbx].frame)) { + return false; // There's a mailbox whose priority is higher or equal the priority of the new frame. + } + } - return true; // This new frame will be added to a free TX mailbox in the next @ref send(). + return true; // This new frame will be added to a free TX mailbox in the next @ref send(). } bool CanIface::isRxBufferEmpty() const { - CriticalSectionLocker lock; - return rx_queue_.getLength() == 0; + CriticalSectionLocker lock; + return rx_queue_.getLength() == 0; } uavcan::uint64_t CanIface::getErrorCount() const { - CriticalSectionLocker lock; - return error_cnt_ + rx_queue_.getOverflowCount(); + CriticalSectionLocker lock; + return error_cnt_ + rx_queue_.getOverflowCount(); } unsigned CanIface::getRxQueueLength() const { - CriticalSectionLocker lock; - return rx_queue_.getLength(); + CriticalSectionLocker lock; + return rx_queue_.getLength(); } bool CanIface::hadActivity() { - CriticalSectionLocker lock; - const bool ret = had_activity_; - had_activity_ = false; - return ret; + CriticalSectionLocker lock; + const bool ret = had_activity_; + had_activity_ = false; + return ret; } /* * CanDriver */ -uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const +uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces]) const { - uavcan::CanSelectMasks msk; + uavcan::CanSelectMasks msk; - // Iface 0 - msk.read = if0_.isRxBufferEmpty() ? 0 : 1; + // Iface 0 + msk.read = if0_.isRxBufferEmpty() ? 0 : 1; - if (pending_tx[0] != UAVCAN_NULLPTR) - { - msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0; - } + if (pending_tx[0] != UAVCAN_NULLPTR) { + msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0; + } - // Iface 1 + // Iface 1 #if UAVCAN_STM32_NUM_IFACES > 1 - if (!if1_.isRxBufferEmpty()) - { - msk.read |= 1 << 1; - } - if (pending_tx[1] != UAVCAN_NULLPTR) - { - if (if1_.canAcceptNewTxFrame(*pending_tx[1])) - { - msk.write |= 1 << 1; - } - } + if (!if1_.isRxBufferEmpty()) { + msk.read |= 1 << 1; + } + + if (pending_tx[1] != UAVCAN_NULLPTR) { + if (if1_.canAcceptNewTxFrame(*pending_tx[1])) { + msk.write |= 1 << 1; + } + } + #endif - return msk; + return msk; } bool CanDriver::hasReadableInterfaces() const @@ -869,86 +832,86 @@ bool CanDriver::hasReadableInterfaces() const #endif } -uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], - const uavcan::MonotonicTime blocking_deadline) +uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks, + const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces], + const uavcan::MonotonicTime blocking_deadline) { - const uavcan::CanSelectMasks in_masks = inout_masks; - const uavcan::MonotonicTime time = clock::getMonotonic(); + const uavcan::CanSelectMasks in_masks = inout_masks; + const uavcan::MonotonicTime time = clock::getMonotonic(); - if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots - { - CriticalSectionLocker cs_locker; - if0_.pollErrorFlagsFromISR(); - } + if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots + { + CriticalSectionLocker cs_locker; + if0_.pollErrorFlagsFromISR(); + } #if UAVCAN_STM32_NUM_IFACES > 1 - if1_.discardTimedOutTxMailboxes(time); - { - CriticalSectionLocker cs_locker; - if1_.pollErrorFlagsFromISR(); - } + if1_.discardTimedOutTxMailboxes(time); + { + CriticalSectionLocker cs_locker; + if1_.pollErrorFlagsFromISR(); + } #endif - inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events - if ((inout_masks.read & in_masks.read) != 0 || - (inout_masks.write & in_masks.write) != 0) - { - return 1; - } + inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events - (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates - inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set - return 1; // Return value doesn't matter as long as it is non-negative + if ((inout_masks.read & in_masks.read) != 0 || + (inout_masks.write & in_masks.write) != 0) { + return 1; + } + + (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates + inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set + return 1; // Return value doesn't matter as long as it is non-negative } void CanDriver::initOnce() { - /* - * CAN1, CAN2 - */ - { - CriticalSectionLocker lock; + /* + * CAN1, CAN2 + */ + { + CriticalSectionLocker lock; #if UAVCAN_STM32_NUTTX - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN1RST, 0); + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN1RST, 0); # if UAVCAN_STM32_NUM_IFACES > 1 - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0); + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0); # endif #else - RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; - RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST; - RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST; + RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST; # if UAVCAN_STM32_NUM_IFACES > 1 - RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; - RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST; - RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST; + RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST; # endif #endif - } + } - /* - * IRQ - */ + /* + * IRQ + */ #if UAVCAN_STM32_NUTTX # define IRQ_ATTACH(irq, handler) \ - { \ - const int res = irq_attach(irq, handler, NULL); \ - (void)res; \ - assert(res >= 0); \ - up_enable_irq(irq); \ - } - IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq); - IRQ_ATTACH(STM32_IRQ_CAN1RX0, can1_irq); - IRQ_ATTACH(STM32_IRQ_CAN1RX1, can1_irq); + { \ + const int res = irq_attach(irq, handler, NULL); \ + (void)res; \ + assert(res >= 0); \ + up_enable_irq(irq); \ + } + IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq); + IRQ_ATTACH(STM32_IRQ_CAN1RX0, can1_irq); + IRQ_ATTACH(STM32_IRQ_CAN1RX1, can1_irq); # if UAVCAN_STM32_NUM_IFACES > 1 - IRQ_ATTACH(STM32_IRQ_CAN2TX, can2_irq); - IRQ_ATTACH(STM32_IRQ_CAN2RX0, can2_irq); - IRQ_ATTACH(STM32_IRQ_CAN2RX1, can2_irq); + IRQ_ATTACH(STM32_IRQ_CAN2TX, can2_irq); + IRQ_ATTACH(STM32_IRQ_CAN2RX0, can2_irq); + IRQ_ATTACH(STM32_IRQ_CAN2RX1, can2_irq); # endif # undef IRQ_ATTACH #endif @@ -956,72 +919,73 @@ void CanDriver::initOnce() int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) { - int res = 0; + int res = 0; - UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); - static bool initialized_once = false; - if (!initialized_once) - { - initialized_once = true; - UAVCAN_STM32_LOG("First initialization"); - initOnce(); - } + static bool initialized_once = false; - /* - * CAN1 - */ - UAVCAN_STM32_LOG("Initing iface 0..."); - ifaces[0] = &if0_; // This link must be initialized first, - res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; - if (res < 0) // a typical race condition. - { - UAVCAN_STM32_LOG("Iface 0 init failed %i", res); - ifaces[0] = UAVCAN_NULLPTR; - goto fail; - } + if (!initialized_once) { + initialized_once = true; + UAVCAN_STM32_LOG("First initialization"); + initOnce(); + } - /* - * CAN2 - */ + /* + * CAN1 + */ + UAVCAN_STM32_LOG("Initing iface 0..."); + ifaces[0] = &if0_; // This link must be initialized first, + res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; + + if (res < 0) { // a typical race condition. + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + + /* + * CAN2 + */ #if UAVCAN_STM32_NUM_IFACES > 1 - UAVCAN_STM32_LOG("Initing iface 1..."); - ifaces[1] = &if1_; // Same thing here. - res = if1_.init(bitrate, mode); - if (res < 0) - { - UAVCAN_STM32_LOG("Iface 1 init failed %i", res); - ifaces[1] = UAVCAN_NULLPTR; - goto fail; - } + UAVCAN_STM32_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + + if (res < 0) { + UAVCAN_STM32_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } + #endif - UAVCAN_STM32_LOG("CAN drv init OK"); - UAVCAN_ASSERT(res >= 0); - return res; + UAVCAN_STM32_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; fail: - UAVCAN_STM32_LOG("CAN drv init failed %i", res); - UAVCAN_ASSERT(res < 0); - return res; + UAVCAN_STM32_LOG("CAN drv init failed %i", res); + UAVCAN_ASSERT(res < 0); + return res; } -CanIface* CanDriver::getIface(uavcan::uint8_t iface_index) +CanIface *CanDriver::getIface(uavcan::uint8_t iface_index) { - if (iface_index < UAVCAN_STM32_NUM_IFACES) - { - return ifaces[iface_index]; - } - return UAVCAN_NULLPTR; + if (iface_index < UAVCAN_STM32_NUM_IFACES) { + return ifaces[iface_index]; + } + + return UAVCAN_NULLPTR; } bool CanDriver::hadActivity() { - bool ret = if0_.hadActivity(); + bool ret = if0_.hadActivity(); #if UAVCAN_STM32_NUM_IFACES > 1 - ret |= if1_.hadActivity(); + ret |= if1_.hadActivity(); #endif - return ret; + return ret; } } // namespace uavcan_stm32 @@ -1034,49 +998,43 @@ extern "C" #if UAVCAN_STM32_NUTTX -static int can1_irq(const int irq, void*, void*) -{ - if (irq == STM32_IRQ_CAN1TX) - { - uavcan_stm32::handleTxInterrupt(0); - } - else if (irq == STM32_IRQ_CAN1RX0) - { - uavcan_stm32::handleRxInterrupt(0, 0); - } - else if (irq == STM32_IRQ_CAN1RX1) - { - uavcan_stm32::handleRxInterrupt(0, 1); - } - else - { - PANIC(); - } - return 0; -} + static int can1_irq(const int irq, void *, void *) + { + if (irq == STM32_IRQ_CAN1TX) { + uavcan_stm32::handleTxInterrupt(0); + + } else if (irq == STM32_IRQ_CAN1RX0) { + uavcan_stm32::handleRxInterrupt(0, 0); + + } else if (irq == STM32_IRQ_CAN1RX1) { + uavcan_stm32::handleRxInterrupt(0, 1); + + } else { + PANIC(); + } + + return 0; + } # if UAVCAN_STM32_NUM_IFACES > 1 -static int can2_irq(const int irq, void*, void*) -{ - if (irq == STM32_IRQ_CAN2TX) - { - uavcan_stm32::handleTxInterrupt(1); - } - else if (irq == STM32_IRQ_CAN2RX0) - { - uavcan_stm32::handleRxInterrupt(1, 0); - } - else if (irq == STM32_IRQ_CAN2RX1) - { - uavcan_stm32::handleRxInterrupt(1, 1); - } - else - { - PANIC(); - } - return 0; -} + static int can2_irq(const int irq, void *, void *) + { + if (irq == STM32_IRQ_CAN2TX) { + uavcan_stm32::handleTxInterrupt(1); + + } else if (irq == STM32_IRQ_CAN2RX0) { + uavcan_stm32::handleRxInterrupt(1, 0); + + } else if (irq == STM32_IRQ_CAN2RX1) { + uavcan_stm32::handleRxInterrupt(1, 1); + + } else { + PANIC(); + } + + return 0; + } # endif @@ -1088,29 +1046,29 @@ static int can2_irq(const int irq, void*, void*) # error "Misconfigured build" #endif -UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler); -UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler) -{ - UAVCAN_STM32_IRQ_PROLOGUE(); - uavcan_stm32::handleTxInterrupt(0); - UAVCAN_STM32_IRQ_EPILOGUE(); -} + UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler); + 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_HANDLER(CAN1_RX0_IRQHandler) -{ - UAVCAN_STM32_IRQ_PROLOGUE(); - uavcan_stm32::handleRxInterrupt(0, 0); - UAVCAN_STM32_IRQ_EPILOGUE(); -} + UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler); + 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_HANDLER(CAN1_RX1_IRQHandler) -{ - UAVCAN_STM32_IRQ_PROLOGUE(); - uavcan_stm32::handleRxInterrupt(0, 1); - UAVCAN_STM32_IRQ_EPILOGUE(); -} + UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler); + UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler) + { + UAVCAN_STM32_IRQ_PROLOGUE(); + uavcan_stm32::handleRxInterrupt(0, 1); + UAVCAN_STM32_IRQ_EPILOGUE(); + } # if UAVCAN_STM32_NUM_IFACES > 1 @@ -1120,29 +1078,29 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler) # error "Misconfigured build" #endif -UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler); -UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler) -{ - UAVCAN_STM32_IRQ_PROLOGUE(); - uavcan_stm32::handleTxInterrupt(1); - UAVCAN_STM32_IRQ_EPILOGUE(); -} + UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler); + 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_HANDLER(CAN2_RX0_IRQHandler) -{ - UAVCAN_STM32_IRQ_PROLOGUE(); - uavcan_stm32::handleRxInterrupt(1, 0); - UAVCAN_STM32_IRQ_EPILOGUE(); -} + UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler); + 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_HANDLER(CAN2_RX1_IRQHandler) -{ - UAVCAN_STM32_IRQ_PROLOGUE(); - uavcan_stm32::handleRxInterrupt(1, 1); - UAVCAN_STM32_IRQ_EPILOGUE(); -} + UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler); + UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler) + { + UAVCAN_STM32_IRQ_PROLOGUE(); + uavcan_stm32::handleRxInterrupt(1, 1); + UAVCAN_STM32_IRQ_EPILOGUE(); + } # endif #endif // UAVCAN_STM32_NUTTX diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 7175b08657..3e4fb715c7 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -82,182 +82,181 @@ uavcan::uint64_t time_utc = 0; void init() { - CriticalSectionLocker lock; - if (initialized) - { - return; - } - initialized = true; + CriticalSectionLocker lock; + + if (initialized) { + return; + } + + initialized = true; # if UAVCAN_STM32_NUTTX - // Attach IRQ - irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL); + // Attach IRQ + irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL); - // Power-on and reset - modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK); - modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK); - modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0); + // Power-on and reset + modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK); + modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK); + modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0); - // Start the timer - putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET)); - putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET)); - putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET)); - putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); - putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately - putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET)); - putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start + // Start the timer + putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET)); + putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET)); + putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET)); + putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); + putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately + putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET)); + putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start - // Prioritize and Enable IRQ + // Prioritize and Enable IRQ // todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting // need to investigate // up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); - up_enable_irq(TIMX_IRQn); + up_enable_irq(TIMX_IRQn); # endif } void setUtc(uavcan::UtcTime time) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); - { - CriticalSectionLocker locker; - time_utc = time.toUSec(); - } + { + CriticalSectionLocker locker; + time_utc = time.toUSec(); + } - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; } static uavcan::uint64_t sampleUtcFromCriticalSection() { # if UAVCAN_STM32_NUTTX - UAVCAN_ASSERT(initialized); - UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE); + UAVCAN_ASSERT(initialized); + UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE); - volatile uavcan::uint64_t time = time_utc; - volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + volatile uavcan::uint64_t time = time_utc; + volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); - if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) - { - cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); - const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + - (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; - time = uavcan::uint64_t(uavcan::int64_t(time) + add); - } - return time + cnt; + if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) { + cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + + (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; + time = uavcan::uint64_t(uavcan::int64_t(time) + add); + } + + return time + cnt; # endif } uavcan::uint64_t getUtcUSecFromCanInterrupt() { - return utc_set ? sampleUtcFromCriticalSection() : 0; + return utc_set ? sampleUtcFromCriticalSection() : 0; } uavcan::MonotonicTime getMonotonic() { - uavcan::uint64_t usec = 0; - // Scope Critical section - { - CriticalSectionLocker locker; + uavcan::uint64_t usec = 0; + // Scope Critical section + { + CriticalSectionLocker locker; - volatile uavcan::uint64_t time = time_mono; + volatile uavcan::uint64_t time = time_mono; # if UAVCAN_STM32_NUTTX - volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); - if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) - { - cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); + if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) { + cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); # endif - time += USecPerOverflow; - } - usec = time + cnt; + time += USecPerOverflow; + } + + usec = time + cnt; # ifndef NDEBUG - static uavcan::uint64_t prev_usec = 0; // Self-test - UAVCAN_ASSERT(prev_usec <= usec); - (void)prev_usec; - prev_usec = usec; + static uavcan::uint64_t prev_usec = 0; // Self-test + UAVCAN_ASSERT(prev_usec <= usec); + (void)prev_usec; + prev_usec = usec; # endif - } // End Scope Critical section + } // End Scope Critical section - return uavcan::MonotonicTime::fromUSec(usec); + return uavcan::MonotonicTime::fromUSec(usec); } uavcan::UtcTime getUtc() { - if (utc_set) - { - uavcan::uint64_t usec = 0; - { - CriticalSectionLocker locker; - usec = sampleUtcFromCriticalSection(); - } - return uavcan::UtcTime::fromUSec(usec); - } - return uavcan::UtcTime(); + if (utc_set) { + uavcan::uint64_t usec = 0; + { + CriticalSectionLocker locker; + usec = sampleUtcFromCriticalSection(); + } + return uavcan::UtcTime::fromUSec(usec); + } + + return uavcan::UtcTime(); } static float lowpass(float xold, float xnew, float corner, float dt) { - const float tau = 1.F / corner; - return (dt * xnew + tau * xold) / (dt + tau); + const float tau = 1.F / corner; + return (dt * xnew + tau * xold) / (dt + tau); } static void updateRatePID(uavcan::UtcDuration adjustment) { - const uavcan::MonotonicTime ts = getMonotonic(); - const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F; - prev_utc_adj_at = ts; - const float adj_usec = float(adjustment.toUSec()); + const uavcan::MonotonicTime ts = getMonotonic(); + const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F; + prev_utc_adj_at = ts; + const float adj_usec = float(adjustment.toUSec()); - /* - * Target relative rate in PPM - * Positive to go faster - */ - const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p; + /* + * Target relative rate in PPM + * Positive to go faster + */ + const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p; - /* - * Current relative rate in PPM - * Positive if the local clock is faster - */ - const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM - utc_prev_adj = adj_usec; - utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt); + /* + * Current relative rate in PPM + * Positive if the local clock is faster + */ + const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM + utc_prev_adj = adj_usec; + utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt); - const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; + const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; - if (dt > 10) - { - utc_rel_rate_error_integral = 0; - } - else - { - utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; - utc_rel_rate_error_integral = - uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); - utc_rel_rate_error_integral = - uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm); - } + if (dt > 10) { + utc_rel_rate_error_integral = 0; - /* - * Rate controller - */ - float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral; - total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); - total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); + } else { + utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; + utc_rel_rate_error_integral = + uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); + utc_rel_rate_error_integral = + uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm); + } - utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F)); + /* + * Rate controller + */ + float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral; + total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); + total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); + + utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F)); // syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n", // adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm, @@ -266,96 +265,91 @@ static void updateRatePID(uavcan::UtcDuration adjustment) void adjustUtc(uavcan::UtcDuration adjustment) { - MutexLocker mlocker(mutex); - UAVCAN_ASSERT(initialized); + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); - if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) - { - const uavcan::int64_t adj_usec = adjustment.toUSec(); + if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { + const uavcan::int64_t adj_usec = adjustment.toUSec(); - { - CriticalSectionLocker locker; - if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) - { - time_utc = 1; - } - else - { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); - } - } + { + CriticalSectionLocker locker; - utc_set = true; - utc_locked = false; - utc_jump_cnt++; - utc_prev_adj = 0; - utc_rel_rate_ppm = 0; - } - else - { - updateRatePID(adjustment); + if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) { + time_utc = 1; - if (!utc_locked) - { - utc_locked = - (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && - (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); - } - } + } else { + time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); + } + } + + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; + + } else { + updateRatePID(adjustment); + + if (!utc_locked) { + utc_locked = + (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && + (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); + } + } } float getUtcRateCorrectionPPM() { - MutexLocker mlocker(mutex); - const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); - return 1e6F * rate_correction_mult; + MutexLocker mlocker(mutex); + const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); + return 1e6F * rate_correction_mult; } uavcan::uint32_t getUtcJumpCount() { - MutexLocker mlocker(mutex); - return utc_jump_cnt; + MutexLocker mlocker(mutex); + return utc_jump_cnt; } bool isUtcLocked() { - MutexLocker mlocker(mutex); - return utc_locked; + MutexLocker mlocker(mutex); + return utc_locked; } UtcSyncParams getUtcSyncParams() { - MutexLocker mlocker(mutex); - return utc_sync_params; + MutexLocker mlocker(mutex); + return utc_sync_params; } -void setUtcSyncParams(const UtcSyncParams& params) +void setUtcSyncParams(const UtcSyncParams ¶ms) { - MutexLocker mlocker(mutex); - // Add some sanity check - utc_sync_params = params; + MutexLocker mlocker(mutex); + // Add some sanity check + utc_sync_params = params; } } // namespace clock -SystemClock& SystemClock::instance() +SystemClock &SystemClock::instance() { - static union SystemClockStorage - { - uavcan::uint8_t buffer[sizeof(SystemClock)]; - long long _aligner_1; - long double _aligner_2; - } storage; + static union SystemClockStorage { + uavcan::uint8_t buffer[sizeof(SystemClock)]; + long long _aligner_1; + long double _aligner_2; + } storage; - SystemClock* const ptr = reinterpret_cast(storage.buffer); + SystemClock *const ptr = reinterpret_cast(storage.buffer); - if (!clock::initialized) - { - MutexLocker mlocker(clock::mutex); - clock::init(); - new (ptr)SystemClock(); - } - return *ptr; + if (!clock::initialized) { + MutexLocker mlocker(clock::mutex); + clock::init(); + new (ptr)SystemClock(); + } + + return *ptr; } } // namespace uavcan_stm32 @@ -368,43 +362,39 @@ SystemClock& SystemClock::instance() extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler) { - UAVCAN_STM32_IRQ_PROLOGUE(); + UAVCAN_STM32_IRQ_PROLOGUE(); # if UAVCAN_STM32_NUTTX - putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); + putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); # endif - using namespace uavcan_stm32::clock; - UAVCAN_ASSERT(initialized); + using namespace uavcan_stm32::clock; + UAVCAN_ASSERT(initialized); - time_mono += USecPerOverflow; + time_mono += USecPerOverflow; - if (utc_set) - { - time_utc += USecPerOverflow; - utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; - if (std::abs(utc_accumulated_correction_nsec) >= 1000) - { - time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000); - utc_accumulated_correction_nsec %= 1000; - } + if (utc_set) { + time_utc += USecPerOverflow; + utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; - // Correction decay - 1 nsec per 65536 usec - if (utc_correction_nsec_per_overflow > 0) - { - utc_correction_nsec_per_overflow--; - } - else if (utc_correction_nsec_per_overflow < 0) - { - utc_correction_nsec_per_overflow++; - } - else - { - ; // Zero - } - } + if (std::abs(utc_accumulated_correction_nsec) >= 1000) { + time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000); + utc_accumulated_correction_nsec %= 1000; + } - UAVCAN_STM32_IRQ_EPILOGUE(); + // Correction decay - 1 nsec per 65536 usec + if (utc_correction_nsec_per_overflow > 0) { + utc_correction_nsec_per_overflow--; + + } else if (utc_correction_nsec_per_overflow < 0) { + utc_correction_nsec_per_overflow++; + + } else { + ; // Zero + } + } + + UAVCAN_STM32_IRQ_EPILOGUE(); } #endif diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index c29caa92ca..7e1f21bc6e 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -13,50 +13,53 @@ namespace uavcan_stm32 #if UAVCAN_STM32_NUTTX -BusEvent::BusEvent(CanDriver& can_driver) +BusEvent::BusEvent(CanDriver &can_driver) { - sem_init(&sem_, 0, 0); - sem_setprotocol(&sem_, SEM_PRIO_NONE); + sem_init(&sem_, 0, 0); + sem_setprotocol(&sem_, SEM_PRIO_NONE); } BusEvent::~BusEvent() { - sem_destroy(&sem_); + sem_destroy(&sem_); } bool BusEvent::wait(uavcan::MonotonicDuration duration) { - if (duration.isPositive()) { - timespec abstime; + if (duration.isPositive()) { + timespec abstime; - if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { - const unsigned billion = 1000 * 1000 * 1000; - uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; - abstime.tv_sec += nsecs / billion; - nsecs -= (nsecs / billion) * billion; - abstime.tv_nsec = nsecs; + if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { + const unsigned billion = 1000 * 1000 * 1000; + uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; + abstime.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + abstime.tv_nsec = nsecs; - int ret; - while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); - if (ret == -1) { // timed out or error - return false; - } - return true; - } - } - return false; + int ret; + + while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); + + if (ret == -1) { // timed out or error + return false; + } + + return true; + } + } + + return false; } void BusEvent::signalFromInterrupt() { - if (sem_.semcount <= 0) - { - (void)sem_post(&sem_); - } - if (signal_cb_) - { - signal_cb_(); - } + if (sem_.semcount <= 0) { + (void)sem_post(&sem_); + } + + if (signal_cb_) { + signal_cb_(); + } } #endif