diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 900d90b444..e9fd446026 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -223,6 +223,8 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable 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]) diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp index 77b8378a02..5db7288e29 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp @@ -488,10 +488,26 @@ bool CanIface::waitMsrINakBitStateChange(bool target_state) int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) { - int res = 0; + /* + * 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"); + return -1; + } /* - * Object state + * Object state - interrupts are disabled, so it's safe to modify it now */ rx_queue_.reset(); error_cnt_ = 0; @@ -505,27 +521,17 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) * CAN timings for this bitrate */ Timings timings; - res = computeTimings(bitrate, timings); - if (res < 0) + const int timings_res = computeTimings(bitrate, timings); + if (timings_res < 0) { - goto leave; + 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 + * Hardware initialization (the hardware has already confirmed initialization mode, see above) */ - can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode - can_->MCR |= bxcan::MCR_INRQ; // Request init - - if (!waitMsrINakBitStateChange(true)) - { - UAVCAN_STM32_LOG("MSR INAK not set"); - res = -1; - goto leave; - } - can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648 can_->BTR = ((timings.sjw & 3U) << 24) | @@ -545,8 +551,7 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) if (!waitMsrINakBitStateChange(false)) { UAVCAN_STM32_LOG("MSR INAK not cleared"); - res = -1; - goto leave; + return -1; } /* @@ -579,8 +584,7 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) can_->FMR &= ~bxcan::FMR_FINIT; } -leave: - return res; + return 0; } void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec) @@ -875,68 +879,37 @@ static void nvicEnableVector(int irq, uint8_t prio) NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); - } #endif -int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) +void CanDriver::initOnce() { - int res = 0; - - UAVCAN_STM32_LOG("Bitrate %lu", static_cast(bitrate)); - /* - * CAN1 + * CAN1, CAN2 */ { CriticalSectionLocker lock; #if UAVCAN_STM32_NUTTX - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN); + 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; -#endif - } - - UAVCAN_STM32_LOG("Initing iface 0..."); - res = if0_.init(bitrate, mode); - if (res < 0) - { - UAVCAN_STM32_LOG("Iface 0 init failed %i", res); - goto fail; - } - ifaces[0] = &if0_; - - /* - * CAN2 - */ -#if UAVCAN_STM32_NUM_IFACES > 1 - { - CriticalSectionLocker lock; -# if UAVCAN_STM32_NUTTX - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0); -# else +# if UAVCAN_STM32_NUM_IFACES > 1 RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST; RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST; # endif - } - - UAVCAN_STM32_LOG("Initing iface 1..."); - res = if1_.init(bitrate, mode); - if (res < 0) - { - UAVCAN_STM32_LOG("Iface 1 init failed %i", res); - goto fail; - } - ifaces[1] = &if1_; #endif + } /* * IRQ @@ -944,12 +917,9 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod #if UAVCAN_STM32_NUTTX # define IRQ_ATTACH(irq, handler) \ { \ - res = irq_attach(irq, handler); \ - if (res < 0) \ - { \ - UAVCAN_STM32_LOG("IRQ attach failed %i", irq); \ - goto fail; \ - } \ + const int res = irq_attach(irq, handler); \ + (void)res; \ + assert(res >= 0); \ up_enable_irq(irq); \ } IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq); @@ -978,6 +948,47 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod # 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..."); + res = if0_.init(bitrate, mode); + if (res < 0) + { + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + goto fail; + } + ifaces[0] = &if0_; + + /* + * CAN2 + */ +#if UAVCAN_STM32_NUM_IFACES > 1 + UAVCAN_STM32_LOG("Initing iface 1..."); + res = if1_.init(bitrate, mode); + if (res < 0) + { + UAVCAN_STM32_LOG("Iface 1 init failed %i", res); + goto fail; + } + ifaces[1] = &if1_; +#endif UAVCAN_STM32_LOG("CAN drv init OK"); UAVCAN_ASSERT(res >= 0); @@ -986,21 +997,6 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod fail: UAVCAN_STM32_LOG("CAN drv init failed %i", res); UAVCAN_ASSERT(res < 0); - - CriticalSectionLocker lock; - -#if UAVCAN_STM32_NUTTX - // TODO: Unattach and disable all IRQs - modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_CAN1EN, 0); -# if UAVCAN_STM32_NUM_IFACES > 1 - modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_CAN2EN, 0); -# endif -#else - RCC->APB1ENR &= ~RCC_APB1ENR_CAN1EN; -# if UAVCAN_STM32_NUM_IFACES > 1 - RCC->APB1ENR &= ~RCC_APB1ENR_CAN2EN; -# endif -#endif return res; } diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp index 28779986db..f1e3d7a923 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp +++ b/libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp @@ -28,7 +28,9 @@ namespace board void init() { halInit(); + chibios_rt::System::init(); + sdStart(&STDOUT_SD, NULL); } @@ -79,6 +81,19 @@ void boardInit(void) AFIO_MAPR_CAN_REMAP_REMAP3 | AFIO_MAPR_CAN2_REMAP | AFIO_MAPR_USART2_REMAP; + + /* + * Enabling the CAN controllers, then configuring GPIO functions for CAN_TX. + * Order matters, otherwise the CAN_TX pins will twitch, disturbing the CAN bus. + * This is why we can't perform this initialization using ChibiOS GPIO configuration. + */ + RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; +#if UAVCAN_STM32_NUM_IFACES > 1 + RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; +#endif + + palSetPadMode(GPIOD, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL); + palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL); } } diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h b/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h index 7de04ac5e4..c0608f24c1 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h +++ b/libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h @@ -20,6 +20,8 @@ #define GPIO_PORT_LED GPIOB #define GPIO_PIN_LED 9 +// GPIOD 10 is configured as OUTPUT, it is used as board reboot monitor. + /* * I/O ports initial setup, this configuration is established soon after reset * in the initialization code. @@ -48,7 +50,7 @@ #define VAL_GPIOACRH 0x88888888 // 15..8 #define VAL_GPIOAODR 0x00000000 -#define VAL_GPIOBCRL 0x8B488888 +#define VAL_GPIOBCRL 0x84488888 // CAN2 TX initialized as INPUT, it must be configured later! #define VAL_GPIOBCRH 0x88888828 #define VAL_GPIOBODR 0x00000000 @@ -56,9 +58,9 @@ #define VAL_GPIOCCRH 0x88888888 #define VAL_GPIOCODR 0x00000000 -#define VAL_GPIODCRL 0x88b888B4 -#define VAL_GPIODCRH 0x88888888 -#define VAL_GPIODODR 0x00000000 +#define VAL_GPIODCRL 0x88b88844 // CAN1 TX initialized as INPUT, it must be configured later! +#define VAL_GPIODCRH 0x88888288 +#define VAL_GPIODODR ((1 << 10)) #define VAL_GPIOECRL 0x88888888 #define VAL_GPIOECRH 0x88888888