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

@@ -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)
+    for (uint8_t i=0; i<ARRAY_SIZE(I2CD); i++) {
-    clear_bus(HAL_GPIO_PIN_I2C1_SCL, HAL_I2C1_SCL_AF);
+        clear_bus(i);
-#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
 }
 
-//This code blocks!
+/*
-void I2CBus::clear_bus(ioline_t scl_line, uint8_t scl_af)
+  clear a stuck bus (bus held by a device that is holding SDA low) by
+  clocking out pulses on SCL to let the device complete its
+  transaction
+ */
+void I2CBus::clear_bus(uint8_t busidx)
 {
-    //send dummy clock
+    const struct I2CInfo &info = I2CD[busidx];
-    palSetLineMode(scl_line, PAL_MODE_OUTPUT_PUSHPULL);
+    const iomode_t mode_saved = palReadLineMode(info.scl_line);
-    for(int i = 0; i < 20; i++) {
+    palSetLineMode(info.scl_line, PAL_MODE_OUTPUT_PUSHPULL);
-        palToggleLine(scl_line);
+    for(uint8_t j = 0; j < 20; j++) {
-        hal.scheduler->delay_microseconds(200);
+        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);

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 {

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))
+            % (n, n, n, n, 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)
-            )
     f.write('\n#define HAL_I2C_DEVICE_LIST %s\n\n' % ','.join(devlist))
 
 def parse_timer(str):