HAL: Add support for Extended Addresses and I2C Multiplexer

Change I2C addresses from uint8_t to I2CAddress, in preparation for MUX support.  Currently, by default, I2CAddress is typedef'd to uint8_t.
MUX support implemented for AVR and Wire versions.

I2CManager.cpp

@@ -58,13 +58,46 @@ void I2CManagerClass::begin(void) {
     _setClock(100000);
     unsigned long originalTimeout = timeout;
     setTimeout(1000);       // use 1ms timeout for probes
+
+  #if defined(I2C_EXTENDED_ADDRESS)
+    // First switch off all multiplexer subbuses.
+    for (uint8_t muxNo=I2CMux_0; muxNo <= I2CMux_7; muxNo++) {
+      I2CManager.muxSelectSubBus({(I2CMux)muxNo, SubBus_None});  // Deselect Mux
+    }
+  #endif
+
     bool found = false;
-    for (byte addr=1; addr<127; addr++) {
+    for (uint8_t addr=0x08; addr<0x78; addr++) {
       if (exists(addr)) {
         found = true; 
         DIAG(F("I2C Device found at x%x"), 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
+    // 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)) {
+        for (uint8_t subBus=0; subBus<=7; 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);
+            }
+          }
+        }
+        // Probe mux address again with SubBus_None to deselect all
+        // subBuses for that mux.  Otherwise its devices will continue to
+        // respond when other muxes are being probed.
+        I2CManager.muxSelectSubBus({(I2CMux)muxNo, SubBus_None});  // Deselect Mux
+      } 
+    }
+#endif
     if (!found) DIAG(F("No I2C Devices found"));
     _setClock(_clockSpeed);
     setTimeout(originalTimeout);      // set timeout back to original
