From 6e69df2da8bbac6ef9e583a1f8b377d500beda4b Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Sat, 14 Jan 2023 18:18:57 +0000
Subject: [PATCH] Add I2C retries to Wire and to non-blocking I2CManager.

---
 I2CManager.h             |  6 ++++++
 I2CManager_NonBlocking.h | 27 ++++++++++++++++++---------
 I2CManager_Wire.h        | 37 +++++++++++++++++++++++--------------
 3 files changed, 47 insertions(+), 23 deletions(-)

diff --git a/I2CManager.h b/I2CManager.h
index 7df0803..f966d77 100644
--- a/I2CManager.h
+++ b/I2CManager.h
@@ -111,6 +111,11 @@
  * 
  */
 
+// Maximum number of retries on an I2C operation
+// A value of zero will disable retries.
+// Maximum value is 254 (unsigned byte counter)
+#define MAX_I2C_RETRIES 2
+
 // Add following line to config.h to enable Wire library instead of native I2C drivers
 //#define I2C_USE_WIRE
 
@@ -265,6 +270,7 @@ private:
     static volatile unsigned long startTime;
 
     static unsigned long timeout; // Transaction timeout in microseconds.  0=disabled.
+    static uint8_t retryCounter;  // Count of retries
     
     void startTransaction();
     
diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h
index fbcb98a..2793823 100644
--- a/I2CManager_NonBlocking.h
+++ b/I2CManager_NonBlocking.h
@@ -223,16 +223,15 @@ void I2CManagerClass::handleInterrupt() {
   // Update hardware state machine
   I2C_handleInterrupt();
 
-  // Enable interrupts to minimise effect on other interrupt code
-  interrupts();
-
   // Check if current request has completed.  If there's a current request
   // and state isn't active then state contains the completion status of the request.
   if (state != I2C_STATE_ACTIVE && currentRequest != NULL) {
-    // Remove completed request from head of queue
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    // Operation has completed.
+    if (state == I2C_STATUS_OK || ++retryCounter > MAX_I2C_RETRIES) {
+      // Status is OK, or has failed and retry count exceeded.
+      // Remove completed request from head of queue
       I2CRB * t = queueHead;
-      if (t == queueHead) {
+      if (t == currentRequest) {
         queueHead = t->nextRequest;
         if (!queueHead) queueTail = queueHead;
         t->nBytes = rxCount;
@@ -241,12 +240,21 @@ void I2CManagerClass::handleInterrupt() {
         // I2C state machine is now free for next request
         currentRequest = NULL;
         state = I2C_STATE_FREE;
-
-        // Start next request (if any)
-        I2CManager.startTransaction();
       }
+      retryCounter = 0;
+    } else {
+      // Status is failed and retry permitted.
+      // Retry previous request.
+      state = I2C_STATE_FREE;  
     }
   }
+
+  if (state == I2C_STATE_FREE && queueHead != NULL) {
+    // Allow any pending interrupts before starting the next request.
+    interrupts();
+    // Start next request
+    I2CManager.startTransaction();
+  }
 }
 
 // Fields in I2CManager class specific to Non-blocking implementation.
@@ -261,5 +269,6 @@ volatile uint8_t I2CManagerClass::bytesToSend;
 volatile uint8_t I2CManagerClass::bytesToReceive;
 volatile unsigned long I2CManagerClass::startTime;
 unsigned long I2CManagerClass::timeout = 0;
+uint8_t I2CManagerClass::retryCounter = 0;
 
 #endif
\ No newline at end of file
diff --git a/I2CManager_Wire.h b/I2CManager_Wire.h
index 87152e7..aa0189f 100644
--- a/I2CManager_Wire.h
+++ b/I2CManager_Wire.h
@@ -49,9 +49,14 @@ void I2CManagerClass::_setClock(unsigned long i2cClockSpeed) {
  *  Initiate a write to an I2C device (blocking operation on Wire)
  ***************************************************************************/
 uint8_t I2CManagerClass::write(uint8_t address, const uint8_t buffer[], uint8_t size, I2CRB *rb) {
-  Wire.beginTransmission(address);
-  if (size > 0) Wire.write(buffer, size);
-  rb->status = Wire.endTransmission();
+  uint8_t status = I2C_STATUS_OK;
+  uint8_t retryCount = 0;
+  do {
+    Wire.beginTransmission(address);
+    if (size > 0) Wire.write(buffer, size);
+    status = Wire.endTransmission();
+  } while (!(status == I2C_STATUS_OK || ++retryCount > MAX_I2C_RETRIES));
+  rb->status = status;
   return I2C_STATUS_OK;
 }
 
@@ -75,17 +80,21 @@ uint8_t I2CManagerClass::read(uint8_t address, uint8_t readBuffer[], uint8_t rea
 {
   uint8_t status = I2C_STATUS_OK;
   uint8_t nBytes = 0;
-  if (writeSize > 0) {
-    Wire.beginTransmission(address);
-    Wire.write(writeBuffer, writeSize);
-    status = Wire.endTransmission(false); // Don't free bus yet
-  }
-  if (status == I2C_STATUS_OK) {
-    Wire.requestFrom(address, (size_t)readSize);
-    while (Wire.available() && nBytes < readSize) 
-      readBuffer[nBytes++] = Wire.read();
-    if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
-  }
+  uint8_t retryCount = 0;
+  do {
+    if (writeSize > 0) {
+      Wire.beginTransmission(address);
+      Wire.write(writeBuffer, writeSize);
+      status = Wire.endTransmission(false); // Don't free bus yet
+    }
+    if (status == I2C_STATUS_OK) {
+      Wire.requestFrom(address, (size_t)readSize);
+      while (Wire.available() && nBytes < readSize) 
+        readBuffer[nBytes++] = Wire.read();
+      if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
+    }
+  } while (!(status == I2C_STATUS_OK || ++retryCount > MAX_I2C_RETRIES));
+
   rb->nBytes = nBytes;
   rb->status = status;
   return I2C_STATUS_OK;