From b7a480b45ba9a9d70832199dd88938bf13262105 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 29 Oct 2019 15:02:47 +0100 Subject: [PATCH] refactor uavcan: add stm32 and kinetis drivers directly --- Tools/astyle/files_to_check_code_style.sh | 1 + src/drivers/uavcan/CMakeLists.txt | 30 +- .../uavcan/uavcan_drivers/kinetis/README.md | 59 + .../kinetis/driver/CMakeLists.txt | 17 + .../include/uavcan_kinetis/build_config.hpp | 41 + .../driver/include/uavcan_kinetis/can.hpp | 419 ++++++ .../driver/include/uavcan_kinetis/clock.hpp | 131 ++ .../driver/include/uavcan_kinetis/flexcan.hpp | 661 +++++++++ .../driver/include/uavcan_kinetis/thread.hpp | 108 ++ .../include/uavcan_kinetis/uavcan_kinetis.hpp | 12 + .../kinetis/driver/src/internal.hpp | 72 + .../kinetis/driver/src/uc_kinetis_clock.cpp | 374 +++++ .../kinetis/driver/src/uc_kinetis_flexcan.cpp | 1076 ++++++++++++++ .../kinetis/driver/src/uc_kinetis_thread.cpp | 159 +++ .../uavcan/uavcan_drivers/stm32/README.md | 11 + .../stm32/driver/CMakeLists.txt | 17 + .../include/uavcan_stm32/build_config.hpp | 40 + .../driver/include/uavcan_stm32/bxcan.hpp | 289 ++++ .../stm32/driver/include/uavcan_stm32/can.hpp | 382 +++++ .../driver/include/uavcan_stm32/clock.hpp | 121 ++ .../driver/include/uavcan_stm32/thread.hpp | 239 ++++ .../include/uavcan_stm32/uavcan_stm32.hpp | 11 + .../stm32/driver/src/internal.hpp | 159 +++ .../stm32/driver/src/uc_stm32_can.cpp | 1235 +++++++++++++++++ .../stm32/driver/src/uc_stm32_clock.cpp | 496 +++++++ .../stm32/driver/src/uc_stm32_thread.cpp | 287 ++++ src/drivers/uavcanesc/CMakeLists.txt | 15 +- src/drivers/uavcannode/CMakeLists.txt | 15 +- 28 files changed, 6458 insertions(+), 19 deletions(-) create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/README.md create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/CMakeLists.txt create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/uavcan_kinetis.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp create mode 100644 src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/README.md create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/CMakeLists.txt create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp create mode 100644 src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index d28407df3b..9301004de7 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -11,6 +11,7 @@ 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/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index e13ffb597e..e6a4dc5b1b 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -34,19 +34,20 @@ px4_add_git_submodule(TARGET git_uavcan PATH "libuavcan") set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") +set(UAVCAN_PLATFORM "generic") if(CONFIG_ARCH_CHIP) if (${CONFIG_ARCH_CHIP} MATCHES "kinetis") - set(UAVCAN_PLATFORM "kinetis") + set(UAVCAN_DRIVER "kinetis") set(UAVCAN_TIMER 1) elseif (${CONFIG_ARCH_CHIP} MATCHES "stm32") - set(UAVCAN_PLATFORM "stm32") + set(UAVCAN_DRIVER "stm32") set(UAVCAN_TIMER 5) # The default timer the 5 endif() endif() -if(NOT DEFINED UAVCAN_PLATFORM) - message(FATAL_ERROR "UAVCAN_PLATFORM not set") +if(NOT DEFINED UAVCAN_DRIVER) + message(FATAL_ERROR "UAVCAN_DRIVER not set") endif() if(NOT config_uavcan_num_ifaces) @@ -54,13 +55,13 @@ if(NOT config_uavcan_num_ifaces) endif() string(TOUPPER "${PX4_PLATFORM}" OS_UPPER) -string(TOUPPER "${UAVCAN_PLATFORM}" UAVCAN_PLATFORM_UPPER) +string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER) add_definitions( - -DUAVCAN_${UAVCAN_PLATFORM_UPPER}_${OS_UPPER}=1 - -DUAVCAN_${UAVCAN_PLATFORM_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} - -DUAVCAN_${UAVCAN_PLATFORM_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER} + -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 + -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} + -DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER} -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - -DUAVCAN_DRIVER=uavcan_${UAVCAN_PLATFORM} + -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS @@ -71,6 +72,13 @@ add_compile_options(-Wno-cast-align) # TODO: fix and enable add_subdirectory(libuavcan EXCLUDE_FROM_ALL) add_dependencies(uavcan prebuild_targets) +# driver +add_subdirectory(uavcan_drivers/${UAVCAN_DRIVER}/driver EXCLUDE_FROM_ALL) +target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC + ./libuavcan/libuavcan/include + ./libuavcan/libuavcan/include/dsdlc_generated + ) + # generated DSDL set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/dsdl/uavcan") @@ -98,7 +106,7 @@ px4_add_module( libuavcan/libuavcan/include libuavcan/libuavcan/include/dsdlc_generated libuavcan/libuavcan_drivers/posix/include - libuavcan/libuavcan_drivers/${UAVCAN_PLATFORM}/driver/include + uavcan_drivers/${UAVCAN_DRIVER}/driver/include SRCS # Main uavcan_main.cpp @@ -124,8 +132,8 @@ px4_add_module( version git_uavcan + uavcan_${UAVCAN_DRIVER}_driver # within libuavcan uavcan - uavcan_${UAVCAN_PLATFORM}_driver ) diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/README.md b/src/drivers/uavcan/uavcan_drivers/kinetis/README.md new file mode 100644 index 0000000000..ac3811fede --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/README.md @@ -0,0 +1,59 @@ +# libuavcan_kinetis +Libuavcan platform driver for Kinetis FlexCAN + +The Directory contains the Kinetis FlexCAN platform driver for Libuavcan on NuttX. + +Configuation is set by the NuttX board.h for the following: + +| Driver | board.h | +|--------|----------------| +| OSCERCLK | BOARD_EXTAL_FREQ | +| busclck | BOARD_BUS_FREQ | + +and the following commandline defines: + +| Setting | Description | +|-------------------|-----------------------------------------------------------------------------------| +|UAVCAN_KINETIS_NUM_IFACES | - {1..2} Sets the number of CAN interfaces the SW will support | +|UAVCAN_KINETIS_TIMER_NUMBER | - {0..3} Sets the Periodic Interrupt Timer (PITn) channel | + +Things that could be improved: +1. Support for Bare metal and other OS' (ChibiOS, FREERTOS) +2. Build time command line configuartion of Mailbox/FIFO and filters +3. Build time command line configuartion clock source + - Curently the driver use `const uavcan::uint8_t CLOCKSEL = 0;` To Select OSCERCLK +4. Dynamic filter disable. There are no filter enable bits on the FlexCAN, just the number of Filters + can be set. But this changes the memory map. So the configuration show below has been chosen. + +``` +/* Layout of Fifo, filters and Message buffers */ + +enum { FiFo = 0 }; +/* 0 */ +/* 1 */ +/* 2 */ +/* 3 Fifo */ +/* 4 */ +/* 5 */ +enum { FirstFilter = 6 }; +/* 6 */ +/* 7 */ +/* 8 Filters */ +/* 9 */ +enum { NumHWFilters = 16 }; +enum { NumMBinFiFoAndFilters = 10 }; +/* 10 */ +/* 11 */ +/* 12 */ +/* 13 Tx Message Buffers */ +/* 14 */ +/* 15 */ +/*-- ----------------------*/ +``` + +A dedicated example application may be added later here. +For now, please consider the following open source projects as a reference: + +- https://github.com/PX4/sapog +- https://github.com/Zubax/zubax_gnss +- https://github.com/PX4/Firmware diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/CMakeLists.txt new file mode 100644 index 0000000000..f4cc48168a --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/CMakeLists.txt @@ -0,0 +1,17 @@ +include_directories( + ./include + ) + +add_library(uavcan_kinetis_driver STATIC + ./src/uc_kinetis_flexcan.cpp + ./src/uc_kinetis_clock.cpp + ./src/uc_kinetis_thread.cpp + ) + +add_dependencies(uavcan_kinetis_driver uavcan) + +install(DIRECTORY include/uavcan_kinetis DESTINATION include) +install(TARGETS uavcan_kinetis_driver DESTINATION lib) + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :) + diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp new file mode 100644 index 0000000000..d3d0837d09 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2015, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + +/** + * OS detection + */ +#ifndef UAVCAN_KINETIS_CHIBIOS +# define UAVCAN_KINETIS_CHIBIOS 0 +#endif + +#ifndef UAVCAN_KINETIS_NUTTX +# define UAVCAN_KINETIS_NUTTX 0 +#endif + +#ifndef UAVCAN_KINETIS_BAREMETAL +# define UAVCAN_KINETIS_BAREMETAL 0 +#endif + +#ifndef UAVCAN_KINETIS_FREERTOS +# define UAVCAN_KINETIS_FREERTOS 0 +#endif + +/** + * Number of interfaces must be enabled explicitly + */ +#if !defined(UAVCAN_KINETIS_NUM_IFACES) || (UAVCAN_KINETIS_NUM_IFACES != 1 && UAVCAN_KINETIS_NUM_IFACES != 2) +# error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2" +#endif + +/** + * Any PIT timer channel (PIT0-PIT3) + * e.g. -DUAVCAN_KINETIS_TIMER_NUMBER=2 + */ +#ifndef UAVCAN_KINETIS_TIMER_NUMBER +// In this case the clock driver should be implemented by the application +# define UAVCAN_KINETIS_TIMER_NUMBER 0 +#endif 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 new file mode 100644 index 0000000000..6f2b701e85 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp @@ -0,0 +1,419 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + +#include +#include +#include +#include + +namespace uavcan_kinetis +{ +/** + * Driver error codes. + * These values can be returned from driver functions negated. + */ +//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use +static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented +static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported +static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error +static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc) +static const uavcan::int16_t ErrMcrLPMAckNotSet = 1005; ///< MCR_LPMACK bit of the MCR register is not 1 +static const uavcan::int16_t ErrMcrLPMAckNotCleared = 1006; ///< MCR_LPMACK bit of the MCR register is not 0 +static const uavcan::int16_t ErrMcrFRZACKAckNotSet = 1007; ///< MCR_FRZACK bit of the MCR register is not 1 +static const uavcan::int16_t ErrMcrFRZACKAckNotCleared = 1008; ///< MCR_FRZACK bit of the MCR register is not 0 +static const uavcan::int16_t ErrBitRateNotDetected = 1009; ///< Auto bit rate detection could not be finished +static const uavcan::int16_t ErrFilterNumConfigs = 1010; ///< Number of filters is more than supported +static const uavcan::int16_t ErrMcrSOFTRSTNotCleared = 1011; ///< MCR_SOFTRST bit of the MCR register is not 0 +/** + * 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) + { + } +}; + +/** + * Single CAN iface. + * The application shall not use this directly. + */ +class CanIface : public uavcan::ICanIface + , 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_; + + void registerOverflow(); + + public: + RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) + : buf_(buf), + capacity_(capacity), + in_(0), + out_(0), + len_(0), + overflow_cnt_(0) + { + } + + void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags); + void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags); + + void reset(); + unsigned getLength() const { + return len_; + } + + 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; + + 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; + + TxItem() + : pending(free), + loopback(false), + abort_on_error(false) + { + } + }; + + enum { NumTxMesgBuffers = 6 }; + enum { NumFilters = 16 }; + + 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_; + + 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 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::uint16_t getNumFilters() const { + return NumFilters; + } + + 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 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 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); + } + +/** + * 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); + +/** + * 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); + + 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(); + } + +/** + * 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_; + } + +/** + * 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(); + +/** + * 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); + } +}; + +/** + * CAN driver, incorporates all available CAN ifaces. + * Please avoid direct use, prefer @ref CanInitHelper instead. + */ +class CanDriver : public uavcan::ICanDriver + , uavcan::Noncopyable +{ + BusEvent update_event_; + CanIface if0_; +#if UAVCAN_KINETIS_NUM_IFACES > 1 + CanIface if1_; +#endif + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline); + + 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) +#if UAVCAN_KINETIS_NUM_IFACES > 1 + , if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) +#endif + { + 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; + +/** + * 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); + + virtual CanIface* getIface(uavcan::uint8_t iface_index); + + 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(); +}; + +/** + * Helper class. + * Normally only this class should be used by the application. + * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns. + */ +template +class CanInitHelper +{ + CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]; + +public: + enum { BitRateAutoDetect = 0 }; + + CanDriver driver; + + 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 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 + }; + + for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) + { + inout_bitrate = StandardBitRates[br]; + + const int res = driver.init(inout_bitrate, CanIface::SilentMode); + + delay_callable(); + + 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); + } + } + } + } + + 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 new file mode 100644 index 0000000000..2dec34e3c4 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + +#include +#include + +namespace uavcan_kinetis +{ + +namespace clock +{ +/** + * Starts the clock. + * Can be called multiple times, only the first call will be effective. + */ +void init(); + +/** + * Returns current monotonic time since the moment when clock::init() was called. + * This function is thread safe. + */ +uavcan::MonotonicTime getMonotonic(); + +/** + * Sets the driver's notion of the system UTC. It should be called + * at startup and any time the system clock is updated from an + * external source that is not the UAVCAN Timesync master. + * This function is thread safe. + */ +void setUtc(uavcan::UtcTime time); + +/** + * Returns UTC time if it has been set, otherwise returns zero time. + * This function is thread safe. + */ +uavcan::UtcTime getUtc(); + +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +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 + + 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)) + { + } +}; + +/** + * Clock rate error. + * Positive if the hardware timer is slower than reference time. + * This function is thread safe. + */ +float getUtcRateCorrectionPPM(); + +/** + * Number of non-gradual adjustments performed so far. + * Ideally should be zero. + * This function is thread safe. + */ +uavcan::uint32_t getUtcJumpCount(); + +/** + * Whether UTC is synchronized and locked. + * This function is thread safe. + */ +bool isUtcLocked(); + +/** + * UTC sync params get/set. + * Both functions are thread safe. + */ +UtcSyncParams getUtcSyncParams(); +void setUtcSyncParams(const UtcSyncParams& params); + +} + +/** + * Adapter for uavcan::ISystemClock. + */ +class SystemClock : public uavcan::ISystemClock + , uavcan::Noncopyable +{ + SystemClock() { + } + + 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(); + } + +/** + * 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 new file mode 100644 index 0000000000..cc4f2d7acc --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp @@ -0,0 +1,661 @@ +/* + * Copyright (C) 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + * Bit definitions were copied from NuttX KINETIS CAN driver. + */ + +#pragma once + +#include + +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +// #undef'ed at the end of this file +# define constexpr const +#endif + +namespace uavcan_kinetis +{ +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 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; +}; + +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 + */ +}; + +/* Layout of Fifo, filters and Message buffers */ + +enum { FiFo = 0 }; +/* 0 */ +/* 1 */ +/* 2 */ +/* 3 Fifo */ +/* 4 */ +/* 5 */ +enum { FirstFilter = 6 }; +/* 6 */ +/* 7 */ +/* 8 Filters */ +/* 9 */ +enum { NumHWFilters = 16 }; +enum { NumMBinFiFoAndFilters = 10 }; +/* 10 */ +/* 11 */ +/* 12 */ +/* 13 Tx Message Buffers */ +/* 14 */ +/* 15 */ +/*-- ----------------------*/ + +enum { TXMBMask = (0b111111 << NumMBinFiFoAndFilters) }; + +/** + * CANx register sets + */ + +/* Address of the CAN registers */ + +CanType* const Can[UAVCAN_KINETIS_NUM_IFACES] = +{ + reinterpret_cast(0x40024000) // CAN0 +#if UAVCAN_KINETIS_NUM_IFACES > 1 + , + 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); +/* 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_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 */ + +/* 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_PRESDIV_SHIFT = (24); /* Bits 24-31: Prescaler Division Factor */ +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); +/* 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 */ + +/* 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 */ + +/* 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 */ + +/* 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); +/* 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 */ +/* 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 */ + +/* 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 */ + +/* 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 */ + +/* 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_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_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_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 */ + +/* Control 2 Register */ +/* Bits 0-15: Reserved */ +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_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); +constexpr unsigned long CTRL2_RFFN_24MB(2U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_32MB(3U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_40MB(4U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_48MB(5U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_56MB(6U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_64MB(7U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_72MB(8U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_80MB(9U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_88MB(10U << CTRL2_RFFN_SHIFT); +constexpr unsigned long CTRL2_RFFN_96MB(11U << CTRL2_RFFN_SHIFT); +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 */ + +/* 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); +/* 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 + 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); +/* 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_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 */ +} +} + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# undef constexpr +#endif 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 new file mode 100644 index 0000000000..726721312b --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_kinetis +{ + +class CanDriver; + + +/** + * All bus events are reported as POLLIN. + */ +class BusEvent : uavcan::Noncopyable +{ + static const unsigned MaxPollWaiters = 8; + + ::file_operations file_ops_; + ::pollfd* pollset_[MaxPollWaiters]; + CanDriver& can_driver_; + bool signal_; + + static int openTrampoline(::file* filp); + static int closeTrampoline(::file* filp); + static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup); + + int open(::file* filp); + int close(::file* filp); + int poll(::file* filp, ::pollfd* fds, bool setup); + + int addPollWaiter(::pollfd* fds); + int removePollWaiter(::pollfd* fds); + +public: + static const char* const DevName; + + BusEvent(CanDriver& can_driver); + ~BusEvent(); + + bool wait(uavcan::MonotonicDuration duration); + + void signalFromInterrupt(); +}; + +class Mutex +{ + pthread_mutex_t mutex_; + +public: + Mutex() + { + init(); + } + + int init() + { + return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); + } + + int deinit() + { + return pthread_mutex_destroy(&mutex_); + } + + void lock() + { + (void)pthread_mutex_lock(&mutex_); + } + + void unlock() + { + (void)pthread_mutex_unlock(&mutex_); + } +}; + + +class MutexLocker +{ + Mutex& mutex_; + +public: + MutexLocker(Mutex& mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } +}; + +} diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/uavcan_kinetis.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/uavcan_kinetis.hpp new file mode 100644 index 0000000000..b225491367 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/uavcan_kinetis.hpp @@ -0,0 +1,12 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + +#include + +#include +#include +#include diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp new file mode 100644 index 0000000000..64a98032a4 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + +#include + +#if UAVCAN_KINETIS_NUTTX +# include +# include "up_arch.h" +# include +# include +# include +# include +#else +# error "Unknown OS" +#endif + +/** + * Debug output + */ +#ifndef UAVCAN_KINETIS_LOG +# if 1 +# define UAVCAN_KINETIS_LOG(fmt, ...) syslog(LOG_INFO, "uavcan_kinetis: " fmt "\n", ## __VA_ARGS__) +# else +# define UAVCAN_KINETIS_LOG(...) ((void)0) +# endif +#endif + +/** + * IRQ handler macros + */ +#define UAVCAN_KINETIS_IRQ_HANDLER(id) int id(int irq, FAR void* context, FAR void *arg) + + +/** + * Glue macros + */ +#define UAVCAN_KINETIS_GLUE2_(A, B) A ## B +#define UAVCAN_KINETIS_GLUE2(A, B) UAVCAN_KINETIS_GLUE2_(A, B) + +#define UAVCAN_KINETIS_GLUE3_(A, B, C) A ## B ## C +#define UAVCAN_KINETIS_GLUE3(A, B, C) UAVCAN_KINETIS_GLUE3_(A, B, C) + +namespace uavcan_kinetis +{ +#if UAVCAN_KINETIS_NUTTX + +struct CriticalSectionLocker +{ + const irqstate_t flags_; + + CriticalSectionLocker() + : flags_(enter_critical_section()) + { + } + + ~CriticalSectionLocker() + { + leave_critical_section(flags_); + } +}; + +#endif + +namespace clock +{ +uavcan::uint64_t getUtcUSecFromCanInterrupt(); +} +} 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 new file mode 100644 index 0000000000..a3abdc5535 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp @@ -0,0 +1,374 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#include +#include +#include "internal.hpp" + +#if UAVCAN_KINETIS_TIMER_NUMBER +# include +# include + +/* + * Timer instance + * todo:Consider using Lifetime Timer support + */ +# define TIMX_IRQHandler UAVCAN_KINETIS_GLUE3(PIT, UAVCAN_KINETIS_TIMER_NUMBER, _IRQHandler) +# define TIMX (KINETIS_PIT_BASE + (UAVCAN_KINETIS_TIMER_NUMBER << 4)) +# define TMR_REG(o) (TIMX + (o)) +# define TIMX_INPUT_CLOCK BOARD_BUS_FREQ +# define TIMX_INTERRUPT_FREQ 16 +# define TIMX_IRQn UAVCAN_KINETIS_GLUE2(KINETIS_IRQ_PITCH, UAVCAN_KINETIS_TIMER_NUMBER) + +# if UAVCAN_KINETIS_TIMER_NUMBER >= 0 && UAVCAN_KINETIS_TIMER_NUMBER <= 3 +# define KINETIS_PIT_LDVAL_OFFSET KINETIS_PIT_LDVAL0_OFFSET +# define KINETIS_PIT_CVAL_OFFSET KINETIS_PIT_CVAL0_OFFSET +# define KINETIS_PIT_TCTRL_OFFSET KINETIS_PIT_TCTRL0_OFFSET +# define KINETIS_PIT_TFLG_OFFSET KINETIS_PIT_TFLG0_OFFSET +# else +# error "This UAVCAN_KINETIS_TIMER_NUMBER is not supported yet" +# endif + +extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler); + +namespace uavcan_kinetis +{ +namespace clock +{ +namespace +{ + +const uavcan::uint32_t CountsPerPeriod = (TIMX_INPUT_CLOCK / TIMX_INTERRUPT_FREQ); +const uavcan::uint32_t CountsPerUs = (TIMX_INPUT_CLOCK / 1000000); +const uavcan::uint32_t USecPerOverflow = (1000000 / TIMX_INTERRUPT_FREQ); + +Mutex mutex; + +bool initialized = false; + +bool utc_set = false; +bool utc_locked = false; +uavcan::uint32_t utc_jump_cnt = 0; +UtcSyncParams utc_sync_params; +float utc_prev_adj = 0; +float utc_rel_rate_ppm = 0; +float utc_rel_rate_error_integral = 0; +uavcan::int32_t utc_accumulated_correction_nsec = 0; +uavcan::int32_t utc_correction_nsec_per_overflow = 0; +uavcan::MonotonicTime prev_utc_adj_at; + +uavcan::uint64_t time_mono = 0; +uavcan::uint64_t time_utc = 0; + +} + +void init() +{ + CriticalSectionLocker lock; + if (initialized) + { + return; + } + initialized = true; + + // Attach IRQ + irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL); + + // Power-on Clock + modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT); + + // Enable module + putreg32(0, KINETIS_PIT_MCR); + + // 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); +#endif + up_enable_irq(TIMX_IRQn); +} + +void setUtc(uavcan::UtcTime time) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + { + CriticalSectionLocker locker; + time_utc = time.toUSec(); + } + + 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); + + 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); +} + +uavcan::uint64_t getUtcUSecFromCanInterrupt() +{ + return utc_set ? sampleUtcFromCriticalSection() : 0; +} + +uavcan::MonotonicTime getMonotonic() +{ + 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)); + + 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); + + } // 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(); +} + +static float lowpass(float xold, float xnew, float corner, float dt) +{ + 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()); + + /* + * 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); + + 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); + } + + /* + * 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, +// total_rate_correction_ppm); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + 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); + } + } + + 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; +} + +uavcan::uint32_t getUtcJumpCount() +{ + MutexLocker mlocker(mutex); + return utc_jump_cnt; +} + +bool isUtcLocked() +{ + MutexLocker mlocker(mutex); + return utc_locked; +} + +UtcSyncParams getUtcSyncParams() +{ + MutexLocker mlocker(mutex); + return utc_sync_params; +} + +void setUtcSyncParams(const UtcSyncParams& params) +{ + MutexLocker mlocker(mutex); + // Add some sanity check + utc_sync_params = params; +} + +} // namespace clock + +SystemClock& SystemClock::instance() +{ + 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); + + if (!clock::initialized) + { + MutexLocker mlocker(clock::mutex); + clock::init(); + new (ptr)SystemClock(); + } + return *ptr; +} + +} // namespace uavcan_kinetis + + +/** + * Timer interrupt handler + */ + +extern "C" +UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler) +{ + putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET)); + + using namespace uavcan_kinetis::clock; + UAVCAN_ASSERT(initialized); + + 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; + } + + // 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 new file mode 100644 index 0000000000..25c0716739 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp @@ -0,0 +1,1076 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#include +#include +#include +#include +#include "internal.hpp" + +#if UAVCAN_KINETIS_NUTTX +# include +# include +# include +#else +# error "Unknown OS" +#endif + + +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 +} +namespace flexcan +{ +const uavcan::uint32_t OSCERCLK = BOARD_EXTAL_FREQ; +const uavcan::uint32_t busclck = BOARD_BUS_FREQ; +const uavcan::uint8_t CLOCKSEL = 0; // Select OSCERCLK +const uavcan::uint32_t canclk = CLOCKSEL ? busclck : OSCERCLK; // Is Clock +const uavcan::uint8_t useFIFO = 1; // Fifo is enabled +const uavcan::uint32_t numberFIFOFilters = flexcan::CTRL2_RFFN_16MB; // 16 Rx FIFO filters +} +namespace +{ + +CanIface* ifaces[UAVCAN_KINETIS_NUM_IFACES] = +{ + UAVCAN_NULLPTR +#if UAVCAN_KINETIS_NUM_IFACES > 1 + , UAVCAN_NULLPTR +#endif +}; + +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); +} + +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); +} + +} // namespace + +/* + * CanIface::RxQueue + */ +void CanIface::RxQueue::registerOverflow() +{ + if (overflow_cnt_ < 0xFFFFFFFF) + { + overflow_cnt_++; + } +} + +void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags) +{ + buf_[in_].frame = frame; + buf_[in_].utc_usec = utc_usec; + buf_[in_].flags = flags; + in_++; + if (in_ >= capacity_) + { + in_ = 0; + } + len_++; + if (len_ > capacity_) + { + len_ = capacity_; + registerOverflow(); + out_++; + if (out_ >= capacity_) + { + out_ = 0; + } + } +} + +void CanIface::RxQueue::pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags) +{ + if (len_ > 0) + { + out_frame = buf_[out_].frame; + out_utc_usec = buf_[out_].utc_usec; + out_flags = buf_[out_].flags; + out_++; + if (out_ >= capacity_) + { + out_ = 0; + } + len_--; + } + else + { + UAVCAN_ASSERT(0); + } +} + +void CanIface::RxQueue::reset() +{ + in_ = 0; + out_ = 0; + len_ = 0; + overflow_cnt_ = 0; +} + +/* + * CanIface + */ + +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings) +{ + 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 + */ + + /* Maximize NBT + * NBT * Prescaler = canclk / baud rate. + * + */ + + 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. + */ + + /* 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 + } + + 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; + + /* 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) */ + + 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); + + 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) +{ + 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; + + /* + * 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 mb_bit = 1 << (flexcan::NumMBinFiFoAndFilters + mbi); + + while (mbi < NumTxMesgBuffers) + { + if (can_->MB[flexcan::NumMBinFiFoAndFilters + mbi].mb.CS.code != flexcan::TxMbDataOrRemote) + { + can_->IFLAG1 = mb_bit; + break; + } + mb_bit <<= 1; + mbi++; + } + + if (mbi == NumTxMesgBuffers) + { + return 0; // No transmission for you! + } + + peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, (uavcan::uint8_t) mbi); // Statistics + + 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; + } + + 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) +{ + 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) +{ + if (num_configs <= NumFilters) + { + CriticalSectionLocker lock; + setFreeze(true); + if (!waitFreezeAckChange(true)) + { + return -ErrMcrFRZACKAckNotSet; + } + + volatile flexcan::FilterType* filterBase = reinterpret_cast(&can_->MB[flexcan:: + FirstFilter].mb); + + 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]; + + if (i < num_configs) + { + + const uavcan::CanFilterConfig* const cfg = filter_configs + i; + + 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; +} + +void CanIface::setMCR(uavcan::uint32_t mask, bool target_state) +{ + if (target_state) + { + can_->MCR |= mask; + } + else + { + can_->MCR &= ~mask; + } + +} + +bool CanIface::waitFreezeAckChange(bool 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; + + } + } +} + + +bool CanIface::setEnable(bool enable_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; + } + + 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); + + setMCR(flexcan::MCR_SUPV, false); + + 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::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 */ + + if (!setEnable(false)) + { + UAVCAN_KINETIS_LOG("MCR MCR_LPMACK not set"); + return -ErrMcrLPMAckNotSet; + } + + /* Set the Clock to OSCERCLK or ) */ + + can_->CTRL1 &= ~flexcan::CTRL1_CLKSRC; + can_->CTRL1 |= flexcan::CLOCKSEL; + + /* Set the module enabled */ + + 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 + */ + + 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 + */ + + + uavcan::int16_t rv = doReset(timings); + if (rv != 0) + { + UAVCAN_KINETIS_LOG("doReset returned %d", rv); + return -rv; + } + + 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; + + /* + * 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 + + 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); + + had_activity_ = had_activity_ || txok; + + TxItem& txi = pending_tx_[mailbox_index]; + + if (txi.loopback && txok && txi.pending == TxItem::busy) + { + rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); + } + + txi.pending = TxItem::free; +} + +void CanIface::handleTxInterrupt(uavcan::uint32_t tx_iflags, const uavcan::uint64_t utc_usec) +{ + + /* 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; + + } + + /* 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::CanFrame frame; + + 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_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; + + /* + * 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.rtr) + { + frame.id |= uavcan::CanFrame::FlagRTR; + } + + frame.dlc = rf.CS.dlc; + + 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; + + 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(); +} + +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; + + // 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_++; + } + } +} + +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; + } + + } + + /* + * 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. + } + } + + 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; +} + +uavcan::uint64_t CanIface::getErrorCount() const +{ + CriticalSectionLocker lock; + return error_cnt_ + rx_queue_.getOverflowCount(); +} + +unsigned CanIface::getRxQueueLength() const +{ + CriticalSectionLocker lock; + return rx_queue_.getLength(); +} + +bool CanIface::hadActivity() +{ + 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 msk; + + // Iface 0 + msk.read = if0_.isRxBufferEmpty() ? 0 : 1; + + if (pending_tx[0] != UAVCAN_NULLPTR) + { + msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0; + } + + // 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; + } + } +#endif + return msk; +} + +bool CanDriver::hasReadableInterfaces() const +{ +#if UAVCAN_KINETIS_NUM_IFACES == 1 + return !if0_.isRxBufferEmpty(); +#elif UAVCAN_KINETIS_NUM_IFACES == 2 + 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) +{ + 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(); + } + +#if UAVCAN_KINETIS_NUM_IFACES > 1 + 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; + } + + (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); +#if UAVCAN_KINETIS_NUM_IFACES > 1 + modifyreg32(KINETIS_SIM_SCGC3, 0, SIM_SCGC3_FLEXCAN1); +#endif + } + + /* + * 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); +#if UAVCAN_KINETIS_NUM_IFACES > 1 + 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; + + 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(); + } + + /* + * 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; + } +#endif + + 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; +} + +CanIface* CanDriver::getIface(uavcan::uint8_t iface_index) +{ + if (iface_index < UAVCAN_KINETIS_NUM_IFACES) + { + return ifaces[iface_index]; + } + return UAVCAN_NULLPTR; +} + +bool CanDriver::hadActivity() +{ + bool ret = if0_.hadActivity(); +#if UAVCAN_KINETIS_NUM_IFACES > 1 + ret |= if1_.hadActivity(); +#endif + return ret; +} + + +/* + * Interrupt handlers + */ +extern "C" +{ + +static int can0_irq(const int irq, void*, void* args) +{ + + 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 UAVCAN_KINETIS_NUM_IFACES > 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; +} + +#endif +} // extern "C" +} // namespace uavcan_kinetis 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 new file mode 100644 index 0000000000..4c7db5402b --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#include +#include +#include +#include "internal.hpp" + +namespace uavcan_kinetis +{ + + +const unsigned BusEvent::MaxPollWaiters; +const char* const BusEvent::DevName = "/dev/uavcan/busevent"; + +int BusEvent::openTrampoline(::file* filp) +{ + return static_cast(filp->f_inode->i_private)->open(filp); +} + +int BusEvent::closeTrampoline(::file* filp) +{ + return static_cast(filp->f_inode->i_private)->close(filp); +} + +int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup) +{ + return static_cast(filp->f_inode->i_private)->poll(filp, fds, setup); +} + +int BusEvent::open(::file* filp) +{ + (void)filp; + return 0; +} + +int BusEvent::close(::file* filp) +{ + (void)filp; + return 0; +} + +int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) +{ + CriticalSectionLocker locker; + int ret = -1; + + if (setup) + { + ret = addPollWaiter(fds); + if (ret == 0) + { + /* + * Two events can be reported via POLLIN: + * - The RX queue is not empty. This event is level-triggered. + * - Transmission complete. This event is edge-triggered. + * FIXME Since TX event is edge-triggered, it can be lost between poll() calls. + */ + fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0); + if (fds->revents != 0) + { + (void)sem_post(fds->sem); + } + } + } + else + { + ret = removePollWaiter(fds); + } + + return ret; +} + +int BusEvent::addPollWaiter(::pollfd* fds) +{ + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + if (pollset_[i] == UAVCAN_NULLPTR) + { + pollset_[i] = fds; + return 0; + } + } + return -ENOMEM; +} + +int BusEvent::removePollWaiter(::pollfd* fds) +{ + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + if (fds == pollset_[i]) + { + pollset_[i] = UAVCAN_NULLPTR; + return 0; + } + } + return -EINVAL; +} + +BusEvent::BusEvent(CanDriver& can_driver) + : can_driver_(can_driver), + signal_(false) +{ + std::memset(&file_ops_, 0, sizeof(file_ops_)); + std::memset(pollset_, 0, sizeof(pollset_)); + file_ops_.open = &BusEvent::openTrampoline; + file_ops_.close = &BusEvent::closeTrampoline; + file_ops_.poll = &BusEvent::pollTrampoline; + // TODO: move to init(), add proper error handling + if (register_driver(DevName, &file_ops_, 0666, static_cast(this)) != 0) + { + std::abort(); + } +} + +BusEvent::~BusEvent() +{ + (void)unregister_driver(DevName); +} + +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + // TODO blocking wait + const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration; + while (clock::getMonotonic() < deadline) + { + { + CriticalSectionLocker locker; + if (signal_) + { + signal_ = false; + return true; + } + } + ::usleep(1000); + } + return false; +} + +void BusEvent::signalFromInterrupt() +{ + signal_ = true; // HACK + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + ::pollfd* const fd = pollset_[i]; + if (fd != UAVCAN_NULLPTR) + { + fd->revents |= fd->events & POLLIN; + if ((fd->revents != 0) && (fd->sem->semcount <= 0)) + { + (void)sem_post(fd->sem); + } + } + } +} + +} diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/README.md b/src/drivers/uavcan/uavcan_drivers/stm32/README.md new file mode 100644 index 0000000000..11cf01662f --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/README.md @@ -0,0 +1,11 @@ +STM32 platform driver +===================== + +The directory `driver` contains the STM32 platform driver for Libuavcan. + +A dedicated example application may be added later here. +For now, please consider the following open source projects as a reference: + +- https://github.com/PX4/sapog +- https://github.com/Zubax/zubax_gnss +- https://github.com/PX4/Firmware diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/stm32/driver/CMakeLists.txt new file mode 100644 index 0000000000..ce8ef00234 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/CMakeLists.txt @@ -0,0 +1,17 @@ +include_directories( + ./include + ) + +add_library(uavcan_stm32_driver STATIC + ./src/uc_stm32_can.cpp + ./src/uc_stm32_clock.cpp + ./src/uc_stm32_thread.cpp + ) + +add_dependencies(uavcan_stm32_driver uavcan) + +install(DIRECTORY include/uavcan_stm32 DESTINATION include) +install(TARGETS uavcan_stm32_driver DESTINATION lib) + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :) + diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp new file mode 100644 index 0000000000..0160cc662d --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +/** + * OS detection + */ +#ifndef UAVCAN_STM32_CHIBIOS +# define UAVCAN_STM32_CHIBIOS 0 +#endif + +#ifndef UAVCAN_STM32_NUTTX +# define UAVCAN_STM32_NUTTX 0 +#endif + +#ifndef UAVCAN_STM32_BAREMETAL +# define UAVCAN_STM32_BAREMETAL 0 +#endif + +#ifndef UAVCAN_STM32_FREERTOS +# define UAVCAN_STM32_FREERTOS 0 +#endif + +/** + * Number of interfaces must be enabled explicitly + */ +#if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2) +# error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2" +#endif + +/** + * Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5) + * e.g. -DUAVCAN_STM32_TIMER_NUMBER=2 + */ +#ifndef UAVCAN_STM32_TIMER_NUMBER +// In this case the clock driver should be implemented by the application +# define UAVCAN_STM32_TIMER_NUMBER 0 +#endif 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 new file mode 100644 index 0000000000..7e4679443a --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp @@ -0,0 +1,289 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + * Bit definitions were copied from NuttX STM32 CAN driver. + */ + +#pragma once + +#include + +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +// #undef'ed at the end of this file +# define constexpr const +#endif + +namespace uavcan_stm32 +{ +namespace bxcan +{ + +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 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 */ +}; + +/** + * CANx register sets + */ +CanType* const Can[UAVCAN_STM32_NUM_IFACES] = +{ + reinterpret_cast(0x40006400) +#if UAVCAN_STM32_NUM_IFACES > 1 + , + 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 */ + +/* 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 */ + +/* 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 */ + +/* 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 */ + +/* 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 */ + +/* 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); + +/* 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_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); + +/* 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); + +/* 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); + +/* 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); + +/* 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); + +/* 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); + +/* 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); + +/* 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); + +/* CAN filter master register */ + +constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */ + +} +} + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# undef constexpr +#endif 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 new file mode 100644 index 0000000000..1e743296f4 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -0,0 +1,382 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +namespace uavcan_stm32 +{ +/** + * Driver error codes. + * These values can be returned from driver functions negated. + */ +//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use +static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented +static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported +static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error +static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc) +static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1 +static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0 +static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished +static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported + +/** + * 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) + { } +}; + +/** + * Single CAN iface. + * The application shall not use this directly. + */ +class CanIface : public uavcan::ICanIface, uavcan::Noncopyable +{ + class RxQueue + { + CanRxItem* const buf_; + const uavcan::uint8_t capacity_; + uavcan::uint8_t in_; + uavcan::uint8_t out_; + uavcan::uint8_t len_; + uavcan::uint32_t overflow_cnt_; + + void registerOverflow(); + + public: + RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) + : buf_(buf) + , capacity_(capacity) + , in_(0) + , out_(0) + , len_(0) + , overflow_cnt_(0) + { } + + void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags); + void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags); + + void reset(); + + unsigned getLength() const { return len_; } + + uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; } + }; + + struct Timings + { + uavcan::uint16_t prescaler; + uavcan::uint8_t sjw; + uavcan::uint8_t bs1; + uavcan::uint8_t bs2; + + Timings() + : prescaler(0) + , sjw(0) + , bs1(0) + , bs2(0) + { } + }; + + struct TxItem + { + uavcan::MonotonicTime deadline; + uavcan::CanFrame frame; + bool pending; + bool loopback; + bool abort_on_error; + + TxItem() + : pending(false) + , loopback(false) + , abort_on_error(false) + { } + }; + + enum { NumTxMailboxes = 3 }; + enum { NumFilters = 14 }; + + 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_; + + 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 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::uint16_t getNumFilters() const { return NumFilters; } + + void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec); + + bool waitMsrINakBitStateChange(bool target_state); + +public: + enum { MaxRxQueueCapacity = 254 }; + + 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); + } + + /** + * 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); + + /** + * 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); + + 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(); } + + /** + * 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_; } + + /** + * 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(); + + /** + * 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); } +}; + +/** + * CAN driver, incorporates all available CAN ifaces. + * Please avoid direct use, prefer @ref CanInitHelper instead. + */ +class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable +{ + BusEvent update_event_; + CanIface if0_; +#if UAVCAN_STM32_NUM_IFACES > 1 + CanIface if1_; +#endif + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline); + + 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) +#if UAVCAN_STM32_NUM_IFACES > 1 + , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) +#endif + { + 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; + + /** + * 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); + + virtual CanIface* getIface(uavcan::uint8_t iface_index); + + 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(); +}; + +/** + * Helper class. + * Normally only this class should be used by the application. + * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns. + */ +template +class CanInitHelper +{ + CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]; + +public: + enum { BitRateAutoDetect = 0 }; + + CanDriver driver; + + 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 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 + }; + + for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) + { + inout_bitrate = StandardBitRates[br]; + + const int res = driver.init(inout_bitrate, CanIface::SilentMode); + + delay_callable(); + + 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); + } + } + } + } + + 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 new file mode 100644 index 0000000000..1f422f02dd --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include + +namespace uavcan_stm32 +{ + +namespace clock +{ +/** + * Starts the clock. + * Can be called multiple times, only the first call will be effective. + */ +void init(); + +/** + * Returns current monotonic time since the moment when clock::init() was called. + * This function is thread safe. + */ +uavcan::MonotonicTime getMonotonic(); + +/** + * Sets the driver's notion of the system UTC. It should be called + * at startup and any time the system clock is updated from an + * external source that is not the UAVCAN Timesync master. + * This function is thread safe. + */ +void setUtc(uavcan::UtcTime time); + +/** + * Returns UTC time if it has been set, otherwise returns zero time. + * This function is thread safe. + */ +uavcan::UtcTime getUtc(); + +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +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 + + 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)) + { } +}; + +/** + * Clock rate error. + * Positive if the hardware timer is slower than reference time. + * This function is thread safe. + */ +float getUtcRateCorrectionPPM(); + +/** + * Number of non-gradual adjustments performed so far. + * Ideally should be zero. + * This function is thread safe. + */ +uavcan::uint32_t getUtcJumpCount(); + +/** + * Whether UTC is synchronized and locked. + * This function is thread safe. + */ +bool isUtcLocked(); + +/** + * UTC sync params get/set. + * Both functions are thread safe. + */ +UtcSyncParams getUtcSyncParams(); +void setUtcSyncParams(const UtcSyncParams& params); + +} + +/** + * Adapter for uavcan::ISystemClock. + */ +class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable +{ + SystemClock() { } + + 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(); } + + /** + * 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 new file mode 100644 index 0000000000..b2eac2a65c --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -0,0 +1,239 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +#if UAVCAN_STM32_CHIBIOS +# include +#elif UAVCAN_STM32_NUTTX +# include +# include +# include +# include +# include +# include +# include +#elif UAVCAN_STM32_BAREMETAL +#elif UAVCAN_STM32_FREERTOS +# include +#else +# error "Unknown OS" +#endif + +#include + +namespace uavcan_stm32 +{ + +class CanDriver; + +#if UAVCAN_STM32_CHIBIOS + +class BusEvent +{ + chibios_rt::CounterSemaphore sem_; + +public: + BusEvent(CanDriver& can_driver) + : sem_(0) + { + (void)can_driver; + } + + bool wait(uavcan::MonotonicDuration duration); + + void signal(); + + void signalFromInterrupt(); +}; + +class Mutex +{ + chibios_rt::Mutex mtx_; + +public: + void lock(); + void unlock(); +}; + +#elif UAVCAN_STM32_NUTTX + +/** + * All bus events are reported as POLLIN. + */ +class BusEvent : uavcan::Noncopyable +{ + static const unsigned MaxPollWaiters = 8; + + ::file_operations file_ops_; + ::pollfd* pollset_[MaxPollWaiters]; + CanDriver& can_driver_; + bool signal_; + + static int openTrampoline(::file* filp); + static int closeTrampoline(::file* filp); + static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup); + + int open(::file* filp); + int close(::file* filp); + int poll(::file* filp, ::pollfd* fds, bool setup); + + int addPollWaiter(::pollfd* fds); + int removePollWaiter(::pollfd* fds); + +public: + static const char* const DevName; + + BusEvent(CanDriver& can_driver); + ~BusEvent(); + + bool wait(uavcan::MonotonicDuration duration); + + void signalFromInterrupt(); +}; + +class Mutex +{ + pthread_mutex_t mutex_; + +public: + Mutex() + { + init(); + } + + int init() + { + return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); + } + + int deinit() + { + return pthread_mutex_destroy(&mutex_); + } + + void lock() + { + (void)pthread_mutex_lock(&mutex_); + } + + void unlock() + { + (void)pthread_mutex_unlock(&mutex_); + } +}; +#elif UAVCAN_STM32_BAREMETAL + +class BusEvent +{ + volatile bool ready; + +public: + BusEvent(CanDriver& can_driver) + : ready(false) + { + (void)can_driver; + } + + bool wait(uavcan::MonotonicDuration duration) + { + (void)duration; + bool lready = ready; + #if defined ( __CC_ARM ) + return __sync_lock_test_and_set(&lready, false); + #elif defined ( __GNUC__ ) + return __atomic_exchange_n (&lready, false, __ATOMIC_SEQ_CST); + #else + # error "This compiler is not supported" + #endif + } + + void signal() + { + #if defined ( __CC_ARM ) + __sync_lock_release(&ready); + #elif defined ( __GNUC__ ) + __atomic_store_n (&ready, true, __ATOMIC_SEQ_CST); + #else + # error "This compiler is not supported" + #endif + } + + void signalFromInterrupt() + { + #if defined ( __CC_ARM ) + __sync_lock_release(&ready); + #elif defined ( __GNUC__ ) + __atomic_store_n (&ready, true, __ATOMIC_SEQ_CST); + #else + # error "This compiler is not supported" + #endif + } +}; + +class Mutex +{ +public: + void lock() { } + void unlock() { } +}; + +#elif UAVCAN_STM32_FREERTOS + +class BusEvent +{ + SemaphoreHandle_t sem_; + BaseType_t higher_priority_task_woken; + +public: + BusEvent(CanDriver& can_driver) + { + (void)can_driver; + sem_ = xSemaphoreCreateBinary(); + } + + bool wait(uavcan::MonotonicDuration duration); + + void signal(); + + void signalFromInterrupt(); + + void yieldFromISR(); +}; + +class Mutex +{ + SemaphoreHandle_t mtx_; + +public: + Mutex(void) + { + mtx_ = xSemaphoreCreateMutex(); + } + void lock(); + void unlock(); +}; + +#endif + + +class MutexLocker +{ + Mutex& mutex_; + +public: + MutexLocker(Mutex& mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } +}; + +} diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp new file mode 100644 index 0000000000..e55d48506c --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/uavcan_stm32.hpp @@ -0,0 +1,11 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +#include +#include +#include diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp new file mode 100644 index 0000000000..8f70a1f9b0 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +#if UAVCAN_STM32_CHIBIOS +# include +#elif UAVCAN_STM32_NUTTX +# include +# include +# include +# include +#elif UAVCAN_STM32_BAREMETAL +#include // See http://uavcan.org/Implementations/Libuavcan/Platforms/STM32/ +#elif UAVCAN_STM32_FREERTOS +# include +# include +#else +# error "Unknown OS" +#endif + +/** + * Debug output + */ +#ifndef UAVCAN_STM32_LOG +// syslog() crashes the system in this context +// # if UAVCAN_STM32_NUTTX && CONFIG_ARCH_LOWPUTC +# if 0 +# define UAVCAN_STM32_LOG(fmt, ...) syslog("uavcan_stm32: " fmt "\n", ##__VA_ARGS__) +# else +# define UAVCAN_STM32_LOG(...) ((void)0) +# endif +#endif + +/** + * IRQ handler macros + */ +#if UAVCAN_STM32_CHIBIOS +# define UAVCAN_STM32_IRQ_HANDLER(id) CH_IRQ_HANDLER(id) +# define UAVCAN_STM32_IRQ_PROLOGUE() CH_IRQ_PROLOGUE() +# define UAVCAN_STM32_IRQ_EPILOGUE() CH_IRQ_EPILOGUE() +#elif UAVCAN_STM32_NUTTX +# define UAVCAN_STM32_IRQ_HANDLER(id) int id(int irq, FAR void* context, FAR void *arg) +# define UAVCAN_STM32_IRQ_PROLOGUE() +# define UAVCAN_STM32_IRQ_EPILOGUE() return 0; +#else +# define UAVCAN_STM32_IRQ_HANDLER(id) void id(void) +# define UAVCAN_STM32_IRQ_PROLOGUE() +# define UAVCAN_STM32_IRQ_EPILOGUE() +#endif + +#if UAVCAN_STM32_CHIBIOS +/** + * Priority mask for timer and CAN interrupts. + */ +# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK +# if (CH_KERNEL_MAJOR == 2) +# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY) +# else // ChibiOS 3+ +# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_MAX_KERNEL_PRIORITY +# endif +# endif +#endif + +#if UAVCAN_STM32_BAREMETAL +/** + * Priority mask for timer and CAN interrupts. + */ +# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK +# define UAVCAN_STM32_IRQ_PRIORITY_MASK 0 +# endif +#endif + +#if UAVCAN_STM32_FREERTOS +/** + * Priority mask for timer and CAN interrupts. + */ +# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK +# define UAVCAN_STM32_IRQ_PRIORITY_MASK configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY +# endif +#endif + +/** + * Glue macros + */ +#define UAVCAN_STM32_GLUE2_(A, B) A##B +#define UAVCAN_STM32_GLUE2(A, B) UAVCAN_STM32_GLUE2_(A, B) + +#define UAVCAN_STM32_GLUE3_(A, B, C) A##B##C +#define UAVCAN_STM32_GLUE3(A, B, C) UAVCAN_STM32_GLUE3_(A, B, C) + +namespace uavcan_stm32 +{ +#if UAVCAN_STM32_CHIBIOS + +struct CriticalSectionLocker +{ + CriticalSectionLocker() { chSysSuspend(); } + ~CriticalSectionLocker() { chSysEnable(); } +}; + +#elif UAVCAN_STM32_NUTTX + +struct CriticalSectionLocker +{ + const irqstate_t flags_; + + CriticalSectionLocker() + : flags_(enter_critical_section()) + { } + + ~CriticalSectionLocker() + { + leave_critical_section(flags_); + } +}; + +#elif UAVCAN_STM32_BAREMETAL + +struct CriticalSectionLocker +{ + + CriticalSectionLocker() + { + __disable_irq(); + } + + ~CriticalSectionLocker() + { + __enable_irq(); + } +}; + +#elif UAVCAN_STM32_FREERTOS + +struct CriticalSectionLocker +{ + + CriticalSectionLocker() + { + taskENTER_CRITICAL(); + } + + ~CriticalSectionLocker() + { + taskEXIT_CRITICAL(); + } +}; + +#endif + +namespace clock +{ +uavcan::uint64_t getUtcUSecFromCanInterrupt(); +} +} 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 new file mode 100644 index 0000000000..2fe8ad67e0 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp @@ -0,0 +1,1235 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "internal.hpp" + +#if UAVCAN_STM32_CHIBIOS +# include +#elif UAVCAN_STM32_NUTTX +# include +# include +# include +#elif UAVCAN_STM32_BAREMETAL +#include // See http://uavcan.org/Implementations/Libuavcan/Platforms/STM32/ +#elif UAVCAN_STM32_FREERTOS +#else +# error "Unknown OS" +#endif + +#if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL +# if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F3XX) || defined(STM32F4XX)) +// IRQ numbers +# define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn +# define CAN1_TX_IRQn USB_HP_CAN1_TX_IRQn +// IRQ vectors +# if !defined(CAN1_RX0_IRQHandler) || !defined(CAN1_TX_IRQHandler) +# define CAN1_TX_IRQHandler USB_HP_CAN1_TX_IRQHandler +# define CAN1_RX0_IRQHandler USB_LP_CAN1_RX0_IRQHandler +# endif +# endif +#endif + +#if (UAVCAN_STM32_CHIBIOS && (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5)) +#define CAN1_TX_IRQHandler STM32_CAN1_TX_HANDLER +#define CAN1_RX0_IRQHandler STM32_CAN1_RX0_HANDLER +#define CAN1_RX1_IRQHandler STM32_CAN1_RX1_HANDLER +#define CAN2_TX_IRQHandler STM32_CAN2_TX_HANDLER +#define CAN2_RX0_IRQHandler STM32_CAN2_RX0_HANDLER +#define CAN2_RX1_IRQHandler STM32_CAN2_RX1_HANDLER +#endif + +#if UAVCAN_STM32_NUTTX +# if !defined(STM32_IRQ_CAN1TX) && !defined(STM32_IRQ_CAN1RX0) +# define STM32_IRQ_CAN1TX STM32_IRQ_USBHPCANTX +# define STM32_IRQ_CAN1RX0 STM32_IRQ_USBLPCANRX0 +# endif +extern "C" +{ +static int can1_irq(const int irq, void*, void*); +#if UAVCAN_STM32_NUM_IFACES > 1 +static int can2_irq(const int irq, void*, void*); +#endif +} +#endif + +/* STM32F3's only CAN inteface does not have a number. */ +#if defined(STM32F3XX) +#define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN +#define RCC_APB1RSTR_CAN1RST RCC_APB1RSTR_CANRST +#define CAN1_TX_IRQn CAN_TX_IRQn +#define CAN1_RX0_IRQn CAN_RX0_IRQn +#define CAN1_RX1_IRQn CAN_RX1_IRQn +# if UAVCAN_STM32_BAREMETAL +# define CAN1_TX_IRQHandler CAN_TX_IRQHandler +# define CAN1_RX0_IRQHandler CAN_RX0_IRQHandler +# define CAN1_RX1_IRQHandler CAN_RX1_IRQHandler +# endif +#endif + + +namespace uavcan_stm32 +{ +namespace +{ + +CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] = +{ + UAVCAN_NULLPTR +#if UAVCAN_STM32_NUM_IFACES > 1 + , 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); + } +} + +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); + } +} + +} // namespace + +/* + * CanIface::RxQueue + */ +void CanIface::RxQueue::registerOverflow() +{ + if (overflow_cnt_ < 0xFFFFFFFF) + { + overflow_cnt_++; + } +} + +void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags) +{ + buf_[in_].frame = frame; + buf_[in_].utc_usec = utc_usec; + buf_[in_].flags = flags; + in_++; + if (in_ >= capacity_) + { + in_ = 0; + } + len_++; + if (len_ > capacity_) + { + len_ = capacity_; + registerOverflow(); + out_++; + if (out_ >= capacity_) + { + out_ = 0; + } + } +} + +void CanIface::RxQueue::pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags) +{ + if (len_ > 0) + { + out_frame = buf_[out_].frame; + out_utc_usec = buf_[out_].utc_usec; + out_flags = buf_[out_].flags; + out_++; + if (out_ >= capacity_) + { + out_ = 0; + } + len_--; + } + else { UAVCAN_ASSERT(0); } +} + +void CanIface::RxQueue::reset() +{ + 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 +}; + +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings) +{ + if (target_bitrate < 1) + { + return -ErrInvalidBitRate; + } + + /* + * Hardware configuration + */ +#if UAVCAN_STM32_BAREMETAL + const uavcan::uint32_t pclk = STM32_PCLK1; +#elif UAVCAN_STM32_CHIBIOS + const uavcan::uint32_t pclk = STM32_PCLK1; +#elif UAVCAN_STM32_NUTTX + const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY; +#elif UAVCAN_STM32_FREERTOS + const uavcan::uint32_t pclk = HAL_RCC_GetPCLK1Freq(); +#else +# error "Unknown OS" +#endif + + 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; + + UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); + + 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; + + /* + * 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--; + } + + const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); + if ((prescaler < 1U) || (prescaler > 1024U)) + { + return -ErrInvalidBitRate; // No solution + } + + /* + * 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; + + BsPair() : + bs1(0), + bs2(0), + sample_point_permill(0) + { } + + 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); + } + + bool isValid() const { return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); } + }; + + // First attempt with rounding to nearest + BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); + + 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)); + } + + /* + * 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) +{ + 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; + + /* + * 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. + } + + peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics + + /* + * 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); + } + + 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) +{ + 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) +{ + if (num_configs <= NumFilters) + { + CriticalSectionLocker lock; + + can_->FMR |= bxcan::FMR_FINIT; + + // 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 + + 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; + + const uavcan::CanFilterConfig* const cfg = filter_configs + i; + + 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. + } + + if (cfg->id & uavcan::CanFrame::FlagRTR) + { + id |= bxcan::RIR_RTR; + } + + if (cfg->mask & uavcan::CanFrame::FlagEFF) + { + mask |= bxcan::RIR_IDE; + } + + if (cfg->mask & uavcan::CanFrame::FlagRTR) + { + mask |= bxcan::RIR_RTR; + } + + can_->FilterRegister[filter_start_index + i].FR1 = id; + can_->FilterRegister[filter_start_index + i].FR2 = mask; + + can_->FA1R |= (1 << (filter_start_index + i)); + } + 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 || UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_FREERTOS + const unsigned Timeout = 1000; +#else + 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; + } +#if UAVCAN_STM32_NUTTX + ::usleep(1000); +#endif +#if UAVCAN_STM32_CHIBIOS +#ifdef MS2ST + ::chThdSleep(MS2ST(1)); +#else + ::chThdSleep(TIME_MS2I(1)); +#endif +#endif +#if UAVCAN_STM32_FREERTOS + ::osDelay(1); +#endif + } + 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; + + 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 + } + + 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; + + /* + * 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)); + + /* + * 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_->BTR = ((timings.sjw & 3U) << 24) | + ((timings.bs1 & 15U) << 16) | + ((timings.bs2 & 7U) << 20) | + (timings.prescaler & 1023U) | + ((mode == SilentMode) ? bxcan::BTR_SILM : 0); + + 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 + + can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode + + if (!waitMsrINakBitStateChange(false)) + { + UAVCAN_STM32_LOG("MSR INAK not cleared"); + can_->MCR = bxcan::MCR_RESET; + return -ErrMsrInakNotCleared; + } + + /* + * 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 +#else + can_->FS1R = 0x1fff; + can_->FilterRegister[0].FR1 = 0; + can_->FilterRegister[0].FR2 = 0; + can_->FA1R = 1; +#endif + + can_->FMR &= ~bxcan::FMR_FINIT; + } + + return 0; +} + +void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec) +{ + UAVCAN_ASSERT(mailbox_index < NumTxMailboxes); + + had_activity_ = had_activity_ || txok; + + TxItem& txi = pending_tx_[mailbox_index]; + + if (txi.loopback && txok && txi.pending) + { + rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); + } + + txi.pending = false; +} + +void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) +{ + // TXOK == false means that there was a hardware failure + if (can_->TSR & 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(); + + pollErrorFlagsFromISR(); + + #if UAVCAN_STM32_FREERTOS + update_event_.yieldFromISR(); + #endif +} + +void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec) +{ + 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; + } + + /* + * Register overflow as a hardware error + */ + if ((*rfr_reg & bxcan::RFR_FOVR) != 0) + { + error_cnt_++; + } + + /* + * Read the frame contents + */ + uavcan::CanFrame frame; + const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index]; + + 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; + } + + if ((rf.RIR & bxcan::RIR_RTR) != 0) + { + frame.id |= uavcan::CanFrame::FlagRTR; + } + + frame.dlc = rf.RDTR & 15; + + 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)); + + *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(); + + #if UAVCAN_STM32_FREERTOS + update_event_.yieldFromISR(); + #endif +} + +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_++; + + // 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_++; + } + } +} + +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; + + if (tme == TME) // All TX mailboxes are free (as in freedom). + { + return true; + } + + if (tme == 0) // All TX mailboxes are busy transmitting. + { + return false; + } + } + + /* + * 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. + } + } + + 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; +} + +uavcan::uint64_t CanIface::getErrorCount() const +{ + CriticalSectionLocker lock; + return error_cnt_ + rx_queue_.getOverflowCount(); +} + +unsigned CanIface::getRxQueueLength() const +{ + CriticalSectionLocker lock; + return rx_queue_.getLength(); +} + +bool CanIface::hadActivity() +{ + 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 msk; + + // Iface 0 + msk.read = if0_.isRxBufferEmpty() ? 0 : 1; + + if (pending_tx[0] != UAVCAN_NULLPTR) + { + msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0; + } + + // 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; + } + } +#endif + return msk; +} + +bool CanDriver::hasReadableInterfaces() const +{ +#if UAVCAN_STM32_NUM_IFACES == 1 + return !if0_.isRxBufferEmpty(); +#elif UAVCAN_STM32_NUM_IFACES == 2 + return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty(); +#else +# error UAVCAN_STM32_NUM_IFACES +#endif +} + +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(); + + 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(); + } +#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; + } + + (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 UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + +static void nvicEnableVector(IRQn_Type irq, uint8_t prio) +{ + #if !defined (USE_HAL_DRIVER) + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + #else + HAL_NVIC_SetPriority(irq, prio, 0); + HAL_NVIC_EnableIRQ(irq); + #endif +} + +#endif + +void CanDriver::initOnce() +{ + /* + * 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); +# 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); +# endif +#else + 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; +# endif +#endif + } + + /* + * 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); +# 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); +# endif +# undef IRQ_ATTACH +#elif UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + { + CriticalSectionLocker lock; + nvicEnableVector(CAN1_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN1_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN1_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); +# if UAVCAN_STM32_NUM_IFACES > 1 + nvicEnableVector(CAN2_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN2_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN2_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); +# endif + } +#endif +} + +int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) +{ + int res = 0; + + 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(); + } + + /* + * 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; + } +#endif + + 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; +} + +CanIface* CanDriver::getIface(uavcan::uint8_t iface_index) +{ + if (iface_index < UAVCAN_STM32_NUM_IFACES) + { + return ifaces[iface_index]; + } + return UAVCAN_NULLPTR; +} + +bool CanDriver::hadActivity() +{ + bool ret = if0_.hadActivity(); +#if UAVCAN_STM32_NUM_IFACES > 1 + ret |= if1_.hadActivity(); +#endif + return ret; +} + +} // namespace uavcan_stm32 + +/* + * Interrupt handlers + */ +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; +} + +# 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; +} + +# endif + +#else // UAVCAN_STM32_NUTTX + +#if !defined(CAN1_TX_IRQHandler) ||\ + !defined(CAN1_RX0_IRQHandler) ||\ + !defined(CAN1_RX1_IRQHandler) +# 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_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(); +} + +# if UAVCAN_STM32_NUM_IFACES > 1 + +#if !defined(CAN2_TX_IRQHandler) ||\ + !defined(CAN2_RX0_IRQHandler) ||\ + !defined(CAN2_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_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(); +} + +# endif +#endif // UAVCAN_STM32_NUTTX + +} // extern "C" 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 new file mode 100644 index 0000000000..1a4cd1e682 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -0,0 +1,496 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "internal.hpp" + +#if UAVCAN_STM32_TIMER_NUMBER + +#include +#include + +/* + * Timer instance + */ +# if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS +# define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER) +# define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn) +# define TIMX_INPUT_CLOCK STM32_TIMCLK1 +# endif + +# if (UAVCAN_STM32_CHIBIOS && (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5)) +# define TIMX UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER) +# define TIMX_IRQn UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER) +# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER) +# define TIMX_INPUT_CLOCK STM32_TIMCLK1 +# else +# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler) +# endif + +# if UAVCAN_STM32_NUTTX +# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE) +# define TMR_REG(o) (TIMX + (o)) +# define TIMX_INPUT_CLOCK UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN) + +# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER) +# endif + +# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7 +# define TIMX_RCC_ENR RCC->APB1ENR +# define TIMX_RCC_RSTR RCC->APB1RSTR +# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN) +# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST) +# else +# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet" +# endif + +/** + * UAVCAN_STM32_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed. + * This is useful at least with certain versions of ChibiOS which do not support the bit + * RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active, + * the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed. + * Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870 + * A normal way to use the override feature is to provide an alternative macro, e.g.: + * + * -DUAVCAN_STM32_TIMX_INPUT_CLOCK=STM32_HCLK + * + * Alternatively, the new clock rate can be specified directly. + */ +# ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK +# undef TIMX_INPUT_CLOCK +# define TIMX_INPUT_CLOCK UAVCAN_STM32_TIMX_INPUT_CLOCK +# endif + +extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler); + +namespace uavcan_stm32 +{ +namespace clock +{ +namespace +{ + +const uavcan::uint32_t USecPerOverflow = 65536; + +Mutex mutex; + +bool initialized = false; + +bool utc_set = false; +bool utc_locked = false; +uavcan::uint32_t utc_jump_cnt = 0; +UtcSyncParams utc_sync_params; +float utc_prev_adj = 0; +float utc_rel_rate_ppm = 0; +float utc_rel_rate_error_integral = 0; +uavcan::int32_t utc_accumulated_correction_nsec = 0; +uavcan::int32_t utc_correction_nsec_per_overflow = 0; +uavcan::MonotonicTime prev_utc_adj_at; + +uavcan::uint64_t time_mono = 0; +uavcan::uint64_t time_utc = 0; + +} + +#if UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + +static void nvicEnableVector(IRQn_Type irq, uint8_t prio) +{ + #if !defined (USE_HAL_DRIVER) + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + #else + HAL_NVIC_SetPriority(irq, prio, 0); + HAL_NVIC_EnableIRQ(irq); + #endif + +} + +#endif + +void init() +{ + CriticalSectionLocker lock; + if (initialized) + { + return; + } + initialized = true; + + +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + // Power-on and reset + TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK; + TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK; + TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK; + + // Enable IRQ + nvicEnableVector(TIMX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + +# if (TIMX_INPUT_CLOCK % 1000000) != 0 +# error "No way, timer clock must be divisible by 1e6. FIXME!" +# endif + + // Start the timer + TIMX->ARR = 0xFFFF; + TIMX->PSC = (TIMX_INPUT_CLOCK / 1000000) - 1; // 1 tick == 1 microsecond + TIMX->CR1 = TIM_CR1_URS; + TIMX->SR = 0; + TIMX->EGR = TIM_EGR_UG; // Reload immediately + TIMX->DIER = TIM_DIER_UIE; + TIMX->CR1 = TIM_CR1_CEN; // Start + +# endif + +# if UAVCAN_STM32_NUTTX + + // 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); + + + // 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 +// 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); + +# endif +} + +void setUtc(uavcan::UtcTime time) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + { + CriticalSectionLocker locker; + time_utc = time.toUSec(); + } + + 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_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + UAVCAN_ASSERT(initialized); + UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE); + + volatile uavcan::uint64_t time = time_utc; + volatile uavcan::uint32_t cnt = TIMX->CNT; + + if (TIMX->SR & TIM_SR_UIF) + { + cnt = TIMX->CNT; + 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 + +# if UAVCAN_STM32_NUTTX + + 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)); + + 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; +} + +uavcan::MonotonicTime getMonotonic() +{ + uavcan::uint64_t usec = 0; + // Scope Critical section + { + CriticalSectionLocker locker; + + volatile uavcan::uint64_t time = time_mono; + +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + + volatile uavcan::uint32_t cnt = TIMX->CNT; + if (TIMX->SR & TIM_SR_UIF) + { + cnt = TIMX->CNT; +# endif + +# if UAVCAN_STM32_NUTTX + + 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)); +# endif + 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; +# endif + } // 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(); +} + +static float lowpass(float xold, float xnew, float corner, float dt) +{ + 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()); + + /* + * 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); + + 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); + } + + /* + * 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, +// total_rate_correction_ppm); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + 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); + } + } + + 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; +} + +uavcan::uint32_t getUtcJumpCount() +{ + MutexLocker mlocker(mutex); + return utc_jump_cnt; +} + +bool isUtcLocked() +{ + MutexLocker mlocker(mutex); + return utc_locked; +} + +UtcSyncParams getUtcSyncParams() +{ + MutexLocker mlocker(mutex); + return utc_sync_params; +} + +void setUtcSyncParams(const UtcSyncParams& params) +{ + MutexLocker mlocker(mutex); + // Add some sanity check + utc_sync_params = params; +} + +} // namespace clock + +SystemClock& SystemClock::instance() +{ + 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); + + if (!clock::initialized) + { + MutexLocker mlocker(clock::mutex); + clock::init(); + new (ptr)SystemClock(); + } + return *ptr; +} + +} // namespace uavcan_stm32 + + +/** + * Timer interrupt handler + */ + +extern "C" +UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + +# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + TIMX->SR = 0; +# endif +# if UAVCAN_STM32_NUTTX + putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); +# endif + + using namespace uavcan_stm32::clock; + UAVCAN_ASSERT(initialized); + + 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; + } + + // 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 new file mode 100644 index 0000000000..a347dcb0f8 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -0,0 +1,287 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "internal.hpp" + + +namespace uavcan_stm32 +{ + +#if UAVCAN_STM32_CHIBIOS +/* + * BusEvent + */ +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + // set maximum time to allow for 16 bit timers running at 1MHz + static const uavcan::int64_t MaxDelayUSec = 0x000FFFF; + + const uavcan::int64_t usec = duration.toUSec(); + msg_t ret = msg_t(); + + if (usec <= 0) + { +# if (CH_KERNEL_MAJOR == 2) + ret = sem_.waitTimeout(TIME_IMMEDIATE); +# else // ChibiOS 3+ + ret = sem_.wait(TIME_IMMEDIATE); +# endif + } + else + { +# if (CH_KERNEL_MAJOR == 2) + ret = sem_.waitTimeout((usec > MaxDelayUSec) ? US2ST(MaxDelayUSec) : US2ST(usec)); +# elif defined(MS2ST) // ChibiOS 3+ + ret = sem_.wait((usec > MaxDelayUSec) ? US2ST(MaxDelayUSec) : US2ST(usec)); +# else // ChibiOS 17+ + ret = sem_.wait(::systime_t((usec > MaxDelayUSec) ? TIME_US2I(MaxDelayUSec) : TIME_US2I(usec))); +# endif + } +# if (CH_KERNEL_MAJOR == 2) + return ret == RDY_OK; +# else // ChibiOS 3+ + return ret == MSG_OK; +# endif +} + +void BusEvent::signal() +{ + sem_.signal(); +} + +void BusEvent::signalFromInterrupt() +{ +# if (CH_KERNEL_MAJOR == 2) + chSysLockFromIsr(); + sem_.signalI(); + chSysUnlockFromIsr(); +# else // ChibiOS 3+ + chSysLockFromISR(); + sem_.signalI(); + chSysUnlockFromISR(); +# endif +} + +/* + * Mutex + */ +void Mutex::lock() +{ + mtx_.lock(); +} + +void Mutex::unlock() +{ +# if (CH_KERNEL_MAJOR == 2) + chibios_rt::BaseThread::unlockMutex(); +# else // ChibiOS 3+ + mtx_.unlock(); +# endif +} + + +#elif UAVCAN_STM32_FREERTOS + +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF; + + const uavcan::int64_t msec = duration.toMSec(); + + BaseType_t ret; + + if (msec <= 0) + { + ret = xSemaphoreTake( sem_, ( TickType_t ) 0 ); + } + else + { + ret = xSemaphoreTake( sem_, (msec > MaxDelayMSec) ? (MaxDelayMSec/portTICK_RATE_MS) : (msec/portTICK_RATE_MS)); + } + return ret == pdTRUE; +} + +void BusEvent::signal() +{ + xSemaphoreGive( sem_ ); +} + +void BusEvent::signalFromInterrupt() +{ + higher_priority_task_woken = pdFALSE; + + xSemaphoreGiveFromISR( sem_, &higher_priority_task_woken ); +} + +void BusEvent::yieldFromISR() +{ + portYIELD_FROM_ISR( higher_priority_task_woken ); +} + +/* + * Mutex + */ +void Mutex::lock() +{ + xSemaphoreTake( mtx_, portMAX_DELAY ); +} + +void Mutex::unlock() +{ + xSemaphoreGive( mtx_ ); +} + + +#elif UAVCAN_STM32_NUTTX + +const unsigned BusEvent::MaxPollWaiters; +const char* const BusEvent::DevName = "/dev/uavcan/busevent"; + +int BusEvent::openTrampoline(::file* filp) +{ + return static_cast(filp->f_inode->i_private)->open(filp); +} + +int BusEvent::closeTrampoline(::file* filp) +{ + return static_cast(filp->f_inode->i_private)->close(filp); +} + +int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup) +{ + return static_cast(filp->f_inode->i_private)->poll(filp, fds, setup); +} + +int BusEvent::open(::file* filp) +{ + (void)filp; + return 0; +} + +int BusEvent::close(::file* filp) +{ + (void)filp; + return 0; +} + +int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) +{ + CriticalSectionLocker locker; + int ret = -1; + + if (setup) + { + ret = addPollWaiter(fds); + if (ret == 0) + { + /* + * Two events can be reported via POLLIN: + * - The RX queue is not empty. This event is level-triggered. + * - Transmission complete. This event is edge-triggered. + * FIXME Since TX event is edge-triggered, it can be lost between poll() calls. + */ + fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0); + if (fds->revents != 0) + { + (void)sem_post(fds->sem); + } + } + } + else + { + ret = removePollWaiter(fds); + } + + return ret; +} + +int BusEvent::addPollWaiter(::pollfd* fds) +{ + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + if (pollset_[i] == UAVCAN_NULLPTR) + { + pollset_[i] = fds; + return 0; + } + } + return -ENOMEM; +} + +int BusEvent::removePollWaiter(::pollfd* fds) +{ + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + if (fds == pollset_[i]) + { + pollset_[i] = UAVCAN_NULLPTR; + return 0; + } + } + return -EINVAL; +} + +BusEvent::BusEvent(CanDriver& can_driver) + : can_driver_(can_driver) + , signal_(false) +{ + std::memset(&file_ops_, 0, sizeof(file_ops_)); + std::memset(pollset_, 0, sizeof(pollset_)); + file_ops_.open = &BusEvent::openTrampoline; + file_ops_.close = &BusEvent::closeTrampoline; + file_ops_.poll = &BusEvent::pollTrampoline; + // TODO: move to init(), add proper error handling + if (register_driver(DevName, &file_ops_, 0666, static_cast(this)) != 0) + { + std::abort(); + } +} + +BusEvent::~BusEvent() +{ + (void)unregister_driver(DevName); +} + +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + // TODO blocking wait + const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration; + while (clock::getMonotonic() < deadline) + { + { + CriticalSectionLocker locker; + if (signal_) + { + signal_ = false; + return true; + } + } + ::usleep(1000); + } + return false; +} + +void BusEvent::signalFromInterrupt() +{ + signal_ = true; // HACK + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + ::pollfd* const fd = pollset_[i]; + if (fd != UAVCAN_NULLPTR) + { + fd->revents |= fd->events & POLLIN; + if ((fd->revents != 0) && (fd->sem->semcount <= 0)) + { + (void)sem_post(fd->sem); + } + } + } +} + +#endif + +} diff --git a/src/drivers/uavcanesc/CMakeLists.txt b/src/drivers/uavcanesc/CMakeLists.txt index 093a557cfd..41314c4625 100644 --- a/src/drivers/uavcanesc/CMakeLists.txt +++ b/src/drivers/uavcanesc/CMakeLists.txt @@ -32,11 +32,12 @@ ############################################################################ set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) +set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") -set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") +set(UAVCAN_PLATFORM generic CACHE STRING "uavcan platform") string(TOUPPER "${PX4_PLATFORM}" OS_UPPER) add_definitions( @@ -44,7 +45,7 @@ add_definitions( -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS - -DUAVCAN_PLATFORM=stm32 + -DUAVCAN_PLATFORM=generic -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_STM32_NUM_IFACES=1 -DUAVCAN_STM32_TIMER_NUMBER=2 @@ -54,13 +55,18 @@ add_definitions( add_compile_options(-Wno-cast-align) # TODO: fix and enable add_subdirectory(${LIBUAVCAN_DIR} uavcanesc_libuavcan) +add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/stm32/driver uavcanesc_uavcan_drivers) +target_include_directories(uavcan_stm32_driver PUBLIC + ${LIBUAVCAN_DIR}/libuavcan/include + ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated + ) add_dependencies(uavcan prebuild_targets) include_directories(${PX4_SOURCE_DIR}/src/modules/systemlib/flashparams) include_directories(${LIBUAVCAN_DIR}/libuavcan/include) include_directories(${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated) include_directories(${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include) -include_directories(${LIBUAVCAN_DIR}/libuavcan_drivers/stm32/driver/include) +include_directories(${LIBUAVCAN_DIR_DRIVERS}/stm32/driver/include) include_directories(${PX4_SOURCE_DIR}/src/drivers/bootloaders) @@ -82,10 +88,11 @@ px4_add_module( version cdev + uavcan_stm32_driver + # within libuavcan libuavcan_dsdlc uavcan - uavcan_stm32_driver ) add_subdirectory(commands) diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index f586fc82e5..af2cc514db 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -32,11 +32,12 @@ ############################################################################ set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) +set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") -set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") +set(UAVCAN_PLATFORM generic CACHE STRING "uavcan platform") string(TOUPPER "${PX4_PLATFORM}" OS_UPPER) add_definitions( @@ -44,7 +45,7 @@ add_definitions( -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS - -DUAVCAN_PLATFORM=stm32 + -DUAVCAN_PLATFORM=generic -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_STM32_NUM_IFACES=1 -DUAVCAN_STM32_TIMER_NUMBER=2 @@ -54,6 +55,11 @@ add_definitions( add_compile_options(-Wno-cast-align) # TODO: fix and enable add_subdirectory(${LIBUAVCAN_DIR} uavcannode_libuavcan) +add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/stm32/driver uavcanesc_uavcan_drivers) +target_include_directories(uavcan_stm32_driver PUBLIC + ${LIBUAVCAN_DIR}/libuavcan/include + ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated + ) add_dependencies(uavcan prebuild_targets) px4_add_module( @@ -64,7 +70,7 @@ px4_add_module( ${LIBUAVCAN_DIR}/libuavcan/include ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include - ${LIBUAVCAN_DIR}/libuavcan_drivers/stm32/driver/include + ${LIBUAVCAN_DIR_DRIVERS}/stm32/driver/include COMPILE_FLAGS -Wframe-larger-than=1500 -Wno-deprecated-declarations @@ -81,10 +87,11 @@ px4_add_module( git_uavcan version + uavcan_stm32_driver + # within libuavcan libuavcan_dsdlc uavcan - uavcan_stm32_driver ) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES uavcan_stm32_driver)