From 371aaca4010859aecf26ecdc420e6743f5299121 Mon Sep 17 00:00:00 2001
From: Witold Markowski <witold.a.markowski@gmail.com>
Date: Wed, 28 Feb 2018 20:59:45 +0000
Subject: [PATCH 01/16] #1476 Extend API by adding a possibility to set a
 global timeout in milliseconds.

---
 libraries/Wire/src/Wire.cpp      | 5 +++++
 libraries/Wire/src/Wire.h        | 1 +
 libraries/Wire/src/utility/twi.c | 6 ++++++
 libraries/Wire/src/utility/twi.h | 1 +
 4 files changed, 13 insertions(+)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index 58916ce93..5186373a7 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -86,6 +86,11 @@ void TwoWire::setClock(uint32_t clock)
   twi_setFrequency(clock);
 }
 
+void TwoWire::setTimeoutInMillis(uint8_t timeout)
+{
+  twi_setTimeoutInMillis(timeout);
+}
+
 uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
 {
   if (isize > 0) {
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index 702f37d64..b3ce61a3a 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -54,6 +54,7 @@ class TwoWire : public Stream
     void begin(int);
     void end();
     void setClock(uint32_t);
+    void setTimeoutInMillis(uint8_t);
     void beginTransmission(uint8_t);
     void beginTransmission(int);
     uint8_t endTransmission(void);
diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 1a3514630..7716be964 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -42,6 +42,7 @@ static volatile uint8_t twi_state;
 static volatile uint8_t twi_slarw;
 static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
 static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
+static volatile uint8_t twi_timeout_ms = 0;
 
 static void (*twi_onSlaveTransmit)(void);
 static void (*twi_onSlaveReceive)(uint8_t*, int);
@@ -396,6 +397,11 @@ void twi_releaseBus(void)
   twi_state = TWI_READY;
 }
 
+void twi_setTimeoutInMillis(uint8_t timeout)
+{
+   twi_timeout_ms = timeout;
+}
+
 ISR(TWI_vect)
 {
   switch(TW_STATUS){
diff --git a/libraries/Wire/src/utility/twi.h b/libraries/Wire/src/utility/twi.h
index d27325ea7..d52b5d8a8 100644
--- a/libraries/Wire/src/utility/twi.h
+++ b/libraries/Wire/src/utility/twi.h
@@ -50,6 +50,7 @@
   void twi_reply(uint8_t);
   void twi_stop(void);
   void twi_releaseBus(void);
+  void twi_setTimeoutInMillis(uint8_t);
 
 #endif
 

From 36933f3d1e9822da7ae5e32ded48c7c1b16e218c Mon Sep 17 00:00:00 2001
From: Witold Markowski <witold.a.markowski@gmail.com>
Date: Thu, 1 Mar 2018 13:49:04 +0000
Subject: [PATCH 02/16] #1476. Introduce timeout when checking twi_state
 variable.

---
 libraries/Wire/src/utility/twi.c | 30 +++++++++++++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 7716be964..66d1c49bc 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -25,7 +25,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <compat/twi.h>
-#include "Arduino.h" // for digitalWrite
+#include "Arduino.h" // for digitalWrite and millis
 
 #ifndef cbi
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
@@ -155,7 +155,14 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   }
 
   // wait until twi is ready, become master receiver
+  uint32_t startMillis = millis();
   while(TWI_READY != twi_state){
+    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+      //timeout
+      twi_init();
+
+      return 0;
+    }
     continue;
   }
   twi_state = TWI_MRX;
@@ -194,7 +201,14 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
     TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
 
   // wait for read operation to complete
+  startMillis = millis();
   while(TWI_MRX == twi_state){
+    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+      //timeout
+      twi_init();
+
+      return 0;
+    }
     continue;
   }
 
@@ -234,7 +248,14 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   }
 
   // wait until twi is ready, become master transmitter
+  uint32_t startMillis = millis();
   while(TWI_READY != twi_state){
+    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+      //timeout
+      twi_init();
+
+      return 4;
+    }
     continue;
   }
   twi_state = TWI_MTX;
@@ -276,7 +297,14 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs
 
   // wait for write operation to complete
+  startMillis = millis();
   while(wait && (TWI_MTX == twi_state)){
+    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+      //timeout
+      twi_init();
+
+      return 4;
+    }
     continue;
   }
   

From 4653ce41bebe2237c7bcd51ab750dab3b4b08e4c Mon Sep 17 00:00:00 2001
From: Witold Markowski <witold.a.markowski@gmail.com>
Date: Sat, 3 Mar 2018 21:11:28 +0000
Subject: [PATCH 03/16] #1476 Add timout in twi_stop(void) method.

---
 libraries/Wire/src/utility/twi.c | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 66d1c49bc..45d08ba76 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -402,7 +402,16 @@ void twi_stop(void)
 
   // wait for stop condition to be exectued on bus
   // TWINT is not set after a stop condition!
+  uint32_t counter = 0;
   while(TWCR & _BV(TWSTO)){
+    counter++;
+    if((twi_timeout_ms > 0) && (counter >= 25000)) {
+      // timeout
+      twi_init();
+
+      return;
+    }
+
     continue;
   }
 

