diff --git a/I2CManager.cpp b/I2CManager.cpp
index b00fc8a..00aa71b 100644
--- a/I2CManager.cpp
+++ b/I2CManager.cpp
@@ -41,6 +41,28 @@
 #endif
 
 
+// Helper function for listing device types
+static const FSH * guessI2CDeviceType(uint8_t address) {
+  if (address >= 0x20 && address <= 0x26)
+    return F("GPIO Expander");
+  else if (address == 0x27)
+    return F("GPIO Expander or LCD Display");
+  else if (address == 0x29)
+    return F("Time-of-flight sensor");
+  else if (address >= 0x3c && address <= 0x3c)
+    return F("OLED Display");
+  else if (address >= 0x48 && address <= 0x4f)
+    return F("Analogue Inputs or PWM");
+  else if (address >= 0x40 && address <= 0x4f)
+    return F("PWM");
+  else if (address >= 0x50 && address <= 0x5f) 
+    return F("EEPROM"); 
+  else if (address >= 0x70 && address <= 0x77)
+    return F("I2C Mux");
+  else
+    return F("?");
+}
+
 // If not already initialised, initialise I2C
 void I2CManagerClass::begin(void) {
   if (!_beginCompleted) {
@@ -60,34 +82,46 @@ void I2CManagerClass::begin(void) {
     setTimeout(1000);       // use 1ms timeout for probes
 
   #if defined(I2C_EXTENDED_ADDRESS)
-    // First switch off all multiplexer subbuses.
+    // First count the multiplexers and switch off all subbuses
+    _muxCount = 0;
     for (uint8_t muxNo=I2CMux_0; muxNo <= I2CMux_7; muxNo++) {
-      I2CManager.muxSelectSubBus({(I2CMux)muxNo, SubBus_None});  // Deselect Mux
+      if (I2CManager.muxSelectSubBus({(I2CMux)muxNo, SubBus_None})==I2C_STATUS_OK)
+        _muxCount++;
     }
   #endif
 
+    // Enumerate devices that are visible
     bool found = false;
     for (uint8_t addr=0x08; addr<0x78; addr++) {
       if (exists(addr)) {
         found = true; 
-        DIAG(F("I2C Device found at x%x"), addr);
+        DIAG(F("I2C Device found at x%x, %S?"), addr, guessI2CDeviceType(addr));
       }
     }
 
 #if defined(I2C_EXTENDED_ADDRESS)
     // Enumerate all I2C devices that are connected via multiplexer, 
-    // i.e. respond when only one multiplexer has one subBus enabled
+    // i.e. that respond when only one multiplexer has one subBus enabled
     // and the device doesn't respond when the mux subBus is disabled.
     for (uint8_t muxNo=I2CMux_0; muxNo <= I2CMux_7; muxNo++) {
       uint8_t muxAddr = I2C_MUX_BASE_ADDRESS + muxNo;
       if (exists(muxAddr)) {
+        // Select Mux Subbus
         for (uint8_t subBus=0; subBus<=7; subBus++) {
+          muxSelectSubBus({(I2CMux)muxNo, (I2CSubBus)subBus});
           for (uint8_t addr=0x08; addr<0x78; addr++) {
-            if (exists({(I2CMux)muxNo, (I2CSubBus)subBus, addr})
-                && !exists({(I2CMux)muxNo, SubBus_None, addr})) {
-              found = true; 
-              DIAG(F("I2C Device found at {I2CMux_%d,SubBus_%d,x%x}"), 
-                muxNo, subBus, addr);
+            if (exists(addr)) {
+              // De-select subbus
+              muxSelectSubBus({(I2CMux)muxNo, SubBus_None});
+              if (!exists(addr)) {
+                // Device responds when subbus selected but not when
+                // subbus disabled - ergo it must be on subbus!
+                found = true; 
+                DIAG(F("I2C Device found at {I2CMux_%d,SubBus_%d,x%x}, %S?"), 
+                  muxNo, subBus, addr, guessI2CDeviceType(addr));
+              }
+              // Re-select subbus
+              muxSelectSubBus({(I2CMux)muxNo, (I2CSubBus)subBus});
             }
           }
         }
@@ -232,6 +266,14 @@ I2CManagerClass I2CManager = I2CManagerClass();
 // try, and failure from timeout does not get retried.
 unsigned long I2CManagerClass::timeout = 100000UL;
 
+#if defined(I2C_EXTENDED_ADDRESS)
+// Count of I2C multiplexers found when initialising.  If there is only one
+// MUX then the subbus does not de-selecting after use; however, if there
+// is two or more, then the subbus must be deselected to avoid multiple
+// sub-bus legs on different multiplexers being accessible simultaneously.
+uint8_t I2CManagerClass::_muxCount = 0;
+#endif
+
 
 /////////////////////////////////////////////////////////////////////////////
 // Helper functions associated with I2C Request Block
diff --git a/I2CManager.h b/I2CManager.h
index 4ecf155..2a3d80c 100644
--- a/I2CManager.h
+++ b/I2CManager.h
@@ -195,7 +195,7 @@ private:
 public:
   // Constructors
   // For I2CAddress "{Mux_0, SubBus_0, 0x23}" syntax.
-  I2CAddress(I2CMux muxNumber, I2CSubBus subBus, uint8_t deviceAddress) {
+  I2CAddress(const I2CMux muxNumber, const I2CSubBus subBus, const uint8_t deviceAddress) {
     _muxNumber = muxNumber;
     _subBus = subBus;
     _deviceAddress = deviceAddress;
@@ -218,6 +218,11 @@ public:
   I2CAddress(const I2CMux muxNumber, const I2CSubBus subBus) :
     I2CAddress(muxNumber, subBus, 0x00) {}
 
+  // For I2CAddress in form "{i2cAddress, deviceAddress}"
+  // where deviceAddress is to be on the same subbus as i2cAddress.
+  I2CAddress(I2CAddress firstAddress, uint8_t newDeviceAddress) :
+    I2CAddress(firstAddress._muxNumber, firstAddress._subBus, newDeviceAddress) {}
+
   // Conversion operator from I2CAddress to uint8_t
   // For "uint8_t address = i2cAddress;" syntax
   // (device assumed to be on the main I2C bus or on a currently selected subbus.
@@ -240,7 +245,7 @@ public:
   // Field accessors
   I2CMux muxNumber() { return _muxNumber; }
   I2CSubBus subBus() { return _subBus; }
-  uint8_t address() { return _deviceAddress; }
+  uint8_t deviceAddress() { return _deviceAddress; }
 };
 
 #else
@@ -271,6 +276,7 @@ enum : uint8_t {
   I2C_STATE_ACTIVE=253,
   I2C_STATE_FREE=254,
   I2C_STATE_CLOSING=255,
+  I2C_STATE_COMPLETED=252,
 };
 
 typedef enum : uint8_t
@@ -369,20 +375,23 @@ private:
   bool _beginCompleted = false;
   bool _clockSpeedFixed = false;
   static uint8_t retryCounter;  // Count of retries
-#if defined(__arm__)
-  uint32_t _clockSpeed = 32000000L;  // 3.2MHz max on SAMD and STM32
-#else
-  uint32_t _clockSpeed = 400000L;  // 400kHz max on Arduino.
-#endif
+  // Clock speed must be no higher than 400kHz on AVR. Higher is possible on 4809, SAMD
+  // and STM32 but most popular I2C devices are 400kHz so in practice the higher speeds
+  // will not be useful.  The speed can be overridden by I2CManager::forceClock().
+  uint32_t _clockSpeed = I2C_FREQ;  
   static unsigned long timeout; // Transaction timeout in microseconds.  0=disabled.
     
-
   // Finish off request block by waiting for completion and posting status.
   uint8_t finishRB(I2CRB *rb, uint8_t status);
 
   void _initialise();
   void _setClock(unsigned long);
 
+#if defined(I2C_EXTENDED_ADDRESS)
+  static uint8_t _muxCount;
+  uint8_t getMuxCount() { return _muxCount; }
+#endif
+
 #if !defined(I2C_USE_WIRE)
     // I2CRB structs are queued on the following two links.
     // If there are no requests, both are NULL.
@@ -395,6 +404,7 @@ private:
     static I2CRB * volatile queueHead;
     static I2CRB * volatile queueTail;
     static volatile uint8_t state;
+    static uint8_t completionStatus;
 
     static I2CRB * volatile currentRequest;
     static volatile uint8_t txCount;
@@ -403,7 +413,7 @@ private:
     static volatile uint8_t bytesToReceive;
     static volatile uint8_t operation;
     static volatile unsigned long startTime;
-    static volatile uint8_t muxSendStep;
+    static volatile uint8_t muxPhase;
 
     volatile uint32_t pendingClockSpeed = 0;
 
diff --git a/I2CManager_AVR.h b/I2CManager_AVR.h
index 24f6376..e5e1863 100644
--- a/I2CManager_AVR.h
+++ b/I2CManager_AVR.h
@@ -101,8 +101,9 @@ void I2CManagerClass::I2C_sendStart() {
 #if defined(I2C_EXTENDED_ADDRESS) 
   if (currentRequest->i2cAddress.muxNumber() != I2CMux_None) {
     // Send request to multiplexer
-    muxSendStep = 1;  // When start bit interrupt comes in, send SLA+W to MUX
-  }
+    muxPhase = MuxPhase_PROLOG;  // When start bit interrupt comes in, send SLA+W to MUX
+  } else
+    muxPhase = 0;
 #endif
   TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA)|(1<<TWSTA);  // Send Start
 
@@ -130,38 +131,132 @@ void I2CManagerClass::I2C_close() {
  *  if I2C_USE_INTERRUPTS isn't defined, from the I2CManagerClass::loop() function
  *  (and therefore, indirectly, from I2CRB::wait() and I2CRB::isBusy()).
  ***************************************************************************/
+
 void I2CManagerClass::I2C_handleInterrupt() {
   if (!(TWCR & (1<<TWINT))) return;  // Nothing to do.
 
   uint8_t twsr = TWSR & 0xF8;
 
+#if defined(I2C_EXTENDED_ADDRESS)
+  // First process the MUX state machine.
+  // This does not need to be entered during passthru phase unless the 
+  // application's send and receive have both completed.
+  if (muxPhase > MuxPhase_OFF && !(muxPhase==MuxPhase_PASSTHRU && (bytesToSend || bytesToReceive))) {
+    switch (twsr) {
+      case TWI_MTX_ADR_ACK:       // SLA+W has been transmitted and ACK received
+        if (muxPhase == MuxPhase_PROLOG) {
+          // Send MUX selecter mask to follow address
+          I2CSubBus subBus = currentRequest->i2cAddress.subBus();
+          TWDR =  (subBus==SubBus_All) ? 0xff :
+                  (subBus==SubBus_None) ? 0x00 :
+                  1 << subBus;
+          TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT);
+          return;
+        } else if (muxPhase == MuxPhase_EPILOG) {
+          TWDR = 0x00;  // Disable all subbuses
+          TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT);
+          return;
+        }
+        break;
+
+      case TWI_MTX_DATA_ACK:      // Data byte has been transmitted and ACK received
+        if (muxPhase == MuxPhase_PASSTHRU && !bytesToSend && !bytesToReceive) {
+          if (_muxCount > 1) {
+            // Device transaction complete, prepare to deselect MUX by sending start bit
+            TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWSTO)|(1<<TWSTA);
+            muxPhase = MuxPhase_EPILOG;
+            return;
+          } else {
+            // Only one MUX so no need to deselect it.  Just finish off
+            TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTO);
+            state = I2C_STATE_COMPLETED;
+            muxPhase = MuxPhase_OFF;
+            return;
+          }
+        } else if (muxPhase == MuxPhase_PROLOG) {
+          // If device address is zero, then finish here (i.e. send mux subBus mask only)
+          if (currentRequest->i2cAddress.deviceAddress() == 0) {
+            // Send stop and post rb.
+            TWDR = 0xff;
+            TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTO);
+            state = I2C_STATE_COMPLETED;
+            muxPhase = MuxPhase_OFF;
+            return;
+          } else {
+            // Send stop followed by start, preparing to send device address
+            TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWSTO)|(1<<TWSTA);
+            muxPhase = MuxPhase_PASSTHRU;
+            return;
+          } 
+        } else if (muxPhase == MuxPhase_EPILOG) {
+          // Send stop and allow RB to be posted.
+          TWDR = 0xff;
+          TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTO);
+          state = I2C_STATE_COMPLETED;
+          muxPhase = MuxPhase_OFF;
+          return;
+        }
+        break;
+
+      case TWI_MRX_DATA_NACK:     // Last data byte has been received and NACK transmitted
+        // We must read the data before processing the MUX, so do this here.
+        if (bytesToReceive > 0) {
+          currentRequest->readBuffer[rxCount++] = TWDR;
+          bytesToReceive--;
+        }
+        if (muxPhase == MuxPhase_PASSTHRU && _muxCount > 1) {
+          // Prepare to transmit epilog to mux - first send the stop bit and start bit
+          //  (we don't need to reset mux if there is only one.
+          TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWSTO)|(1<<TWSTA);
+          muxPhase = MuxPhase_EPILOG;
+          return;
+        } else {
+          // Finish up.
+          TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTO);  // Send Stop
+          state = I2C_STATE_COMPLETED;
+          muxPhase = MuxPhase_OFF;
+          return;
+        }
+        break;
+        
+      case TWI_START:             // START has been transmitted  
+      case TWI_REP_START:         // Repeated START has been transmitted
+        if (muxPhase == MuxPhase_PROLOG || muxPhase == MuxPhase_EPILOG) {
+          // Send multiplexer address first
+          uint8_t muxAddress = I2C_MUX_BASE_ADDRESS + currentRequest->i2cAddress.muxNumber();
+          TWDR = (muxAddress << 1) | 0;   // MUXaddress+Write
+          TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT);
+          return;
+        }
+        break;
+   
+      case TWI_MTX_ADR_NACK:      // SLA+W has been transmitted and NACK received
+      case TWI_MRX_ADR_NACK:      // SLA+R has been transmitted and NACK received
+      case TWI_MTX_DATA_NACK:     // Data byte has been transmitted and NACK received
+        if (muxPhase == MuxPhase_PASSTHRU) {
+          // Data transaction was nak'd, update RB status but continue with mux cleardown
+          completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+          TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWSTO)|(1<<TWSTA);  // Send Stop and start
+          muxPhase = MuxPhase_EPILOG;
+          return;
+        } else if (muxPhase > MuxPhase_EPILOG) {
+          // Mux Cleardown was NAK'd, send stop and then finish.
+          TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTO);  // Send Stop
+          state = I2C_STATE_COMPLETED;
+          return;
+        }
+        break;
+
+    }
+  }
+  #endif
+
+  // Now the main I2C interrupt handler, used for the device communications.
+  //
   // Cases are ordered so that the most frequently used ones are tested first.
   switch (twsr) {
     case TWI_MTX_DATA_ACK:      // Data byte has been transmitted and ACK received
     case TWI_MTX_ADR_ACK:       // SLA+W has been transmitted and ACK received
-#if defined(I2C_EXTENDED_ADDRESS)   // Support multiplexer selection
-      if (muxSendStep == 2) {
-        muxSendStep = 3;
-        // Send MUX selecter mask following address
-        I2CSubBus subBus = currentRequest->i2cAddress.subBus();
-        uint8_t subBusMask = (subBus==SubBus_All) ? 0xff :
-                             (subBus==SubBus_None) ? 0x00 :
-                             1 << subBus;
-        TWDR = subBusMask;
-        TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT);
-      } else if (muxSendStep == 3) {
-        muxSendStep = 0;  // Mux command complete, reset sequence step number
-        // If device address is zero, then finish here (i.e. send mux subBus mask only)
-        if (currentRequest->i2cAddress.address() == 0 && bytesToSend == 0) {
-          // Send stop and post rb.
-          TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWSTO);
-          state = I2C_STATUS_OK;
-        } else {
-          // Send stop followed by start, preparing to send device address
-          TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWSTO)|(1<<TWSTA);
-        }
-      } else
-#endif
       if (bytesToSend) {  // Send first.
         if (operation == OPERATION_SEND_P)
           TWDR = GETFLASH(currentRequest->writeBuffer + (txCount++));
@@ -173,18 +268,20 @@ void I2CManagerClass::I2C_handleInterrupt() {
         // Don't need to wait for stop, as the interface won't send the start until
         // any in-progress stop condition has been sent.
         TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWSTA);  // Send Start
-      } else {  // Nothing left to send or receive
-        TWDR = 0xff;  // Default condition = SDA released
+      } else {  
+         // Nothing left to send or receive
         TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-        state = I2C_STATUS_OK;
+        state = I2C_STATE_COMPLETED;
       }
       break;