@@ -92,7 +125,7 @@ void I2CManagerClass::forceClock(uint32_t speed) {
 // Check if specified I2C address is responding (blocking operation)
 // Returns I2C_STATUS_OK (0) if OK, or error code.
 // Suppress retries.  If it doesn't respond first time it's out of the running.
-uint8_t I2CManagerClass::checkAddress(uint8_t address) {
+uint8_t I2CManagerClass::checkAddress(I2CAddress address) {
   I2CRB rb;
   rb.setWriteParams(address, NULL, 0);
   rb.suppressRetries(true);
@@ -104,7 +137,7 @@ uint8_t I2CManagerClass::checkAddress(uint8_t address) {
 /***************************************************************************
  *  Write a transmission to I2C using a list of data (blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::write(uint8_t address, uint8_t nBytes, ...) {
+uint8_t I2CManagerClass::write(I2CAddress address, uint8_t nBytes, ...) {
   uint8_t buffer[nBytes];
   va_list args;
   va_start(args, nBytes);
@@ -117,7 +150,7 @@ uint8_t I2CManagerClass::write(uint8_t address, uint8_t nBytes, ...) {
 /***************************************************************************
  *  Initiate a write to an I2C device (blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::write(uint8_t i2cAddress, const uint8_t writeBuffer[], uint8_t writeLen) {
+uint8_t I2CManagerClass::write(I2CAddress i2cAddress, const uint8_t writeBuffer[], uint8_t writeLen) {
   I2CRB req;
   uint8_t status = write(i2cAddress, writeBuffer, writeLen, &req);
   return finishRB(&req, status);
@@ -126,7 +159,7 @@ uint8_t I2CManagerClass::write(uint8_t i2cAddress, const uint8_t writeBuffer[],
 /***************************************************************************
  *  Initiate a write from PROGMEM (flash) to an I2C device (blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::write_P(uint8_t i2cAddress, const uint8_t * data, uint8_t dataLen) {
+uint8_t I2CManagerClass::write_P(I2CAddress i2cAddress, const uint8_t * data, uint8_t dataLen) {
   I2CRB req;
   uint8_t status = write_P(i2cAddress, data, dataLen, &req);
   return finishRB(&req, status);
@@ -135,7 +168,7 @@ uint8_t I2CManagerClass::write_P(uint8_t i2cAddress, const uint8_t * data, uint8
 /***************************************************************************
  *  Initiate a write (optional) followed by a read from the I2C device (blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::read(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen, 
+uint8_t I2CManagerClass::read(I2CAddress i2cAddress, uint8_t *readBuffer, uint8_t readLen, 
     const uint8_t *writeBuffer, uint8_t writeLen)
 {
   I2CRB req;
@@ -146,7 +179,7 @@ uint8_t I2CManagerClass::read(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t r
 /***************************************************************************
  *  Overload of read() to allow command to be specified as a series of bytes (blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::read(uint8_t address, uint8_t readBuffer[], uint8_t readSize, 
+uint8_t I2CManagerClass::read(I2CAddress address, uint8_t readBuffer[], uint8_t readSize, 
                                   uint8_t writeSize, ...) {
   va_list args;
   // Copy the series of bytes into an array.
@@ -230,7 +263,7 @@ bool I2CRB::isBusy() {
 /***************************************************************************
  *  Helper functions to fill the I2CRequest structure with parameters.
  ***************************************************************************/
-void I2CRB::setReadParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen) {
+void I2CRB::setReadParams(I2CAddress i2cAddress, uint8_t *readBuffer, uint8_t readLen) {
   this->i2cAddress = i2cAddress;
   this->writeLen = 0;
   this->readBuffer = readBuffer;
@@ -239,7 +272,7 @@ void I2CRB::setReadParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readL
   this->status = I2C_STATUS_OK;
 }
 
-void I2CRB::setRequestParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen, 
+void I2CRB::setRequestParams(I2CAddress i2cAddress, uint8_t *readBuffer, uint8_t readLen, 
     const uint8_t *writeBuffer, uint8_t writeLen) {
   this->i2cAddress = i2cAddress;
   this->writeBuffer = writeBuffer;
@@ -250,7 +283,7 @@ void I2CRB::setRequestParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t re
   this->status = I2C_STATUS_OK;
 }
 
-void I2CRB::setWriteParams(uint8_t i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen) {
+void I2CRB::setWriteParams(I2CAddress i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen) {
   this->i2cAddress = i2cAddress;
   this->writeBuffer = writeBuffer;
   this->writeLen = writeLen;

I2CManager.h

@@ -131,6 +131,124 @@
 #define I2C_USE_INTERRUPTS
 #endif
 
+// I2C Extended Address support I2C Multiplexers and allows various properties to be 
+// associated with an I2C address such as the MUX and SubBus.  In the future, this
+// may be extended to include multiple buses, and other features. 
+// Uncomment to enable extended address.
+//
+// WARNING: When I2CAddress is passed to formatting commands such as DIAG, LCD etc,
+// it should be cast to (int) to ensure that the address value is passed rather than
+// the struct.
+
+//#define I2C_EXTENDED_ADDRESS
+
+
+// Type to hold I2C address
+#if defined(I2C_EXTENDED_ADDRESS)
+
+/////////////////////////////////////////////////////////////////////////////////////
+// Extended I2C Address type to facilitate extended I2C addresses including
+// I2C multiplexer support.
+/////////////////////////////////////////////////////////////////////////////////////
+
+// Currently I2CAddress supports one I2C bus, with up to eight
+// multipexers (MUX) attached.  Each MUX can have up to eight sub-buses.
+enum I2CMux : uint8_t {
+  I2CMux_0 = 0,
+  I2CMux_1 = 1,
+  I2CMux_2 = 2,
+  I2CMux_3 = 3,
+  I2CMux_4 = 4,
+  I2CMux_5 = 5,
+  I2CMux_6 = 6,
+  I2CMux_7 = 7,
+  I2CMux_None = 255,   // Address doesn't need mux switching
+};
+  
+enum I2CSubBus : uint8_t {
+  SubBus_0 = 0,        // Enable individual sub-buses...
+  SubBus_1 = 1,
+  SubBus_2 = 2,
+  SubBus_3 = 3,
+  SubBus_4 = 4,
+  SubBus_5 = 5,
+  SubBus_6 = 6,
+  SubBus_7 = 7,
+  SubBus_None = 254,   // Disable all sub-buses on selected mux
+  SubBus_All = 255,    // Enable all sub-buses
+};
+
+// First MUX address (they range between 0x70-0x77).
+#define I2C_MUX_BASE_ADDRESS 0x70
+
+// Currently I2C address supports one I2C bus, with up to eight
+// multiplexers (MUX) attached.  Each MUX can have up to eight sub-buses.
+// This structure could be extended in the future (if there is a need) 
+// to support 10-bit I2C addresses, different I2C clock speed for each
+// sub-bus, multiple I2C buses, and other features not yet thought of.
+struct I2CAddress {
+private:
+  // Fields
+  I2CMux _muxNumber;
+  I2CSubBus _subBus;
+  uint8_t _deviceAddress;
+public:
+  // Constructors
+  // For I2CAddress "{Mux_0, SubBus_0, 0x23}" syntax.
+  I2CAddress(I2CMux muxNumber, I2CSubBus subBus, uint8_t deviceAddress) {
+    _muxNumber = muxNumber;
+    _subBus = subBus;
+    _deviceAddress = deviceAddress;
+  }
+
+  // Basic constructor
+  I2CAddress() : I2CAddress(I2CMux_None, SubBus_None, 0) {}
+
+  // For I2CAddress in form "{SubBus_0, 0x23}" - assume Mux0 (0x70)
+  I2CAddress(I2CSubBus subBus, uint8_t deviceAddress) : 
+    I2CAddress(I2CMux_0, subBus, deviceAddress) {}
+
+  // Conversion from uint8_t to I2CAddress
+  // For I2CAddress in form "0x23"
+  // (device assumed to be on the main I2C bus).
+  I2CAddress(const uint8_t deviceAddress) : 
+    I2CAddress(I2CMux_None, SubBus_None, deviceAddress) {}
+
+  // For I2CAddress in form "{I2CMux_0, SubBus_0}" (mux selector)
+  I2CAddress(const I2CMux muxNumber, const I2CSubBus subBus) :
+    I2CAddress(muxNumber, subBus, 0x00) {}
+
+  // 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.
+  operator uint8_t () const { return _deviceAddress; }
+
+  // Comparison operator
+  int operator == (I2CAddress &a) const {
+    if (_deviceAddress != a._deviceAddress) 
+      return false; // Different device address so no match
+    if (_muxNumber == I2CMux_None || a._muxNumber == I2CMux_None)
+      return true;  // Same device address, one or other on main bus
+    if (_subBus == SubBus_None || a._subBus == SubBus_None) 
+      return true;  // Same device address, one or other on main bus
+    if (_muxNumber != a._muxNumber) 
+      return false; // Connected to a subbus on a different mux
+    if (_subBus != a._subBus)
+      return false;  // different subbus
+    return true;  // Same address on same mux and same subbus
+  }
+  // Field accessors
+  I2CMux muxNumber() { return _muxNumber; }
+  I2CSubBus subBus() { return _subBus; }
+  uint8_t address() { return _deviceAddress; }
+};
+
+#else
+// Legacy single-byte I2C address type for compact code and smooth changeover.
+typedef uint8_t I2CAddress;
+#endif // I2C_EXTENDED_ADDRESS
+
+
 // Status codes for I2CRB structures.
 enum : uint8_t {
   // Codes used by Wire and by native drivers
@@ -181,19 +299,19 @@ public:
   uint8_t wait();
   bool isBusy();
 
-  void setReadParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen);
+  void setReadParams(I2CAddress i2cAddress, uint8_t *readBuffer, uint8_t readLen);
-  void setRequestParams(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen, const uint8_t *writeBuffer, uint8_t writeLen);
+  void setRequestParams(I2CAddress i2cAddress, uint8_t *readBuffer, uint8_t readLen, const uint8_t *writeBuffer, uint8_t writeLen);
-  void setWriteParams(uint8_t i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen);
+  void setWriteParams(I2CAddress i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen);
   void suppressRetries(bool suppress);
 
   uint8_t writeLen;
   uint8_t readLen;
   uint8_t operation;
-  uint8_t i2cAddress;
+  I2CAddress i2cAddress;
   uint8_t *readBuffer;
   const uint8_t *writeBuffer;
 #if !defined(I2C_USE_WIRE)
-  I2CRB *nextRequest;
+  I2CRB *nextRequest;  // Used by non-blocking devices for I2CRB queue management.
 #endif
 };
 
@@ -210,25 +328,30 @@ public:
   // setTimeout sets the timout value for I2C transactions (milliseconds).
   void setTimeout(unsigned long);
   // Check if specified I2C address is responding.
-  uint8_t checkAddress(uint8_t address);
+  uint8_t checkAddress(I2CAddress address);
-  inline bool exists(uint8_t address) {
+  inline bool exists(I2CAddress address) {
     return checkAddress(address)==I2C_STATUS_OK;
   }
+  // Select/deselect Mux Sub-Bus (if using legacy addresses, just checks address)
+  // E.g. muxSelectSubBus({I2CMux_0, SubBus_3});
+  uint8_t muxSelectSubBus(I2CAddress address) {
+    return checkAddress(address);
+  }
   // Write a complete transmission to I2C from an array in RAM
-  uint8_t write(uint8_t address, const uint8_t buffer[], uint8_t size);
+  uint8_t write(I2CAddress address, const uint8_t buffer[], uint8_t size);
-  uint8_t write(uint8_t address, const uint8_t buffer[], uint8_t size, I2CRB *rb);
+  uint8_t write(I2CAddress address, const uint8_t buffer[], uint8_t size, I2CRB *rb);
   // Write a complete transmission to I2C from an array in Flash
-  uint8_t write_P(uint8_t address, const uint8_t buffer[], uint8_t size);
+  uint8_t write_P(I2CAddress address, const uint8_t buffer[], uint8_t size);
-  uint8_t write_P(uint8_t address, const uint8_t buffer[], uint8_t size, I2CRB *rb);
+  uint8_t write_P(I2CAddress address, const uint8_t buffer[], uint8_t size, I2CRB *rb);
   // Write a transmission to I2C from a list of bytes.
-  uint8_t write(uint8_t address, uint8_t nBytes, ...);
+  uint8_t write(I2CAddress address, uint8_t nBytes, ...);
   // Write a command from an array in RAM and read response
-  uint8_t read(uint8_t address, uint8_t readBuffer[], uint8_t readSize, 
+  uint8_t read(I2CAddress address, uint8_t readBuffer[], uint8_t readSize, 
     const uint8_t writeBuffer[]=NULL, uint8_t writeSize=0);
-  uint8_t read(uint8_t address, uint8_t readBuffer[], uint8_t readSize, 
+  uint8_t read(I2CAddress address, uint8_t readBuffer[], uint8_t readSize, 
     const uint8_t writeBuffer[], uint8_t writeSize, I2CRB *rb);
   // Write a command from an arbitrary list of bytes and read response
-  uint8_t read(uint8_t address, uint8_t readBuffer[], uint8_t readSize, 
+  uint8_t read(I2CAddress address, uint8_t readBuffer[], uint8_t readSize, 
     uint8_t writeSize, ...);
   void queueRequest(I2CRB *req);
 
@@ -280,6 +403,7 @@ private:
     static volatile uint8_t bytesToReceive;
     static volatile uint8_t operation;
     static volatile unsigned long startTime;
+    static volatile uint8_t muxSendStep;
 
     volatile uint32_t pendingClockSpeed = 0;
 

I2CManager_AVR.h

@@ -98,7 +98,14 @@ void I2CManagerClass::I2C_sendStart() {
   bytesToReceive = currentRequest->readLen;
   rxCount = 0;
   txCount = 0;
+#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
+  }
+#endif
   TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA)|(1<<TWSTA);  // Send Start
+
 }
 
 /***************************************************************************
@@ -132,17 +139,40 @@ void I2CManagerClass::I2C_handleInterrupt() {
   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++));
         else
           TWDR = currentRequest->writeBuffer[txCount++];
         bytesToSend--;
-        TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
+        TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT);
       } else if (bytesToReceive) {  // All sent, anything to receive?
         // 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<<TWEA)|(1<<TWSTA);  // Send Start
+        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
         TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);  // Send Stop
@@ -173,11 +203,22 @@ void I2CManagerClass::I2C_handleInterrupt() {
       break;
     case TWI_START:             // START has been transmitted  
     case TWI_REP_START:         // Repeated START has been transmitted
-      // Set up address and R/W
+#if defined(I2C_EXTENDED_ADDRESS)
-      if (operation == OPERATION_READ || (operation==OPERATION_REQUEST && !bytesToSend))
+      if (muxSendStep == 1) {
-        TWDR = (currentRequest->i2cAddress << 1) | 1; // SLA+R
+        muxSendStep = 2;
-      else
+        // Send multiplexer address first
-        TWDR = (currentRequest->i2cAddress << 1) | 0; // SLA+W
+        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;
+        if (operation == OPERATION_READ || (operation==OPERATION_REQUEST && !bytesToSend))
+          TWDR = (deviceAddress << 1) | 1; // SLA+R
+        else
+          TWDR = (deviceAddress << 1) | 0; // SLA+W
+      }
       TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
       break;
     case TWI_MTX_ADR_NACK:      // SLA+W has been transmitted and NACK received

I2CManager_NonBlocking.h

@@ -145,7 +145,7 @@ void I2CManagerClass::queueRequest(I2CRB *req) {
 /***************************************************************************
  *  Initiate a write to an I2C device (non-blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::write(uint8_t i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen, I2CRB *req) {
+uint8_t I2CManagerClass::write(I2CAddress i2cAddress, const uint8_t *writeBuffer, uint8_t writeLen, I2CRB *req) {
   // Make sure previous request has completed.
   req->wait();
   req->setWriteParams(i2cAddress, writeBuffer, writeLen);
@@ -156,7 +156,7 @@ uint8_t I2CManagerClass::write(uint8_t i2cAddress, const uint8_t *writeBuffer, u
 /***************************************************************************
  *  Initiate a write from PROGMEM (flash) to an I2C device (non-blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::write_P(uint8_t i2cAddress, const uint8_t * writeBuffer, uint8_t writeLen, I2CRB *req) {
+uint8_t I2CManagerClass::write_P(I2CAddress i2cAddress, const uint8_t * writeBuffer, uint8_t writeLen, I2CRB *req) {
   // Make sure previous request has completed.
   req->wait();
   req->setWriteParams(i2cAddress, writeBuffer, writeLen);
@@ -169,7 +169,7 @@ uint8_t I2CManagerClass::write_P(uint8_t i2cAddress, const uint8_t * writeBuffer
  *  Initiate a read from the I2C device, optionally preceded by a write 
  *   (non-blocking operation)
  ***************************************************************************/
-uint8_t I2CManagerClass::read(uint8_t i2cAddress, uint8_t *readBuffer, uint8_t readLen, 
+uint8_t I2CManagerClass::read(I2CAddress i2cAddress, uint8_t *readBuffer, uint8_t readLen, 
     const uint8_t *writeBuffer, uint8_t writeLen, I2CRB *req)
 {
   // Make sure previous request has completed.
@@ -201,7 +201,7 @@ void I2CManagerClass::checkForTimeout() {
       unsigned long elapsed = micros() - startTime;
       if (elapsed > timeout) { 
 #ifdef DIAG_IO
-        //DIAG(F("I2CManager Timeout on x%x, I2CRB=x%x"), t->i2cAddress, currentRequest);
+        //DIAG(F("I2CManager Timeout on x%x, I2CRB=x%x"), (int)t->i2cAddress, currentRequest);
 #endif
         // Excessive time. Dequeue request
         queueHead = t->nextRequest;
@@ -305,4 +305,8 @@ volatile uint8_t I2CManagerClass::bytesToReceive;
 volatile unsigned long I2CManagerClass::startTime;
 uint8_t I2CManagerClass::retryCounter = 0;
 
+#if defined(I2C_EXTENDED_ADDRESS) 
+volatile uint8_t I2CManagerClass::muxSendStep = 0;
+#endif
+
 #endif

I2CManager_Wire.h

@@ -65,18 +65,40 @@ void I2CManagerClass::setTimeout(unsigned long value) {
 #endif
 }
 
+/********************************************************
+ * Helper function for I2C Multiplexer operations
+ ********************************************************/
+#ifdef I2C_EXTENDED_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
+}
+#endif
+
 /***************************************************************************
  *  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) {
+uint8_t I2CManagerClass::write(I2CAddress address, const uint8_t buffer[], uint8_t size, I2CRB *rb) {
   uint8_t status = I2C_STATUS_OK;
   uint8_t retryCount = 0;
   // If request fails, retry up to the defined limit, unless the NORETRY flag is set
   // in the request block.
   do {
-    Wire.beginTransmission(address);
+    status = I2C_STATUS_OK;
-    if (size > 0) Wire.write(buffer, size);
+#ifdef I2C_EXTENDED_ADDRESS
-    status = Wire.endTransmission();
+    if (address.muxNumber() != I2CMux_None) {
+      status = muxSelect(address);
+    }
+#endif
+    // Only send new transaction if address and size are both nonzero.
+    if (status == I2C_STATUS_OK && address != 0 && size != 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));
   rb->status = status;
@@ -86,7 +108,7 @@ uint8_t I2CManagerClass::write(uint8_t address, const uint8_t buffer[], uint8_t
 /***************************************************************************
  *  Initiate a write from PROGMEM (flash) to an I2C device (blocking operation on Wire)
  ***************************************************************************/
-uint8_t I2CManagerClass::write_P(uint8_t address, const uint8_t buffer[], uint8_t size, I2CRB *rb) {
+uint8_t I2CManagerClass::write_P(I2CAddress address, const uint8_t buffer[], uint8_t size, I2CRB *rb) {
   uint8_t ramBuffer[size];
   const uint8_t *p1 = buffer;
   for (uint8_t i=0; i<size; i++)
@@ -98,7 +120,7 @@ uint8_t I2CManagerClass::write_P(uint8_t address, const uint8_t buffer[], uint8_
  *  Initiate a write (optional) followed by a read from the I2C device (blocking operation on Wire)
  *  If fewer than the number of requested bytes are received, status is I2C_STATUS_TRUNCATED.
  ***************************************************************************/
-uint8_t I2CManagerClass::read(uint8_t address, uint8_t readBuffer[], uint8_t readSize,
+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;
@@ -107,27 +129,37 @@ uint8_t I2CManagerClass::read(uint8_t address, uint8_t readBuffer[], uint8_t rea
   // If request fails, retry up to the defined limit, unless the NORETRY flag is set
   // in the request block.
   do {
-    if (writeSize > 0) {
+    status = I2C_STATUS_OK;
-      Wire.beginTransmission(address);
+#ifdef I2C_EXTENDED_ADDRESS
-      Wire.write(writeBuffer, writeSize);
+    if (address.muxNumber() != I2CMux_None) {
-      status = Wire.endTransmission(false); // Don't free bus yet
+      status = muxSelect(address);
     }
-    if (status == I2C_STATUS_OK) {
-#ifdef WIRE_HAS_TIMEOUT
-      Wire.clearWireTimeoutFlag();
 #endif
-      Wire.requestFrom(address, (size_t)readSize);
+    // Only start new transaction if address and readSize are both nonzero.
-#ifdef WIRE_HAS_TIMEOUT
+    if (status == I2C_STATUS_OK && address != 0 && writeSize > 0) {
-      if (!Wire.getWireTimeoutFlag()) {
+      if (writeSize > 0) {
-#endif
+        Wire.beginTransmission(address);
-        while (Wire.available() && nBytes < readSize) 
+        Wire.write(writeBuffer, writeSize);
-          readBuffer[nBytes++] = Wire.read();
+        status = Wire.endTransmission(false); // Don't free bus yet
-        if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
-#ifdef WIRE_HAS_TIMEOUT
-      } else {
-        status = I2C_STATUS_TIMEOUT;
       }
+      if (status == I2C_STATUS_OK) {
+#ifdef WIRE_HAS_TIMEOUT
+        Wire.clearWireTimeoutFlag();
+        Wire.requestFrom(address, (size_t)readSize);
+        if (!Wire.getWireTimeoutFlag()) {
+          while (Wire.available() && nBytes < readSize) 
+            readBuffer[nBytes++] = Wire.read();
+          if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
+        } else {
+          status = I2C_STATUS_TIMEOUT;
+        }
+#else
+        Wire.requestFrom(address, (size_t)readSize);
+          while (Wire.available() && nBytes < readSize) 
+            readBuffer[nBytes++] = Wire.read();
+          if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
 #endif
+      }
     }
   } while (!(status == I2C_STATUS_OK || ++retryCount > MAX_I2C_RETRIES
     || rb->operation & OPERATION_NORETRY));
@@ -137,6 +169,7 @@ uint8_t I2CManagerClass::read(uint8_t address, uint8_t readBuffer[], uint8_t rea
   return I2C_STATUS_OK;
 }
 
+
 /***************************************************************************
  *  Function to queue a request block and initiate operations.
  * 

IODevice.h

@@ -239,7 +239,7 @@ protected:
   // Common object fields.
   VPIN _firstVpin;
   int _nPins;
-  uint8_t _I2CAddress;
+  I2CAddress _I2CAddress;
   // Flag whether the device supports callbacks.
   bool _hasCallback = false;
 
@@ -272,7 +272,7 @@ private:
  
 class PCA9685 : public IODevice {
 public:
-  static void create(VPIN vpin, int nPins, uint8_t I2CAddress);
+  static void create(VPIN vpin, int nPins, I2CAddress i2cAddress);
   enum ProfileType : uint8_t {
     Instant = 0,  // Moves immediately between positions (if duration not specified)
     UseDuration = 0, // Use specified duration
@@ -285,7 +285,7 @@ public:
 
 private:
   // Constructor
-  PCA9685(VPIN vpin, int nPins, uint8_t I2CAddress);
+  PCA9685(VPIN vpin, int nPins, I2CAddress i2cAddress);
   // Device-specific initialisation
   void _begin() override;
   bool _configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, int params[]) override;

IO_GPIOBase.h

@@ -34,7 +34,7 @@ class GPIOBase : public IODevice {
 
 protected:
   // Constructor
-  GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin);
+  GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin);
   // Device-specific initialisation
   void _begin() override;
   // Device-specific pin configuration function.  
@@ -80,11 +80,11 @@ protected:
 
 // Constructor
 template <class T>
-GPIOBase<T>::GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin) :
+GPIOBase<T>::GPIOBase(FSH *deviceName, VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin) :
   IODevice(firstVpin, nPins)
 {
   _deviceName = deviceName;
-  _I2CAddress = I2CAddress;
+  _I2CAddress = i2cAddress;
   _gpioInterruptPin = interruptPin;
   _hasCallback = true;
   // Add device to list of devices.
@@ -110,7 +110,7 @@ void GPIOBase<T>::_begin() {
     _setupDevice();
     _deviceState = DEVSTATE_NORMAL;
   } else {
-    DIAG(F("%S I2C:x%x Device not detected"), _deviceName, _I2CAddress);
+    DIAG(F("%S I2C:x%x Device not detected"), _deviceName, (int)_I2CAddress);
     _deviceState = DEVSTATE_FAILED;
   }
 }
@@ -125,7 +125,7 @@ bool GPIOBase<T>::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCoun
   bool pullup = params[0];
   int pin = vpin - _firstVpin;
   #ifdef DIAG_IO
-  DIAG(F("%S I2C:x%x Config Pin:%d Val:%d"), _deviceName, _I2CAddress, pin, pullup);
+  DIAG(F("%S I2C:x%x Config Pin:%d Val:%d"), _deviceName, (int)_I2CAddress, pin, pullup);
   #endif
   uint16_t mask = 1 << pin;
   if (pullup) 
@@ -155,7 +155,7 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
       _deviceState = DEVSTATE_NORMAL;
     } else {
       _deviceState = DEVSTATE_FAILED;
-      DIAG(F("%S I2C:x%x Error:%d %S"), _deviceName, _I2CAddress, status, 
+      DIAG(F("%S I2C:x%x Error:%d %S"), _deviceName, (int)_I2CAddress, status, 
         I2CManager.getErrorMessage(status));
     }
     _processCompletion(status);
@@ -178,7 +178,7 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
 
     #ifdef DIAG_IO
     if (differences)
-      DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, _I2CAddress, _portInputState);
+      DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, (int)_I2CAddress, _portInputState);
     #endif
   }
 
@@ -199,7 +199,7 @@ void GPIOBase<T>::_loop(unsigned long currentMicros) {
 
 template <class T>
 void GPIOBase<T>::_display() {
-  DIAG(F("%S I2C:x%x Configured on Vpins:%d-%d %S"), _deviceName, _I2CAddress, 
+  DIAG(F("%S I2C:x%x Configured on Vpins:%d-%d %S"), _deviceName, (int)_I2CAddress, 
     _firstVpin, _firstVpin+_nPins-1, (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
 }
 
@@ -208,7 +208,7 @@ void GPIOBase<T>::_write(VPIN vpin, int value) {
   int pin = vpin - _firstVpin;
   T mask = 1 << pin;
   #ifdef DIAG_IO
-  DIAG(F("%S I2C:x%x Write Pin:%d Val:%d"), _deviceName, _I2CAddress, pin, value);
+  DIAG(F("%S I2C:x%x Write Pin:%d Val:%d"), _deviceName, (int)_I2CAddress, pin, value);
   #endif
 
   // Set port mode output if currently not output mode
@@ -244,7 +244,7 @@ int GPIOBase<T>::_read(VPIN vpin) {
   // Set unused pin and write mode pin value to 1
     _portInputState |= ~_portInUse | _portMode;
     #ifdef DIAG_IO
-    DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, _I2CAddress, _portInputState);
+    DIAG(F("%S I2C:x%x PortStates:%x"), _deviceName, (int)_I2CAddress, _portInputState);
     #endif
   }
   return (_portInputState & mask) ? 0 : 1;  // Invert state (5v=0, 0v=1)

IO_MCP23008.h

@@ -25,14 +25,14 @@
 
 class MCP23008 : public GPIOBase<uint8_t> {
 public:
-  static void create(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1) {
+  static void create(VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1) {
-    if (checkNoOverlap(firstVpin, nPins,I2CAddress)) new MCP23008(firstVpin, nPins, I2CAddress, interruptPin);
+    if (checkNoOverlap(firstVpin, nPins,i2cAddress)) new MCP23008(firstVpin, nPins, i2cAddress, interruptPin);
   }
 
 private:
   // Constructor
-  MCP23008(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
+  MCP23008(VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1)
-    : GPIOBase<uint8_t>((FSH *)F("MCP23008"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin) {
+    : GPIOBase<uint8_t>((FSH *)F("MCP23008"), firstVpin, min(nPins, (uint8_t)8), i2cAddress, interruptPin) {
 
     requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
       outputBuffer, sizeof(outputBuffer));

IO_MCP23017.h

@@ -30,14 +30,14 @@
  
 class MCP23017 : public GPIOBase<uint16_t> {
 public:
-  static void create(VPIN vpin, int nPins, uint8_t I2CAddress, int interruptPin=-1) {
+  static void create(VPIN vpin, int nPins, I2CAddress i2cAddress, int interruptPin=-1) {
-    if (checkNoOverlap(vpin, nPins, I2CAddress)) new MCP23017(vpin, min(nPins,16), I2CAddress, interruptPin);
+    if (checkNoOverlap(vpin, nPins, i2cAddress)) new MCP23017(vpin, min(nPins,16), i2cAddress, interruptPin);
   }
 
 private:  
   // Constructor
-  MCP23017(VPIN vpin, int nPins, uint8_t I2CAddress, int interruptPin=-1) 
+  MCP23017(VPIN vpin, int nPins, I2CAddress i2cAddress, int interruptPin=-1) 
-    : GPIOBase<uint16_t>((FSH *)F("MCP23017"), vpin, nPins, I2CAddress, interruptPin) 
+    : GPIOBase<uint16_t>((FSH *)F("MCP23017"), vpin, nPins, i2cAddress, interruptPin) 
   {
     requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
       outputBuffer, sizeof(outputBuffer));

IO_PCA9685.cpp

@@ -38,8 +38,8 @@ static const uint32_t MAX_I2C_SPEED = 1000000L; // PCA9685 rated up to 1MHz I2C
 static void writeRegister(byte address, byte reg, byte value);
 
 // Create device driver instance.
-void PCA9685::create(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
+void PCA9685::create(VPIN firstVpin, int nPins, I2CAddress i2cAddress) {
-  if (checkNoOverlap(firstVpin, nPins,I2CAddress)) new PCA9685(firstVpin, nPins, I2CAddress);
+  if (checkNoOverlap(firstVpin, nPins,i2cAddress)) new PCA9685(firstVpin, nPins, i2cAddress);
 }
 
 // Configure a port on the PCA9685.
@@ -73,10 +73,10 @@ bool PCA9685::_configure(VPIN vpin, ConfigTypeEnum configType, int paramCount, i
 }
 
 // Constructor
-PCA9685::PCA9685(VPIN firstVpin, int nPins, uint8_t I2CAddress) {
+PCA9685::PCA9685(VPIN firstVpin, int nPins, I2CAddress i2cAddress) {
   _firstVpin = firstVpin;
   _nPins = min(nPins, 16);
-  _I2CAddress = I2CAddress;
+  _I2CAddress = i2cAddress;
   // To save RAM, space for servo configuration is not allocated unless a pin is used.
   // Initialise the pointers to NULL.
   for (int i=0; i<_nPins; i++)
@@ -239,13 +239,13 @@ void PCA9685::updatePosition(uint8_t pin) {
 // between 0 and 4095 for the PWM mark-to-period ratio, with 4095 being 100%.
 void PCA9685::writeDevice(uint8_t pin, int value) {
   #ifdef DIAG_IO
-  DIAG(F("PCA9685 I2C:x%x WriteDevice Pin:%d Value:%d"), _I2CAddress, pin, value);
+  DIAG(F("PCA9685 I2C:x%x WriteDevice Pin:%d Value:%d"), (int)_I2CAddress, pin, value);
   #endif
   // Wait for previous request to complete
   uint8_t status = requestBlock.wait();
   if (status != I2C_STATUS_OK) {
     _deviceState = DEVSTATE_FAILED;
-    DIAG(F("PCA9685 I2C:x%x failed %S"), _I2CAddress, I2CManager.getErrorMessage(status));
+    DIAG(F("PCA9685 I2C:x%x failed %S"), (int)_I2CAddress, I2CManager.getErrorMessage(status));
   } else {
     // Set up new request.
     outputBuffer[0] = PCA9685_FIRST_SERVO + 4 * pin;
@@ -259,7 +259,7 @@ void PCA9685::writeDevice(uint8_t pin, int value) {
 
 // Display details of this device.
 void PCA9685::_display() {
-  DIAG(F("PCA9685 I2C:x%x Configured on Vpins:%d-%d %S"), _I2CAddress, (int)_firstVpin, 
+  DIAG(F("PCA9685 I2C:x%x Configured on Vpins:%d-%d %S"), (int)_I2CAddress, (int)_firstVpin, 
     (int)_firstVpin+_nPins-1, (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
 }
 

IO_PCF8574.h

@@ -43,13 +43,13 @@
 
 class PCF8574 : public GPIOBase<uint8_t> {
 public:
-  static void create(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1) {
+  static void create(VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1) {
-    if (checkNoOverlap(firstVpin, nPins,I2CAddress)) new PCF8574(firstVpin, nPins, I2CAddress, interruptPin);
+    if (checkNoOverlap(firstVpin, nPins, i2cAddress)) new PCF8574(firstVpin, nPins, i2cAddress, interruptPin);
   }
 
 private:
-  PCF8574(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
+  PCF8574(VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1)
-    : GPIOBase<uint8_t>((FSH *)F("PCF8574"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin) 
+    : GPIOBase<uint8_t>((FSH *)F("PCF8574"), firstVpin, min(nPins, (uint8_t)8), i2cAddress, interruptPin) 
   {
     requestBlock.setReadParams(_I2CAddress, inputBuffer, 1);
   }

IO_PCF8575.h

@@ -44,13 +44,13 @@
 
 class PCF8575 : public GPIOBase<uint16_t> {
 public:
-  static void create(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1) {
+  static void create(VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1) {
-    if (checkNoOverlap(firstVpin, nPins, I2CAddress)) new PCF8575(firstVpin, min(nPins,(uint8_t)16), I2CAddress, interruptPin);
+    if (checkNoOverlap(firstVpin, nPins, i2cAddress)) new PCF8575(firstVpin, min(nPins,(uint8_t)16), i2cAddress, interruptPin);
   }
 
 private:
-  PCF8575(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
+  PCF8575(VPIN firstVpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1)
-    : GPIOBase<uint16_t>((FSH *)F("PCF8575"), firstVpin, nPins, I2CAddress, interruptPin)
+    : GPIOBase<uint16_t>((FSH *)F("PCF8575"), firstVpin, nPins, i2cAddress, interruptPin)
   {
     requestBlock.setReadParams(_I2CAddress, inputBuffer, sizeof(inputBuffer));
   }

IO_VL53L0X.h

@@ -97,7 +97,6 @@
 
 class VL53L0X : public IODevice {
 private: 
-  uint8_t _i2cAddress;
   uint16_t _ambient;
   uint16_t _distance;
   uint16_t _signal;
@@ -145,7 +144,7 @@ protected:
   VL53L0X(VPIN firstVpin, int nPins, uint8_t i2cAddress, uint16_t onThreshold, uint16_t offThreshold, VPIN xshutPin = VPIN_NONE) {
     _firstVpin = firstVpin;
     _nPins = min(nPins, 3);
-    _i2cAddress = i2cAddress;
+    _I2CAddress = i2cAddress;
     _onThreshold = onThreshold;
     _offThreshold = offThreshold;
     _xshutPin = xshutPin;
@@ -157,7 +156,7 @@ protected:
     //  the device will not respond on its default address if it has 
     //  already been changed.  Therefore, we skip the address configuration if the 
     //  desired address is already responding on the I2C bus.
-    if (_xshutPin == VPIN_NONE && I2CManager.exists(_i2cAddress)) {
+    if (_xshutPin == VPIN_NONE && I2CManager.exists(_I2CAddress)) {
       // Device already present on this address, so skip the address initialisation.
       _nextState = STATE_CONFIGUREDEVICE;
     }
@@ -194,7 +193,7 @@ protected:
       case STATE_CONFIGUREADDRESS:
         // Then write the desired I2C address to the device, while this is the only
         //  module responding to the default address.
-        I2CManager.write(VL53L0X_I2C_DEFAULT_ADDRESS, 2, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, _i2cAddress);
+        I2CManager.write(VL53L0X_I2C_DEFAULT_ADDRESS, 2, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS, _I2CAddress);
         _addressConfigInProgress = false;
         _nextState = STATE_SKIP;
         break;
@@ -204,7 +203,7 @@ protected:
         break;
       case STATE_CONFIGUREDEVICE:
         // On next entry, check if device address has been set.
-        if (I2CManager.exists(_i2cAddress)) {
+        if (I2CManager.exists(_I2CAddress)) {
           #ifdef DIAG_IO
           _display();
           #endif
@@ -213,7 +212,7 @@ protected:
             read_reg(VL53L0X_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01);
           _nextState = STATE_INITIATESCAN;
         } else {
-          DIAG(F("VL53L0X I2C:x%x device not responding"), _i2cAddress);
+          DIAG(F("VL53L0X I2C:x%x device not responding"), (int)_I2CAddress);
           _deviceState = DEVSTATE_FAILED;
           _nextState = STATE_FAILED;
         }
@@ -222,7 +221,7 @@ protected:
         // Not scanning, so initiate a scan
         _outBuffer[0] = VL53L0X_REG_SYSRANGE_START;
         _outBuffer[1] = 0x01;
-        I2CManager.write(_i2cAddress, _outBuffer, 2, &_rb);
+        I2CManager.write(_I2CAddress, _outBuffer, 2, &_rb);
         _nextState = STATE_CHECKSTATUS;
         break;
       case STATE_CHECKSTATUS:
@@ -238,7 +237,7 @@ protected:
       case STATE_GETRESULTS:
         // Ranging completed.  Request results
         _outBuffer[0] = VL53L0X_REG_RESULT_RANGE_STATUS;
-        I2CManager.read(_i2cAddress, _inBuffer, 12, _outBuffer, 1, &_rb);
+        I2CManager.read(_I2CAddress, _inBuffer, 12, _outBuffer, 1, &_rb);
         _nextState = 3;
         delayUntil(currentMicros + 5000); // Allow 5ms to get data
         _nextState = STATE_DECODERESULTS;
@@ -278,7 +277,7 @@ protected:
 
   // Function to report a failed I2C operation.  Put the device off-line.
   void reportError(uint8_t status) {
-    DIAG(F("VL53L0X I2C:x%x Error:%d %S"), _i2cAddress, status, I2CManager.getErrorMessage(status));
+    DIAG(F("VL53L0X I2C:x%x Error:%d %S"), (int)_I2CAddress, status, I2CManager.getErrorMessage(status));
     _deviceState = DEVSTATE_FAILED;
     _value = false;
   }
@@ -308,7 +307,7 @@ protected:
 
   void _display() override {
     DIAG(F("VL53L0X I2C:x%x Configured on Vpins:%d-%d On:%dmm Off:%dmm %S"),
-      _i2cAddress, _firstVpin, _firstVpin+_nPins-1, _onThreshold, _offThreshold,
+      (int)_I2CAddress, _firstVpin, _firstVpin+_nPins-1, _onThreshold, _offThreshold,
       (_deviceState==DEVSTATE_FAILED) ? F("OFFLINE") : F(""));
   }
   
@@ -322,11 +321,11 @@ private:
     uint8_t outBuffer[2];
     outBuffer[0] = reg;
     outBuffer[1] = data;
-    return I2CManager.write(_i2cAddress, outBuffer, 2);
+    return I2CManager.write(_I2CAddress, outBuffer, 2);
   }
   uint8_t read_reg(uint8_t reg) {
     // read byte from register and return value
-    I2CManager.read(_i2cAddress, _inBuffer, 1, &reg, 1);
+    I2CManager.read(_I2CAddress, _inBuffer, 1, &reg, 1);
     return _inBuffer[0];
   }
 };

LiquidCrystal_I2C.cpp

@@ -41,7 +41,7 @@
 // can't assume that its in that state when a sketch starts (and the
 // LiquidCrystal constructor is called).
 
-LiquidCrystal_I2C::LiquidCrystal_I2C(uint8_t lcd_Addr, uint8_t lcd_cols,
+LiquidCrystal_I2C::LiquidCrystal_I2C(I2CAddress lcd_Addr, uint8_t lcd_cols,
                                      uint8_t lcd_rows) {
   _Addr = lcd_Addr;
   lcdRows = lcd_rows;

LiquidCrystal_I2C.h

@@ -64,7 +64,7 @@
 
 class LiquidCrystal_I2C : public LCDDisplay {
 public:
-  LiquidCrystal_I2C(uint8_t lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows);
+  LiquidCrystal_I2C(I2CAddress lcd_Addr,uint8_t lcd_cols,uint8_t lcd_rows);
   void begin();
   void clearNative() override;
   void setRowNative(byte line) override;

SSD1306Ascii.cpp

@@ -144,9 +144,32 @@ const uint8_t FLASH SSD1306AsciiWire::SH1106_132x64init[] = {
 //------------------------------------------------------------------------------
  
 // Constructor
+SSD1306AsciiWire::SSD1306AsciiWire() {}
+
+// CS auto-detect and configure constructor
 SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
+  I2CManager.begin();
+  I2CManager.setClock(400000L);  // Set max supported I2C speed
+
+  // Probe for I2C device on 0x3c and 0x3d.
+  for (uint8_t address = 0x3c; address <= 0x3d; address++) {
+    if (I2CManager.exists(address)) {
+      begin(address, width, height);
+      // Set singleton Address so CS is able to call it.
+      lcdDisplay = this;
+      return;
+    }
+  }
+  DIAG(F("OLED display not found"));
+}
+
+bool SSD1306AsciiWire::begin(I2CAddress address, int width, int height) {
+  if (m_initialised) return true;
+
+  m_i2cAddr = address;
   m_displayWidth = width;
   m_displayHeight = height;
+  
   // Set size in characters in base class
   lcdRows = height / 8;
   lcdCols = width / 6;
@@ -154,35 +177,25 @@ SSD1306AsciiWire::SSD1306AsciiWire(int width, int height) {
   m_row = 0;
   m_colOffset = 0;
 
-  I2CManager.begin();
+  if (m_displayWidth==132 && m_displayHeight==64) {
-  I2CManager.setClock(400000L);  // Set max supported I2C speed
+    // SH1106 display.  This uses 128x64 centered within a 132x64 OLED.
-  for (byte address = 0x3c; address <= 0x3d; address++) {
+    m_colOffset = 2;
-    if (I2CManager.exists(address)) {
+    I2CManager.write_P(m_i2cAddr, SH1106_132x64init, sizeof(SH1106_132x64init));
-      m_i2cAddr = address;
+  } else if (m_displayWidth==128 && (m_displayHeight==64 || m_displayHeight==32)) {
-      if (m_displayWidth==132 && m_displayHeight==64) {
+    // SSD1306 128x64 or 128x32
-        // SH1106 display.  This uses 128x64 centered within a 132x64 OLED.
+    I2CManager.write_P(m_i2cAddr, Adafruit128xXXinit, sizeof(Adafruit128xXXinit));
-        m_colOffset = 2;
+    if (m_displayHeight == 32) 
-        I2CManager.write_P(address, SH1106_132x64init, sizeof(SH1106_132x64init));
+      I2CManager.write(m_i2cAddr, 5, 0, // Set command mode
-      } else if (m_displayWidth==128 && (m_displayHeight==64 || m_displayHeight==32)) {
+        SSD1306_SETMULTIPLEX, 0x1F,     // ratio 32
-        // SSD1306 128x64 or 128x32
+        SSD1306_SETCOMPINS, 0x02);      // sequential COM pins, disable remap
-        I2CManager.write_P(address, Adafruit128xXXinit, sizeof(Adafruit128xXXinit));
+  } else {
-        if (m_displayHeight == 32) 
+    DIAG(F("OLED configuration option not recognised"));
-          I2CManager.write(address, 5, 0, // Set command mode
+    return false;
-            SSD1306_SETMULTIPLEX, 0x1F,     // ratio 32
-            SSD1306_SETCOMPINS, 0x02);      // sequential COM pins, disable remap
-      } else {
-        DIAG(F("OLED configuration option not recognised"));
-        return;
-      }
-      // Device found
-      DIAG(F("%dx%d OLED display configured on I2C:x%x"), width, height, address);
-      // Set singleton address
-      lcdDisplay = this;
-      clear();
-      return;
-    }
   }
-  DIAG(F("OLED display not found"));
+  // Device found
+  DIAG(F("%dx%d OLED display configured on I2C:x%x"), m_displayWidth, m_displayHeight, (uint8_t)m_i2cAddr);
+  clear();
+  return true;
 }
 
 /* Clear screen by writing blank pixels. */

SSD1306Ascii.h

@@ -36,11 +36,12 @@
 class SSD1306AsciiWire : public LCDDisplay {
  public:
 
-  // Constructor
+  // Constructors
-  SSD1306AsciiWire(int width, int height);
+  SSD1306AsciiWire(int width, int height); // Auto-detects I2C address
+  SSD1306AsciiWire();  // Requires call to 'begin()'
 
   // Initialize the display controller.
-  void begin(uint8_t i2cAddr);
+  bool begin(I2CAddress address, int width, int height);
 
   // Clear the display and set the cursor to (0, 0).
   void clearNative() override;
@@ -66,6 +67,8 @@ class SSD1306AsciiWire : public LCDDisplay {
   uint8_t m_colOffset = 0;
   // Current font.
   const uint8_t* const m_font = System5x7;
+  // Flag to prevent calling begin() twice
+  uint8_t m_initialised = false;
 
   // Only fixed size 5x7 fonts in a 6x8 cell are supported.
   static const uint8_t fontWidth = 5;
@@ -74,7 +77,7 @@ class SSD1306AsciiWire : public LCDDisplay {
   static const uint8_t m_fontFirstChar = 0x20;
   static const uint8_t m_fontCharCount = 0x61;
 
-  uint8_t m_i2cAddr;
+  I2CAddress m_i2cAddr;
 
   I2CRB requestBlock;
   uint8_t outputBuffer[fontWidth+letterSpacing+1];