From a150c2ea54219004f1ac7ec6d6f845e35684dced Mon Sep 17 00:00:00 2001
From: Alexandre Bourdiol <alexandre.bourdiol@st.com>
Date: Tue, 22 Feb 2022 09:49:21 +0100
Subject: [PATCH] Wire: implement Clear Bus sequence to recover I2C bus.
 Source: https://www.nxp.com/docs/en/user-guide/UM10204.pdf
 https://bits4device.wordpress.com/2017/07/28/i2c-bus-recovery/

Useful in case of bus stuck after a reset for example
Fixes #1661

Signed-off-by: Alexandre Bourdiol <alexandre.bourdiol@st.com>
---
 libraries/Wire/src/Wire.cpp | 24 ++++++++++++++++++++++++
 libraries/Wire/src/Wire.h   |  1 +
 2 files changed, 25 insertions(+)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index dd75e57a9a..34519633d8 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -83,6 +83,8 @@ void TwoWire::begin(uint8_t address, bool generalCall)
 
   _i2c.generalCall = (generalCall == true) ? 1 : 0;
 
+  recoverBus(); // in case I2C bus (device) is stuck after a reset for example
+
   i2c_custom_init(&_i2c, 100000, I2C_ADDRESSINGMODE_7BIT, ownAddress);
 
   if (_i2c.isMaster == 0) {
@@ -501,6 +503,28 @@ inline void TwoWire::resetTxBuffer(void)
   }
 }
 
+// Send clear bus (clock pulse) sequence to recover bus.
+// Useful in case of bus stuck after a reset for example
+// a mix implementation of Clear Bus from
+// https://www.nxp.com/docs/en/user-guide/UM10204.pdf
+// https://bits4device.wordpress.com/2017/07/28/i2c-bus-recovery/
+void TwoWire::recoverBus(void)
+{
+  pinMode(pinNametoDigitalPin(_i2c.sda), INPUT);
+
+  if (digitalReadFast(_i2c.sda) == LOW) {
+    pinMode(pinNametoDigitalPin(_i2c.scl), OUTPUT);
+
+    for (int i = 0; i < 20; i++) {
+      digitalWriteFast(_i2c.scl, LOW);
+      delayMicroseconds(10);
+      digitalWriteFast(_i2c.scl, HIGH);
+      delayMicroseconds(10);
+    }
+    pinMode(pinNametoDigitalPin(_i2c.scl), INPUT);
+  }
+}
+
 // Preinstantiate Objects //////////////////////////////////////////////////////
 
 TwoWire Wire = TwoWire(); //D14-D15
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index 206fe27508..cd1dfe925d 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -66,6 +66,7 @@ class TwoWire : public Stream {
 
     void resetRxBuffer(void);
     void resetTxBuffer(void);
+    void recoverBus(void);
 
   public:
     TwoWire();