+
     case TWI_MRX_DATA_ACK:      // Data byte has been received and ACK transmitted
       if (bytesToReceive > 0) {
         currentRequest->readBuffer[rxCount++] = TWDR;
         bytesToReceive--;
       }
       /* fallthrough */
+
     case TWI_MRX_ADR_ACK:      // SLA+R has been sent and ACK received
       if (bytesToReceive <= 1) {
         TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT); // Send NACK after next reception
@@ -193,24 +290,18 @@ void I2CManagerClass::I2C_handleInterrupt() {
         TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
       }
       break;
+
     case TWI_MRX_DATA_NACK:     // Data byte has been received and NACK transmitted
       if (bytesToReceive > 0) {
         currentRequest->readBuffer[rxCount++] = TWDR;
         bytesToReceive--;
       }
       TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-      state = I2C_STATUS_OK;
+      state = I2C_STATE_COMPLETED;
       break;
+
     case TWI_START:             // START has been transmitted  
     case TWI_REP_START:         // Repeated START has been transmitted
-#if defined(I2C_EXTENDED_ADDRESS)
-      if (muxSendStep == 1) {
-        muxSendStep = 2;
-        // Send multiplexer address first
-        uint8_t muxAddress = I2C_MUX_BASE_ADDRESS + currentRequest->i2cAddress.muxNumber();
-        TWDR = (muxAddress << 1) | 0;   // MUXaddress+Write
-      } else
-#endif
       {
         // Set up address and R/W
         uint8_t deviceAddress = currentRequest->i2cAddress;
@@ -218,25 +309,29 @@ void I2CManagerClass::I2C_handleInterrupt() {
           TWDR = (deviceAddress << 1) | 1; // SLA+R
         else
           TWDR = (deviceAddress << 1) | 0; // SLA+W
+        TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
       }
-      TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
       break;
+
     case TWI_MTX_ADR_NACK:      // SLA+W has been transmitted and NACK received
     case TWI_MRX_ADR_NACK:      // SLA+R has been transmitted and NACK received
     case TWI_MTX_DATA_NACK:     // Data byte has been transmitted and NACK received
-      TWDR = 0xff;  // Default condition = SDA released
       TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-      state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      state = I2C_STATE_COMPLETED;
       break;
+
     case TWI_ARB_LOST:          // Arbitration lost
       // Restart transaction from start.
       I2C_sendStart();
       break;
+
     case TWI_BUS_ERROR:         // Bus error due to an illegal START or STOP condition
     default:
       TWDR = 0xff;  // Default condition = SDA released
       TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
-      state = I2C_STATUS_TRANSMIT_ERROR;
+      completionStatus = I2C_STATUS_TRANSMIT_ERROR;
+      state = I2C_STATE_COMPLETED;
   }
 }
 
