esp8266
diff --git a/‎cores/esp8266/core_esp8266_si2c.cpp
Lines changed: 38 additions & 14 deletions b/‎cores/esp8266/core_esp8266_si2c.cpp
Lines changed: 38 additions & 14 deletions
@@ -113,6 +113,12 @@ class Twi
         return (GPI & (1 << twi_scl)) != 0;
     }
+    // ICACHE_RAM_ATTR used to have a constant delay.
+    void ICACHE_RAM_ATTR twi_wait_clockStretchLimit(void);
+
+    // Generate a clock "valley" (at the end of a segment, just before a repeated start)
+    void twi_scl_valley(void);
+
 public:
     void setClock(unsigned int freq);
     void setClockStretchLimit(uint32_t limit);
@@ -268,12 +274,11 @@ bool Twi::write_start(void)
 
 bool Twi::write_stop(void)
 {
-    uint32_t i = 0;
     SCL_LOW();
     SDA_LOW();
     delay(twi_dcount);
     SCL_HIGH();
-    while (SCL_READ() == 0 && (i++) < twi_clockStretchLimit); // Clock stretching
+    twi_wait_clockStretchLimit();
     delay(twi_dcount);
     SDA_HIGH();
     delay(twi_dcount);
@@ -282,7 +287,6 @@ bool Twi::write_stop(void)
 
 bool Twi::write_bit(bool bit)
 {
-    uint32_t i = 0;
     SCL_LOW();
     if (bit)
     {
@@ -294,19 +298,18 @@ bool Twi::write_bit(bool bit)
     }
     delay(twi_dcount + 1);
     SCL_HIGH();
-    while (SCL_READ() == 0 && (i++) < twi_clockStretchLimit);// Clock stretching
+    twi_wait_clockStretchLimit();
     delay(twi_dcount);
     return true;
 }
 
 bool Twi::read_bit(void)
 {
-    uint32_t i = 0;
     SCL_LOW();
     SDA_HIGH();
     delay(twi_dcount + 2);
-    while (SCL_READ() == 0 && (i++) < twi_clockStretchLimit);// Clock stretching
+    twi_wait_clockStretchLimit();
     bool bit = SDA_READ();
     delay(twi_dcount);
     return bit;
@@ -364,14 +367,18 @@ unsigned char Twi::writeTo(unsigned char address, unsigned char * buf, unsigned
     if (sendStop)
     {
         write_stop();
+    } 
+    else
+    {
+        twi_scl_valley();
+        twi_wait_clockStretchLimit();
+        // TD-er: Also twi_delay here?
+        // delay(twi_dcount);
     }
     i = 0;
     while (SDA_READ() == 0 && (i++) < 10)
     {
-        SCL_LOW();
-        delay(twi_dcount);
-        SCL_HIGH();
-        unsigned int t = 0; while (SCL_READ() == 0 && (t++) < twi_clockStretchLimit); // twi_clockStretchLimit
+        twi_scl_valley();
         delay(twi_dcount);
     }
     return 0;
@@ -401,13 +408,17 @@ unsigned char Twi::readFrom(unsigned char address, unsigned char* buf, unsigned
     {
         write_stop();
     }
+    else
+    {
+        twi_scl_valley();
+        twi_wait_clockStretchLimit();
+        // TD-er: Also twi_delay here?
+        // delay(twi_dcount);
+    }
     i = 0;
     while (SDA_READ() == 0 && (i++) < 10)
     {
-        SCL_LOW();
-        delay(twi_dcount);
-        SCL_HIGH();
-        unsigned int t = 0; while (SCL_READ() == 0 && (t++) < twi_clockStretchLimit); // twi_clockStretchLimit
+        twi_scl_valley();
         delay(twi_dcount);
     }
     return 0;
@@ -628,6 +639,19 @@ void ICACHE_RAM_ATTR Twi::onTwipEvent(uint8_t status)
     }
 }
 
+void ICACHE_RAM_ATTR Twi::twi_wait_clockStretchLimit(void)
+{
+    uint32_t t=0; 
+    while(SCL_READ() == 0 && (t++) < twi_clockStretchLimit); // Clock stretching
+}
+
+void Twi::twi_scl_valley(void)
+{
+    SCL_LOW();
+    delay(twi_dcount);
+    SCL_HIGH();
+}
+
 void ICACHE_RAM_ATTR Twi::onTimer(void *unused)
 {
     (void)unused;