diff --git a/boards/hex/cube-orange/default.cmake b/boards/hex/cube-orange/default.cmake index 54835b25d6..2b01605f50 100644 --- a/boards/hex/cube-orange/default.cmake +++ b/boards/hex/cube-orange/default.cmake @@ -10,7 +10,7 @@ px4_add_board( BUILD_BOOTLOADER IO px4_io-v2_default TESTING - UAVCAN_INTERFACES 2 # - No H7 or FD can support in UAVCAN + UAVCAN_INTERFACES 2 SERIAL_PORTS # IO DEBUG:/dev/ttyS0 TEL1:/dev/ttyS1 @@ -55,7 +55,7 @@ px4_add_board( telemetry # all available telemetry drivers test_ppm tone_alarm -# uavcan - No H7 or FD can support in UAVCAN yet + uavcan MODULES airspeed_selector attitude_estimator_q diff --git a/boards/hex/cube-orange/nuttx-config/include/board.h b/boards/hex/cube-orange/nuttx-config/include/board.h index ab7ef2d526..c3e9cc17dd 100644 --- a/boards/hex/cube-orange/nuttx-config/include/board.h +++ b/boards/hex/cube-orange/nuttx-config/include/board.h @@ -192,6 +192,9 @@ #define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ #define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 773ba57b78..8ecc63fdf8 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -43,9 +43,12 @@ if(CONFIG_ARCH_CHIP) if(${CONFIG_ARCH_CHIP} MATCHES "kinetis") set(UAVCAN_DRIVER "kinetis") set(UAVCAN_TIMER 1) + elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") + set(UAVCAN_DRIVER "stm32h7") + set(UAVCAN_TIMER 5) # The default timer is TIM5 elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32") set(UAVCAN_DRIVER "stm32") - set(UAVCAN_TIMER 5) # The default timer the 5 + set(UAVCAN_TIMER 5) # The default timer is TIM5 endif() endif() diff --git a/src/drivers/uavcan/uavcan_driver.hpp b/src/drivers/uavcan/uavcan_driver.hpp index bd951b5e61..6a681427a6 100644 --- a/src/drivers/uavcan/uavcan_driver.hpp +++ b/src/drivers/uavcan/uavcan_driver.hpp @@ -40,6 +40,8 @@ # include #elif defined(UAVCAN_STM32_NUTTX) # include +#elif defined(UAVCAN_STM32H7_NUTTX) +# include #else # error "Unsupported driver" #endif diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/README.md b/src/drivers/uavcan/uavcan_drivers/stm32h7/README.md new file mode 100644 index 0000000000..8902c19bf6 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/README.md @@ -0,0 +1,11 @@ +STM32H7 platform driver +===================== + +The directory `driver` contains the STM32H7 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/stm32h7/driver/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/CMakeLists.txt new file mode 100644 index 0000000000..d405e7b1f5 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/CMakeLists.txt @@ -0,0 +1,16 @@ +include_directories( + ./include + ) + +add_library(uavcan_stm32h7_driver STATIC + ./src/uc_stm32h7_can.cpp + ./src/uc_stm32h7_clock.cpp + ./src/uc_stm32h7_thread.cpp + ) + +add_dependencies(uavcan_stm32h7_driver uavcan) + +install(DIRECTORY include/uavcan_stm32h7 DESTINATION include) +install(TARGETS uavcan_stm32h7_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/stm32h7/driver/include.mk b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include.mk new file mode 100644 index 0000000000..003b24171a --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include.mk @@ -0,0 +1,9 @@ +# +# Copyright (C) 2014 Pavel Kirienko +# + +LIBUAVCAN_STM32H7_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_STM32H7_SRC := $(shell find $(LIBUAVCAN_STM32H7_DIR)src -type f -name '*.cpp') + +LIBUAVCAN_STM32H7_INC := $(LIBUAVCAN_STM32H7_DIR)include/ diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp new file mode 100644 index 0000000000..769545f6d2 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +/** + * OS detection + */ +#ifndef UAVCAN_STM32H7_NUTTX +# error "Only NuttX is supported" +#endif + +/** + * Number of interfaces must be enabled explicitly + */ +#if !defined(UAVCAN_STM32H7_NUM_IFACES) || (UAVCAN_STM32H7_NUM_IFACES != 1 && UAVCAN_STM32H7_NUM_IFACES != 2) +# error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2" +#endif + +/** + * Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5) + * e.g. -DUAVCAN_STM32H7_TIMER_NUMBER=2 + */ +#ifndef UAVCAN_STM32H7_TIMER_NUMBER +// In this case the clock driver should be implemented by the application +# define UAVCAN_STM32H7_TIMER_NUMBER 0 +#endif diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp new file mode 100644 index 0000000000..3253cef688 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp @@ -0,0 +1,384 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +namespace uavcan_stm32h7 +{ +/** + * 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; + uavcan::uint8_t index; + //bool pending; + bool loopback; + bool abort_on_error; + + TxItem() + : /*pending(false) + ,*/ loopback(false) + , abort_on_error(false) + { } + }; + + struct MessageRam { + uavcan::uint32_t StdIdFilterSA; + uavcan::uint32_t ExtIdFilterSA; + uavcan::uint32_t RxFIFO0SA; + uavcan::uint32_t RxFIFO1SA; + uavcan::uint32_t TxQueueSA; + } message_ram_; + + enum { NumTxMailboxes = 32 }; // Should match the number of Tx FIFOs available in message RAM + enum { NumFilters = 14 }; + + RxQueue rx_queue_; + fdcan::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; } + +public: + enum { MaxRxQueueCapacity = 64 }; + + enum OperatingMode { + NormalMode, + SilentMode + }; + + CanIface(fdcan::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_STM32H7_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); + + /** + * 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, NumTxMailboxes]. + * Value at max 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_STM32H7_NUM_IFACES > 1 + CanIface if1_; +#endif + uint8_t num_ifaces_; + uint32_t enabledInterfaces_; + + 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_STM32H7_NUM_IFACES][RxQueueCapacity]) + : update_event_(*this) + , if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) +#if UAVCAN_STM32H7_NUM_IFACES > 1 + , if1_(fdcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) + , num_ifaces_(2) +#else + , num_ifaces_(1) +#endif + , enabledInterfaces_(0x7) + { + 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, const uavcan::uint32_t EnabledInterfaces); + + virtual CanIface *getIface(uavcan::uint8_t iface_index); + + virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32H7_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(); + + BusEvent &updateEvent() { return update_event_; } +}; + +/** + * 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_STM32H7_NUM_IFACES][RxQueueCapacity]; + +public: + enum { BitRateAutoDetect = 0 }; + + CanDriver driver; + uint32_t enabledInterfaces_; + + CanInitHelper(const uavcan::uint32_t EnabledInterfaces = 0x7) : + driver(queue_storage_), + enabledInterfaces_(EnabledInterfaces) + { } + + /** + * 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, enabledInterfaces_); + } + + /** + * 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, enabledInterfaces_); + + } 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, enabledInterfaces_); + + 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, enabledInterfaces_); + } + } + } + } + + 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/stm32h7/driver/include/uavcan_stm32h7/clock.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp new file mode 100644 index 0000000000..98d1c2988e --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/clock.hpp @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include + +namespace uavcan_stm32h7 +{ + +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 ¶ms); + +} + +/** + * 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/stm32h7/driver/include/uavcan_stm32h7/fdcan.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/fdcan.hpp new file mode 100644 index 0000000000..1704f43252 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/fdcan.hpp @@ -0,0 +1,65 @@ +/* + * 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_stm32h7 +{ +namespace fdcan +{ +#ifdef CONFIG_ARCH_CHIP_STM32H743ZI +#include "fdcan_h743xx.h" +#else +# error "Unsupported STM32H7 MCU" +#endif + +typedef FDCAN_GlobalTypeDef CanType; + +constexpr unsigned long IDE = (0x40000000U); // Identifier Extension +constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask +constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask +constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request +constexpr unsigned long ESI = (0x80000000U); // Error Frame +constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code +constexpr uint32_t T0_STID_Pos = 18; +constexpr uint32_t T0_RTR_Pos = 29; +constexpr uint32_t T0_XTD_Pos = 30; +constexpr uint32_t T0_ESI_Pos = 31; +constexpr uint32_t T1_DLC_Pos = 16; +constexpr uint32_t T1_BRS_Pos = 20; +constexpr uint32_t T1_FDF_Pos = 21; +constexpr uint32_t T1_EFC_Pos = 23; +constexpr uint32_t T1_MM_Pos = 24; + +/** + * CANx register sets + */ +CanType *const Can[UAVCAN_STM32H7_NUM_IFACES] = { + reinterpret_cast(FDCAN1_BASE) +#if UAVCAN_STM32H7_NUM_IFACES > 1 + , + reinterpret_cast(FDCAN2_BASE) +#endif +}; +} +} + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# undef constexpr +#endif diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/fdcan_h743xx.h b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/fdcan_h743xx.h new file mode 100644 index 0000000000..36167de780 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/fdcan_h743xx.h @@ -0,0 +1,1596 @@ +/******************************************************************************* + * The following FDCAN register definitions were taken from: + * @file stm32h743xx.h + * @author MCD Application Team + * @version V1.1.0 + * @date 31-August-2017 + * @brief CMSIS STM32H743xx Device Peripheral Access Layer Header File. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2017 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + +/* ------ Adapted for UAVCAN v0.9 by: Jacob Crabill ------ */ + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +#include + +#define FDCAN1_IT0_IRQn 19 /*!< FDCAN1 Interrupt line 0 */ +#define FDCAN2_IT0_IRQn 20 /*!< FDCAN2 Interrupt line 0 */ +#define FDCAN1_IT1_IRQn 21 /*!< FDCAN1 Interrupt line 1 */ +#define FDCAN2_IT1_IRQn 22 /*!< FDCAN2 Interrupt line 1 */ + +typedef struct { + uint32_t CREL; /*!< FDCAN Core Release register, Address offset: 0x000 */ + uint32_t ENDN; /*!< FDCAN Endian register, Address offset: 0x004 */ + uint32_t RESERVED1; /*!< Reserved, 0x008 */ + uint32_t DBTP; /*!< FDCAN Data Bit Timing & Prescaler register, Address offset: 0x00C */ + uint32_t TEST; /*!< FDCAN Test register, Address offset: 0x010 */ + uint32_t RWD; /*!< FDCAN RAM Watchdog register, Address offset: 0x014 */ + uint32_t CCCR; /*!< FDCAN CC Control register, Address offset: 0x018 */ + uint32_t NBTP; /*!< FDCAN Nominal Bit Timing & Prescaler register, Address offset: 0x01C */ + uint32_t TSCC; /*!< FDCAN Timestamp Counter Configuration register, Address offset: 0x020 */ + uint32_t TSCV; /*!< FDCAN Timestamp Counter Value register, Address offset: 0x024 */ + uint32_t TOCC; /*!< FDCAN Timeout Counter Configuration register, Address offset: 0x028 */ + uint32_t TOCV; /*!< FDCAN Timeout Counter Value register, Address offset: 0x02C */ + uint32_t RESERVED2[4]; /*!< Reserved, 0x030 - 0x03C */ + uint32_t ECR; /*!< FDCAN Error Counter register, Address offset: 0x040 */ + uint32_t PSR; /*!< FDCAN Protocol Status register, Address offset: 0x044 */ + uint32_t TDCR; /*!< FDCAN Transmitter Delay Compensation register, Address offset: 0x048 */ + uint32_t RESERVED3; /*!< Reserved, 0x04C */ + uint32_t IR; /*!< FDCAN Interrupt register, Address offset: 0x050 */ + uint32_t IE; /*!< FDCAN Interrupt Enable register, Address offset: 0x054 */ + uint32_t ILS; /*!< FDCAN Interrupt Line Select register, Address offset: 0x058 */ + uint32_t ILE; /*!< FDCAN Interrupt Line Enable register, Address offset: 0x05C */ + uint32_t RESERVED4[8]; /*!< Reserved, 0x060 - 0x07C */ + uint32_t GFC; /*!< FDCAN Global Filter Configuration register, Address offset: 0x080 */ + uint32_t SIDFC; /*!< FDCAN Standard ID Filter Configuration register, Address offset: 0x084 */ + uint32_t XIDFC; /*!< FDCAN Extended ID Filter Configuration register, Address offset: 0x088 */ + uint32_t RESERVED5; /*!< Reserved, 0x08C */ + uint32_t XIDAM; /*!< FDCAN Extended ID AND Mask register, Address offset: 0x090 */ + uint32_t HPMS; /*!< FDCAN High Priority Message Status register, Address offset: 0x094 */ + uint32_t NDAT1; /*!< FDCAN New Data 1 register, Address offset: 0x098 */ + uint32_t NDAT2; /*!< FDCAN New Data 2 register, Address offset: 0x09C */ + uint32_t RXF0C; /*!< FDCAN Rx FIFO 0 Configuration register, Address offset: 0x0A0 */ + uint32_t RXF0S; /*!< FDCAN Rx FIFO 0 Status register, Address offset: 0x0A4 */ + uint32_t RXF0A; /*!< FDCAN Rx FIFO 0 Acknowledge register, Address offset: 0x0A8 */ + uint32_t RXBC; /*!< FDCAN Rx Buffer Configuration register, Address offset: 0x0AC */ + uint32_t RXF1C; /*!< FDCAN Rx FIFO 1 Configuration register, Address offset: 0x0B0 */ + uint32_t RXF1S; /*!< FDCAN Rx FIFO 1 Status register, Address offset: 0x0B4 */ + uint32_t RXF1A; /*!< FDCAN Rx FIFO 1 Acknowledge register, Address offset: 0x0B8 */ + uint32_t RXESC; /*!< FDCAN Rx Buffer/FIFO Element Size Configuration register, Address offset: 0x0BC */ + uint32_t TXBC; /*!< FDCAN Tx Buffer Configuration register, Address offset: 0x0C0 */ + uint32_t TXFQS; /*!< FDCAN Tx FIFO/Queue Status register, Address offset: 0x0C4 */ + uint32_t TXESC; /*!< FDCAN Tx Buffer Element Size Configuration register, Address offset: 0x0C8 */ + uint32_t TXBRP; /*!< FDCAN Tx Buffer Request Pending register, Address offset: 0x0CC */ + uint32_t TXBAR; /*!< FDCAN Tx Buffer Add Request register, Address offset: 0x0D0 */ + uint32_t TXBCR; /*!< FDCAN Tx Buffer Cancellation Request register, Address offset: 0x0D4 */ + uint32_t TXBTO; /*!< FDCAN Tx Buffer Transmission Occurred register, Address offset: 0x0D8 */ + uint32_t TXBCF; /*!< FDCAN Tx Buffer Cancellation Finished register, Address offset: 0x0DC */ + uint32_t TXBTIE; /*!< FDCAN Tx Buffer Transmission Interrupt Enable register, Address offset: 0x0E0 */ + uint32_t TXBCIE; /*!< FDCAN Tx Buffer Cancellation Finished Interrupt Enable register, Address offset: 0x0E4 */ + uint32_t RESERVED6[2]; /*!< Reserved, 0x0E8 - 0x0EC */ + uint32_t TXEFC; /*!< FDCAN Tx Event FIFO Configuration register, Address offset: 0x0F0 */ + uint32_t TXEFS; /*!< FDCAN Tx Event FIFO Status register, Address offset: 0x0F4 */ + uint32_t TXEFA; /*!< FDCAN Tx Event FIFO Acknowledge register, Address offset: 0x0F8 */ + uint32_t RESERVED7; /*!< Reserved, 0x0FC */ +} FDCAN_GlobalTypeDef; + +/** + * @brief TTFD Controller Area Network + */ + +typedef struct { + uint32_t TTTMC; /*!< TT Trigger Memory Configuration register, Address offset: 0x100 */ + uint32_t TTRMC; /*!< TT Reference Message Configuration register, Address offset: 0x104 */ + uint32_t TTOCF; /*!< TT Operation Configuration register, Address offset: 0x108 */ + uint32_t TTMLM; /*!< TT Matrix Limits register, Address offset: 0x10C */ + uint32_t TURCF; /*!< TUR Configuration register, Address offset: 0x110 */ + uint32_t TTOCN; /*!< TT Operation Control register, Address offset: 0x114 */ + uint32_t TTGTP; /*!< TT Global Time Preset register, Address offset: 0x118 */ + uint32_t TTTMK; /*!< TT Time Mark register, Address offset: 0x11C */ + uint32_t TTIR; /*!< TT Interrupt register, Address offset: 0x120 */ + uint32_t TTIE; /*!< TT Interrupt Enable register, Address offset: 0x124 */ + uint32_t TTILS; /*!< TT Interrupt Line Select register, Address offset: 0x128 */ + uint32_t TTOST; /*!< TT Operation Status register, Address offset: 0x12C */ + uint32_t TURNA; /*!< TT TUR Numerator Actual register, Address offset: 0x130 */ + uint32_t TTLGT; /*!< TT Local and Global Time register, Address offset: 0x134 */ + uint32_t TTCTC; /*!< TT Cycle Time and Count register, Address offset: 0x138 */ + uint32_t TTCPT; /*!< TT Capture Time register, Address offset: 0x13C */ + uint32_t TTCSM; /*!< TT Cycle Sync Mark register, Address offset: 0x140 */ + uint32_t RESERVED1[111]; /*!< Reserved, 0x144 - 0x2FC */ + uint32_t TTTS; /*!< TT Trigger Select register, Address offset: 0x300 */ +} TTCAN_TypeDef; + +/** + * @brief FD Controller Area Network + */ + +typedef struct { + uint32_t CREL; /*!< Clock Calibration Unit Core Release register, Address offset: 0x00 */ + uint32_t CCFG; /*!< Calibration Configuration register, Address offset: 0x04 */ + uint32_t CSTAT; /*!< Calibration Status register, Address offset: 0x08 */ + uint32_t CWD; /*!< Calibration Watchdog register, Address offset: 0x0C */ + uint32_t IR; /*!< CCU Interrupt register, Address offset: 0x10 */ + uint32_t IE; /*!< CCU Interrupt Enable register, Address offset: 0x14 */ +} FDCAN_ClockCalibrationUnit_TypeDef; + + +/******************************************************************************/ +/* Peripheral Registers_Bits_Definition */ +/******************************************************************************/ + +#define PERIPH_BASE ((uint32_t)0x40000000) /*!< Base address of : AHB/ABP Peripherals */ +#define D2_APB1PERIPH_BASE PERIPH_BASE + +#define FDCAN1_BASE (D2_APB1PERIPH_BASE + 0xA000) +#define FDCAN2_BASE (D2_APB1PERIPH_BASE + 0xA400) +#define FDCAN_CCU_BASE (D2_APB1PERIPH_BASE + 0xA800) +#define SRAMCAN_BASE (D2_APB1PERIPH_BASE + 0xAC00) + +/******************************************************************************/ +/* */ +/* Flexible Datarate Controller Area Network */ +/* */ +/******************************************************************************/ +/*! + */ + +#pragma once + +#include + +#if UAVCAN_STM32H7_NUTTX +# include +# include +# include +# include +# include +# include +# include +#else +# error "Unknown OS" +#endif + +#include + +namespace uavcan_stm32h7 +{ + +class CanDriver; + +#if UAVCAN_STM32H7_NUTTX + +/** + * All bus events are reported as POLLIN. + */ +class BusEvent : uavcan::Noncopyable +{ + using SignalCallbackHandler = void(*)(); + + SignalCallbackHandler signal_cb_{nullptr}; + sem_t sem_; + +public: + BusEvent(CanDriver &can_driver); + ~BusEvent(); + + void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } + + 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_); + } +}; +#endif + + +class MutexLocker +{ + Mutex &mutex_; + +public: + MutexLocker(Mutex &mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } +}; + +} diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/uavcan_stm32h7.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/uavcan_stm32h7.hpp new file mode 100644 index 0000000000..68b0ab99f2 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/uavcan_stm32h7.hpp @@ -0,0 +1,11 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +#include +#include +#include diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/internal.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/internal.hpp new file mode 100644 index 0000000000..b73f50f4b4 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/internal.hpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include "board_config.h" + +#if UAVCAN_STM32H7_NUTTX +# include +# include +# include +# include +#else +# error "Unknown OS" +#endif + +/** + * Debug output + */ +#ifndef UAVCAN_STM32H7_LOG +// syslog() crashes the system in this context +// # if UAVCAN_STM32H7_NUTTX && CONFIG_ARCH_LOWPUTC +# if 1 +# define UAVCAN_STM32H7_LOG(fmt, ...) printf("uavcan_stm32: \n" fmt "\n", ##__VA_ARGS__) +# else +# define UAVCAN_STM32H7_LOG(...) ((void)0) +# endif +#endif + +/** + * IRQ handler macros + */ +#if UAVCAN_STM32H7_NUTTX +# define UAVCAN_STM32H7_IRQ_HANDLER(id) int id(int irq, FAR void* context, FAR void *arg) +# define UAVCAN_STM32H7_IRQ_PROLOGUE() +# define UAVCAN_STM32H7_IRQ_EPILOGUE() return 0; +#endif + +/** + * Glue macros + */ +#define UAVCAN_STM32H7_GLUE2_(A, B) A##B +#define UAVCAN_STM32H7_GLUE2(A, B) UAVCAN_STM32H7_GLUE2_(A, B) + +#define UAVCAN_STM32H7_GLUE3_(A, B, C) A##B##C +#define UAVCAN_STM32H7_GLUE3(A, B, C) UAVCAN_STM32H7_GLUE3_(A, B, C) + +namespace uavcan_stm32h7 +{ +#if UAVCAN_STM32H7_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/stm32h7/driver/src/uc_stm32h7_can.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp new file mode 100644 index 0000000000..753e6f91e3 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp @@ -0,0 +1,1162 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uc_stm32h7_can.cpp + * + * STM32H7 FDCAN driver for libuavcan + * + * @author Jacob Crabill + */ + +/* + * Original libuavcan driver for stm32: + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "internal.hpp" + +#if UAVCAN_STM32H7_NUTTX +# include +# include +# include +#else +# error "Unknown OS" +#endif + +#define WORD_LENGTH 4U +#define FIFO_ELEMENT_SIZE 4U // size in words of a FIFO element in message RAM + +// Rx FIFO element definition for classic CAN frame +// RM0433 page 2536 +typedef struct __attribute__((__packed__)) +{ + // word R0 + uint32_t id : 29; + uint32_t RTR : 1; + uint32_t XTD : 1; + uint32_t ESI : 1; + + // word R1 + uint16_t RXTS; + uint8_t DLC : 4; // Data Length Code + uint8_t BRS : 1; // Bit Rate Switching + uint8_t FDF : 1; // CAN-FD Flag + uint8_t RES : 2; // Reserved + uint8_t FIDX : 7; // Filter Index + uint8_t ANMF : 1; // Accepted non-matching frame + + // words R2, R3 + uint8_t data[8]; + +} RxFifoElement; + +// Tx FIFO element definition for classic CAN frame +// RM0433 page 2538 +typedef struct { + // word T0 + uint32_t id : 29; + uint32_t RTR : 1; + uint32_t XTD : 1; + uint32_t ESI : 1; + + // word T1 + uint16_t _reserved; + uint8_t DLC : 4; + uint8_t BRS : 1; + uint8_t FDF : 1; + uint8_t RES : 1; + uint8_t EFC : 1; + uint8_t MM; + + // words T2, T3 + uint8_t data[8]; + +} TxFifoElement; + +extern "C" +{ + static int can1_irq(const int irq, void *, void *); +#if UAVCAN_STM32H7_NUM_IFACES > 1 + static int can2_irq(const int irq, void *, void *); +#endif +} + +namespace uavcan_stm32h7 +{ +namespace +{ + +CanIface *ifaces[UAVCAN_STM32H7_NUM_IFACES] = { + UAVCAN_NULLPTR +#if UAVCAN_STM32H7_NUM_IFACES > 1 + , UAVCAN_NULLPTR +#endif +}; + +inline void handleTxInterrupt(uavcan::uint8_t iface_index) +{ + UAVCAN_ASSERT(iface_index < UAVCAN_STM32H7_NUM_IFACES); + + if (ifaces[iface_index] == UAVCAN_NULLPTR) { + fdcan::Can[iface_index]->IR = FDCAN_IR_TC; + UAVCAN_ASSERT(0); + return; + } + + if (fdcan::Can[iface_index]->IR & FDCAN_IR_TC) { + fdcan::Can[iface_index]->IR = FDCAN_IR_TC; + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + + if (utc_usec > 0) { + utc_usec--; + } + + ifaces[iface_index]->handleTxInterrupt(utc_usec); + } +} + +inline void handleRxInterrupt(uavcan::uint8_t iface_index) +{ + UAVCAN_ASSERT(iface_index < UAVCAN_STM32H7_NUM_IFACES); + + /* + * Rx-Related Interrupt Definitions + * + * FDCAN_IR_RFxN - FIFOx New message -- Enabled + * FDCAN_IR_RFxW - FIFOx Watermark Reached + * FDCAN_IR_RFxF - FIFOx Full -- Enabled + * FDCAN_IR_RFxL - FIFOx Message Lost + */ + + if (ifaces[iface_index] == UAVCAN_NULLPTR) { + // Bad interface - reset flags and return + fdcan::Can[iface_index]->IR = FDCAN_IR_RF0N; + fdcan::Can[iface_index]->IR = FDCAN_IR_RF1N; + fdcan::Can[iface_index]->IR = FDCAN_IR_RF0F; + fdcan::Can[iface_index]->IR = FDCAN_IR_RF1F; + UAVCAN_ASSERT(0); + return; + } + + const uint32_t IR = fdcan::Can[iface_index]->IR; + + // Check for our enabled interrupts: FIFO 0 + if ((IR & (FDCAN_IR_RF0N | FDCAN_IR_RF0F)) > 0) { + fdcan::Can[iface_index]->IR = (FDCAN_IR_RF0N | FDCAN_IR_RF0F); + ifaces[iface_index]->handleRxInterrupt(0); + + } else if ((IR & (FDCAN_IR_RF1N | FDCAN_IR_RF1F)) > 0) { + fdcan::Can[iface_index]->IR = (FDCAN_IR_RF1N | FDCAN_IR_RF1F); + ifaces[iface_index]->handleRxInterrupt(1); + + } 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 + */ + +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings &out_timings) +{ + if (target_bitrate < 1) { + return -ErrInvalidBitRate; + } + + /* + * Hardware configuration + */ + const uavcan::uint32_t pclk = STM32_FDCANCLK; + + 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_STM32H7_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%", + int(1 + solution.bs1 + solution.bs2), double(solution.sample_point_permill) / 10.); + + 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; + + // First, check if there are any slots available in the queue + if ((can_->TXFQS & FDCAN_TXFQS_TFQF) > 0) { + // Tx FIFO / Queue is full + return 0; + } + + // Next, get the next available queue index from the controller + const uint8_t index = (can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos; + + // Now, we can copy the CAN frame to the queue (in message RAM) + uint32_t *txbuf = (uint32_t *)(message_ram_.TxQueueSA + (index * FIFO_ELEMENT_SIZE * WORD_LENGTH)); + + // Copy the ID; special case for standard ID frames + if (frame.isExtended()) { + txbuf[0] = (frame.id & fdcan::EXID_MASK) | fdcan::IDE; + + } else { + // Standard ID frames must be entered into bits [28:18] + txbuf[0] = (frame.id << fdcan::T0_STID_Pos) & fdcan::STID_MASK; + } + + if (frame.isRemoteTransmissionRequest()) { + txbuf[0] |= fdcan::RTR; + } + + if (frame.isErrorFrame()) { + txbuf[0] |= fdcan::ESI; + } + + txbuf[1] = (frame.dlc << fdcan::T1_DLC_Pos); + + txbuf[1] &= ~(1 << fdcan::T1_FDF_Pos); // Classic CAN frame, not CAN-FD + txbuf[1] &= ~(1 << fdcan::T1_BRS_Pos); // No bitrate switching + txbuf[1] &= ~(1 << fdcan::T1_EFC_Pos); // Don't store Tx events + txbuf[1] |= (index << fdcan::T1_MM_Pos); // Marker for our use; just give it the FIFO index + + // Store the data bytes + txbuf[2] = (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); + txbuf[3] = (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); + + // Submit the transmission request for this element + can_->TXBAR = 1 << index; + + /* + * Registering the pending transmission so we can track its deadline and loopback it as needed + */ + TxItem &txi = pending_tx_[index]; + txi.deadline = tx_deadline; + txi.frame = frame; + txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0; + txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; + txi.index = index; + + 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) +{ + /* + * The FDCAN controller handles standard ID and extended ID filters separately. + * We must scan through the requested filter configurations, and group them by + * ID type. We can then setup the filters and assign message RAM. + */ + + // Filter config registers are protected; write access is only available + // when bit CCE and bit INIT of FDCAN_CCCR register are set to 1. + + // CriticalSectionLocker lock; + + // // Request Init mode, then wait for completion + // can_->CCCR |= FDCAN_CCCR_INIT; + // while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {} + + // // Configuration Chane Enable + // can_->CCCR |= FDCAN_CCCR_CCE; + + // /// TODO ------------------------- + + // // Leave Init mode + // can_->CCCR &= ~FDCAN_CCCR_INIT; + + return 0; +} + +int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) +{ + /* + * Wake up the device and enable configuration changes + */ + { + CriticalSectionLocker lock; + + // Exit Power-down / Sleep mode, then wait for acknowledgement + can_->CCCR &= ~FDCAN_CCCR_CSR; + + // TODO: add timeout + while ((can_->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA) {} + + // Request Init mode, then wait for completion + can_->CCCR |= FDCAN_CCCR_INIT; + + while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {} + + // Configuration Changes Enable. Can only be set during Init mode; + // cleared when INIT bit is cleared. + can_->CCCR |= FDCAN_CCCR_CCE; + + // Disable interrupts while we configure the hardware + can_->IE = 0; + } + + /* + * 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_->CCCR &= ~FDCAN_CCCR_INIT; + return timings_res; + } + + UAVCAN_STM32H7_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u", + unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); + + /* + * Set bit timings and prescalers (Nominal and Data bitrates) + */ + + // We're not using CAN-FD, so set same timings for both + can_->NBTP = ((timings.sjw << FDCAN_NBTP_NSJW_Pos) | + (timings.bs1 << FDCAN_NBTP_NTSEG1_Pos) | + (timings.bs2 << FDCAN_NBTP_TSEG2_Pos) | + (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); + + can_->DBTP = ((timings.sjw << FDCAN_DBTP_DSJW_Pos) | + (timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | + (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | + (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); + + /* + * Operation Configuration + */ + + // Disable CAN-FD communications ("classic" CAN only) + can_->CCCR &= ~FDCAN_CCCR_FDOE; + + // Disable Time Triggered (TT) operation -- TODO (must use TTCAN_TypeDef) + //ttcan_->TTOCF &= ~FDCAN_TTOCF_OM + + /* + * Configure Interrupts + */ + + // Clear all interrupt flags + // Note: A flag is cleared by writing a 1 to the corresponding bit position + can_->IR = 0xFFFFFFFF; //FDCAN_IR_MASK; + + // Enable relevant interrupts + can_->IE = FDCAN_IE_TCE // Transmit Complete + | FDCAN_IE_RF0NE // Rx FIFO 0 new message + | FDCAN_IE_RF0FE // Rx FIFO 0 FIFO full + | FDCAN_IE_RF1NE // Rx FIFO 1 new message + | FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO full + + // Keep Rx interrupts on Line 0; move Tx to Line 1 + can_->ILS = FDCAN_ILS_TCL; // TC interrupt on line 1 + + // Enable Tx buffer transmission interrupt + can_->TXBTIE = FDCAN_TXBTIE_TIE; + + // Enable both interrupt lines + can_->ILE = FDCAN_ILE_EINT0 | FDCAN_ILE_EINT1; + + /* + * Configure Message RAM + * + * The available 2560 words (10 kiB) of RAM are shared between both FDCAN + * interfaces. It is up to us to ensure each interface has its own non- + * overlapping region of RAM assigned to it by properly assignin the start and + * end addresses for all regions of RAM. + * + * We will give each interface half of the available RAM. + * + * Rx buffers are only used in conjunction with acceptance filters; we don't + * have any specific need for this, so we will only use Rx FIFOs. + * + * Each FIFO can hold up to 64 elements, where each element (for a classic CAN + * 2.0B frame) is up to 4 words long (8 bytes data + header bits) + * + * Let's make use of the full 64 FIFO elements for FIFO0. We have no need to + * separate messages between FIFO0 and FIFO1, so ignore FIFO1 for simplicity. + * + * Note that the start addresses given to FDCAN are in terms of _words_, not + * bytes, so when we go to read/write to/from the message RAM, there will be a + * factor of 4 necessary in the address relative to the SA register values. + */ + + // Location of this interface's message RAM - address in CPU memory address + // and relative address (in words) used for configuration + const uint32_t ram_base = (2560 / 2) * self_index_; + const uint32_t gl_ram_base = ram_base + SRAMCAN_BASE; + uint32_t ram_offset = ram_base; + + // Standard ID Filters: Allow space for 128 filters (128 words) + const uint8_t n_stdid = 128; + message_ram_.StdIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH; + can_->SIDFC = ((n_stdid << FDCAN_SIDFC_LSS_Pos) + | ram_offset << FDCAN_SIDFC_FLSSA_Pos); + ram_offset += n_stdid; + + // Extended ID Filters: Allow space for 128 filters (128 words) + const uint8_t n_extid = 128; + message_ram_.ExtIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH; + can_->XIDFC = ((n_extid << FDCAN_XIDFC_LSE_Pos) + | ram_offset << FDCAN_XIDFC_FLESA_Pos); + ram_offset += n_extid; + + // Set size of each element in the Rx/Tx buffers and FIFOs + can_->RXESC = 0; // 8 byte space for every element (Rx buffer, FIFO1, FIFO0) + can_->TXESC = 0; // 8 byte space for every element (Tx buffer) + + // Rx FIFO0 (64 elements max) + const uint8_t n_fifo0 = 64; + message_ram_.RxFIFO0SA = gl_ram_base + ram_offset * WORD_LENGTH; + can_->RXF0C = ram_offset << FDCAN_RXF0C_F0SA_Pos; + can_->RXF0C |= n_fifo0 << FDCAN_RXF0C_F0S_Pos; + ram_offset += n_fifo0 * FIFO_ELEMENT_SIZE; + + // Set Tx queue size (32 elements max) + message_ram_.TxQueueSA = gl_ram_base + ram_offset * WORD_LENGTH; + can_->TXBC = 32U << FDCAN_TXBC_TFQS_Pos; + can_->TXBC |= FDCAN_TXBC_TFQM; // Queue mode (vs. FIFO) + can_->TXBC |= ram_offset << FDCAN_TXBC_TBSA_Pos; + + /* + * Default filter configuration + * + * Accept all messages into Rx FIFO0 by default + */ + can_->GFC &= ~FDCAN_GFC_ANFS; // Accept non-matching stdid frames into FIFO0 + can_->GFC &= ~FDCAN_GFC_ANFE; // Accept non-matching extid frames into FIFO0 + + /* + * Exit Initialization mode + */ + can_->CCCR &= ~FDCAN_CCCR_INIT; + + return 0; +} + +void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) +{ + // Update counters for successful transmissions + // Process loopback messages + for (uint8_t i = 0; i < NumTxMailboxes; i++) { + if ((can_->TXBTO & (1 << i)) > 0) { + // Transmission Occurred in buffer i + if (pending_tx_[i].loopback && had_activity_) { + rx_queue_.push(pending_tx_[i].frame, utc_usec, uavcan::CanIOFlagLoopback); + } + } + } + + pollErrorFlagsFromISR(); +} + +void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index) +{ + UAVCAN_ASSERT(fifo_index < 2); + + // Bitwise register definitions are the same for FIFO 0/1 + constexpr uint32_t FDCAN_RXFnC_FnS = FDCAN_RXF0C_F0S; // Rx FIFO Size + constexpr uint32_t FDCAN_RXFnS_RFnL = FDCAN_RXF0S_RF0L; // Rx Message Lost + constexpr uint32_t FDCAN_RXFnS_FnFL = FDCAN_RXF0S_F0FL; // Rx FIFO Fill Level + constexpr uint32_t FDCAN_RXFnS_FnGI = FDCAN_RXF0S_F0GI; // Rx FIFO Get Index + constexpr uint32_t FDCAN_RXFnS_FnGI_Pos = FDCAN_RXF0S_F0GI_Pos; + //constexpr uint32_t FDCAN_RXFnS_FnPI = FDCAN_RXF0S_F0PI; // Rx FIFO Put Index + //constexpr uint32_t FDCAN_RXFnS_FnPI_Pos = FDCAN_RXF0S_F0PI_Pos; + //constexpr uint32_t FDCAN_RXFnS_FnF = FDCAN_RXF0S_F0F; // Rx FIFO Full + + volatile uint32_t *const RXFnC = (fifo_index == 0) ? &(can_->RXF0C) : &(can_->RXF1C); + volatile uint32_t *const RXFnS = (fifo_index == 0) ? &(can_->RXF0S) : &(can_->RXF1S); + volatile uint32_t *const RXFnA = (fifo_index == 0) ? &(can_->RXF0A) : &(can_->RXF1A); + + bool had_error = false; + + // Check number of elements in message RAM allocated to this FIFO + if ((*RXFnC & FDCAN_RXFnC_FnS) == 0) { + UAVCAN_ASSERT(0); + return; + } + + // Check for message lost; count an error + if ((*RXFnS & FDCAN_RXFnS_RFnL) != 0) { + error_cnt_++; + } + + // Check number of elements available (fill level) + const uint8_t n_elem = (*RXFnS & FDCAN_RXFnS_FnFL); + + if (n_elem == 0) { + // No elements available? + UAVCAN_ASSERT(0); + return; + } + + while ((*RXFnS & FDCAN_RXFnS_FnFL) > 0) { + // Copy the message from message RAM + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + uavcan::CanFrame frame; + + const uint8_t index = (*RXFnS & FDCAN_RXFnS_FnGI) >> FDCAN_RXFnS_FnGI_Pos; + + RxFifoElement *rx = (RxFifoElement *)(message_ram_.RxFIFO0SA + (index * FIFO_ELEMENT_SIZE * WORD_LENGTH)); + + // Note that we must shift Standard IDs to the lowest bit range of ID + if (rx->XTD) { + frame.id = (rx->id & fdcan::EXID_MASK) & uavcan::CanFrame::MaskExtID; + frame.id |= uavcan::CanFrame::FlagEFF; + + } else { + frame.id = (rx->id >> 18) & uavcan::CanFrame::MaskStdID; + } + + if (rx->RTR) { + frame.id |= uavcan::CanFrame::FlagRTR; + } + + if (rx->ESI) { + frame.id |= uavcan::CanFrame::FlagERR; + } + + frame.dlc = rx->DLC; + + for (uint8_t i = 0; i < 8; i++) { + frame.data[i] = rx->data[i]; + } + + // Acknowledge receipt of this FIFO element + *RXFnA = index; + + // Push the frame into the application queue + rx_queue_.push(frame, utc_usec, 0); + + had_activity_ = true; + } + + if (had_error) { + error_cnt_++; + } + + update_event_.signalFromInterrupt(); + + pollErrorFlagsFromISR(); +} + +void CanIface::pollErrorFlagsFromISR() +{ + // Read CAN Error Logging counter (This also resets the error counter) + const uavcan::uint8_t cel = ((can_->ECR & FDCAN_ECR_CEL) >> FDCAN_ECR_CEL_Pos); + + if (cel > 0) { + + // Serve abort requests + for (uint8_t i = 0; i < NumTxMailboxes; i++) { + TxItem &txi = pending_tx_[i]; + + if (txi.abort_on_error && ((1 << txi.index) & can_->TXBRP)) { + // Request to Cancel Tx item + can_->TXBCR = (1 << txi.index); + error_cnt_++; + served_aborts_cnt_++; + } + } + } +} + +void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) +{ + CriticalSectionLocker lock; + + for (uint8_t i = 0; i < NumTxMailboxes; i++) { + TxItem &txi = pending_tx_[i]; + + if (((1 << txi.index) & can_->TXBRP) && txi.deadline < current_time) { + // Request to Cancel Tx item + can_->TXBCR = (1 << txi.index); + error_cnt_++; + } + } +} + +bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame &frame) const +{ + // Check that we even _have_ a Tx FIFO allocated + if ((can_->TXBC & FDCAN_TXBC_TFQS) == 0) { + // Your queue size is 0, you did something wrong + return false; + } + + // Check if the Tx queue is full + if ((can_->TXFQS & FDCAN_TXFQS_TFQF) == FDCAN_TXFQS_TFQF) { + // Sorry, out of room, try back later + return false; + } + + return true; +} + +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; + + for (uint8_t i = 0; i < num_ifaces_; i++) { + msk.read |= (ifaces[i]->isRxBufferEmpty() ? 0 : 1) << i; + + if (pending_tx[i] != UAVCAN_NULLPTR) { + msk.write |= (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i]) ? 1 : 0) << i; + } + } + + return msk; +} + +bool CanDriver::hasReadableInterfaces() const +{ + for (uint8_t i = 0; i < num_ifaces_; i++) { + if (!ifaces[i]->isRxBufferEmpty()) { + return true; + } + } + + return false; +} + +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_STM32H7_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() +{ + /* + * FDCAN1, FDCAN2 - Enable peripheral, clock + */ + { + CriticalSectionLocker lock; +#if UAVCAN_STM32H7_NUTTX + modifyreg32(STM32_RCC_APB1HENR, 0, RCC_APB1HENR_FDCANEN); + modifyreg32(STM32_RCC_APB1HRSTR, 0, RCC_APB1HRSTR_FDCANRST); + modifyreg32(STM32_RCC_APB1HRSTR, RCC_APB1HRSTR_FDCANRST, 0); +#else + RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; + RCC->APB1HRSTR |= RCC_APB1HRSTR_FDCANRST; + RCC->APB1HRSTR &= ~RCC_APB1HRSTR_FDCANRST; +#endif + } + + /* + * IRQ + */ +#if UAVCAN_STM32H7_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_FDCAN1_0, can1_irq); + IRQ_ATTACH(STM32_IRQ_FDCAN1_1, can1_irq); +# if UAVCAN_STM32H7_NUM_IFACES > 1 + IRQ_ATTACH(STM32_IRQ_FDCAN2_0, can2_irq); + IRQ_ATTACH(STM32_IRQ_FDCAN2_1, can2_irq); +# endif +# undef IRQ_ATTACH +#endif +} + +int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, + const uavcan::uint32_t enabledInterfaces) +{ + int res = 0; + + UAVCAN_STM32H7_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + + static bool initialized_once = false; + + if (!initialized_once) { + initialized_once = true; + UAVCAN_STM32H7_LOG("First initialization"); + initOnce(); + } + + /* + * FDCAN1 + */ + if (enabledInterfaces & 1) { + UAVCAN_STM32H7_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_STM32H7_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + } + + /* + * FDCAN2 + */ +#if UAVCAN_STM32H7_NUM_IFACES > 1 + + if (enabledInterfaces & 2) { + UAVCAN_STM32H7_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + + if (res < 0) { + UAVCAN_STM32H7_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } + } + +#endif + + UAVCAN_STM32H7_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; + +fail: + UAVCAN_STM32H7_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_STM32H7_NUM_IFACES) { + return ifaces[iface_index]; + } + + return UAVCAN_NULLPTR; +} + +bool CanDriver::hadActivity() +{ + bool ret = if0_.hadActivity(); +#if UAVCAN_STM32H7_NUM_IFACES > 1 + ret |= if1_.hadActivity(); +#endif + return ret; +} + +} // namespace uavcan_stm32 + +/* + * Interrupt handlers + */ +extern "C" +{ + +#if UAVCAN_STM32H7_NUTTX + + static int can1_irq(const int irq, void *, void *) + { + if (irq == STM32_IRQ_FDCAN1_0) { + // We've put only Rx interrupts on Line 0 + uavcan_stm32h7::handleRxInterrupt(0); + + } else if (irq == STM32_IRQ_FDCAN1_1) { + // And only Tx interrupts on Line 1 + uavcan_stm32h7::handleTxInterrupt(0); + + } else { + PANIC(); + } + + return 0; + } + +# if UAVCAN_STM32H7_NUM_IFACES > 1 + + static int can2_irq(const int irq, void *, void *) + { + if (irq == STM32_IRQ_FDCAN2_0) { + // We've put only Rx interrupts on Line 0 + uavcan_stm32h7::handleRxInterrupt(1); + + } else if (irq == STM32_IRQ_FDCAN2_1) { + // And only Tx interrupts on Line 1 + uavcan_stm32h7::handleTxInterrupt(1); + + } else { + PANIC(); + } + + return 0; + } + +# endif + +#else // UAVCAN_STM32H7_NUTTX + +#if !defined(FDCAN1_IT0_IRQHandler) ||\ + !defined(FDCAN1_IT1_IRQHandler) +# error "Misconfigured build" +#endif + + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN1_IT0_IRQHandler); + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN1_IT0_IRQHandler) + { + UAVCAN_STM32H7_IRQ_PROLOGUE(); + uavcan_stm32h7::handleInterrupt(0, 0); + UAVCAN_STM32H7_IRQ_EPILOGUE(); + } + + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN1_IT1_IRQHandler); + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN1_IT1_IRQHandler) + { + UAVCAN_STM32H7_IRQ_PROLOGUE(); + uavcan_stm32h7::handleInterrupt(0, 1); + UAVCAN_STM32H7_IRQ_EPILOGUE(); + } + +# if UAVCAN_STM32H7_NUM_IFACES > 1 + +#if !defined(FDCAN2_IT0_IRQHandler) || \ + !defined(FDCAN2_IT1_IRQHandler) +# error "Misconfigured build" +#endif + + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN2_IT0_IRQHandler); + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN2_IT0_IRQHandler) + { + UAVCAN_STM32H7_IRQ_PROLOGUE(); + uavcan_stm32h7::handleInterrupt(1, 0); + UAVCAN_STM32H7_IRQ_EPILOGUE(); + } + + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN2_IT1_IRQHandler); + UAVCAN_STM32H7_IRQ_HANDLER(FDCAN2_IT1_IRQHandler) + { + UAVCAN_STM32H7_IRQ_PROLOGUE(); + uavcan_stm32h7::handleInterrupt(1, 1); + UAVCAN_STM32H7_IRQ_EPILOGUE(); + } + +# endif +#endif // UAVCAN_STM32H7_NUTTX + +} // extern "C" diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp new file mode 100644 index 0000000000..72c4170a1b --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp @@ -0,0 +1,400 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "internal.hpp" + +#if UAVCAN_STM32H7_TIMER_NUMBER + +#include +#include + +/* + * Timer instance + */ +# if UAVCAN_STM32H7_NUTTX +# define TIMX UAVCAN_STM32H7_GLUE3(STM32_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _BASE) +# define TMR_REG(o) (TIMX + (o)) +# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_GLUE3(STM32_APB1_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _CLKIN) + +# define TIMX_IRQn UAVCAN_STM32H7_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32H7_TIMER_NUMBER) +# endif + +# if UAVCAN_STM32H7_TIMER_NUMBER >= 2 && UAVCAN_STM32H7_TIMER_NUMBER <= 7 +# define TIMX_RCC_ENR RCC->APB1ENR +# define TIMX_RCC_RSTR RCC->APB1RSTR +# define TIMX_RCC_ENR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, EN) +# define TIMX_RCC_RSTR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, RST) +# else +# error "This UAVCAN_STM32H7_TIMER_NUMBER is not supported yet" +# endif + +/** + * UAVCAN_STM32H7_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_STM32H7_TIMX_INPUT_CLOCK=STM32_HCLK + * + * Alternatively, the new clock rate can be specified directly. + */ +# ifdef UAVCAN_STM32H7_TIMX_INPUT_CLOCK +# undef TIMX_INPUT_CLOCK +# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_TIMX_INPUT_CLOCK +# endif + +extern "C" UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler); + +namespace uavcan_stm32h7 +{ +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; + +} + + +void init() +{ + CriticalSectionLocker lock; + + if (initialized) { + return; + } + + initialized = true; + + +# if UAVCAN_STM32H7_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_STM32H7_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_STM32H7_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 ¶ms) +{ + 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_stm32h7 + + +/** + * Timer interrupt handler + */ + +extern "C" +UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler) +{ + UAVCAN_STM32H7_IRQ_PROLOGUE(); + +# if UAVCAN_STM32H7_NUTTX + putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); +# endif + + using namespace uavcan_stm32h7::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_STM32H7_IRQ_EPILOGUE(); +} + +#endif diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_thread.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_thread.cpp new file mode 100644 index 0000000000..9af0a40a53 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_thread.cpp @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "internal.hpp" + + +namespace uavcan_stm32h7 +{ + +#if UAVCAN_STM32H7_NUTTX + +BusEvent::BusEvent(CanDriver &can_driver) +{ + sem_init(&sem_, 0, 0); + sem_setprotocol(&sem_, SEM_PRIO_NONE); +} + +BusEvent::~BusEvent() +{ + sem_destroy(&sem_); +} + +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + if (duration.isPositive()) { + timespec abstime; + + if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { + const unsigned billion = 1000 * 1000 * 1000; + uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; + abstime.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + abstime.tv_nsec = nsecs; + + int ret; + + while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); + + if (ret == -1) { // timed out or error + return false; + } + + return true; + } + } + + return false; +} + +void BusEvent::signalFromInterrupt() +{ + if (sem_.semcount <= 0) { + (void)sem_post(&sem_); + } + + if (signal_cb_) { + signal_cb_(); + } +} + +#endif + +}