From f57b1b9c4bed8c37fb7a341cce822e479efd96bf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 12 Mar 2021 14:08:23 +1100 Subject: [PATCH] HAL_ChibiOS: support logical/physical CAN bus mapping allow for first CAN interface to be CAN2 or CAN3 --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 84 ++++++++++---- libraries/AP_HAL_ChibiOS/CANFDIface.h | 13 +-- libraries/AP_HAL_ChibiOS/CANIface.h | 10 +- libraries/AP_HAL_ChibiOS/CanIface.cpp | 109 +++++++++++++----- .../hwdef/scripts/chibios_hwdef.py | 41 +++++-- 5 files changed, 183 insertions(+), 74 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 689cd1e7ff..cb6743efc1 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -96,21 +96,32 @@ using namespace ChibiOS; #endif constexpr CANIface::CanType* const CANIface::Can[]; -static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr}; +static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES]; + +uint8_t CANIface::next_interface; + +// mapping from logical interface to physical. First physical is 0, first logical is 0 +static constexpr uint8_t can_interfaces[HAL_NUM_CAN_IFACES] = { HAL_CAN_INTERFACE_LIST }; + +// mapping from physical interface back to logical. First physical is 0, first logical is 0 +static constexpr int8_t can_iface_to_idx[3] = { HAL_CAN_INTERFACE_REV_LIST }; + #define REG_SET_TIMEOUT 250 // if it takes longer than 250ms for setting a register we have failed + static inline bool driver_initialised(uint8_t iface_index) { - if (iface_index >= HAL_NUM_CAN_IFACES) { - return false; - } if (can_ifaces[iface_index] == nullptr) { return false; } return true; } -static inline void handleCANInterrupt(uint8_t iface_index, uint8_t line_index) +static inline void handleCANInterrupt(uint8_t phys_index, uint8_t line_index) { + const int8_t iface_index = can_iface_to_idx[phys_index]; + if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) { + return; + } if (!driver_initialised(iface_index)) { //Just reset all the interrupts and return CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N; @@ -161,6 +172,11 @@ CANIface::CANIface(uint8_t index) : } } +// constructor suitable for array +CANIface::CANIface() : + CANIface(next_interface++) +{} + void CANIface::handleBusOffInterrupt() { _detected_bus_off = true; @@ -548,16 +564,20 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) */ if (!irq_init_) { CriticalSectionLocker lock; - if (self_index_ == 0) { + switch (can_interfaces[self_index_]) { + case 0: nvicEnableVector(FDCAN1_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); nvicEnableVector(FDCAN1_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); - } -# if HAL_NUM_CAN_IFACES > 1 - else if (self_index_ == 1) { + break; + case 1: nvicEnableVector(FDCAN2_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); nvicEnableVector(FDCAN2_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; + case 2: + nvicEnableVector(FDCAN3_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + nvicEnableVector(FDCAN3_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; } -# endif irq_init_ = true; } @@ -679,12 +699,13 @@ void CANIface::clear_rx() void CANIface::setupMessageRam() { #if defined(STM32G4) - memset((void*)SRAMCAN_BASE, 0, 0x350); - MessageRam_.StandardFilterSA = SRAMCAN_BASE; - MessageRam_.ExtendedFilterSA = SRAMCAN_BASE + 0x70; - MessageRam_.RxFIFO0SA = SRAMCAN_BASE + 0xB0; - MessageRam_.RxFIFO1SA = SRAMCAN_BASE + 0x188; - MessageRam_.TxFIFOQSA = SRAMCAN_BASE + 0x278; + const uint32_t base = SRAMCAN_BASE + 0x350 * can_interfaces[self_index_]; + memset((void*)base, 0, 0x350); + MessageRam_.StandardFilterSA = base; + MessageRam_.ExtendedFilterSA = base + 0x70; + MessageRam_.RxFIFO0SA = base + 0xB0; + MessageRam_.RxFIFO1SA = base + 0x188; + MessageRam_.TxFIFOQSA = base + 0x278; can_->TXBC = 0; // fifo mode #else @@ -997,7 +1018,7 @@ bool CANIface::select(bool &read, bool &write, } time = AP_HAL::micros(); } - return true; // Return value doesn't matter as long as it is non-negative + return false; } #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) @@ -1032,7 +1053,8 @@ void CANIface::get_stats(ExpandingString &str) */ extern "C" { - +#ifdef HAL_CAN_IFACE1_ENABLE + // FDCAN1 CH_IRQ_HANDLER(FDCAN1_IT0_IRQHandler); CH_IRQ_HANDLER(FDCAN1_IT0_IRQHandler) { @@ -1048,10 +1070,10 @@ extern "C" handleCANInterrupt(0, 1); CH_IRQ_EPILOGUE(); } +#endif - -# if HAL_NUM_CAN_IFACES > 1 - +#ifdef HAL_CAN_IFACE2_ENABLE + // FDCAN2 CH_IRQ_HANDLER(FDCAN2_IT0_IRQHandler); CH_IRQ_HANDLER(FDCAN2_IT0_IRQHandler) { @@ -1067,9 +1089,27 @@ extern "C" handleCANInterrupt(1, 1); CH_IRQ_EPILOGUE(); } +#endif -# endif +#ifdef HAL_CAN_IFACE3_ENABLE + // FDCAN3 + CH_IRQ_HANDLER(FDCAN3_IT0_IRQHandler); + CH_IRQ_HANDLER(FDCAN3_IT0_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(2, 0); + CH_IRQ_EPILOGUE(); + } + CH_IRQ_HANDLER(FDCAN3_IT1_IRQHandler); + CH_IRQ_HANDLER(FDCAN3_IT1_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(2, 1); + CH_IRQ_EPILOGUE(); + } +#endif + } // extern "C" #endif //defined(STM32H7XX) || defined(STM32G4) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 0edb08530c..e9218a5c79 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -166,6 +166,8 @@ public: * Common CAN methods * * ****************************************/ CANIface(uint8_t index); + CANIface(); + static uint8_t next_interface; // Initialise CAN Peripheral bool init(const uint32_t bitrate, const OperatingMode mode) override; @@ -232,14 +234,9 @@ public: // droping the frame, and counting errors void pollErrorFlagsFromISR(void); - // CAN Peripheral register structure - static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { - reinterpret_cast(FDCAN1_BASE) -#if HAL_NUM_CAN_IFACES > 1 - , - reinterpret_cast(FDCAN2_BASE) -#endif - }; + // CAN Peripheral register structure, pointing at base + // register. Indexed by locical interface number + static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST }; }; diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 9224ada640..19393256c4 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -161,6 +161,8 @@ public: * Common CAN methods * * ****************************************/ CANIface(uint8_t index); + CANIface(); + static uint8_t next_interface; // Initialise CAN Peripheral bool init(const uint32_t bitrate, const OperatingMode mode) override; @@ -233,13 +235,7 @@ public: void pollErrorFlagsFromISR(void); // CAN Peripheral register structure - static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { - reinterpret_cast(uintptr_t(0x40006400U)) -#if HAL_NUM_CAN_IFACES > 1 - , - reinterpret_cast(uintptr_t(0x40006800U)) -#endif - }; + static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST }; }; #endif //HAL_NUM_CAN_IFACES #endif //# if defined(STM32H7XX) || defined(STM32G4) diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index d4256eaa82..0c2a666bac 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -88,11 +88,20 @@ extern AP_HAL::HAL& hal; using namespace ChibiOS; constexpr bxcan::CanType* const CANIface::Can[]; -static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr}; +static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES]; -static inline void handleTxInterrupt(uint8_t iface_index) +uint8_t CANIface::next_interface; + +// mapping from logical interface to physical. First physical is 0, first logical is 0 +static constexpr uint8_t can_interfaces[HAL_NUM_CAN_IFACES] = { HAL_CAN_INTERFACE_LIST }; + +// mapping from physical interface back to logical. First physical is 0, first logical is 0 +static constexpr int8_t can_iface_to_idx[3] = { HAL_CAN_INTERFACE_REV_LIST }; + +static inline void handleTxInterrupt(uint8_t phys_index) { - if (iface_index >= HAL_NUM_CAN_IFACES) { + const int8_t iface_index = can_iface_to_idx[phys_index]; + if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) { return; } uint64_t precise_time = AP_HAL::micros64(); @@ -104,9 +113,10 @@ static inline void handleTxInterrupt(uint8_t iface_index) } } -static inline void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index) +static inline void handleRxInterrupt(uint8_t phys_index, uint8_t fifo_index) { - if (iface_index >= HAL_NUM_CAN_IFACES) { + const int8_t iface_index = can_iface_to_idx[phys_index]; + if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) { return; } uint64_t precise_time = AP_HAL::micros64(); @@ -140,6 +150,11 @@ CANIface::CANIface(uint8_t index) : } } +// constructor suitable for array +CANIface::CANIface() : + CANIface(next_interface++) +{} + bool CANIface::computeTimings(uint32_t target_bitrate, Timings& out_timings) { if (target_bitrate < 1) { @@ -735,18 +750,27 @@ void CANIface::initOnce(bool enable_irq) */ { CriticalSectionLocker lock; - if (self_index_ == 0) { + switch (can_interfaces[self_index_]) { + case 0: RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST; RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST; - } -# if HAL_NUM_CAN_IFACES > 1 - else if (self_index_ == 1) { + break; +#ifdef RCC_APB1ENR_CAN2EN + case 1: RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST; RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST; + break; +#endif +#ifdef RCC_APB1ENR_CAN3EN + case 2: + RCC->APB1ENR |= RCC_APB1ENR_CAN3EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN3RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN3RST; + break; +#endif } -# endif } /* @@ -754,18 +778,27 @@ void CANIface::initOnce(bool enable_irq) */ if (!irq_init_ && enable_irq) { CriticalSectionLocker lock; - if (self_index_ == 0) { + switch (can_interfaces[self_index_]) { + case 0: nvicEnableVector(CAN1_TX_IRQn, CORTEX_MAX_KERNEL_PRIORITY); nvicEnableVector(CAN1_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); nvicEnableVector(CAN1_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); - } -# if HAL_NUM_CAN_IFACES > 1 - else if (self_index_ == 1) { + break; +#ifdef CAN2_TX_IRQn + case 1: nvicEnableVector(CAN2_TX_IRQn, CORTEX_MAX_KERNEL_PRIORITY); nvicEnableVector(CAN2_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); nvicEnableVector(CAN2_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; +#endif +#ifdef CAN3_TX_IRQn + case 2: + nvicEnableVector(CAN3_TX_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + nvicEnableVector(CAN3_RX0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + nvicEnableVector(CAN3_RX1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; +#endif } -# endif irq_init_ = true; } } @@ -935,7 +968,8 @@ void CANIface::get_stats(ExpandingString &str) */ extern "C" { - +#ifdef HAL_CAN_IFACE1_ENABLE + // CAN1 CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler); CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler) { @@ -959,21 +993,10 @@ extern "C" handleRxInterrupt(0, 1); CH_IRQ_EPILOGUE(); } - -#if HAL_NUM_CAN_IFACES > 1 - -#if !defined(CAN2_TX_IRQ_Handler) -# error "Misconfigured build1" -#endif - -#if !defined(CAN2_RX0_IRQ_Handler) -# error "Misconfigured build2" -#endif - -#if !defined(CAN2_RX1_IRQ_Handler) -# error "Misconfigured build3" #endif +#ifdef HAL_CAN_IFACE2_ENABLE + // CAN2 CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler); CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler) { @@ -997,9 +1020,35 @@ extern "C" handleRxInterrupt(1, 1); CH_IRQ_EPILOGUE(); } +#endif -# endif +#ifdef HAL_CAN_IFACE3_ENABLE + // CAN3 + CH_IRQ_HANDLER(CAN3_TX_IRQ_Handler); + CH_IRQ_HANDLER(CAN3_TX_IRQ_Handler) + { + CH_IRQ_PROLOGUE(); + handleTxInterrupt(2); + CH_IRQ_EPILOGUE(); + } + CH_IRQ_HANDLER(CAN3_RX0_IRQ_Handler); + CH_IRQ_HANDLER(CAN3_RX0_IRQ_Handler) + { + CH_IRQ_PROLOGUE(); + handleRxInterrupt(2, 0); + CH_IRQ_EPILOGUE(); + } + + CH_IRQ_HANDLER(CAN3_RX1_IRQ_Handler); + CH_IRQ_HANDLER(CAN3_RX1_IRQ_Handler) + { + CH_IRQ_PROLOGUE(); + handleRxInterrupt(2, 1); + CH_IRQ_EPILOGUE(); + } +#endif + } // extern "C" #endif //!defined(STM32H7XX) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index d00e27d457..d7371a16df 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -615,10 +615,40 @@ def make_line(label): return line -def enable_can(f, num_ifaces): +def enable_can(f): '''setup for a CAN enabled board''' - f.write('#define HAL_NUM_CAN_IFACES %d\n' % num_ifaces) - env_vars['HAL_NUM_CAN_IFACES'] = str(num_ifaces) + global mcu_series + if mcu_series.startswith("STM32H7") or mcu_series.startswith("STM32G4"): + prefix = "FDCAN" + cast = "CanType" + else: + prefix = "CAN" + cast = "bxcan::CanType" + + # allow for optional CAN_ORDER option giving bus order + can_order_str = get_config('CAN_ORDER', required=False, aslist=True) + if can_order_str: + can_order = [int(s) for s in can_order_str] + else: + can_order = [] + for i in range(1,3): + if 'CAN%u' % i in bytype: + can_order.append(i) + + base_list = [] + for i in can_order: + base_list.append("reinterpret_cast<%s*>(uintptr_t(%s%s_BASE))" % (cast, prefix, i)) + f.write("#define HAL_CAN_IFACE%u_ENABLE\n" % i) + + can_rev_order = [-1]*3 + for i in range(len(can_order)): + can_rev_order[can_order[i]-1] = i + + f.write('#define HAL_CAN_INTERFACE_LIST %s\n' % ','.join([str(i-1) for i in can_order])) + f.write('#define HAL_CAN_INTERFACE_REV_LIST %s\n' % ','.join([str(i) for i in can_rev_order])) + f.write('#define HAL_CAN_BASE_LIST %s\n' % ','.join(base_list)) + f.write('#define HAL_NUM_CAN_IFACES %d\n' % len(base_list)) + env_vars['HAL_NUM_CAN_IFACES'] = str(len(base_list)) def has_sdcard_spi(): @@ -710,10 +740,7 @@ def write_mcu_config(f): f.write('#define %s\n' % d[7:]) if have_type_prefix('CAN') and not using_chibios_can: - if 'CAN1' in bytype and 'CAN2' in bytype: - enable_can(f, 2) - else: - enable_can(f, 1) + enable_can(f) flash_size = get_config('FLASH_SIZE_KB', type=int) f.write('#define BOARD_FLASH_SIZE %u\n' % flash_size) env_vars['BOARD_FLASH_SIZE'] = flash_size