diff --git a/I2CManager.cpp b/I2CManager.cpp
index d0d8550..1d1387e 100644
--- a/I2CManager.cpp
+++ b/I2CManager.cpp
@@ -92,7 +92,7 @@ void I2CManagerClass::begin(void) {
     // Probe and list devices.  Use standard mode 
     //  (clock speed 100kHz) for best device compatibility.
     _setClock(100000);
-    unsigned long originalTimeout = _timeout;
+    uint32_t originalTimeout = _timeout;
     setTimeout(1000);       // use 1ms timeout for probes
 
   #if defined(I2C_EXTENDED_ADDRESS)
diff --git a/I2CManager.h b/I2CManager.h
index ede30cc..08d81d4 100644
--- a/I2CManager.h
+++ b/I2CManager.h
@@ -485,7 +485,7 @@ private:
   // When retries are enabled, the timeout applies to each
   // try, and failure from timeout does not get retried.
   // A value of 0 means disable timeout monitoring.
-  unsigned long _timeout = 100000UL;
+  uint32_t _timeout = 100000UL;
     
   // Finish off request block by waiting for completion and posting status.
   uint8_t finishRB(I2CRB *rb, uint8_t status);
@@ -532,7 +532,7 @@ private:
     uint8_t bytesToSend = 0;
     uint8_t bytesToReceive = 0;
     uint8_t operation = 0;
-    unsigned long startTime = 0;
+    uint32_t startTime = 0;
     uint8_t muxPhase = 0;
     uint8_t muxAddress = 0;
     uint8_t muxData[1];
diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h
index fb5bae5..59bbcaf 100644
--- a/I2CManager_NonBlocking.h
+++ b/I2CManager_NonBlocking.h
@@ -172,6 +172,10 @@ void I2CManagerClass::startTransaction() {
  *  Function to queue a request block and initiate operations.
  ***************************************************************************/
 void I2CManagerClass::queueRequest(I2CRB *req) {
+
+  if (((req->operation & OPERATION_MASK) == OPERATION_READ) && req->readLen == 0)
+    return;  // Ignore null read
+
   req->status = I2C_STATUS_PENDING;
   req->nextRequest = NULL;
   ATOMIC_BLOCK() {
@@ -184,6 +188,7 @@ void I2CManagerClass::queueRequest(I2CRB *req) {
 
 }
 
+
 /***************************************************************************
  *  Initiate a write to an I2C device (non-blocking operation)
  ***************************************************************************/
@@ -240,8 +245,8 @@ void I2CManagerClass::checkForTimeout() {
     I2CRB *t = queueHead;
     if (state==I2C_STATE_ACTIVE && t!=0 && t==currentRequest && _timeout > 0) {
       // Check for timeout
-      unsigned long elapsed = micros() - startTime;
-      if (elapsed > _timeout) { 
+      int32_t elapsed = micros() - startTime;
+      if (elapsed > (int32_t)_timeout) { 
 #ifdef DIAG_IO
         //DIAG(F("I2CManager Timeout on %s"), t->i2cAddress.toString());
 #endif
@@ -300,12 +305,12 @@ void I2CManagerClass::handleInterrupt() {
 
   // 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_COMPLETED && currentRequest != NULL) {
+  if (state == I2C_STATE_COMPLETED && currentRequest != NULL && currentRequest == queueHead) {
     // Operation has completed.
     if (completionStatus == I2C_STATUS_OK || ++retryCounter > MAX_I2C_RETRIES
       || currentRequest->operation & OPERATION_NORETRY) 
     {
-      // Status is OK, or has failed and retry count exceeded, or retries disabled.
+      // Status is OK, or has failed and retry count exceeded, or failed and retries disabled.
 #if defined(I2C_EXTENDED_ADDRESS)
       if (muxPhase == MuxPhase_PROLOG ) {
         overallStatus = completionStatus;
diff --git a/I2CManager_STM32.h b/I2CManager_STM32.h
index 5132640..eac331a 100644
--- a/I2CManager_STM32.h
+++ b/I2CManager_STM32.h
@@ -232,6 +232,7 @@ void I2CManagerClass::I2C_sendStart() {
   // multi-master bus, the bus may be BUSY under control of another master,
   // in which case we can avoid some arbitration failures by waiting until
   // the bus state is IDLE.  We don't do that here.
+  //while (s->SR2 & I2C_SR2_BUSY) {}
 
   // Check there's no STOP still in progress.  If we OR the START bit into CR1
   // and the STOP bit is already set, we could output multiple STOP conditions.
@@ -247,6 +248,7 @@ void I2CManagerClass::I2C_sendStart() {
  ***************************************************************************/
 void I2CManagerClass::I2C_sendStop() {
   s->CR1 |= I2C_CR1_STOP; // Stop I2C
+  //while (s->CR1 & I2C_CR1_STOP) {}  // Wait for STOP bit to reset
 }
 
 /***************************************************************************
@@ -273,6 +275,9 @@ void I2CManagerClass::I2C_close() {
 void I2CManagerClass::I2C_handleInterrupt() {
   volatile uint16_t temp_sr1, temp_sr2;
 
+  pinMode(D2, OUTPUT);
+  digitalWrite(D2, 1);
+
   temp_sr1 = s->SR1;
 
   // Check for errors first
@@ -302,7 +307,8 @@ void I2CManagerClass::I2C_handleInterrupt() {
       completionStatus = I2C_STATUS_BUS_ERROR;
       state = I2C_STATE_COMPLETED;
     }
-  } else {
+  } 
+  else {
     // No error flags, so process event according to current state.
     switch (transactionState) {
       case TS_START:
@@ -324,6 +330,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
         break;
 
       case TS_W_ADDR:
+        temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
         if (temp_sr1 & I2C_SR1_ADDR) {
           // Event EV6
           // Address sent successfully, device has ack'd in response.
@@ -333,10 +340,25 @@ void I2CManagerClass::I2C_handleInterrupt() {
             completionStatus = I2C_STATUS_OK;
             state = I2C_STATE_COMPLETED;
           } else {
-            transactionState = TS_W_DATA;
+            if (bytesToSend <= 2) {
+              // After this interrupt, we will have no more data to send. 
+              // Next event of interest will be the BTF interrupt, so disable TXE interrupt
+              s->CR2 &= ~I2C_CR2_ITBUFEN;
+              transactionState = TS_W_STOP;
+            } else {
+              // More data to send, enable TXE interrupt.
+              s->CR2 |= I2C_CR2_ITBUFEN;
+              transactionState = TS_W_DATA;
+            }
+            // Put one or two bytes into DR to avoid interrupts
+            s->DR = sendBuffer[txCount++];
+            bytesToSend--;
+            if (bytesToSend) {
+              s->DR = sendBuffer[txCount++];
+              bytesToSend--;
+            }
           }
         }
-        temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
         break;
 
       case TS_W_DATA:
@@ -344,21 +366,24 @@ void I2CManagerClass::I2C_handleInterrupt() {
           // Event EV8_1/EV8/EV8_2
           // Transmitter empty, write a byte to it.
           if (bytesToSend) {
+            if (bytesToSend == 1) {
+              // We will next need to wait for BTF.
+              // TXE becomes set one byte before BTF is set, so disable 
+              // TXE interrupt while we're waiting for BTF, to suppress 
+              // repeated interrupts during that period.
+              s->CR2 &= ~I2C_CR2_ITBUFEN;
+              transactionState = TS_W_STOP;
+            }
             s->DR = sendBuffer[txCount++];
             bytesToSend--;
           }
-          // See if we're finished sending
-          if (!bytesToSend) {
-            // Wait for last byte to be sent.
-            transactionState = TS_W_STOP;
-          }
         }
         break;
 
       case TS_W_STOP:
         if ((temp_sr1 & I2C_SR1_BTF) && (temp_sr1 & I2C_SR1_TXE)) {
           // Event EV8_2
-          // Write finished.
+          // All writes finished.
           if (bytesToReceive) {
               // Start a read operation by sending (re)start
               I2C_sendStart();
@@ -383,17 +408,22 @@ void I2CManagerClass::I2C_handleInterrupt() {
             // Receive 1 byte
             s->CR1 &= ~I2C_CR1_ACK;  // Disable ack
             temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
+            // Next step will occur after a RXNE interrupt, so enable it
+            s->CR2 |= I2C_CR2_ITBUFEN;
             transactionState = TS_R_STOP;
-            // Next step will occur after a BTF interrupt
           } else if (bytesToReceive == 2) {
             // Receive 2 bytes
             s->CR1 &= ~I2C_CR1_ACK;  // Disable ACK for final byte
             s->CR1 |= I2C_CR1_POS;  // set POS flag to delay effect of ACK flag
+            // Next step will occur after a BTF interrupt, so disable RXNE interrupt
+            s->CR2 &= ~I2C_CR2_ITBUFEN;
             temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
             transactionState = TS_R_STOP;
           } else {
             // >2 bytes, just wait for bytes to come in and ack them for the time being
             // (ack flag has already been set).
+            // Next step will occur after a BTF interrupt, so disable RXNE interrupt
+            s->CR2 &= ~I2C_CR2_ITBUFEN;
             temp_sr2 = s->SR2; // read SR2 to complete clearing the ADDR bit
             transactionState = TS_R_DATA;
           }
@@ -448,6 +478,8 @@ void I2CManagerClass::I2C_handleInterrupt() {
         break;
     }
   }
+  delayMicroseconds(1);
+  digitalWrite(D2, 0);
 }
 
 #endif /* I2CMANAGER_STM32_H */