From adf7fefc2e51d93df4e1d778a20cf030beb940a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Nov 2018 21:25:29 +1100 Subject: [PATCH] HAL_ChibiOS: detect stuck I2C bus and clear with SCL This detects the I2C bus becoming stuck with SDA low after a timeout and clears the bus by toggling SCL. Many thanks to @jhw84 for the suggestion --- libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 66 ++++++++++++------- libraries/AP_HAL_ChibiOS/I2CDevice.h | 3 +- .../hwdef/scripts/chibios_hwdef.py | 13 ++-- 3 files changed, 50 insertions(+), 32 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index eef0a0c554..0549a19d3c 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -30,6 +30,8 @@ static const struct I2CInfo { struct I2CDriver *i2c; uint8_t dma_channel_rx; uint8_t dma_channel_tx; + ioline_t scl_line; + ioline_t sda_line; } I2CD[] = { HAL_I2C_DEVICE_LIST }; using namespace ChibiOS; @@ -51,6 +53,13 @@ I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(I2CD)]; #define HAL_I2C_F7_100_TIMINGR 0x20404768 #define HAL_I2C_F7_400_TIMINGR 0x6000030D +/* + enable clear (toggling SCL) on I2C bus timeouts which leave SDA stuck low + */ +#ifndef HAL_I2C_CLEAR_ON_TIMEOUT +#define HAL_I2C_CLEAR_ON_TIMEOUT 1 +#endif + // get a handle for DMA sharing DMA channels with other subsystems void I2CBus::dma_init(void) { @@ -63,33 +72,39 @@ void I2CBus::dma_init(void) // Clear Bus to avoid bus lockup void I2CBus::clear_all() { -#if defined(HAL_GPIO_PIN_I2C1_SCL) && defined(HAL_I2C1_SCL_AF) - clear_bus(HAL_GPIO_PIN_I2C1_SCL, HAL_I2C1_SCL_AF); -#endif - -#if defined(HAL_GPIO_PIN_I2C2_SCL) && defined(HAL_I2C2_SCL_AF) - clear_bus(HAL_GPIO_PIN_I2C2_SCL, HAL_I2C2_SCL_AF); -#endif - -#if defined(HAL_GPIO_PIN_I2C3_SCL) && defined(HAL_I2C3_SCL_AF) - clear_bus(HAL_GPIO_PIN_I2C3_SCL, HAL_I2C3_SCL_AF); -#endif - -#if defined(HAL_GPIO_PIN_I2C4_SCL) && defined(HAL_I2C4_SCL_AF) - clear_bus(HAL_GPIO_PIN_I2C4_SCL, HAL_I2C4_SCL_AF); -#endif + for (uint8_t i=0; idelay_microseconds(200); + const struct I2CInfo &info = I2CD[busidx]; + const iomode_t mode_saved = palReadLineMode(info.scl_line); + palSetLineMode(info.scl_line, PAL_MODE_OUTPUT_PUSHPULL); + for(uint8_t j = 0; j < 20; j++) { + palToggleLine(info.scl_line); + hal.scheduler->delay_microseconds(10); } - palSetLineMode(scl_line, PAL_MODE_ALTERNATE(scl_af) | PAL_STM32_OSPEED_MID2 | PAL_STM32_OTYPE_OPENDRAIN); + palSetLineMode(info.scl_line, mode_saved); +} + +/* + read SDA on a bus, to check if it may be stuck + */ +uint8_t I2CBus::read_sda(uint8_t busidx) +{ + const struct I2CInfo &info = I2CD[busidx]; + const iomode_t mode_saved = palReadLineMode(info.sda_line); + palSetLineMode(info.sda_line, PAL_MODE_INPUT); + uint8_t ret = palReadLine(info.sda_line); + palSetLineMode(info.sda_line, mode_saved); + return ret; } // setup I2C buses @@ -260,6 +275,11 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len, i2cReleaseBus(I2CD[bus.busnum].i2c); return true; } +#if HAL_I2C_CLEAR_ON_TIMEOUT + if (ret == MSG_TIMEOUT && I2CBus::read_sda(bus.busnum) == 0) { + I2CBus::clear_bus(bus.busnum); + } +#endif } bus.bouncebuffer_finish(send, recv, recv_len); i2cReleaseBus(I2CD[bus.busnum].i2c); diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h index a53cb059a3..5a7448b4a7 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.h +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h @@ -50,7 +50,8 @@ public: void dma_deallocate(Shared_DMA *); void dma_init(void); static void clear_all(void); - static void clear_bus(ioline_t scl_line, uint8_t scl_af); + static void clear_bus(uint8_t busidx); + static uint8_t read_sda(uint8_t busidx); }; class I2CDevice : public AP_HAL::I2CDevice { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 2b3eb85ad5..3904e49f0c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -872,6 +872,8 @@ def write_I2C_config(f): if len(i2c_list) == 0: error("I2C_ORDER invalid") devlist = [] + + # write out config structures for dev in i2c_list: if not dev.startswith('I2C') or dev[3] not in "1234": error("Bad I2C_ORDER element %s" % dev) @@ -879,17 +881,12 @@ def write_I2C_config(f): devlist.append('HAL_I2C%u_CONFIG' % n) f.write(''' #if defined(STM32_I2C_I2C%u_RX_DMA_STREAM) && defined(STM32_I2C_I2C%u_TX_DMA_STREAM) -#define HAL_I2C%u_CONFIG { &I2CD%u, STM32_I2C_I2C%u_RX_DMA_STREAM, STM32_I2C_I2C%u_TX_DMA_STREAM } +#define HAL_I2C%u_CONFIG { &I2CD%u, STM32_I2C_I2C%u_RX_DMA_STREAM, STM32_I2C_I2C%u_TX_DMA_STREAM, HAL_GPIO_PIN_I2C%u_SCL, HAL_GPIO_PIN_I2C%u_SDA } #else -#define HAL_I2C%u_CONFIG { &I2CD%u, SHARED_DMA_NONE, SHARED_DMA_NONE } +#define HAL_I2C%u_CONFIG { &I2CD%u, SHARED_DMA_NONE, SHARED_DMA_NONE, HAL_GPIO_PIN_I2C%u_SCL, HAL_GPIO_PIN_I2C%u_SDA } #endif ''' - % (n, n, n, n, n, n, n, n)) - if dev + "_SCL" in bylabel: - p = bylabel[dev + "_SCL"] - f.write( - '#define HAL_%s_SCL_AF %d\n' % (dev, p.af) - ) + % (n, n, n, n, n, n, n, n, n, n, n, n)) f.write('\n#define HAL_I2C_DEVICE_LIST %s\n\n' % ','.join(devlist)) def parse_timer(str):