From 24037cc828407a2aa1ad773b1252a7b865dbace7 Mon Sep 17 00:00:00 2001
From: Witold Markowski <witold.a.markowski@gmail.com>
Date: Mon, 5 Mar 2018 20:35:25 +0000
Subject: [PATCH 04/16] #1476 Disable TWI before init.

---
 libraries/Wire/src/utility/twi.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 45d08ba76..8dcee043d 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -159,6 +159,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   while(TWI_READY != twi_state){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
       //timeout
+      twi_disable();
       twi_init();
 
       return 0;
@@ -205,6 +206,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   while(TWI_MRX == twi_state){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
       //timeout
+      twi_disable();
       twi_init();
 
       return 0;
@@ -252,6 +254,7 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   while(TWI_READY != twi_state){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
       //timeout
+      twi_disable();
       twi_init();
 
       return 4;
@@ -301,6 +304,7 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   while(wait && (TWI_MTX == twi_state)){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
       //timeout
+      twi_disable();
       twi_init();
 
       return 4;
@@ -407,6 +411,7 @@ void twi_stop(void)
     counter++;
     if((twi_timeout_ms > 0) && (counter >= 25000)) {
       // timeout
+      twi_disable();
       twi_init();
 
       return;

From 4f907bcf76cf9b768a492167e9343ed8727bcc24 Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Mon, 16 Sep 2019 12:14:34 +0100
Subject: [PATCH 05/16] move timout handling into its own function

---
 libraries/Wire/src/utility/twi.c | 45 +++++++++++++++++---------------
 libraries/Wire/src/utility/twi.h |  1 +
 2 files changed, 25 insertions(+), 21 deletions(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 8dcee043d..d625cf0dc 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -158,10 +158,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   uint32_t startMillis = millis();
   while(TWI_READY != twi_state){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
-      //timeout
-      twi_disable();
-      twi_init();
-
+      twi_handleTimeout();
       return 0;
     }
     continue;
@@ -205,10 +202,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   startMillis = millis();
   while(TWI_MRX == twi_state){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
-      //timeout
-      twi_disable();
-      twi_init();
-
+      twi_handleTimeout();
       return 0;
     }
     continue;
@@ -253,10 +247,7 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   uint32_t startMillis = millis();
   while(TWI_READY != twi_state){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
-      //timeout
-      twi_disable();
-      twi_init();
-
+      twi_handleTimeout();
       return 4;
     }
     continue;
@@ -303,10 +294,7 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   startMillis = millis();
   while(wait && (TWI_MTX == twi_state)){
     if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
-      //timeout
-      twi_disable();
-      twi_init();
-
+      twi_handleTimeout();
       return 4;
     }
     continue;
@@ -410,10 +398,7 @@ void twi_stop(void)
   while(TWCR & _BV(TWSTO)){
     counter++;
     if((twi_timeout_ms > 0) && (counter >= 25000)) {
-      // timeout
-      twi_disable();
-      twi_init();
-
+      twi_handleTimeout();
       return;
     }
 
@@ -439,9 +424,27 @@ void twi_releaseBus(void)
   twi_state = TWI_READY;
 }
 
+/* 
+ * Function twi_setTimeoutInMillis
+ * Desc     set a global timeout for while loops that we might get stuck in
+ * Input    timeout value in milliseconds
+ * Output   none
+ */
 void twi_setTimeoutInMillis(uint8_t timeout)
 {
-   twi_timeout_ms = timeout;
+  twi_timeout_ms = timeout;
+}
+
+/* 
+ * Function twi_handleTimeout
+ * Desc     do this stuff when a while loop here has run for longer than twi_timeout_ms milliseconds
+ * Input    none
+ * Output   none
+ */
+void twi_handleTimeout(void)
+{
+  twi_disable();
+  twi_init();
 }
 
 ISR(TWI_vect)
diff --git a/libraries/Wire/src/utility/twi.h b/libraries/Wire/src/utility/twi.h
index d52b5d8a8..ba6ae5b7e 100644
--- a/libraries/Wire/src/utility/twi.h
+++ b/libraries/Wire/src/utility/twi.h
@@ -51,6 +51,7 @@
   void twi_stop(void);
   void twi_releaseBus(void);
   void twi_setTimeoutInMillis(uint8_t);
+  void twi_handleTimeout(void);
 
 #endif
 

From 612742352ed093df1fb636792bccfe8442f4504f Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Mon, 16 Sep 2019 13:55:23 +0100
Subject: [PATCH 06/16] change timeout from milliseconds to microseconds

---
 libraries/Wire/src/Wire.cpp      |  4 ++--
 libraries/Wire/src/Wire.h        |  2 +-
 libraries/Wire/src/utility/twi.c | 32 ++++++++++++++++----------------
 libraries/Wire/src/utility/twi.h |  2 +-
 4 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index 5186373a7..04ffc5175 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -86,9 +86,9 @@ void TwoWire::setClock(uint32_t clock)
   twi_setFrequency(clock);
 }
 
-void TwoWire::setTimeoutInMillis(uint8_t timeout)
+void TwoWire::setTimeoutInMicros(uint16_t timeout)
 {
-  twi_setTimeoutInMillis(timeout);
+  twi_setTimeoutInMicros(timeout);
 }
 
 uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index b3ce61a3a..f944b349c 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -54,7 +54,7 @@ class TwoWire : public Stream
     void begin(int);
     void end();
     void setClock(uint32_t);
-    void setTimeoutInMillis(uint8_t);
+    void setTimeoutInMicros(uint16_t);
     void beginTransmission(uint8_t);
     void beginTransmission(int);
     uint8_t endTransmission(void);
diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index d625cf0dc..cccaa4be0 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -25,7 +25,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <compat/twi.h>
-#include "Arduino.h" // for digitalWrite and millis
+#include "Arduino.h" // for digitalWrite and micros
 
 #ifndef cbi
 #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
@@ -42,7 +42,7 @@ static volatile uint8_t twi_state;
 static volatile uint8_t twi_slarw;
 static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
 static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
-static volatile uint8_t twi_timeout_ms = 0;
+static volatile uint16_t twi_timeout_us = 0;
 
 static void (*twi_onSlaveTransmit)(void);
 static void (*twi_onSlaveReceive)(uint8_t*, int);
@@ -155,9 +155,9 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   }
 
   // wait until twi is ready, become master receiver
-  uint32_t startMillis = millis();
+  uint32_t startMicros = micros();
   while(TWI_READY != twi_state){
-    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 0;
     }
@@ -199,9 +199,9 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
     TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
 
   // wait for read operation to complete
-  startMillis = millis();
+  startMicros = micros();
   while(TWI_MRX == twi_state){
-    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 0;
     }
@@ -244,9 +244,9 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   }
 
   // wait until twi is ready, become master transmitter
-  uint32_t startMillis = millis();
+  uint32_t startMicros = micros();
   while(TWI_READY != twi_state){
-    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 4;
     }
@@ -291,9 +291,9 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs
 
   // wait for write operation to complete
-  startMillis = millis();
+  startMicros = micros();
   while(wait && (TWI_MTX == twi_state)){
-    if((twi_timeout_ms > 0) && (millis() - startMillis > twi_timeout_ms)) {
+    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 4;
     }
@@ -397,7 +397,7 @@ void twi_stop(void)
   uint32_t counter = 0;
   while(TWCR & _BV(TWSTO)){
     counter++;
-    if((twi_timeout_ms > 0) && (counter >= 25000)) {
+    if((twi_timeout_us > 0) && (counter >= 25000)) {
       twi_handleTimeout();
       return;
     }
@@ -425,19 +425,19 @@ void twi_releaseBus(void)
 }
 
 /* 
- * Function twi_setTimeoutInMillis
+ * Function twi_setTimeoutInMicros
  * Desc     set a global timeout for while loops that we might get stuck in
- * Input    timeout value in milliseconds
+ * Input    timeout value in microseconds
  * Output   none
  */
-void twi_setTimeoutInMillis(uint8_t timeout)
+void twi_setTimeoutInMicros(uint16_t timeout)
 {
-  twi_timeout_ms = timeout;
+  twi_timeout_us = timeout;
 }
 
 /* 
  * Function twi_handleTimeout
- * Desc     do this stuff when a while loop here has run for longer than twi_timeout_ms milliseconds
+ * Desc     do this stuff when a while loop here has run for longer than twi_timeout_us microseconds
  * Input    none
  * Output   none
  */
diff --git a/libraries/Wire/src/utility/twi.h b/libraries/Wire/src/utility/twi.h
index ba6ae5b7e..67189410c 100644
--- a/libraries/Wire/src/utility/twi.h
+++ b/libraries/Wire/src/utility/twi.h
@@ -50,7 +50,7 @@
   void twi_reply(uint8_t);
   void twi_stop(void);
   void twi_releaseBus(void);
-  void twi_setTimeoutInMillis(uint8_t);
+  void twi_setTimeoutInMicros(uint16_t);
   void twi_handleTimeout(void);
 
 #endif

From bc0e699d8df8af26b943ff06fade20aacd2c0d69 Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Mon, 16 Sep 2019 14:06:00 +0100
Subject: [PATCH 07/16] don't forget operating slave address or the bitrate
 when we reset because of a timeout

---
 libraries/Wire/src/utility/twi.c | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index cccaa4be0..a048b79d3 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -443,8 +443,19 @@ void twi_setTimeoutInMicros(uint16_t timeout)
  */
 void twi_handleTimeout(void)
 {
+  // remember the bitrate register value
+  uint8_t previous_TWBR = TWBR;
+
+  // remember the address register value
+  uint8_t previous_TWAR = TWBR;
+
+  // reset the interface
   twi_disable();
   twi_init();
+
+  // reapply the previous register values
+  TWAR = previous_TWAR;
+  TWBR = previous_TWAR;
 }
 
 ISR(TWI_vect)

From 3fc5fb88280789753eacc4e099ce1814f34c76d0 Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Tue, 21 Jan 2020 15:17:07 +0000
Subject: [PATCH 08/16] fix delay datatype uint16_t --> uint32_t

---
 libraries/Wire/src/Wire.cpp      |  2 +-
 libraries/Wire/src/Wire.h        |  2 +-
 libraries/Wire/src/utility/twi.c | 14 +++++++-------
 libraries/Wire/src/utility/twi.h |  2 +-
 4 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index 04ffc5175..ef557e3d2 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -86,7 +86,7 @@ void TwoWire::setClock(uint32_t clock)
   twi_setFrequency(clock);
 }
 
-void TwoWire::setTimeoutInMicros(uint16_t timeout)
+void TwoWire::setTimeoutInMicros(uint32_t timeout)
 {
   twi_setTimeoutInMicros(timeout);
 }
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index f944b349c..4a1b341b1 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -54,7 +54,7 @@ class TwoWire : public Stream
     void begin(int);
     void end();
     void setClock(uint32_t);
-    void setTimeoutInMicros(uint16_t);
+    void setTimeoutInMicros(uint32_t);
     void beginTransmission(uint8_t);
     void beginTransmission(int);
     uint8_t endTransmission(void);
diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index a048b79d3..fdb7b0c23 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -42,7 +42,7 @@ static volatile uint8_t twi_state;
 static volatile uint8_t twi_slarw;
 static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
 static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
-static volatile uint16_t twi_timeout_us = 0;
+static volatile uint32_t twi_timeout_us = 0ul;
 
 static void (*twi_onSlaveTransmit)(void);
 static void (*twi_onSlaveReceive)(uint8_t*, int);
@@ -157,7 +157,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   // wait until twi is ready, become master receiver
   uint32_t startMicros = micros();
   while(TWI_READY != twi_state){
-    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
+    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 0;
     }
@@ -201,7 +201,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   // wait for read operation to complete
   startMicros = micros();
   while(TWI_MRX == twi_state){
-    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
+    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 0;
     }
@@ -246,7 +246,7 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   // wait until twi is ready, become master transmitter
   uint32_t startMicros = micros();
   while(TWI_READY != twi_state){
-    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
+    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 4;
     }
@@ -293,7 +293,7 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   // wait for write operation to complete
   startMicros = micros();
   while(wait && (TWI_MTX == twi_state)){
-    if((twi_timeout_us > 0) && (micros() - startMicros > twi_timeout_us)) {
+    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return 4;
     }
@@ -397,7 +397,7 @@ void twi_stop(void)
   uint32_t counter = 0;
   while(TWCR & _BV(TWSTO)){
     counter++;
-    if((twi_timeout_us > 0) && (counter >= 25000)) {
+    if((twi_timeout_us > 0ul) && (counter >= 25000)) {
       twi_handleTimeout();
       return;
     }
@@ -430,7 +430,7 @@ void twi_releaseBus(void)
  * Input    timeout value in microseconds
  * Output   none
  */
-void twi_setTimeoutInMicros(uint16_t timeout)
+void twi_setTimeoutInMicros(uint32_t timeout)
 {
   twi_timeout_us = timeout;
 }
diff --git a/libraries/Wire/src/utility/twi.h b/libraries/Wire/src/utility/twi.h
index 67189410c..e9c23ee02 100644
--- a/libraries/Wire/src/utility/twi.h
+++ b/libraries/Wire/src/utility/twi.h
@@ -50,7 +50,7 @@
   void twi_reply(uint8_t);
   void twi_stop(void);
   void twi_releaseBus(void);
-  void twi_setTimeoutInMicros(uint16_t);
+  void twi_setTimeoutInMicros(uint32_t);
   void twi_handleTimeout(void);
 
 #endif

From 157688a844e6623bba8e604dd42f05f9e7e0603a Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Sat, 28 Mar 2020 16:08:08 +0000
Subject: [PATCH 09/16] Update libraries/Wire/src/utility/twi.c

fix mix up using TWBR instea of TWAR!

Co-Authored-By: Matthijs Kooijman <matthijs@stdin.nl>
---
 libraries/Wire/src/utility/twi.c | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index fdb7b0c23..702f78408 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -443,11 +443,9 @@ void twi_setTimeoutInMicros(uint32_t timeout)
  */
 void twi_handleTimeout(void)
 {
-  // remember the bitrate register value
+  // remember bitrate and address settings
   uint8_t previous_TWBR = TWBR;
-
-  // remember the address register value
-  uint8_t previous_TWAR = TWBR;
+  uint8_t previous_TWAR = TWAR;
 
   // reset the interface
   twi_disable();
@@ -622,4 +620,3 @@ ISR(TWI_vect)
       break;
   }
 }
-

From d8c03ffb2e958cc6d31bc9130fee6d34772551ce Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Sat, 28 Mar 2020 16:09:37 +0000
Subject: [PATCH 10/16] Update libraries/Wire/src/utility/twi.c

fix 2nd TWBR/TWAR mixup

Co-Authored-By: Matthijs Kooijman <matthijs@stdin.nl>
---
 libraries/Wire/src/utility/twi.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 702f78408..98391b716 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -453,7 +453,7 @@ void twi_handleTimeout(void)
 
   // reapply the previous register values
   TWAR = previous_TWAR;
-  TWBR = previous_TWAR;
+  TWBR = previous_TWBR;
 }
 
 ISR(TWI_vect)

From 68fe5f1dae1bb41183bb37eeda3fb453394a580c Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Sat, 28 Mar 2020 16:47:33 +0000
Subject: [PATCH 11/16] twi_stop() should use the same timeout as everywhere
 else

---
 libraries/Wire/src/utility/twi.c | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 98391b716..0190e0088 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -394,10 +394,9 @@ void twi_stop(void)
 
   // wait for stop condition to be exectued on bus
   // TWINT is not set after a stop condition!
-  uint32_t counter = 0;
+  unit32_t startMicros = micros();
   while(TWCR & _BV(TWSTO)){
-    counter++;
-    if((twi_timeout_us > 0ul) && (counter >= 25000)) {
+    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
       twi_handleTimeout();
       return;
     }

From 36821cf46d452df6e915ea7ca2f5677712460335 Mon Sep 17 00:00:00 2001
From: Grey Christoforo <grey@christoforo.net>
Date: Sat, 28 Mar 2020 17:50:16 +0000
Subject: [PATCH 12/16] all while loops are now protected by timeouts

---
 libraries/Wire/src/utility/twi.c | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index 0190e0088..d8f2646d1 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -189,8 +189,13 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
     // up. Also, don't enable the START interrupt. There may be one pending from the 
     // repeated start that we sent ourselves, and that would really confuse things.
     twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
+    startMicros = micros();
     do {
       TWDR = twi_slarw;
+      if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
+        twi_handleTimeout();
+        return 0;
+      }
     } while(TWCR & _BV(TWWC));
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
   }
@@ -281,8 +286,13 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
     // up. Also, don't enable the START interrupt. There may be one pending from the 
     // repeated start that we sent outselves, and that would really confuse things.
     twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
+    startMicros = micros();
     do {
-      TWDR = twi_slarw;				
+      TWDR = twi_slarw;
+      if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
+        twi_handleTimeout();
+        return 4;
+      }
     } while(TWCR & _BV(TWWC));
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
   }

From a9929dc362539b2349328917b732a91471591181 Mon Sep 17 00:00:00 2001
From: Greyson Christoforo <grey@christoforo.net>
Date: Tue, 31 Mar 2020 11:55:33 +0100
Subject: [PATCH 13/16] Revert "twi_stop() should use the same timeout as
 everywhere else"

This reverts commit 68fe5f1dae1bb41183bb37eeda3fb453394a580c.
---
 libraries/Wire/src/utility/twi.c | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index d8f2646d1..d45a5f801 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -404,9 +404,10 @@ void twi_stop(void)
 
   // wait for stop condition to be exectued on bus
   // TWINT is not set after a stop condition!
-  unit32_t startMicros = micros();
+  uint32_t counter = 0;
   while(TWCR & _BV(TWSTO)){
-    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
+    counter++;
+    if((twi_timeout_us > 0ul) && (counter >= 25000)) {
       twi_handleTimeout();
       return;
     }

From 849c97c7b366095da2e923229bd22cf2f4527b1f Mon Sep 17 00:00:00 2001
From: Greyson Christoforo <grey@christoforo.net>
Date: Tue, 31 Mar 2020 12:00:53 +0100
Subject: [PATCH 14/16] make timeout counter volatile

---
 libraries/Wire/src/utility/twi.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index d45a5f801..c120998e8 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -404,7 +404,7 @@ void twi_stop(void)
 
   // wait for stop condition to be exectued on bus
   // TWINT is not set after a stop condition!
-  uint32_t counter = 0;
+  volatile uint32_t counter = 0;
   while(TWCR & _BV(TWSTO)){
     counter++;
     if((twi_timeout_us > 0ul) && (counter >= 25000)) {

From c45ac98e5f3ec8e7a038f1224646d3218761daa3 Mon Sep 17 00:00:00 2001
From: Greyson Christoforo <grey@christoforo.net>
Date: Tue, 31 Mar 2020 12:52:49 +0100
Subject: [PATCH 15/16] rename timeout function for improved clarity

---
 libraries/Wire/src/Wire.cpp | 2 +-
 libraries/Wire/src/Wire.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index ef557e3d2..7f09ed256 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -86,7 +86,7 @@ void TwoWire::setClock(uint32_t clock)
   twi_setFrequency(clock);
 }
 
-void TwoWire::setTimeoutInMicros(uint32_t timeout)
+void TwoWire::setWireTimeoutUs(uint32_t timeout)
 {
   twi_setTimeoutInMicros(timeout);
 }
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index 4a1b341b1..fca1beab2 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -54,7 +54,7 @@ class TwoWire : public Stream
     void begin(int);
     void end();
     void setClock(uint32_t);
-    void setTimeoutInMicros(uint32_t);
+    void setWireTimeoutUs(uint32_t);
     void beginTransmission(uint8_t);
     void beginTransmission(int);
     uint8_t endTransmission(void);

From da7d85aefde4ab6c6e1797b543be0ce57045a6ee Mon Sep 17 00:00:00 2001
From: Greyson Christoforo <grey@christoforo.net>
Date: Sat, 16 May 2020 13:56:54 +0100
Subject: [PATCH 16/16] - resetting the twi interface on timeouts is now
 optional - timeouts in the ISR are no longer hardcoded and now obey the set
 timeout value - a user-readable flag is now set whenever a timeout occurs   -
 the user can clear this flag whenever they like

---
 libraries/Wire/src/Wire.cpp      |  29 +++++-
 libraries/Wire/src/Wire.h        |   7 +-
 libraries/Wire/src/utility/twi.c | 172 ++++++++++++++++++-------------
 libraries/Wire/src/utility/twi.h |   8 +-
 4 files changed, 138 insertions(+), 78 deletions(-)

diff --git a/libraries/Wire/src/Wire.cpp b/libraries/Wire/src/Wire.cpp
index 7f09ed256..b446f9afe 100644
--- a/libraries/Wire/src/Wire.cpp
+++ b/libraries/Wire/src/Wire.cpp
@@ -18,6 +18,7 @@
  
   Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
   Modified 2017 by Chuck Todd (ctodd@cableone.net) to correct Unconfigured Slave Mode reboot
+  Modified 2020 by Greyson Christoforo (grey@christoforo.net) to implement timeouts
 */
 
 extern "C" {
@@ -86,9 +87,31 @@ void TwoWire::setClock(uint32_t clock)
   twi_setFrequency(clock);
 }
 
-void TwoWire::setWireTimeoutUs(uint32_t timeout)
-{
-  twi_setTimeoutInMicros(timeout);
+/***
+ * Sets the TWI timeout.
+ *
+ * @param timeout a timeout value in microseconds, if zero then timeout checking is disabled
+ * @param reset_with_timeout if true then TWI interface will be automatically reset on timeout
+ *                           if false then TWI interface will not be reset on timeout
+ */
+void TwoWire::setWireTimeoutUs(uint32_t timeout, bool reset_with_timeout){
+  twi_setTimeoutInMicros(timeout, reset_with_timeout);
+}
+
+/***
+ * Returns the TWI timeout flag.
+ *
+ * @return true if timeout has occured
+ */
+bool TwoWire::getWireTimeoutFlag(void){
+  return(twi_manageTimeoutFlag(false));
+}
+
+/***
+ * Clears the TWI timeout flag.
+ */
+void TwoWire::clearWireTimeoutFlag(void){
+  twi_manageTimeoutFlag(true);
 }
 
 uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
diff --git a/libraries/Wire/src/Wire.h b/libraries/Wire/src/Wire.h
index fca1beab2..c8b9a3e0f 100644
--- a/libraries/Wire/src/Wire.h
+++ b/libraries/Wire/src/Wire.h
@@ -17,6 +17,7 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
   Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+  Modified 2020 by Greyson Christoforo (grey@christoforo.net) to implement timeouts
 */
 
 #ifndef TwoWire_h
@@ -54,14 +55,16 @@ class TwoWire : public Stream
     void begin(int);
     void end();
     void setClock(uint32_t);
-    void setWireTimeoutUs(uint32_t);
+    void setWireTimeoutUs(uint32_t, bool);
+    bool getWireTimeoutFlag(void);
+    void clearWireTimeoutFlag(void);
     void beginTransmission(uint8_t);
     void beginTransmission(int);
     uint8_t endTransmission(void);
     uint8_t endTransmission(uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
-	uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t);
+    uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t);
     uint8_t requestFrom(int, int);
     uint8_t requestFrom(int, int, int);
     virtual size_t write(uint8_t);
diff --git a/libraries/Wire/src/utility/twi.c b/libraries/Wire/src/utility/twi.c
index c120998e8..b3096c6ca 100644
--- a/libraries/Wire/src/utility/twi.c
+++ b/libraries/Wire/src/utility/twi.c
@@ -17,6 +17,7 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
   Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+  Modified 2020 by Greyson Christoforo (grey@christoforo.net) to implement timeouts
 */
 
 #include <math.h>
@@ -24,6 +25,7 @@
 #include <inttypes.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include <util/delay.h>
 #include <compat/twi.h>
 #include "Arduino.h" // for digitalWrite and micros
 
@@ -42,7 +44,16 @@ static volatile uint8_t twi_state;
 static volatile uint8_t twi_slarw;
 static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
 static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
+
+// twi_timeout_us > 0 prevents the code from getting stuck in various while loops here
+// if twi_timeout_us == 0 then timeout checking is disabled (the previous Wire lib behavior)
+// at some point in the future, the default twi_timeout_us value could become 25000
+// and twi_do_reset_on_timeout could become true
+// to conform to the SMBus standard
+// http://smbus.org/specs/SMBus_3_1_20180319.pdf
 static volatile uint32_t twi_timeout_us = 0ul;
+static volatile bool twi_timed_out_flag = false;  // a timeout has been seen
+static volatile bool twi_do_reset_on_timeout = false;  // reset the TWI registers on timeout
 
 static void (*twi_onSlaveTransmit)(void);
 static void (*twi_onSlaveReceive)(uint8_t*, int);
@@ -157,11 +168,10 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
   // wait until twi is ready, become master receiver
   uint32_t startMicros = micros();
   while(TWI_READY != twi_state){
-    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
-      twi_handleTimeout();
+    if((twi_timeout_us > 0ul) && ((micros() - startMicros) > twi_timeout_us)) {
+      twi_handleTimeout(twi_do_reset_on_timeout);
       return 0;
     }
-    continue;
   }
   twi_state = TWI_MRX;
   twi_sendStop = sendStop;
@@ -192,35 +202,35 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
     startMicros = micros();
     do {
       TWDR = twi_slarw;
-      if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
-        twi_handleTimeout();
+      if((twi_timeout_us > 0ul) && ((micros() - startMicros) > twi_timeout_us)) {
+        twi_handleTimeout(twi_do_reset_on_timeout);
         return 0;
       }
     } while(TWCR & _BV(TWWC));
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
-  }
-  else
+  } else {
     // send start condition
     TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
+  }
 
   // wait for read operation to complete
   startMicros = micros();
   while(TWI_MRX == twi_state){
-    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
-      twi_handleTimeout();
+    if((twi_timeout_us > 0ul) && ((micros() - startMicros) > twi_timeout_us)) {
+      twi_handleTimeout(twi_do_reset_on_timeout);
       return 0;
     }
-    continue;
   }
 
-  if (twi_masterBufferIndex < length)
+  if (twi_masterBufferIndex < length) {
     length = twi_masterBufferIndex;
+  }
 
   // copy twi buffer to data
   for(i = 0; i < length; ++i){
     data[i] = twi_masterBuffer[i];
   }
-	
+
   return length;
 }
 
@@ -238,6 +248,7 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sen
  *          2 .. address send, NACK received
  *          3 .. data send, NACK received
  *          4 .. other twi error (lost bus arbitration, bus error, ..)
+ *          5 .. timeout
  */
 uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop)
 {
@@ -251,11 +262,10 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   // wait until twi is ready, become master transmitter
   uint32_t startMicros = micros();
   while(TWI_READY != twi_state){
-    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
-      twi_handleTimeout();
-      return 4;
+    if((twi_timeout_us > 0ul) && ((micros() - startMicros) > twi_timeout_us)) {
+      twi_handleTimeout(twi_do_reset_on_timeout);
+      return (5);
     }
-    continue;
   }
   twi_state = TWI_MTX;
   twi_sendStop = sendStop;
@@ -289,25 +299,24 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
     startMicros = micros();
     do {
       TWDR = twi_slarw;
-      if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
-        twi_handleTimeout();
-        return 4;
+      if((twi_timeout_us > 0ul) && ((micros() - startMicros) > twi_timeout_us)) {
+        twi_handleTimeout(twi_do_reset_on_timeout);
+        return (5);
       }
     } while(TWCR & _BV(TWWC));
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
-  }
-  else
+  } else {
     // send start condition
     TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs
+  }
 
   // wait for write operation to complete
   startMicros = micros();
   while(wait && (TWI_MTX == twi_state)){
-    if((twi_timeout_us > 0ul) && (micros() - startMicros > twi_timeout_us)) {
-      twi_handleTimeout();
-      return 4;
+    if((twi_timeout_us > 0ul) && ((micros() - startMicros) > twi_timeout_us)) {
+      twi_handleTimeout(twi_do_reset_on_timeout);
+      return (5);
     }
-    continue;
   }
   
   if (twi_error == 0xFF)
@@ -387,7 +396,7 @@ void twi_reply(uint8_t ack)
   if(ack){
     TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
   }else{
-	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
   }
 }
 
