Update IODevice base class to better support filter drivers

Filter drivers provide extra functionality above a hardware driver.  For example, a hardware driver for a PWM module may just set the PWM ratio, but a separate filter driver could animate motors or servos over time, calling the PWM driver to output the pulses.  This would allow the animations to be easily implemented on a different type of PWM module.

IODevice.cpp

@@ -255,20 +255,26 @@ void IODevice::setGPIOInterruptPin(int16_t pinNumber) {
   _gpioInterruptPin = pinNumber;
 }
 
-// Private helper function to add a device to the chain of devices.
-void IODevice::addDevice(IODevice *newDevice) {
-  // Link new object to the end of the chain.  Thereby, the first devices to be declared/created
-  // will be located faster by findDevice than those which are created later.
-  // Ideally declare/create the digital IO pins first, then servos, then more esoteric devices.
-  IODevice *lastDevice;
-  if (_firstDevice == 0)
+// Helper function to add a new device to the device chain.  If 
+// slaveDevice is NULL then the device is added to the end of the chain.
+// Otherwise, the chain is searched for slaveDevice and the new device linked
+// in front of it (to support filter devices that share the same VPIN range
+// as the devices they control).  If slaveDevice isn't found, then the
+// device is linked to the end of the chain.
+void IODevice::addDevice(IODevice *newDevice, IODevice *slaveDevice /* = NULL */) {
+  if (slaveDevice == _firstDevice) {
+    newDevice->_nextDevice = _firstDevice;
     _firstDevice = newDevice;
-  else {
-    for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice)
-      lastDevice = dev;
-    lastDevice->_nextDevice = newDevice;
+  } else {
+    for (IODevice *dev = _firstDevice; dev != 0; dev = dev->_nextDevice) {
+      if (dev->_nextDevice == slaveDevice || dev->_nextDevice == NULL) {
+          // Link new device between dev and slaveDevice (or at end of chain)
+        newDevice->_nextDevice = dev->_nextDevice;
+        dev->_nextDevice = newDevice;
+        break;
+      }
+    }
   }
-  newDevice->_nextDevice = 0;
   newDevice->_begin();
 }
 
@@ -283,6 +289,17 @@ IODevice *IODevice::findDevice(VPIN vpin) {
   return NULL;
 }
 
+// Instance helper function for filter devices (layered over others).  Looks for 
+//  a device that is further down the chain than the current device.
+IODevice *IODevice::findDeviceFollowing(VPIN vpin) {
+  for (IODevice *dev = _nextDevice; dev != 0; dev = dev->_nextDevice) {
+    VPIN firstVpin = dev->_firstVpin;
+    if (vpin >= firstVpin && vpin < firstVpin+dev->_nPins)
+      return dev;
+  }
+  return NULL;
+}
+
 // Private helper function to check for vpin overlap. Run during setup only.
 // returns true if pins DONT overlap with existing device
 bool IODevice::checkNoOverlap(VPIN firstPin, uint8_t nPins, uint8_t i2cAddress) {
@@ -320,7 +337,7 @@ bool IODevice::checkNoOverlap(VPIN firstPin, uint8_t nPins, uint8_t i2cAddress)
 // Chain of callback blocks (identifying registered callback functions for state changes)
 IONotifyCallback *IONotifyCallback::first = 0;
 
-// Start of chain of devices.
+// Start and end of chain of devices.
 IODevice *IODevice::_firstDevice = 0;
 
 // Reference to next device to be called on _loop() method.

IODevice.h

@@ -163,9 +163,35 @@ public:
   // once the GPIO port concerned has been read.
   void setGPIOInterruptPin(int16_t pinNumber);
 
- // Method to check if pins will overlap before creating new device. 
+  // Method to check if pins will overlap before creating new device. 
   static bool checkNoOverlap(VPIN firstPin, uint8_t nPins=1, uint8_t i2cAddress=0);
-  
+
+  // Method used by IODevice filters to locate slave pins that may be overlayed by their own
+  // pin range.  
+  IODevice *findDeviceFollowing(VPIN vpin);
+
+  // Method to write new state (optionally implemented within device class)
+  virtual void _write(VPIN vpin, int value) {
+    (void)vpin; (void)value;
+  };
+
+  // Method to write an 'analogue' value (optionally implemented within device class)
+  virtual void _writeAnalogue(VPIN vpin, int value, uint8_t param1=0, uint16_t param2=0) {
+    (void)vpin; (void)value; (void) param1; (void)param2;
+  };
+
+  // Method to read digital pin state (optionally implemented within device class)
+  virtual int _read(VPIN vpin) { 
+    (void)vpin; 
+    return 0;
+  };
+
+  // Method to read analogue pin state (optionally implemented within device class)
+  virtual int _readAnalogue(VPIN vpin) { 
+    (void)vpin; 
+    return 0;
+  };
+
 protected:
   
   // Constructor
@@ -188,27 +214,6 @@ protected:
     return false;
   };
 
-  // Method to write new state (optionally implemented within device class)
-  virtual void _write(VPIN vpin, int value) {
-    (void)vpin; (void)value;
-  };
-
-  // Method to write an 'analogue' value (optionally implemented within device class)
-  virtual void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) {
-    (void)vpin; (void)value; (void) param1; (void)param2;
-  };
-
-  // Method to read digital pin state (optionally implemented within device class)
-  virtual int _read(VPIN vpin) { 
-    (void)vpin; 
-    return 0;
-  };
-
-  // Method to read analogue pin state (optionally implemented within device class)
-  virtual int _readAnalogue(VPIN vpin) { 
-    (void)vpin; 
-    return 0;
-  };
   virtual int _configureAnalogIn(VPIN vpin) { 
     (void)vpin; 
     return 0;
@@ -242,7 +247,7 @@ protected:
   int16_t _gpioInterruptPin = -1;
     
   // Static support function for subclass creation
-  static void addDevice(IODevice *newDevice);
+  static void addDevice(IODevice *newDevice, IODevice *slaveDevice = NULL);
 
   // Method to find device handling Vpin
   static IODevice *findDevice(VPIN vpin);