diff --git a/I2CManager_Mega4809.h b/I2CManager_Mega4809.h
index 41c70fe..a788724 100644
--- a/I2CManager_Mega4809.h
+++ b/I2CManager_Mega4809.h
@@ -110,14 +110,17 @@ void I2CManagerClass::I2C_handleInterrupt() {
     I2C_sendStart();   // Reinitiate request
   } else if (currentStatus & TWI_BUSERR_bm) {
     // Bus error
-    state = I2C_STATUS_BUS_ERROR;
+    completionStatus = I2C_STATUS_BUS_ERROR;
+    state = I2C_STATE_COMPLETED;
     TWI0.MSTATUS = currentStatus; // clear all flags
   } else if (currentStatus & TWI_WIF_bm) {
     // Master write completed
     if (currentStatus & TWI_RXACK_bm) {
       // Nacked, send stop.
       TWI0.MCTRLB = TWI_MCMD_STOP_gc;
-      state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      state = I2C_STATE_COMPLETED;
+
     } else if (bytesToSend) {
       // Acked, so send next byte (don't need to use GETFLASH)
       TWI0.MDATA = currentRequest->writeBuffer[txCount++];
@@ -128,7 +131,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     } else {
       // No more data to send/receive. Initiate a STOP condition.
       TWI0.MCTRLB = TWI_MCMD_STOP_gc;
-      state = I2C_STATUS_OK;  // Done
+      state = I2C_STATE_COMPLETED;
     }
   } else if (currentStatus & TWI_RIF_bm) {
     // Master read completed without errors
@@ -142,7 +145,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
     } else {
       // Transaction finished, issue NACK and STOP.
       TWI0.MCTRLB = TWI_ACKACT_bm | TWI_MCMD_STOP_gc;
-      state = I2C_STATUS_OK;
+      state = I2C_STATE_COMPLETED;
     }
   }
 }
diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h
index 5892bc2..4214ef9 100644
--- a/I2CManager_NonBlocking.h
+++ b/I2CManager_NonBlocking.h
@@ -76,6 +76,14 @@ for ( MY_ATOMIC_RESTORESTATE, _done =  my_iCliRetVal();                   \
 #undef I2C_USE_WIRE
 #endif
 
+enum MuxPhase: uint8_t {
+  MuxPhase_OFF = 0,
+  MuxPhase_PROLOG,
+  MuxPhase_PASSTHRU,
+  MuxPhase_EPILOG,
+} ;
+
+
 /***************************************************************************
  * Initialise the I2CManagerAsync class.
  ***************************************************************************/
@@ -108,6 +116,7 @@ void I2CManagerClass::startTransaction() {
   ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
     if ((state == I2C_STATE_FREE) && (queueHead != NULL)) {
       state = I2C_STATE_ACTIVE;
+      completionStatus = I2C_STATUS_OK;
       // Check for pending clock speed change
       if (pendingClockSpeed) {
         // We're about to start a new I2C transaction, so set clock now.
@@ -260,7 +269,7 @@ void I2CManagerClass::handleInterrupt() {
   // and state isn't active then state contains the completion status of the request.
   if (state != I2C_STATE_ACTIVE && currentRequest != NULL) {
     // Operation has completed.
-    if (state == I2C_STATUS_OK || ++retryCounter > MAX_I2C_RETRIES
+    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.
@@ -270,7 +279,7 @@ void I2CManagerClass::handleInterrupt() {
         queueHead = t->nextRequest;
         if (!queueHead) queueTail = queueHead;
         t->nBytes = rxCount;
-        t->status = state;
+        t->status = completionStatus;
         
         // I2C state machine is now free for next request
         currentRequest = NULL;
@@ -297,6 +306,7 @@ I2CRB * volatile I2CManagerClass::queueHead = NULL;
 I2CRB * volatile I2CManagerClass::queueTail = NULL;
 I2CRB * volatile I2CManagerClass::currentRequest = NULL;
 volatile uint8_t I2CManagerClass::state = I2C_STATE_FREE;
+uint8_t I2CManagerClass::completionStatus;
 volatile uint8_t I2CManagerClass::txCount;
 volatile uint8_t I2CManagerClass::rxCount;
 volatile uint8_t I2CManagerClass::operation;
@@ -306,7 +316,7 @@ volatile unsigned long I2CManagerClass::startTime;
 uint8_t I2CManagerClass::retryCounter = 0;
 
 #if defined(I2C_EXTENDED_ADDRESS) 
-volatile uint8_t I2CManagerClass::muxSendStep = 0;
+volatile uint8_t I2CManagerClass::muxPhase = 0;
 #endif
 
 #endif
\ No newline at end of file
diff --git a/I2CManager_SAMD.h b/I2CManager_SAMD.h
index 08e34f2..5eb612b 100644
--- a/I2CManager_SAMD.h
+++ b/I2CManager_SAMD.h
@@ -155,6 +155,8 @@ void I2CManagerClass::I2C_sendStart() {
   // Set counters here in case this is a retry.
   bytesToSend = currentRequest->writeLen;
   bytesToReceive = currentRequest->readLen;
+  txCount = 0;
+  rxCount = 0;
 
   // On a single-master I2C bus, the start bit won't be sent until the bus 
   // state goes to IDLE so we can request it without waiting.  On a 
@@ -207,13 +209,15 @@ void I2CManagerClass::I2C_handleInterrupt() {
     I2C_sendStart();   // Reinitiate request
   } else if (s->I2CM.STATUS.bit.BUSERR) {
     // Bus error
-    state = I2C_STATUS_BUS_ERROR;
+    completionStatus = I2C_STATUS_BUS_ERROR;
+    state = I2C_STATE_COMPLETED;  // Completed with error
   } else if (s->I2CM.INTFLAG.bit.MB) {
     // Master write completed
     if (s->I2CM.STATUS.bit.RXNACK) {
       // Nacked, send stop.
       I2C_sendStop();
-      state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      completionStatus = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
+      state = I2C_STATE_COMPLETED;  // Completed with error
     } else if (bytesToSend) {
       // Acked, so send next byte
       s->I2CM.DATA.bit.DATA = currentRequest->writeBuffer[txCount++];
@@ -222,9 +226,9 @@ void I2CManagerClass::I2C_handleInterrupt() {
       // Last sent byte acked and no more to send.  Send repeated start, address and read bit.
       s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1) | 1;
     } else {
-      // No more data to send/receive. Initiate a STOP condition.
+      // No more data to send/receive. Initiate a STOP condition
       I2C_sendStop();
-      state = I2C_STATUS_OK; // Done
+      state = I2C_STATE_COMPLETED;  // Completed OK
     }
   } else if (s->I2CM.INTFLAG.bit.SB) {
     // Master read completed without errors
@@ -233,7 +237,7 @@ void I2CManagerClass::I2C_handleInterrupt() {
       I2C_sendStop();  // send stop
       currentRequest->readBuffer[rxCount++] = s->I2CM.DATA.bit.DATA;  // Store received byte
       bytesToReceive = 0;
-      state = I2C_STATUS_OK; // done
+      state = I2C_STATE_COMPLETED;  // Completed OK
     } else if (bytesToReceive) {
       s->I2CM.CTRLB.bit.ACKACT = 0;  // ACK all but final byte
       currentRequest->readBuffer[rxCount++] = s->I2CM.DATA.bit.DATA;  // Store received byte
diff --git a/I2CManager_Wire.h b/I2CManager_Wire.h
index 7ea75c5..67ef306 100644
--- a/I2CManager_Wire.h
+++ b/I2CManager_Wire.h
@@ -69,38 +69,53 @@ void I2CManagerClass::setTimeout(unsigned long value) {
  * Helper function for I2C Multiplexer operations
  ********************************************************/
 #ifdef I2C_EXTENDED_ADDRESS
-static uint8_t muxSelect(I2CAddress &address) {
+static uint8_t muxSelect(I2CAddress address) {
   // Select MUX sub bus.
-  Wire.beginTransmission(I2C_MUX_BASE_ADDRESS+address.muxNumber()); 
-  uint8_t data = address.subBus();
-  Wire.write(&data, 1);
-  return Wire.endTransmission(true);  // have to release I2C bus for it to work
+  I2CMux muxNo = address.muxNumber();
+  I2CSubBus subBus = address.subBus();
+  if (muxNo != I2CMux_None) {
+    Wire.beginTransmission(I2C_MUX_BASE_ADDRESS+muxNo); 
+    uint8_t data =  (subBus == SubBus_All) ? 0xff :
+                    (subBus == SubBus_None) ? 0x00 :
+                    (1 << subBus);
+    Wire.write(&data, 1);
+    return Wire.endTransmission(true);  // have to release I2C bus for it to work
+  }
+  return I2C_STATUS_OK;
 }
 #endif
 
+
 /***************************************************************************
  *  Initiate a write to an I2C device (blocking operation on Wire)
  ***************************************************************************/
 uint8_t I2CManagerClass::write(I2CAddress address, const uint8_t buffer[], uint8_t size, I2CRB *rb) {
-  uint8_t status = I2C_STATUS_OK;
+  uint8_t status, muxStatus;
   uint8_t retryCount = 0;
   // If request fails, retry up to the defined limit, unless the NORETRY flag is set
   // in the request block.
   do {
-    status = I2C_STATUS_OK;
+    status = muxStatus = I2C_STATUS_OK;
 #ifdef I2C_EXTENDED_ADDRESS
-    if (address.muxNumber() != I2CMux_None) {
-      status = muxSelect(address);
-    }
+    if (address.muxNumber() != I2CMux_None)
+      muxStatus = muxSelect(address);
 #endif
-    // Only send new transaction if address and size are both nonzero.
-    if (status == I2C_STATUS_OK && address != 0 && size != 0) {
+    // Only send new transaction if address is non-zero.
+    if (muxStatus == I2C_STATUS_OK && address != 0) {
       Wire.beginTransmission(address);
       if (size > 0) Wire.write(buffer, size);
       status = Wire.endTransmission();
     }
-  } while (!(status == I2C_STATUS_OK || ++retryCount > MAX_I2C_RETRIES
-    || rb->operation & OPERATION_NORETRY));
+#ifdef I2C_EXTENDED_ADDRESS
+    // Deselect MUX if there's more than one MUX present, to avoid having multiple ones selected
+    if (_muxCount > 1 && muxStatus == I2C_STATUS_OK 
+          && address.deviceAddress() != 0 && address.muxNumber() != I2CMux_None) {
+      muxSelect({address.muxNumber(), SubBus_None});
+    }
+    if (muxStatus != I2C_STATUS_OK) status = muxStatus;
+#endif
+  } while (!(status == I2C_STATUS_OK
+    || ++retryCount > MAX_I2C_RETRIES || rb->operation & OPERATION_NORETRY));
   rb->status = status;
   return I2C_STATUS_OK;
 }
@@ -123,20 +138,20 @@ uint8_t I2CManagerClass::write_P(I2CAddress address, const uint8_t buffer[], uin
 uint8_t I2CManagerClass::read(I2CAddress address, uint8_t readBuffer[], uint8_t readSize,
                               const uint8_t writeBuffer[], uint8_t writeSize, I2CRB *rb)
 {
-  uint8_t status = I2C_STATUS_OK;
+  uint8_t status, muxStatus;
   uint8_t nBytes = 0;
   uint8_t retryCount = 0;
   // If request fails, retry up to the defined limit, unless the NORETRY flag is set
   // in the request block.
   do {
-    status = I2C_STATUS_OK;
+    status = muxStatus = I2C_STATUS_OK;
 #ifdef I2C_EXTENDED_ADDRESS
     if (address.muxNumber() != I2CMux_None) {
-      status = muxSelect(address);
+      muxStatus = muxSelect(address);
     }
 #endif
-    // Only start new transaction if address and readSize are both nonzero.
-    if (status == I2C_STATUS_OK && address != 0 && writeSize > 0) {
+    // Only start new transaction if address is non-zero.
+    if (muxStatus == I2C_STATUS_OK && address != 0) {
       if (writeSize > 0) {
         Wire.beginTransmission(address);
         Wire.write(writeBuffer, writeSize);
@@ -161,8 +176,16 @@ uint8_t I2CManagerClass::read(I2CAddress address, uint8_t readBuffer[], uint8_t
 #endif
       }
     }
-  } while (!(status == I2C_STATUS_OK || ++retryCount > MAX_I2C_RETRIES
-    || rb->operation & OPERATION_NORETRY));
+#ifdef I2C_EXTENDED_ADDRESS
+    // Deselect MUX if there's more than one MUX present, to avoid having multiple ones selected
+    if (_muxCount > 1 && muxStatus == I2C_STATUS_OK && address != 0 && address.muxNumber() != I2CMux_None) {
+      muxSelect({address.muxNumber(), SubBus_None});
+    }
+    if (muxStatus != I2C_STATUS_OK) status = muxStatus;
+#endif
+
+  } while (!((status == I2C_STATUS_OK) 
+    || ++retryCount > MAX_I2C_RETRIES || rb->operation & OPERATION_NORETRY));
 
   rb->nBytes = nBytes;
   rb->status = status;