@@ -404,15 +413,17 @@ void twi_stop(void)
 
   // wait for stop condition to be exectued on bus
   // TWINT is not set after a stop condition!
-  volatile uint32_t counter = 0;
+  volatile uint32_t counter = twi_timeout_us/10ul; // approximate the timeout
   while(TWCR & _BV(TWSTO)){
-    counter++;
-    if((twi_timeout_us > 0ul) && (counter >= 25000)) {
-      twi_handleTimeout();
-      return;
+    if(twi_timeout_us > 0ul){
+      if (counter > 0ul){
+        _delay_us(10);
+        counter--;
+      } else {
+        twi_handleTimeout(twi_do_reset_on_timeout);
+        return;
+      }
     }
-
-    continue;
   }
 
   // update twi state
@@ -436,34 +447,55 @@ void twi_releaseBus(void)
 
 /* 
  * Function twi_setTimeoutInMicros
- * Desc     set a global timeout for while loops that we might get stuck in
- * Input    timeout value in microseconds
+ * Desc     set a timeout for while loops that twi might get stuck in
+ * Input    timeout value in microseconds (0 means never time out)
+ * Input    reset_with_timeout: true causes timeout events to reset twi
  * Output   none
  */
-void twi_setTimeoutInMicros(uint32_t timeout)
-{
+void twi_setTimeoutInMicros(uint32_t timeout, bool reset_with_timeout){
+  twi_timed_out_flag = false;
   twi_timeout_us = timeout;
+  twi_do_reset_on_timeout = reset_with_timeout;
 }
 
 /* 
  * Function twi_handleTimeout
- * Desc     do this stuff when a while loop here has run for longer than twi_timeout_us microseconds
- * Input    none
+ * Desc     this gets called whenever a while loop here has lasted longer than
+ *          twi_timeout_us microseconds. always sets twi_timed_out_flag
+ * Input    reset: true causes this function to reset the twi hardware interface
  * Output   none
  */
-void twi_handleTimeout(void)
-{
-  // remember bitrate and address settings
-  uint8_t previous_TWBR = TWBR;
-  uint8_t previous_TWAR = TWAR;
+void twi_handleTimeout(bool reset){
+  twi_timed_out_flag = true;
+
+  if (reset) {
+    // remember bitrate and address settings
+    uint8_t previous_TWBR = TWBR;
+    uint8_t previous_TWAR = TWAR;
+
+    // reset the interface
+    twi_disable();
+    twi_init();
 
-  // reset the interface
-  twi_disable();
-  twi_init();
+    // reapply the previous register values
+    TWAR = previous_TWAR;
+    TWBR = previous_TWBR;
+  }
+}
 
-  // reapply the previous register values
-  TWAR = previous_TWAR;
-  TWBR = previous_TWBR;
+/*
+ * Function twi_manageTimeoutFlag
+ * Desc     returns true if twi has seen a timeout
+ *          optionally clears the timeout flag
+ * Input    clear_flag: true if we should reset the hardware
+ * Output   none
+ */
+bool twi_manageTimeoutFlag(bool clear_flag){
+  bool flag = twi_timed_out_flag;
+  if (clear_flag){
+    twi_timed_out_flag = false;
+  }
+  return(flag);
 }
 
 ISR(TWI_vect)
@@ -486,16 +518,16 @@ ISR(TWI_vect)
         TWDR = twi_masterBuffer[twi_masterBufferIndex++];
         twi_reply(1);
       }else{
-	if (twi_sendStop)
+        if (twi_sendStop){
           twi_stop();
-	else {
-	  twi_inRepStart = true;	// we're gonna send the START
-	  // don't enable the interrupt. We'll generate the start, but we 
-	  // avoid handling the interrupt until we're in the next transaction,
-	  // at the point where we would normally issue the start.
-	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
-	  twi_state = TWI_READY;
-	}
+       } else {
+         twi_inRepStart = true;	// we're gonna send the START
+         // don't enable the interrupt. We'll generate the start, but we
+         // avoid handling the interrupt until we're in the next transaction,
+         // at the point where we would normally issue the start.
+         TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
+         twi_state = TWI_READY;
+        }
       }
       break;
     case TW_MT_SLA_NACK:  // address sent, nack received
@@ -527,17 +559,17 @@ ISR(TWI_vect)
     case TW_MR_DATA_NACK: // data received, nack sent
       // put final byte into buffer
       twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
-	if (twi_sendStop)
-          twi_stop();
-	else {
-	  twi_inRepStart = true;	// we're gonna send the START
-	  // don't enable the interrupt. We'll generate the start, but we 
-	  // avoid handling the interrupt until we're in the next transaction,
-	  // at the point where we would normally issue the start.
-	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
-	  twi_state = TWI_READY;
-	}    
-	break;
+      if (twi_sendStop){
+        twi_stop();
+      } else {
+        twi_inRepStart = true;	// we're gonna send the START
+        // don't enable the interrupt. We'll generate the start, but we
+        // avoid handling the interrupt until we're in the next transaction,
+        // at the point where we would normally issue the start.
+        TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
+        twi_state = TWI_READY;
+      }
+      break;
     case TW_MR_SLA_NACK: // address sent, nack received
       twi_stop();
       break;
diff --git a/libraries/Wire/src/utility/twi.h b/libraries/Wire/src/utility/twi.h
index e9c23ee02..85b983794 100644
--- a/libraries/Wire/src/utility/twi.h
+++ b/libraries/Wire/src/utility/twi.h
@@ -15,6 +15,8 @@
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+  Modified 2020 by Greyson Christoforo (grey@christoforo.net) to implement timeouts
 */
 
 #ifndef twi_h
@@ -50,8 +52,8 @@
   void twi_reply(uint8_t);
   void twi_stop(void);
   void twi_releaseBus(void);
-  void twi_setTimeoutInMicros(uint32_t);
-  void twi_handleTimeout(void);
+  void twi_setTimeoutInMicros(uint32_t, bool);
+  void twi_handleTimeout(bool);
+  bool twi_manageTimeoutFlag(bool);
 
 #endif
-